1 /* 2 $License: 3 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 See included License.txt for License information. 5 $ 6 */ 7 8 /** 9 * @defgroup Storage_Manager storage_manager 10 * @brief Motion Library - Stores Data for functions. 11 * 12 * 13 * @{ 14 * @file storage_manager.c 15 * @brief Load and Store Manager. 16 */ 17 #undef MPL_LOG_NDEBUG 18 #define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ 19 #undef MPL_LOG_TAG 20 #define MPL_LOG_TAG "MLLITE" 21 22 #include <string.h> 23 24 #include "storage_manager.h" 25 #include "log.h" 26 #include "ml_math_func.h" 27 #include "mlmath.h" 28 29 /* Must be changed if the format of storage changes */ 30 #define DEFAULT_KEY 29681 31 32 typedef inv_error_t (*load_func_t)(const unsigned char *data); 33 typedef inv_error_t (*save_func_t)(unsigned char *data); 34 /** Max number of entites that can be stored */ 35 #define NUM_STORAGE_BOXES 20 36 37 struct data_header_t { 38 long size; 39 uint32_t checksum; 40 unsigned int key; 41 }; 42 43 struct data_storage_t { 44 int num; /**< Number of differnt save entities */ 45 size_t total_size; /**< Size in bytes to store non volatile data */ 46 load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ 47 save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ 48 struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ 49 }; 50 static struct data_storage_t ds; 51 52 /** Should be called once before using any of the storage methods. Typically 53 * called first by inv_init_mpl().*/ 54 void inv_init_storage_manager() 55 { 56 memset(&ds, 0, sizeof(ds)); 57 ds.total_size = sizeof(struct data_header_t); 58 } 59 60 /** Used to register your mechanism to load and store non-volative data. This should typical be 61 * called during the enable function for your feature. 62 * @param[in] load_func function pointer you will use to receive data that was stored for you. 63 * @param[in] save_func function pointer you will use to save any data you want saved to 64 * non-volatile memory between runs. 65 * @param[in] size The size in bytes of the amount of data you want loaded and saved. 66 * @param[in] key The key associated with your data type should be unique across MPL. 67 * The key should change when your type of data for storage changes. 68 * @return Returns INV_SUCCESS if successful or an error code if not. 69 */ 70 inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), 71 inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) 72 { 73 int kk; 74 // Check if this has been registered already 75 for (kk=0; kk<ds.num; ++kk) { 76 if (key == ds.hd[kk].key) { 77 return INV_ERROR_INVALID_PARAMETER; 78 } 79 } 80 // Make sure there is room 81 if (ds.num >= NUM_STORAGE_BOXES) { 82 return INV_ERROR_INVALID_PARAMETER; 83 } 84 // Add to list 85 ds.hd[ds.num].key = key; 86 ds.hd[ds.num].size = size; 87 ds.load[ds.num] = load_func; 88 ds.save[ds.num] = save_func; 89 ds.total_size += size + sizeof(struct data_header_t); 90 ds.num++; 91 92 return INV_SUCCESS; 93 } 94 95 /** Returns the memory size needed to perform a store 96 * @param[out] size Size in bytes of memory needed to store. 97 * @return Returns INV_SUCCESS if successful or an error code if not. 98 */ 99 inv_error_t inv_get_mpl_state_size(size_t *size) 100 { 101 *size = ds.total_size; 102 return INV_SUCCESS; 103 } 104 105 /** @internal 106 * Finds key in ds.hd[] array and returns location 107 * @return location where key exists in array, -1 if not found. 108 */ 109 static int inv_find_entry(unsigned int key) 110 { 111 int kk; 112 for (kk=0; kk<ds.num; ++kk) { 113 if (key == ds.hd[kk].key) { 114 return kk; 115 } 116 } 117 return -1; 118 } 119 120 /** This function takes a block of data that has been saved in non-volatile memory and pushes 121 * to the proper locations. Multiple error checks are performed on the data. 122 * @param[in] data Data that was saved to be loaded up by MPL 123 * @param[in] length Length of data vector in bytes 124 * @return Returns INV_SUCCESS if successful or an error code if not. 125 */ 126 inv_error_t inv_load_mpl_states(const unsigned char *data, size_t length) 127 { 128 struct data_header_t *hd; 129 int entry; 130 uint32_t checksum; 131 long len; 132 133 len = length; // Important so we get negative numbers 134 if (data == NULL || len == 0) 135 return INV_SUCCESS; 136 if (len < sizeof(struct data_header_t)) 137 return INV_ERROR_CALIBRATION_LOAD; // No data 138 hd = (struct data_header_t *)data; 139 if (hd->key != DEFAULT_KEY) 140 return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption 141 len = MIN(hd->size, len); 142 len = hd->size; 143 len -= sizeof(struct data_header_t); 144 data += sizeof(struct data_header_t); 145 checksum = inv_checksum(data, len); 146 if (checksum != hd->checksum) 147 return INV_ERROR_CALIBRATION_LOAD; // Data corruption 148 149 while (len > (long)sizeof(struct data_header_t)) { 150 hd = (struct data_header_t *)data; 151 entry = inv_find_entry(hd->key); 152 data += sizeof(struct data_header_t); 153 len -= sizeof(struct data_header_t); 154 if (entry >= 0 && len >= hd->size) { 155 if (hd->size != ds.hd[entry].size) 156 return INV_ERROR_CALIBRATION_LEN; 157 checksum = inv_checksum(data, hd->size); 158 if (checksum != hd->checksum) 159 return INV_ERROR_CALIBRATION_LOAD; 160 ds.load[entry](data); 161 } 162 len -= hd->size; 163 if (len >= 0) 164 data = data + hd->size; 165 } 166 167 return INV_SUCCESS; 168 } 169 170 /** This function fills up a block of memory to be stored in non-volatile memory. 171 * @param[out] data Place to store data, size of sz, must be at least size 172 * returned by inv_get_mpl_state_size() 173 * @param[in] sz Size of data. 174 * @return Returns INV_SUCCESS if successful or an error code if not. 175 */ 176 inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) 177 { 178 unsigned char *cur; 179 int kk; 180 struct data_header_t *hd; 181 182 if (data == NULL || sz == 0) 183 return INV_ERROR_CALIBRATION_LOAD; 184 if (sz >= ds.total_size) { 185 cur = data + sizeof(struct data_header_t); 186 for (kk = 0; kk < ds.num; ++kk) { 187 hd = (struct data_header_t *)cur; 188 cur += sizeof(struct data_header_t); 189 ds.save[kk](cur); 190 hd->checksum = inv_checksum(cur, ds.hd[kk].size); 191 hd->size = ds.hd[kk].size; 192 hd->key = ds.hd[kk].key; 193 cur += ds.hd[kk].size; 194 } 195 } else { 196 return INV_ERROR_CALIBRATION_LOAD; 197 } 198 199 hd = (struct data_header_t *)data; 200 hd->checksum = inv_checksum(data + sizeof(struct data_header_t), 201 ds.total_size - sizeof(struct data_header_t)); 202 hd->key = DEFAULT_KEY; 203 hd->size = ds.total_size; 204 205 return INV_SUCCESS; 206 } 207 208 /** 209 * @} 210 */ 211
