diff options
Diffstat (limited to '60xx/libsensors_iio/software/core/mllite/storage_manager.c')
| -rw-r--r-- | 60xx/libsensors_iio/software/core/mllite/storage_manager.c | 204 |
1 files changed, 204 insertions, 0 deletions
diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c new file mode 100644 index 0000000..3e4e619 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.c @@ -0,0 +1,204 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup Storage_Manager storage_manager + * @brief Motion Library - Stores Data for functions. + * + * + * @{ + * @file storage_manager.c + * @brief Load and Store Manager. + */ + +#include <string.h> + +#include "storage_manager.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" + +/* Must be changed if the format of storage changes */ +#define DEFAULT_KEY 29681 + +typedef inv_error_t (*load_func_t)(const unsigned char *data); +typedef inv_error_t (*save_func_t)(unsigned char *data); +/** Max number of entites that can be stored */ +#define NUM_STORAGE_BOXES 20 + +struct data_header_t { + long size; + uint32_t checksum; + unsigned int key; +}; + +struct data_storage_t { + int num; /**< Number of differnt save entities */ + size_t total_size; /**< Size in bytes to store non volatile data */ + load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ + save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ + struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ +}; +static struct data_storage_t ds; + +/** Should be called once before using any of the storage methods. Typically +* called first by inv_init_mpl().*/ +void inv_init_storage_manager() +{ + memset(&ds, 0, sizeof(ds)); + ds.total_size = sizeof(struct data_header_t); +} + +/** Used to register your mechanism to load and store non-volative data. This should typical be +* called during the enable function for your feature. +* @param[in] load_func function pointer you will use to receive data that was stored for you. +* @param[in] save_func function pointer you will use to save any data you want saved to +* non-volatile memory between runs. +* @param[in] size The size in bytes of the amount of data you want loaded and saved. +* @param[in] key The key associated with your data type should be unique across MPL. +* The key should change when your type of data for storage changes. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) +{ + int kk; + // Check if this has been registered already + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return INV_ERROR_INVALID_PARAMETER; + } + } + // Make sure there is room + if (ds.num >= NUM_STORAGE_BOXES) { + return INV_ERROR_INVALID_PARAMETER; + } + // Add to list + ds.hd[ds.num].key = key; + ds.hd[ds.num].size = size; + ds.load[ds.num] = load_func; + ds.save[ds.num] = save_func; + ds.total_size += size + sizeof(struct data_header_t); + ds.num++; + + return INV_SUCCESS; +} + +/** Returns the memory size needed to perform a store +* @param[out] size Size in bytes of memory needed to store. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_get_mpl_state_size(size_t *size) +{ + *size = ds.total_size; + return INV_SUCCESS; +} + +/** @internal + * Finds key in ds.hd[] array and returns location + * @return location where key exists in array, -1 if not found. + */ +static int inv_find_entry(unsigned int key) +{ + int kk; + for (kk=0; kk<ds.num; ++kk) { + if (key == ds.hd[kk].key) { + return kk; + } + } + return -1; +} + +/** This function takes a block of data that has been saved in non-volatile memory and pushes +* to the proper locations. Multiple error checks are performed on the data. +* @param[in] data Data that was saved to be loaded up by MPL +* @param[in] length Length of data vector in bytes +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) +{ + struct data_header_t *hd; + int entry; + uint32_t checksum; + long len; + + len = length; // Important so we get negative numbers + if (len < sizeof(struct data_header_t)) + return INV_ERROR_CALIBRATION_LOAD; // No data + hd = (struct data_header_t *)data; + if (hd->key != DEFAULT_KEY) + return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption + len = MIN(hd->size, len); + len = hd->size; + len -= sizeof(struct data_header_t); + data += sizeof(struct data_header_t); + checksum = inv_checksum(data, len); + if (checksum != hd->checksum) + return INV_ERROR_CALIBRATION_LOAD; // Data corruption + + while (len > (long)sizeof(struct data_header_t)) { + hd = (struct data_header_t *)data; + entry = inv_find_entry(hd->key); + data += sizeof(struct data_header_t); + len -= sizeof(struct data_header_t); + if (entry >= 0 && len >= hd->size) { + if (hd->size == ds.hd[entry].size) { + checksum = inv_checksum(data, hd->size); + if (checksum == hd->checksum) { + ds.load[entry](data); + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + } + } + len -= hd->size; + if (len >= 0) + data = data + hd->size; + } + + return INV_SUCCESS; +} + +/** This function fills up a block of memory to be stored in non-volatile memory. +* @param[out] data Place to store data, size of sz, must be at least size +* returned by inv_get_mpl_state_size() +* @param[in] sz Size of data. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) +{ + unsigned char *cur; + int kk; + struct data_header_t *hd; + + if (sz >= ds.total_size) { + cur = data + sizeof(struct data_header_t); + for (kk = 0; kk < ds.num; ++kk) { + hd = (struct data_header_t *)cur; + cur += sizeof(struct data_header_t); + ds.save[kk](cur); + hd->checksum = inv_checksum(cur, ds.hd[kk].size); + hd->size = ds.hd[kk].size; + hd->key = ds.hd[kk].key; + cur += ds.hd[kk].size; + } + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + + hd = (struct data_header_t *)data; + hd->checksum = inv_checksum(data + sizeof(struct data_header_t), + ds.total_size - sizeof(struct data_header_t)); + hd->key = DEFAULT_KEY; + hd->size = ds.total_size; + + return INV_SUCCESS; +} + +/** + * @} + */ |