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
