149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* 249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $License: 349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow See included License.txt for License information. 549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow $ 649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @defgroup Storage_Manager storage_manager 1049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Motion Library - Stores Data for functions. 1149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * 1349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @{ 1449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @file storage_manager.c 1549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @brief Load and Store Manager. 1649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 1749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#undef MPL_LOG_NDEBUG 1849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ 1949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#undef MPL_LOG_TAG 2049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define MPL_LOG_TAG "MLLITE" 2149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include <string.h> 2349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "storage_manager.h" 2549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "log.h" 2649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "ml_math_func.h" 2749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#include "mlmath.h" 2849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 2949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/* Must be changed if the format of storage changes */ 3049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define DEFAULT_KEY 29681 3149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowtypedef inv_error_t (*load_func_t)(const unsigned char *data); 3349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowtypedef inv_error_t (*save_func_t)(unsigned char *data); 3449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Max number of entites that can be stored */ 3549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow#define NUM_STORAGE_BOXES 20 3649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 3749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct data_header_t { 3849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long size; 3949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t checksum; 4049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned int key; 4149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 4249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 4349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstruct data_storage_t { 4449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int num; /**< Number of differnt save entities */ 4549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow size_t total_size; /**< Size in bytes to store non volatile data */ 4649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ 4749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ 4849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ 4949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow}; 5049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic struct data_storage_t ds; 5149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 5249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Should be called once before using any of the storage methods. Typically 5349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* called first by inv_init_mpl().*/ 5449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowvoid inv_init_storage_manager() 5549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 5649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow memset(&ds, 0, sizeof(ds)); 5749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.total_size = sizeof(struct data_header_t); 5849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 5949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 6049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Used to register your mechanism to load and store non-volative data. This should typical be 6149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* called during the enable function for your feature. 6249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] load_func function pointer you will use to receive data that was stored for you. 6349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] save_func function pointer you will use to save any data you want saved to 6449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* non-volatile memory between runs. 6549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] size The size in bytes of the amount of data you want loaded and saved. 6649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] key The key associated with your data type should be unique across MPL. 6749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* The key should change when your type of data for storage changes. 6849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 6949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 7049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), 7149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) 7249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 7349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk; 7449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Check if this has been registered already 7549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk=0; kk<ds.num; ++kk) { 7649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (key == ds.hd[kk].key) { 7749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 7849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 7949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 8049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Make sure there is room 8149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (ds.num >= NUM_STORAGE_BOXES) { 8249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_INVALID_PARAMETER; 8349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 8449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow // Add to list 8549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.hd[ds.num].key = key; 8649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.hd[ds.num].size = size; 8749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.load[ds.num] = load_func; 8849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.save[ds.num] = save_func; 8949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.total_size += size + sizeof(struct data_header_t); 9049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.num++; 9149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 9349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 9449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 9549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** Returns the memory size needed to perform a store 9649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] size Size in bytes of memory needed to store. 9749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 9849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 9949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_get_mpl_state_size(size_t *size) 10049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 10149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow *size = ds.total_size; 10249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 10349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 10449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 10549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** @internal 10649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * Finds key in ds.hd[] array and returns location 10749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @return location where key exists in array, -1 if not found. 10849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 10949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowstatic int inv_find_entry(unsigned int key) 11049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 11149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk; 11249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk=0; kk<ds.num; ++kk) { 11349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (key == ds.hd[kk].key) { 11449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return kk; 11549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 11649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 11749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return -1; 11849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 11949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 12049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function takes a block of data that has been saved in non-volatile memory and pushes 12149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* to the proper locations. Multiple error checks are performed on the data. 12249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] data Data that was saved to be loaded up by MPL 12349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] length Length of data vector in bytes 12449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 12549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 12649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) 12749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 12849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct data_header_t *hd; 12949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int entry; 13049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow uint32_t checksum; 13149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow long len; 13249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 13349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len = length; // Important so we get negative numbers 13449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data == NULL || len == 0) 13549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 13649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (len < sizeof(struct data_header_t)) 13749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; // No data 13849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd = (struct data_header_t *)data; 13949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hd->key != DEFAULT_KEY) 14049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption 14149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len = MIN(hd->size, len); 14249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len = hd->size; 14349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len -= sizeof(struct data_header_t); 14449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data += sizeof(struct data_header_t); 14549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow checksum = inv_checksum(data, len); 14649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (checksum != hd->checksum) 14749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; // Data corruption 14849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 14949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow while (len > (long)sizeof(struct data_header_t)) { 15049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd = (struct data_header_t *)data; 15149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow entry = inv_find_entry(hd->key); 15249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data += sizeof(struct data_header_t); 15349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len -= sizeof(struct data_header_t); 15449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (entry >= 0 && len >= hd->size) { 15549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (hd->size != ds.hd[entry].size) 15649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LEN; 15749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow checksum = inv_checksum(data, hd->size); 15849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (checksum != hd->checksum) 15949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; 16049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.load[entry](data); 16149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 16249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow len -= hd->size; 16349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (len >= 0) 16449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow data = data + hd->size; 16549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 16649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 16749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 16849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 16949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 17049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** This function fills up a block of memory to be stored in non-volatile memory. 17149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[out] data Place to store data, size of sz, must be at least size 17249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* returned by inv_get_mpl_state_size() 17349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @param[in] sz Size of data. 17449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow* @return Returns INV_SUCCESS if successful or an error code if not. 17549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow*/ 17649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chowinv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) 17749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow{ 17849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow unsigned char *cur; 17949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow int kk; 18049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow struct data_header_t *hd; 18149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 18249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (data == NULL || sz == 0) 18349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; 18449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow if (sz >= ds.total_size) { 18549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cur = data + sizeof(struct data_header_t); 18649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow for (kk = 0; kk < ds.num; ++kk) { 18749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd = (struct data_header_t *)cur; 18849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cur += sizeof(struct data_header_t); 18949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.save[kk](cur); 19049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->checksum = inv_checksum(cur, ds.hd[kk].size); 19149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->size = ds.hd[kk].size; 19249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->key = ds.hd[kk].key; 19349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow cur += ds.hd[kk].size; 19449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 19549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } else { 19649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_ERROR_CALIBRATION_LOAD; 19749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow } 19849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 19949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd = (struct data_header_t *)data; 20049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->checksum = inv_checksum(data + sizeof(struct data_header_t), 20149ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow ds.total_size - sizeof(struct data_header_t)); 20249ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->key = DEFAULT_KEY; 20349ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow hd->size = ds.total_size; 20449ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20549ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow return INV_SUCCESS; 20649ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow} 20749ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow 20849ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow/** 20949ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow * @} 21049ea3e26ca3c6a779e527a0322e49a663333350aRosa Chow */ 211