1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
|
/*
$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 "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;
}
/**
* @}
*/
|
