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#include <stdio.h>
23
24#include "log.h"
25#undef MPL_LOG_TAG
26#define MPL_LOG_TAG "MPL-storeload"
27
28#include "ml_stored_data.h"
29#include "storage_manager.h"
30#include "mlos.h"
31
32#define LOADCAL_DEBUG    0
33#define STORECAL_DEBUG   0
34
35#define DEFAULT_KEY 29681
36
37#define STORECAL_LOG MPL_LOGI
38#define LOADCAL_LOG  MPL_LOGI
39
40inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
41{
42    FILE *fp;
43    inv_error_t result = INV_SUCCESS;
44    size_t fsize;
45
46    fp = fopen(MLCAL_FILE,"rb");
47    if (fp == NULL) {
48        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
49        return INV_ERROR_FILE_OPEN;
50    }
51
52    // obtain file size
53    fseek (fp, 0 , SEEK_END);
54    fsize = ftell (fp);
55    rewind (fp);
56
57    *calData = (unsigned char *)inv_malloc(fsize);
58    if (*calData==NULL) {
59        MPL_LOGE("Could not allocate buffer of %d bytes - "
60                 "aborting\n", fsize);
61        fclose(fp);
62        return INV_ERROR_MEMORY_EXAUSTED;
63    }
64
65    *bytesRead = fread(*calData, 1, fsize, fp);
66    if (*bytesRead != fsize) {
67        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
68                 *bytesRead, fsize);
69        result = INV_ERROR_FILE_READ;
70        goto read_cal_end;
71    }
72    else {
73        MPL_LOGI("Bytes read = %d", *bytesRead);
74    }
75
76read_cal_end:
77    fclose(fp);
78    return result;
79}
80
81inv_error_t inv_write_cal(unsigned char *cal, size_t len)
82{
83    FILE *fp;
84    int bytesWritten;
85    inv_error_t result = INV_SUCCESS;
86
87    if (len <= 0) {
88        MPL_LOGE("Nothing to write");
89        return INV_ERROR_FILE_WRITE;
90    }
91    else {
92        MPL_LOGI("cal data size to write = %d", len);
93    }
94    fp = fopen(MLCAL_FILE,"wb");
95    if (fp == NULL) {
96        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
97        return INV_ERROR_FILE_OPEN;
98    }
99    bytesWritten = fwrite(cal, 1, len, fp);
100    if (bytesWritten != len) {
101        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
102                 bytesWritten, len);
103        result = INV_ERROR_FILE_WRITE;
104    }
105    else {
106        MPL_LOGI("Bytes written = %d", bytesWritten);
107    }
108    fclose(fp);
109    return result;
110}
111
112/**
113 *  @brief  Loads a type 0 set of calibration data.
114 *          It parses a binary data set containing calibration data.
115 *          The binary data set is intended to be loaded from a file.
116 *          This calibrations data format stores values for (in order of
117 *          appearance) :
118 *          - temperature compensation : temperature data points,
119 *          - temperature compensation : gyro biases data points for X, Y,
120 *              and Z axes.
121 *          - accel biases for X, Y, Z axes.
122 *          This calibration data is produced internally by the MPL and its
123 *          size is 2777 bytes (header and checksum included).
124 *          Calibration format type 1 is currently used for ITG3500
125 *
126 *  @pre    inv_init_storage_manager()
127 *          must have been called.
128 *
129 *  @param  calData
130 *              A pointer to an array of bytes to be parsed.
131 *  @param  len
132 *              the length of the calibration
133 *
134 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
135 */
136inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len)
137{
138    inv_error_t result;
139
140    LOADCAL_LOG("Entering inv_load_cal_V0\n");
141
142    /*if (len != expLen) {
143        MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n",
144                 expLen, len);
145        return INV_ERROR_FILE_READ;
146    }*/
147
148    result = inv_load_mpl_states(calData, len);
149    return result;
150}
151
152/**
153 *  @brief  Loads a type 1 set of calibration data.
154 *          It parses a binary data set containing calibration data.
155 *          The binary data set is intended to be loaded from a file.
156 *          This calibrations data format stores values for (in order of
157 *          appearance) :
158 *          - temperature,
159 *          - gyro biases for X, Y, Z axes,
160 *          - accel biases for X, Y, Z axes.
161 *          This calibration data would normally be produced by the MPU Self
162 *          Test and its size is 36 bytes (header and checksum included).
163 *          Calibration format type 1 is produced by the MPU Self Test and
164 *          substitutes the type 0 : inv_load_cal_V0().
165 *
166 *  @pre
167 *
168 *  @param  calData
169 *              A pointer to an array of bytes to be parsed.
170 *  @param  len
171 *              the length of the calibration
172 *
173 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
174 */
175inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len)
176{
177    return INV_SUCCESS;
178}
179
180/**
181 * @brief   Loads a set of calibration data.
182 *          It parses a binary data set containing calibration data.
183 *          The binary data set is intended to be loaded from a file.
184 *
185 * @pre
186 *
187 *
188 * @param   calData
189 *              A pointer to an array of bytes to be parsed.
190 *
191 * @return  INV_SUCCESS if successful, a non-zero error code otherwise.
192 */
193inv_error_t inv_load_cal(unsigned char *calData)
194{
195    int calType = 0;
196    int len = 0;
197    //int ptr;
198    //uint32_t chk = 0;
199    //uint32_t cmp_chk = 0;
200
201    /*load_func_t loaders[] = {
202        inv_load_cal_V0,
203        inv_load_cal_V1,
204    };
205    */
206
207    inv_load_cal_V0(calData, len);
208
209    /* read the header (type and len)
210       len is the total record length including header and checksum */
211    len = 0;
212    len += 16777216L * ((int)calData[0]);
213    len += 65536L * ((int)calData[1]);
214    len += 256 * ((int)calData[2]);
215    len += (int)calData[3];
216
217    calType = ((int)calData[4]) * 256 + ((int)calData[5]);
218    if (calType > 5) {
219        MPL_LOGE("Unsupported calibration file format %d. "
220                 "Valid types 0..5\n", calType);
221        return INV_ERROR_INVALID_PARAMETER;
222    }
223
224    /* call the proper method to read in the data */
225    //return loaders[calType] (calData, len);
226    return 0;
227}
228
229/**
230 *  @brief  Stores a set of calibration data.
231 *          It generates a binary data set containing calibration data.
232 *          The binary data set is intended to be stored into a file.
233 *
234 *  @pre    inv_dmp_open()
235 *
236 *  @param  calData
237 *              A pointer to an array of bytes to be stored.
238 *  @param  length
239 *              The amount of bytes available in the array.
240 *
241 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
242 */
243inv_error_t inv_store_cal(unsigned char *calData, size_t length)
244{
245    inv_error_t res = 0;
246    size_t size;
247
248    STORECAL_LOG("Entering inv_store_cal\n");
249
250    inv_get_mpl_state_size(&size);
251
252    MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
253
254    /* store data */
255    res = inv_save_mpl_states(calData, size);
256    if(res != 0)
257    {
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    }
323    else {
324        MPL_LOGI("mpl state size = %d", length);
325    }
326
327    result = inv_save_mpl_states(calData, length);
328    if (result != INV_SUCCESS) {
329        MPL_LOGE("Could not save mpl states - "
330                 "error %d - aborting\n", result);
331        goto free_mem_n_exit;
332    }
333    else {
334        MPL_LOGE("calData from inv_save_mpl_states, size=%d",
335                 strlen((char *)calData));
336    }
337
338    result = inv_write_cal(calData, length);
339    if (result != INV_SUCCESS) {
340        MPL_LOGE("Could not store calibrated data on file - "
341                 "error %d - aborting\n", result);
342        goto free_mem_n_exit;
343
344    }
345
346free_mem_n_exit:
347    inv_free(calData);
348    return result;
349}
350
351/**
352 *  @}
353 */
354