Home | History | Annotate | Download | only in linux
      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 
     96 inv_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 
    122 inv_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 
    156 inv_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     */
    182 read_cal_end:
    183     fclose(fp);
    184     return result;
    185 }
    186 
    187 inv_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 
    208 inv_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 
    232 inv_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*)(uintptr_t) open(port, O_RDWR);
    240     if((intptr_t)*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 
    252 inv_error_t inv_serial_close(void *sl_handle)
    253 {
    254     INVENSENSE_FUNC_START;
    255 
    256     close((int)(uintptr_t)sl_handle);
    257 
    258     return INV_SUCCESS;
    259 }
    260 
    261 inv_error_t inv_serial_reset(void *sl_handle)
    262 {
    263     return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
    264 }
    265 
    266 inv_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 
    277 inv_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)(uintptr_t)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 
    313 inv_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)(uintptr_t)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 
    352 inv_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)(uintptr_t)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 
    384 inv_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)(uintptr_t)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 
    421 inv_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)(uintptr_t)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 
    457 inv_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)(uintptr_t)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