1/*
2 $License:
3    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
4 $
5 */
6
7/******************************************************************************
8 *
9 * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $
10 *
11 *****************************************************************************/
12
13/**
14 * @defgroup ML_STORED_DATA
15 *
16 * @{
17 *      @file     ml_stored_data.c
18 *      @brief    functions for reading and writing stored data sets.
19 *                Typically, these functions process stored calibration data.
20 */
21
22#undef MPL_LOG_NDEBUG
23#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
24#undef MPL_LOG_TAG
25
26#include <stdio.h>
27
28#include "log.h"
29#undef MPL_LOG_TAG
30#define MPL_LOG_TAG "MPL-storeload"
31
32#include "ml_stored_data.h"
33#include "storage_manager.h"
34#include "mlos.h"
35
36#define LOADCAL_DEBUG    0
37#define STORECAL_DEBUG   0
38
39#define DEFAULT_KEY 29681
40
41#define STORECAL_LOG MPL_LOGI
42#define LOADCAL_LOG  MPL_LOGI
43
44inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
45{
46    FILE *fp;
47    inv_error_t result = INV_SUCCESS;
48    size_t fsize;
49
50    fp = fopen(MLCAL_FILE,"rb");
51    if (fp == NULL) {
52        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
53        return INV_ERROR_FILE_OPEN;
54    }
55
56    // obtain file size
57    fseek (fp, 0 , SEEK_END);
58    fsize = ftell (fp);
59    rewind (fp);
60
61    *calData = (unsigned char *)inv_malloc(fsize);
62    if (*calData==NULL) {
63        MPL_LOGE("Could not allocate buffer of %d bytes - "
64                 "aborting\n", fsize);
65        fclose(fp);
66        return INV_ERROR_MEMORY_EXAUSTED;
67    }
68
69    *bytesRead = fread(*calData, 1, fsize, fp);
70    if (*bytesRead != fsize) {
71        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
72                 *bytesRead, fsize);
73        result = INV_ERROR_FILE_READ;
74        goto read_cal_end;
75    }
76    else {
77        MPL_LOGI("Bytes read = %d", *bytesRead);
78    }
79
80read_cal_end:
81    fclose(fp);
82    return result;
83}
84
85inv_error_t inv_write_cal(unsigned char *cal, size_t len)
86{
87    FILE *fp;
88    int bytesWritten;
89    inv_error_t result = INV_SUCCESS;
90
91    if (len <= 0) {
92        MPL_LOGE("Nothing to write");
93        return INV_ERROR_FILE_WRITE;
94    }
95    else {
96        MPL_LOGI("cal data size to write = %d", len);
97    }
98    fp = fopen(MLCAL_FILE,"wb");
99    if (fp == NULL) {
100        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
101        return INV_ERROR_FILE_OPEN;
102    }
103    bytesWritten = fwrite(cal, 1, len, fp);
104    if (bytesWritten != len) {
105        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
106                 bytesWritten, len);
107        result = INV_ERROR_FILE_WRITE;
108    }
109    else {
110        MPL_LOGI("Bytes written = %d", bytesWritten);
111    }
112    fclose(fp);
113    return result;
114}
115
116/**
117 *  @brief  Loads a type 0 set of calibration data.
118 *          It parses a binary data set containing calibration data.
119 *          The binary data set is intended to be loaded from a file.
120 *          This calibrations data format stores values for (in order of
121 *          appearance) :
122 *          - temperature compensation : temperature data points,
123 *          - temperature compensation : gyro biases data points for X, Y,
124 *              and Z axes.
125 *          - accel biases for X, Y, Z axes.
126 *          This calibration data is produced internally by the MPL and its
127 *          size is 2777 bytes (header and checksum included).
128 *          Calibration format type 1 is currently used for ITG3500
129 *
130 *  @pre    inv_init_storage_manager()
131 *          must have been called.
132 *
133 *  @param  calData
134 *              A pointer to an array of bytes to be parsed.
135 *  @param  len
136 *              the length of the calibration
137 *
138 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
139 */
140inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
141{
142    inv_error_t result;
143
144    LOADCAL_LOG("Entering inv_load_cal_V0\n");
145
146    /*if (len != expLen) {
147        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
148                 expLen, len);
149        return INV_ERROR_FILE_READ;
150    }*/
151
152    result = inv_load_mpl_states(calData, len);
153    return result;
154}
155
156/**
157 *  @brief  Loads a type 1 set of calibration data.
158 *          It parses a binary data set containing calibration data.
159 *          The binary data set is intended to be loaded from a file.
160 *          This calibrations data format stores values for (in order of
161 *          appearance) :
162 *          - temperature,
163 *          - gyro biases for X, Y, Z axes,
164 *          - accel biases for X, Y, Z axes.
165 *          This calibration data would normally be produced by the MPU Self
166 *          Test and its size is 36 bytes (header and checksum included).
167 *          Calibration format type 1 is produced by the MPU Self Test and
168 *          substitutes the type 0 : inv_load_cal_V0().
169 *
170 *  @pre
171 *
172 *  @param  calData
173 *              A pointer to an array of bytes to be parsed.
174 *  @param  len
175 *              the length of the calibration
176 *
177 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
178 */
179inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
180{
181    return INV_SUCCESS;
182}
183
184/**
185 * @brief   Loads a set of calibration data.
186 *          It parses a binary data set containing calibration data.
187 *          The binary data set is intended to be loaded from a file.
188 *
189 * @pre
190 *
191 *
192 * @param   calData
193 *              A pointer to an array of bytes to be parsed.
194 *
195 * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
196 */
197inv_error_t inv_load_cal(unsigned char *calData)
198{
199    int calType = 0;
200    int len = 0;
201    //int ptr;
202    //uint32_t chk = 0;
203    //uint32_t cmp_chk = 0;
204
205    /*load_func_t loaders[] = {
206        inv_load_cal_V0,
207        inv_load_cal_V1,
208    };
209    */
210
211    inv_load_cal_V0(calData, len);
212
213    /* read the header (type and len)
214       len is the total record length including header and checksum */
215    len = 0;
216    len += 16777216L * ((int)calData[0]);
217    len += 65536L * ((int)calData[1]);
218    len += 256 * ((int)calData[2]);
219    len += (int)calData[3];
220
221    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
222    if (calType > 5) {
223        MPL_LOGE("Unsupported calibration file format %d. "
224                 "Valid types 0..5\n", calType);
225        return INV_ERROR_INVALID_PARAMETER;
226    }
227
228    /* call the proper method to read in the data */
229    //return loaders[calType] (calData, len);
230    return 0;
231}
232
233/**
234 *  @brief  Stores a set of calibration data.
235 *          It generates a binary data set containing calibration data.
236 *          The binary data set is intended to be stored into a file.
237 *
238 *  @pre    inv_dmp_open()
239 *
240 *  @param  calData
241 *              A pointer to an array of bytes to be stored.
242 *  @param  length
243 *              The amount of bytes available in the array.
244 *
245 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
246 */
247inv_error_t inv_store_cal(unsigned char *calData, size_t length)
248{
249    inv_error_t res = 0;
250    size_t size;
251
252    STORECAL_LOG("Entering inv_store_cal\n");
253
254    inv_get_mpl_state_size(&size);
255
256    MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
257
258    /* store data */
259    res = inv_save_mpl_states(calData, size);
260    if(res != 0)
261    {
262        MPL_LOGE("inv_save_mpl_states() failed");
263    }
264
265    STORECAL_LOG("Exiting inv_store_cal\n");
266    return INV_SUCCESS;
267}
268
269/**
270 *  @brief  Load a calibration file.
271 *
272 *  @pre    Must be in INV_STATE_DMP_OPENED state.
273 *          inv_dmp_open() or inv_dmp_stop() must have been called.
274 *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
275 *          been called.
276 *
277 *  @return 0 or error code.
278 */
279inv_error_t inv_load_calibration(void)
280{
281    unsigned char *calData= NULL;
282    inv_error_t result = 0;
283    size_t bytesRead = 0;
284
285    result = inv_read_cal(&calData, &bytesRead);
286    if(result != INV_SUCCESS) {
287        MPL_LOGE("Could not load cal file - "
288                 "aborting\n");
289        goto free_mem_n_exit;
290    }
291
292    result = inv_load_mpl_states(calData, bytesRead);
293    if (result != INV_SUCCESS) {
294        MPL_LOGE("Could not load the calibration data - "
295                 "error %d - aborting\n", result);
296        goto free_mem_n_exit;
297    }
298
299free_mem_n_exit:
300    inv_free(calData);
301    return result;
302}
303
304/**
305 *  @brief  Store runtime calibration data to a file
306 *
307 *  @pre    Must be in INV_STATE_DMP_OPENED state.
308 *          inv_dmp_open() or inv_dmp_stop() must have been called.
309 *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
310 *          been called.
311 *
312 *  @return 0 or error code.
313 */
314inv_error_t inv_store_calibration(void)
315{
316    unsigned char *calData;
317    inv_error_t result;
318    size_t length;
319
320    result = inv_get_mpl_state_size(&length);
321    calData = (unsigned char *)inv_malloc(length);
322    if (!calData) {
323        MPL_LOGE("Could not allocate buffer of %d bytes - "
324                 "aborting\n", length);
325        return INV_ERROR_MEMORY_EXAUSTED;
326    }
327    else {
328        MPL_LOGI("inv_get_mpl state size = %d", length);
329    }
330
331    result = inv_save_mpl_states(calData, length);
332    if (result != INV_SUCCESS) {
333        MPL_LOGE("Could not save mpl states - "
334                 "error %d - aborting\n", result);
335        goto free_mem_n_exit;
336    }
337    else {
338        MPL_LOGE("calData from inv_save_mpl_states, size=%d",
339                 strlen((char *)calData));
340    }
341
342    result = inv_write_cal(calData, length);
343    if (result != INV_SUCCESS) {
344        MPL_LOGE("Could not store calibrated data on file - "
345                 "error %d - aborting\n", result);
346        goto free_mem_n_exit;
347
348    }
349
350free_mem_n_exit:
351    inv_free(calData);
352    return result;
353}
354
355/**
356 *  @}
357 */
358