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