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
32typedef inv_error_t (*load_func_t)(const unsigned char *data);
33typedef 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
37struct data_header_t {
38    long size;
39    uint32_t checksum;
40    unsigned int key;
41};
42
43struct 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};
50static 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().*/
54void 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*/
70inv_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*/
99inv_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 */
109static 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*/
126inv_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*/
176inv_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