1/*
2 $License:
3   Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16  $
17 */
18
19/******************************************************************************
20 * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $
21 *****************************************************************************/
22
23/**
24 *  @defgroup MLSL (Motion Library - Serial Layer)
25 *  @brief  The Motion Library System Layer provides the Motion Library the
26 *          interface to the system functions.
27 *
28 *  @{
29 *      @file   mlsl_linux_mpu.c
30 *      @brief  The Motion Library System Layer.
31 *
32 */
33
34/* ------------------ */
35/* - Include Files. - */
36/* ------------------ */
37#include <stdio.h>
38#include <sys/ioctl.h>
39#include <stdlib.h>
40#include <fcntl.h>
41#include <errno.h>
42#include <unistd.h>
43#include <linux/fs.h>
44#include <linux/i2c.h>
45#include <string.h>
46#include <signal.h>
47#include <time.h>
48
49#include "mpu.h"
50#if defined CONFIG_MPU_SENSORS_MPU6050A2
51#    include "mpu6050a2.h"
52#elif defined CONFIG_MPU_SENSORS_MPU6050B1
53#    include "mpu6050b1.h"
54#elif defined CONFIG_MPU_SENSORS_MPU3050
55#  include "mpu3050.h"
56#else
57#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx
58#endif
59
60#include "mlsl.h"
61#include "mlos.h"
62#include "mlmath.h"
63#include "mlinclude.h"
64
65#define MLCAL_ID      (0x0A0B0C0DL)
66#define MLCAL_FILE    "/data/cal.bin"
67#define MLCFG_ID      (0x01020304L)
68#define MLCFG_FILE    "/data/cfg.bin"
69
70#include <log.h>
71#undef MPL_LOG_TAG
72#define MPL_LOG_TAG "MPL-mlsl"
73
74#ifndef I2CDEV
75#define I2CDEV "/dev/mpu"
76#endif
77
78#define SERIAL_FULL_DEBUG (0)
79
80/* --------------- */
81/* - Prototypes. - */
82/* --------------- */
83
84/* ----------------------- */
85/* -  Function Pointers. - */
86/* ----------------------- */
87
88/* --------------------------- */
89/* - Global and Static vars. - */
90/* --------------------------- */
91
92/* ---------------- */
93/* - Definitions. - */
94/* ---------------- */
95
96inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len)
97{
98    FILE *fp;
99    int bytesRead;
100
101    fp = fopen(MLCFG_FILE, "rb");
102    if (fp == NULL) {
103        MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE);
104        return INV_ERROR_FILE_OPEN;
105    }
106    bytesRead = fread(cfg, 1, len, fp);
107    if (bytesRead != len) {
108        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
109                 bytesRead, len);
110        return INV_ERROR_FILE_READ;
111    }
112    fclose(fp);
113
114    if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3])
115        != MLCFG_ID) {
116        return INV_ERROR_ASSERTION_FAILURE;
117    }
118
119    return INV_SUCCESS;
120}
121
122inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len)
123{
124    FILE *fp;
125    int bytesWritten;
126    unsigned char cfgId[4];
127
128    fp = fopen(MLCFG_FILE,"wb");
129    if (fp == NULL) {
130        MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE);
131        return INV_ERROR_FILE_OPEN;
132    }
133
134    cfgId[0] = (unsigned char)(MLCFG_ID >> 24);
135    cfgId[1] = (unsigned char)(MLCFG_ID >> 16);
136    cfgId[2] = (unsigned char)(MLCFG_ID >> 8);
137    cfgId[3] = (unsigned char)(MLCFG_ID);
138    bytesWritten = fwrite(cfgId, 1, 4, fp);
139    if (bytesWritten != 4) {
140        MPL_LOGE("CFG ID could not be written on file\n");
141        return INV_ERROR_FILE_WRITE;
142    }
143
144    bytesWritten = fwrite(cfg, 1, len, fp);
145    if (bytesWritten != len) {
146        MPL_LOGE("bytes write (%d) don't match requested length (%d)\n",
147                 bytesWritten, len);
148        return INV_ERROR_FILE_WRITE;
149    }
150
151    fclose(fp);
152
153    return INV_SUCCESS;
154}
155
156inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len)
157{
158    FILE *fp;
159    int bytesRead;
160    inv_error_t result = INV_SUCCESS;
161
162    fp = fopen(MLCAL_FILE,"rb");
163    if (fp == NULL) {
164        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
165        return INV_ERROR_FILE_OPEN;
166    }
167    bytesRead = fread(cal, 1, len, fp);
168    if (bytesRead != len) {
169        MPL_LOGE("bytes read (%d) don't match requested length (%d)\n",
170                 bytesRead, len);
171        result = INV_ERROR_FILE_READ;
172        goto read_cal_end;
173    }
174
175    /* MLCAL_ID not used
176    if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3])
177        != MLCAL_ID) {
178        result = INV_ERROR_ASSERTION_FAILURE;
179        goto read_cal_end;
180    }
181    */
182read_cal_end:
183    fclose(fp);
184    return result;
185}
186
187inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len)
188{
189    FILE *fp;
190    int bytesWritten;
191    inv_error_t result = INV_SUCCESS;
192
193    fp = fopen(MLCAL_FILE,"wb");
194    if (fp == NULL) {
195        MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
196        return INV_ERROR_FILE_OPEN;
197    }
198    bytesWritten = fwrite(cal, 1, len, fp);
199    if (bytesWritten != len) {
200        MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
201                 bytesWritten, len);
202        result = INV_ERROR_FILE_WRITE;
203    }
204    fclose(fp);
205    return result;
206}
207
208inv_error_t inv_serial_get_cal_length(unsigned int *len)
209{
210    FILE *calFile;
211    *len = 0;
212
213    calFile = fopen(MLCAL_FILE, "rb");
214    if (calFile == NULL) {
215        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
216        return INV_ERROR_FILE_OPEN;
217    }
218
219    *len += (unsigned char)fgetc(calFile) << 24;
220    *len += (unsigned char)fgetc(calFile) << 16;
221    *len += (unsigned char)fgetc(calFile) << 8;
222    *len += (unsigned char)fgetc(calFile);
223
224    fclose(calFile);
225
226    if (*len <= 0)
227        return INV_ERROR_FILE_READ;
228
229    return INV_SUCCESS;
230}
231
232inv_error_t inv_serial_open(char const *port, void **sl_handle)
233{
234    INVENSENSE_FUNC_START;
235
236    if (NULL == port) {
237        port = I2CDEV;
238    }
239    *sl_handle = (void*) open(port, O_RDWR);
240    if(sl_handle < 0) {
241        /* ERROR HANDLING; you can check errno to see what went wrong */
242        MPL_LOGE("inv_serial_open\n");
243        MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port);
244        return INV_ERROR_SERIAL_OPEN_ERROR;
245    } else {
246        MPL_LOGI("inv_serial_open: %s\n", port);
247    }
248
249    return INV_SUCCESS;
250}
251
252inv_error_t inv_serial_close(void *sl_handle)
253{
254    INVENSENSE_FUNC_START;
255
256    close((int)sl_handle);
257
258    return INV_SUCCESS;
259}
260
261inv_error_t inv_serial_reset(void *sl_handle)
262{
263    return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
264}
265
266inv_error_t inv_serial_single_write(void *sl_handle,
267                               unsigned char slaveAddr,
268                               unsigned char registerAddr,
269                               unsigned char data)
270{
271    unsigned char buf[2];
272    buf[0] = registerAddr;
273    buf[1] = data;
274    return inv_serial_write(sl_handle, slaveAddr, 2, buf);
275}
276
277inv_error_t inv_serial_write(void *sl_handle,
278                         unsigned char slaveAddr,
279                         unsigned short length,
280                         unsigned char const *data)
281{
282    INVENSENSE_FUNC_START;
283    struct mpu_read_write msg;
284    inv_error_t result;
285
286    if (NULL == data) {
287        return INV_ERROR_INVALID_PARAMETER;
288    }
289
290    msg.address = 0; /* not used */
291    msg.length  = length;
292    msg.data    = (unsigned char*)data;
293
294    if ((result = ioctl((int)sl_handle, MPU_WRITE, &msg))) {
295        MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n",
296                 data[0], length, result);
297       return result;
298    } else if (SERIAL_FULL_DEBUG) {
299        char data_buff[4096] = "";
300        char strchar[3];
301        int ii;
302        for (ii = 0; ii < length; ii++) {
303            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
304            strncat(data_buff, strchar, sizeof(data_buff));
305        }
306        MPL_LOGI("I2C Write Success %02x %02x: %s \n",
307                 data[0], length, data_buff);
308    }
309
310    return INV_SUCCESS;
311}
312
313inv_error_t inv_serial_read(void *sl_handle,
314                        unsigned char  slaveAddr,
315                        unsigned char  registerAddr,
316                        unsigned short length,
317                        unsigned char  *data)
318{
319    INVENSENSE_FUNC_START;
320    int result = INV_SUCCESS;
321    struct mpu_read_write msg;
322
323    if (NULL == data) {
324        return INV_ERROR_INVALID_PARAMETER;
325    }
326
327    msg.address = registerAddr;
328    msg.length  = length;
329    msg.data    = data;
330
331    result = ioctl((int)sl_handle, MPU_READ, &msg);
332
333    if (result != INV_SUCCESS) {
334        MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n",
335                 result, registerAddr, length);
336        result = INV_ERROR_SERIAL_READ;
337    } else if (SERIAL_FULL_DEBUG) {
338        char data_buff[4096] = "";
339        char strchar[3];
340        int ii;
341        for (ii = 0; ii < length; ii++) {
342            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
343            strncat(data_buff, strchar, sizeof(data_buff));
344        }
345        MPL_LOGI("I2C Read  Success %02x %02x: %s \n",
346                  registerAddr, length, data_buff);
347    }
348
349    return (inv_error_t) result;
350}
351
352inv_error_t inv_serial_write_mem(void *sl_handle,
353                            unsigned char mpu_addr,
354                            unsigned short memAddr,
355                            unsigned short length,
356                            const unsigned char *data)
357{
358    INVENSENSE_FUNC_START;
359    struct mpu_read_write msg;
360    int result;
361
362    msg.address = memAddr;
363    msg.length  = length;
364    msg.data    = (unsigned char *)data;
365
366    result = ioctl((int)sl_handle, MPU_WRITE_MEM, &msg);
367    if (result) {
368        LOG_RESULT_LOCATION(result);
369        return result;
370    } else if (SERIAL_FULL_DEBUG) {
371        char data_buff[4096] = "";
372        char strchar[3];
373        int ii;
374        for (ii = 0; ii < length; ii++) {
375            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
376            strncat(data_buff, strchar, sizeof(data_buff));
377        }
378        MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n",
379                 memAddr, length, data_buff);
380    }
381    return INV_SUCCESS;
382}
383
384inv_error_t inv_serial_read_mem(void *sl_handle,
385                           unsigned char  mpu_addr,
386                           unsigned short memAddr,
387                           unsigned short length,
388                           unsigned char  *data)
389{
390    INVENSENSE_FUNC_START;
391    struct mpu_read_write msg;
392    int result;
393
394    if (NULL == data) {
395        return INV_ERROR_INVALID_PARAMETER;
396    }
397
398    msg.address = memAddr;
399    msg.length  = length;
400    msg.data    = data;
401
402    result = ioctl((int)sl_handle, MPU_READ_MEM, &msg);
403    if (result != INV_SUCCESS) {
404        MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n",
405                 result, memAddr, length);
406        return INV_ERROR_SERIAL_READ;
407    } else if (SERIAL_FULL_DEBUG) {
408        char data_buff[4096] = "";
409        char strchar[3];
410        int ii;
411        for (ii = 0; ii < length; ii++) {
412            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
413            strncat(data_buff, strchar, sizeof(data_buff));
414        }
415        MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n",
416                 memAddr, length, data_buff);
417    }
418    return INV_SUCCESS;
419}
420
421inv_error_t inv_serial_write_fifo(void *sl_handle,
422                             unsigned char mpu_addr,
423                             unsigned short length,
424                             const unsigned char *data)
425{
426    INVENSENSE_FUNC_START;
427    struct mpu_read_write msg;
428    int result;
429
430    if (NULL == data) {
431        return INV_ERROR_INVALID_PARAMETER;
432    }
433
434    msg.address = 0; /* Not used */
435    msg.length  = length;
436    msg.data    = (unsigned char *)data;
437
438    result = ioctl((int)sl_handle, MPU_WRITE_FIFO, &msg);
439    if (result != INV_SUCCESS) {
440        MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n",
441                  MPUREG_FIFO_R_W, length);
442        return INV_ERROR_SERIAL_WRITE;
443    } else if (SERIAL_FULL_DEBUG) {
444        char data_buff[4096] = "";
445        char strchar[3];
446        int ii;
447        for (ii = 0; ii < length; ii++) {
448            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
449            strncat(data_buff, strchar, sizeof(data_buff));
450        }
451        MPL_LOGI("I2C Write Success %02x %02x: %s\n",
452                 MPUREG_FIFO_R_W, length, data_buff);
453    }
454    return (inv_error_t) result;
455}
456
457inv_error_t inv_serial_read_fifo(void *sl_handle,
458                            unsigned char  mpu_addr,
459                            unsigned short length,
460                            unsigned char  *data)
461{
462    INVENSENSE_FUNC_START;
463    struct mpu_read_write msg;
464    int result;
465
466    if (NULL == data) {
467        return INV_ERROR_INVALID_PARAMETER;
468    }
469
470    msg.address = MPUREG_FIFO_R_W; /* Not used */
471    msg.length  = length;
472    msg.data    = data;
473
474    result = ioctl((int)sl_handle, MPU_READ_FIFO, &msg);
475    if (result != INV_SUCCESS) {
476        MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n",
477                 result, MPUREG_FIFO_R_W, length);
478        return INV_ERROR_SERIAL_READ;
479    } else if (SERIAL_FULL_DEBUG) {
480        char data_buff[4096] = "";
481        char strchar[3];
482        int ii;
483        for (ii = 0; ii < length; ii++) {
484            snprintf(strchar, sizeof(strchar), "%02x", data[0]);
485            strncat(data_buff, strchar, sizeof(data_buff));
486        }
487        MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n",
488                 MPUREG_FIFO_R_W, length, data_buff);
489    }
490    return INV_SUCCESS;
491}
492
493/**
494 *  @}
495 */
496
497
498