17494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* 27494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $License: 37494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 47494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall See included License.txt for License information. 57494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall $ 67494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 733ce91b37062fa63af192f5643de93f3beebe854JP Abgrall 87494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 97494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @defgroup Storage_Manager storage_manager 107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Motion Library - Stores Data for functions. 117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * 137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @{ 147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @file storage_manager.c 157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @brief Load and Store Manager. 167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "storage_manager.h" 187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "log.h" 197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "ml_math_func.h" 207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#include "mlmath.h" 217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/* Must be changed if the format of storage changes */ 237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define DEFAULT_KEY 29681 247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralltypedef inv_error_t (*load_func_t)(const unsigned char *data); 267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgralltypedef inv_error_t (*save_func_t)(unsigned char *data); 277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Max number of entites that can be stored */ 287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall#define NUM_STORAGE_BOXES 20 297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstruct data_header_t { 317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long size; 327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall uint32_t checksum; 337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned int key; 347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall}; 357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstruct data_storage_t { 377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int num; /**< Number of differnt save entities */ 387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall size_t total_size; /**< Size in bytes to store non volatile data */ 397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ 407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ 417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ 427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall}; 437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic struct data_storage_t ds; 447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Should be called once before using any of the storage methods. Typically 467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* called first by inv_init_mpl().*/ 477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallvoid inv_init_storage_manager() 487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall memset(&ds, 0, sizeof(ds)); 507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.total_size = sizeof(struct data_header_t); 517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Used to register your mechanism to load and store non-volative data. This should typical be 547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* called during the enable function for your feature. 557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] load_func function pointer you will use to receive data that was stored for you. 567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] save_func function pointer you will use to save any data you want saved to 577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* non-volatile memory between runs. 587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] size The size in bytes of the amount of data you want loaded and saved. 597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] key The key associated with your data type should be unique across MPL. 607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* The key should change when your type of data for storage changes. 617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), 647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) 657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk; 677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Check if this has been registered already 687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk=0; kk<ds.num; ++kk) { 697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (key == ds.hd[kk].key) { 707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_INVALID_PARAMETER; 717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Make sure there is room 747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (ds.num >= NUM_STORAGE_BOXES) { 757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_INVALID_PARAMETER; 767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall // Add to list 787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.hd[ds.num].key = key; 797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.hd[ds.num].size = size; 807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.load[ds.num] = load_func; 817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.save[ds.num] = save_func; 827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.total_size += size + sizeof(struct data_header_t); 837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.num++; 847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** Returns the memory size needed to perform a store 897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] size Size in bytes of memory needed to store. 907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_get_mpl_state_size(size_t *size) 937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall *size = ds.total_size; 957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** @internal 997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * Finds key in ds.hd[] array and returns location 1007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @return location where key exists in array, -1 if not found. 1017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 1027494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallstatic int inv_find_entry(unsigned int key) 1037494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1047494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk; 1057494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk=0; kk<ds.num; ++kk) { 1067494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (key == ds.hd[kk].key) { 1077494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return kk; 1087494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1097494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1107494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return -1; 1117494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1127494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1137494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This function takes a block of data that has been saved in non-volatile memory and pushes 1147494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* to the proper locations. Multiple error checks are performed on the data. 1157494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] data Data that was saved to be loaded up by MPL 1167494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] length Length of data vector in bytes 1177494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 1187494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1197494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) 1207494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1217494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall struct data_header_t *hd; 1227494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int entry; 1237494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall uint32_t checksum; 1247494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall long len; 1257494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1267494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len = length; // Important so we get negative numbers 1277494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (len < sizeof(struct data_header_t)) 1287494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_CALIBRATION_LOAD; // No data 1297494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd = (struct data_header_t *)data; 1307494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (hd->key != DEFAULT_KEY) 1317494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption 1327494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len = MIN(hd->size, len); 1337494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len = hd->size; 1347494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len -= sizeof(struct data_header_t); 1357494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall data += sizeof(struct data_header_t); 1367494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall checksum = inv_checksum(data, len); 1377494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (checksum != hd->checksum) 1387494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_CALIBRATION_LOAD; // Data corruption 1397494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1407494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall while (len > (long)sizeof(struct data_header_t)) { 1417494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd = (struct data_header_t *)data; 1427494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall entry = inv_find_entry(hd->key); 1437494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall data += sizeof(struct data_header_t); 1447494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len -= sizeof(struct data_header_t); 1457494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (entry >= 0 && len >= hd->size) { 1467494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (hd->size == ds.hd[entry].size) { 1477494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall checksum = inv_checksum(data, hd->size); 1487494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (checksum == hd->checksum) { 1497494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.load[entry](data); 1507494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 1517494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_CALIBRATION_LOAD; 1527494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1537494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1547494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1557494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall len -= hd->size; 1567494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (len >= 0) 1577494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall data = data + hd->size; 1587494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1597494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1607494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 1617494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1627494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1637494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** This function fills up a block of memory to be stored in non-volatile memory. 1647494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[out] data Place to store data, size of sz, must be at least size 1657494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* returned by inv_get_mpl_state_size() 1667494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @param[in] sz Size of data. 1677494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall* @return Returns INV_SUCCESS if successful or an error code if not. 1687494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall*/ 1697494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrallinv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) 1707494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall{ 1717494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall unsigned char *cur; 1727494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall int kk; 1737494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall struct data_header_t *hd; 1747494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1757494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall if (sz >= ds.total_size) { 1767494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cur = data + sizeof(struct data_header_t); 1777494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall for (kk = 0; kk < ds.num; ++kk) { 1787494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd = (struct data_header_t *)cur; 1797494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cur += sizeof(struct data_header_t); 1807494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.save[kk](cur); 1817494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->checksum = inv_checksum(cur, ds.hd[kk].size); 1827494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->size = ds.hd[kk].size; 1837494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->key = ds.hd[kk].key; 1847494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall cur += ds.hd[kk].size; 1857494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1867494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } else { 1877494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_ERROR_CALIBRATION_LOAD; 1887494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall } 1897494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1907494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd = (struct data_header_t *)data; 1917494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->checksum = inv_checksum(data + sizeof(struct data_header_t), 1927494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall ds.total_size - sizeof(struct data_header_t)); 1937494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->key = DEFAULT_KEY; 1947494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall hd->size = ds.total_size; 1957494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1967494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall return INV_SUCCESS; 1977494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall} 1987494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall 1997494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall/** 2007494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall * @} 2017494581689b0fc1d8addd016b1c92d74d01f5ad4JP Abgrall */ 202