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
inv_serial_read_cfg(unsigned char * cfg,unsigned int len)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
inv_serial_write_cfg(unsigned char * cfg,unsigned int len)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
inv_serial_read_cal(unsigned char * cal,unsigned int len)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
inv_serial_write_cal(unsigned char * cal,unsigned int len)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
inv_serial_get_cal_length(unsigned int * len)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
inv_serial_open(char const * port,void ** sl_handle)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
inv_serial_close(void * sl_handle)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
inv_serial_reset(void * sl_handle)261 inv_error_t inv_serial_reset(void *sl_handle)
262 {
263 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
264 }
265
inv_serial_single_write(void * sl_handle,unsigned char slaveAddr,unsigned char registerAddr,unsigned char data)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
inv_serial_write(void * sl_handle,unsigned char slaveAddr,unsigned short length,unsigned char const * data)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
inv_serial_read(void * sl_handle,unsigned char slaveAddr,unsigned char registerAddr,unsigned short length,unsigned char * data)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
inv_serial_write_mem(void * sl_handle,unsigned char mpu_addr,unsigned short memAddr,unsigned short length,const unsigned char * data)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
inv_serial_read_mem(void * sl_handle,unsigned char mpu_addr,unsigned short memAddr,unsigned short length,unsigned char * data)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
inv_serial_write_fifo(void * sl_handle,unsigned char mpu_addr,unsigned short length,const unsigned char * data)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
inv_serial_read_fifo(void * sl_handle,unsigned char mpu_addr,unsigned short length,unsigned char * data)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