aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/misc/mpu3050/mldl_cfg.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/misc/mpu3050/mldl_cfg.c')
-rw-r--r--drivers/misc/mpu3050/mldl_cfg.c1739
1 files changed, 1739 insertions, 0 deletions
diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c
new file mode 100644
index 00000000000..9cc4cf69038
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -0,0 +1,1739 @@
1/*
2 $License:
3 Copyright (C) 2010 InvenSense Corporation, All Rights Reserved.
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
14
15 You should have received a copy of the GNU General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 $
18 */
19
20/**
21 * @addtogroup MLDL
22 *
23 * @{
24 * @file mldl_cfg.c
25 * @brief The Motion Library Driver Layer.
26 */
27
28/* ------------------ */
29/* - Include Files. - */
30/* ------------------ */
31
32#include <stddef.h>
33
34#include "mldl_cfg.h"
35#include "mpu.h"
36
37#include "mlsl.h"
38#include "mlos.h"
39
40#include "log.h"
41#undef MPL_LOG_TAG
42#define MPL_LOG_TAG "mldl_cfg:"
43
44/* --------------------- */
45/* - Variables. - */
46/* --------------------- */
47#ifdef M_HW
48#define SLEEP 0
49#define WAKE_UP 7
50#define RESET 1
51#define STANDBY 1
52#else
53/* licteral significance of all parameters used in MLDLPowerMgmtMPU */
54#define SLEEP 1
55#define WAKE_UP 0
56#define RESET 1
57#define STANDBY 1
58#endif
59
60/*---------------------*/
61/*- Prototypes. -*/
62/*---------------------*/
63
64/*----------------------*/
65/*- Static Functions. -*/
66/*----------------------*/
67
68static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
69{
70 unsigned char userCtrlReg;
71 int result;
72
73 if (!mldl_cfg->dmp_is_running)
74 return ML_SUCCESS;
75
76 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
77 MPUREG_USER_CTRL, 1, &userCtrlReg);
78 ERROR_CHECK(result);
79 userCtrlReg = (userCtrlReg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
80 userCtrlReg = (userCtrlReg & (~BIT_DMP_EN)) | BIT_DMP_RST;
81
82 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
83 MPUREG_USER_CTRL, userCtrlReg);
84 ERROR_CHECK(result);
85 mldl_cfg->dmp_is_running = 0;
86
87 return result;
88
89}
90/**
91 * @brief Starts the DMP running
92 *
93 * @return ML_SUCCESS or non-zero error code
94 */
95static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
96{
97 unsigned char userCtrlReg;
98 int result;
99
100 if (pdata->dmp_is_running == pdata->dmp_enable)
101 return ML_SUCCESS;
102
103 result = MLSLSerialRead(mlsl_handle, pdata->addr,
104 MPUREG_USER_CTRL, 1, &userCtrlReg);
105 ERROR_CHECK(result);
106
107 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
108 MPUREG_USER_CTRL,
109 ((userCtrlReg & (~BIT_FIFO_EN))
110 | BIT_FIFO_RST));
111 ERROR_CHECK(result);
112
113 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
114 MPUREG_USER_CTRL, userCtrlReg);
115 ERROR_CHECK(result);
116
117 result = MLSLSerialRead(mlsl_handle, pdata->addr,
118 MPUREG_USER_CTRL, 1, &userCtrlReg);
119 ERROR_CHECK(result);
120
121 if (pdata->dmp_enable)
122 userCtrlReg |= BIT_DMP_EN;
123 else
124 userCtrlReg &= ~BIT_DMP_EN;
125
126 if (pdata->fifo_enable)
127 userCtrlReg |= BIT_FIFO_EN;
128 else
129 userCtrlReg &= ~BIT_FIFO_EN;
130
131 userCtrlReg |= BIT_DMP_RST;
132
133 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
134 MPUREG_USER_CTRL, userCtrlReg);
135 ERROR_CHECK(result);
136 pdata->dmp_is_running = pdata->dmp_enable;
137
138 return result;
139}
140
141/**
142 * @brief enables/disables the I2C bypass to an external device
143 * connected to MPU's secondary I2C bus.
144 * @param enable
145 * Non-zero to enable pass through.
146 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
147 */
148static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
149 void *mlsl_handle,
150 unsigned char enable)
151{
152 unsigned char b;
153 int result;
154
155 if ((mldl_cfg->gyro_is_bypassed && enable) ||
156 (!mldl_cfg->gyro_is_bypassed && !enable))
157 return ML_SUCCESS;
158
159 /*---- get current 'USER_CTRL' into b ----*/
160 result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
161 MPUREG_USER_CTRL, 1, &b);
162 ERROR_CHECK(result);
163
164 b &= ~BIT_AUX_IF_EN;
165
166 if (!enable) {
167 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
168 MPUREG_USER_CTRL,
169 (b | BIT_AUX_IF_EN));
170 ERROR_CHECK(result);
171 } else {
172 /* Coming out of I2C is tricky due to several erratta. Do not
173 * modify this algorithm
174 */
175 /*
176 * 1) wait for the right time and send the command to change
177 * the aux i2c slave address to an invalid address that will
178 * get nack'ed
179 *
180 * 0x00 is broadcast. 0x7F is unlikely to be used by any aux.
181 */
182 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
183 MPUREG_AUX_SLV_ADDR, 0x7F);
184 ERROR_CHECK(result);
185 /*
186 * 2) wait enough time for a nack to occur, then go into
187 * bypass mode:
188 */
189 MLOSSleep(2);
190 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
191 MPUREG_USER_CTRL, (b));
192 ERROR_CHECK(result);
193 /*
194 * 3) wait for up to one MPU cycle then restore the slave
195 * address
196 */
197 MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
198 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
199 MPUREG_AUX_SLV_ADDR,
200 mldl_cfg->pdata->
201 accel.address);
202 ERROR_CHECK(result);
203
204 /*
205 * 4) reset the ime interface
206 */
207#ifdef M_HW
208 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
209 MPUREG_USER_CTRL,
210 (b | BIT_I2C_MST_RST));
211
212#else
213 result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
214 MPUREG_USER_CTRL,
215 (b | BIT_AUX_IF_RST));
216#endif
217 ERROR_CHECK(result);
218 MLOSSleep(2);
219 }
220 mldl_cfg->gyro_is_bypassed = enable;
221
222 return result;
223}
224
225struct tsProdRevMap {
226 unsigned char siliconRev;
227 unsigned short sensTrim;
228};
229
230#define NUM_OF_PROD_REVS (DIM(prodRevsMap))
231
232/* NOTE : 'npp' is a non production part */
233#ifdef M_HW
234#define OLDEST_PROD_REV_SUPPORTED 1
235static struct tsProdRevMap prodRevsMap[] = {
236 {0, 0},
237 {MPU_SILICON_REV_A1, 131}, /* 1 A1 (npp) */
238 {MPU_SILICON_REV_A1, 131}, /* 2 A1 (npp) */
239 {MPU_SILICON_REV_A1, 131}, /* 3 A1 (npp) */
240 {MPU_SILICON_REV_A1, 131}, /* 4 A1 (npp) */
241 {MPU_SILICON_REV_A1, 131}, /* 5 A1 (npp) */
242 {MPU_SILICON_REV_A1, 131}, /* 6 A1 (npp) */
243 {MPU_SILICON_REV_A1, 131}, /* 7 A1 (npp) */
244 {MPU_SILICON_REV_A1, 131}, /* 8 A1 (npp) */
245};
246
247#else /* !M_HW */
248#define OLDEST_PROD_REV_SUPPORTED 11
249
250static struct tsProdRevMap prodRevsMap[] = {
251 {0, 0},
252 {MPU_SILICON_REV_A4, 131}, /* 1 A? OBSOLETED */
253 {MPU_SILICON_REV_A4, 131}, /* 2 | */
254 {MPU_SILICON_REV_A4, 131}, /* 3 V */
255 {MPU_SILICON_REV_A4, 131}, /* 4 */
256 {MPU_SILICON_REV_A4, 131}, /* 5 */
257 {MPU_SILICON_REV_A4, 131}, /* 6 */
258 {MPU_SILICON_REV_A4, 131}, /* 7 */
259 {MPU_SILICON_REV_A4, 131}, /* 8 */
260 {MPU_SILICON_REV_A4, 131}, /* 9 */
261 {MPU_SILICON_REV_A4, 131}, /* 10 */
262 {MPU_SILICON_REV_B1, 131}, /* 11 B1 */
263 {MPU_SILICON_REV_B1, 131}, /* 12 | */
264 {MPU_SILICON_REV_B1, 131}, /* 13 V */
265 {MPU_SILICON_REV_B1, 131}, /* 14 B4 */
266 {MPU_SILICON_REV_B4, 131}, /* 15 | */
267 {MPU_SILICON_REV_B4, 131}, /* 16 V */
268 {MPU_SILICON_REV_B4, 131}, /* 17 */
269 {MPU_SILICON_REV_B4, 131}, /* 18 */
270 {MPU_SILICON_REV_B4, 115}, /* 19 */
271 {MPU_SILICON_REV_B4, 115}, /* 20 */
272 {MPU_SILICON_REV_B6, 131}, /* 21 B6 (B6/A9) */
273 {MPU_SILICON_REV_B4, 115}, /* 22 B4 (B7/A10) */
274 {MPU_SILICON_REV_B6, 0}, /* 23 B6 (npp) */
275 {MPU_SILICON_REV_B6, 0}, /* 24 | (npp) */
276 {MPU_SILICON_REV_B6, 0}, /* 25 V (npp) */
277 {MPU_SILICON_REV_B6, 131}, /* 26 (B6/A11) */
278};
279#endif /* !M_HW */
280
281/**
282 * @internal
283 * @brief Get the silicon revision ID from OTP.
284 * The silicon revision number is in read from OTP bank 0,
285 * ADDR6[7:2]. The corresponding ID is retrieved by lookup
286 * in a map.
287 * @return The silicon revision ID (0 on error).
288 */
289static int MLDLGetSiliconRev(struct mldl_cfg *pdata,
290 void *mlsl_handle)
291{
292 int result;
293 unsigned char index = 0x00;
294 unsigned char bank =
295 (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
296 unsigned short memAddr = ((bank << 8) | 0x06);
297
298 result = MLSLSerialReadMem(mlsl_handle, pdata->addr,
299 memAddr, 1, &index);
300 ERROR_CHECK(result);
301 if (result)
302 return result;
303 index >>= 2;
304
305 /* clean the prefetch and cfg user bank bits */
306 result =
307 MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
308 MPUREG_BANK_SEL, 0);
309 ERROR_CHECK(result);
310 if (result)
311 return result;
312
313 if (index < OLDEST_PROD_REV_SUPPORTED || NUM_OF_PROD_REVS <= index) {
314 pdata->silicon_revision = 0;
315 pdata->trim = 0;
316 MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
317 return ML_ERROR_INVALID_MODULE;
318 }
319
320 pdata->silicon_revision = prodRevsMap[index].siliconRev;
321 pdata->trim = prodRevsMap[index].sensTrim;
322
323 if (pdata->trim == 0) {
324 MPL_LOGE("sensitivity trim is 0"
325 " - unsupported non production part.\n");
326 return ML_ERROR_INVALID_MODULE;
327 }
328
329 return result;
330}
331
332/**
333 * @brief Enable / Disable the use MPU's secondary I2C interface level
334 * shifters.
335 * When enabled the secondary I2C interface to which the external
336 * device is connected runs at VDD voltage (main supply).
337 * When disabled the 2nd interface runs at VDDIO voltage.
338 * See the device specification for more details.
339 *
340 * @note using this API may produce unpredictable results, depending on how
341 * the MPU and slave device are setup on the target platform.
342 * Use of this API should entirely be restricted to system
343 * integrators. Once the correct value is found, there should be no
344 * need to change the level shifter at runtime.
345 *
346 * @pre Must be called after MLSerialOpen().
347 * @note Typically called before MLDmpOpen().
348 *
349 * @param[in] enable:
350 * 0 to run at VDDIO (default),
351 * 1 to run at VDD.
352 *
353 * @return ML_SUCCESS if successfull, a non-zero error code otherwise.
354 */
355static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata,
356 void *mlsl_handle,
357 unsigned char enable)
358{
359#ifndef M_HW
360 int result;
361 unsigned char reg;
362 unsigned char mask;
363 unsigned char regval;
364
365 if (0 == pdata->silicon_revision)
366 return ML_ERROR_INVALID_PARAMETER;
367
368 /*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR --
369 NOTE: this is incompatible with ST accelerometers where the VDDIO
370 bit MUST be set to enable ST's internal logic to autoincrement
371 the register address on burst reads --*/
372 if ((pdata->silicon_revision & 0xf) < MPU_SILICON_REV_B6) {
373 reg = MPUREG_ACCEL_BURST_ADDR;
374 mask = 0x80;
375 } else {
376 /*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 =>
377 the mask is always 0x04 --*/
378 reg = MPUREG_FIFO_EN2;
379 mask = 0x04;
380 }
381
382 result = MLSLSerialRead(mlsl_handle, pdata->addr, reg, 1, &regval);
383 if (result)
384 return result;
385
386 if (enable)
387 regval |= mask;
388 else
389 regval &= ~mask;
390
391 result =
392 MLSLSerialWriteSingle(mlsl_handle, pdata->addr, reg, regval);
393
394 return result;
395#else
396 return ML_SUCCESS;
397#endif
398}
399
400
401#ifdef M_HW
402/**
403 * @internal
404 * @param reset 1 to reset hardware
405 */
406static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata,
407 void *mlsl_handle,
408 unsigned char reset,
409 unsigned char powerselection)
410{
411 unsigned char b;
412 tMLError result;
413
414 if (powerselection < 0 || powerselection > 7)
415 return ML_ERROR_INVALID_PARAMETER;
416
417 result =
418 MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_1, 1,
419 &b);
420 ERROR_CHECK(result);
421
422 b &= ~(BITS_PWRSEL);
423
424 if (reset) {
425 /* Current sillicon has an errata where the reset will get
426 * nacked. Ignore the error code for now. */
427 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
428 MPUREG_PWR_MGM, b | BIT_H_RESET);
429#define M_HW_RESET_ERRATTA
430#ifndef M_HW_RESET_ERRATTA
431 ERROR_CHECK(result);
432#else
433 MLOSSleep(50);
434#endif
435 }
436
437 b |= (powerselection << 4);
438
439 if (b & BITS_PWRSEL)
440 pdata->gyro_is_suspended = FALSE;
441 else
442 pdata->gyro_is_suspended = TRUE;
443
444 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
445 MPUREG_PWR_MGM, b);
446 ERROR_CHECK(result);
447
448 return ML_SUCCESS;
449}
450
451/**
452 * @internal
453 */
454static tMLError MLDLStandByGyros(struct mldl_cfg *pdata,
455 void *mlsl_handle,
456 unsigned char disable_gx,
457 unsigned char disable_gy,
458 unsigned char disable_gz)
459{
460 unsigned char b;
461 tMLError result;
462
463 result =
464 MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
465 &b);
466 ERROR_CHECK(result);
467
468 b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
469 b |= (disable_gx << 2 | disable_gy << 1 | disable_gz);
470
471 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
472 MPUREG_PWR_MGMT_2, b);
473 ERROR_CHECK(result);
474
475 return ML_SUCCESS;
476}
477
478/**
479 * @internal
480 */
481static tMLError MLDLStandByAccels(struct mldl_cfg *pdata,
482 void *mlsl_handle,
483 unsigned char disable_ax,
484 unsigned char disable_ay,
485 unsigned char disable_az)
486{
487 unsigned char b;
488 tMLError result;
489
490 result =
491 MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
492 &b);
493 ERROR_CHECK(result);
494
495 b &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
496 b |= (disable_ax << 2 | disable_ay << 1 | disable_az);
497
498 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
499 MPUREG_PWR_MGMT_2, b);
500 ERROR_CHECK(result);
501
502 return ML_SUCCESS;
503}
504
505#else /* ! M_HW */
506
507/**
508 * @internal
509 * @brief This function controls the power management on the MPU device.
510 * The entire chip can be put to low power sleep mode, or individual
511 * gyros can be turned on/off.
512 *
513 * Putting the device into sleep mode depending upon the changing needs
514 * of the associated applications is a recommended method for reducing
515 * power consuption. It is a safe opearation in that sleep/wake up of
516 * gyros while running will not result in any interruption of data.
517 *
518 * Although it is entirely allowed to put the device into full sleep
519 * while running the DMP, it is not recomended because it will disrupt
520 * the ongoing calculations carried on inside the DMP and consequently
521 * the sensor fusion algorithm. Furthermore, while in sleep mode
522 * read & write operation from the app processor on both registers and
523 * memory are disabled and can only regained by restoring the MPU in
524 * normal power mode.
525 * Disabling any of the gyro axis will reduce the associated power
526 * consuption from the PLL but will not stop the DMP from running
527 * state.
528 *
529 * @param reset
530 * Non-zero to reset the device. Note that this setting
531 * is volatile and the corresponding register bit will
532 * clear itself right after being applied.
533 * @param sleep
534 * Non-zero to put device into full sleep.
535 * @param disable_gx
536 * Non-zero to disable gyro X.
537 * @param disable_gy
538 * Non-zero to disable gyro Y.
539 * @param disable_gz
540 * Non-zero to disable gyro Z.
541 *
542 * @return ML_SUCCESS if successfull; a non-zero error code otherwise.
543 */
544static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
545 void *mlsl_handle,
546 unsigned char reset,
547 unsigned char sleep,
548 unsigned char disable_gx,
549 unsigned char disable_gy,
550 unsigned char disable_gz)
551{
552 unsigned char b;
553 int result;
554
555 result =
556 MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGM, 1,
557 &b);
558 ERROR_CHECK(result);
559
560 /* If we are awake, we need to put it in bypass before resetting */
561 if ((!(b & BIT_SLEEP)) && reset)
562 result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
563
564 /* If we are awake, we need stop the dmp sleeping */
565 if ((!(b & BIT_SLEEP)) && sleep)
566 dmp_stop(pdata, mlsl_handle);
567
568 /* Reset if requested */
569 if (reset) {
570 MPL_LOGV("Reset MPU3050\n");
571 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
572 MPUREG_PWR_MGM, b | BIT_H_RESET);
573 ERROR_CHECK(result);
574 MLOSSleep(5);
575 pdata->gyro_needs_reset = FALSE;
576 /* Some chips are awake after reset and some are asleep,
577 * check the status */
578 result = MLSLSerialRead(mlsl_handle, pdata->addr,
579 MPUREG_PWR_MGM, 1, &b);
580 ERROR_CHECK(result);
581 }
582
583 /* Update the suspended state just in case we return early */
584 if (b & BIT_SLEEP)
585 pdata->gyro_is_suspended = TRUE;
586 else
587 pdata->gyro_is_suspended = FALSE;
588
589 /* if power status match requested, nothing else's left to do */
590 if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
591 (((sleep != 0) * BIT_SLEEP) |
592 ((disable_gx != 0) * BIT_STBY_XG) |
593 ((disable_gy != 0) * BIT_STBY_YG) |
594 ((disable_gz != 0) * BIT_STBY_ZG))) {
595 return ML_SUCCESS;
596 }
597
598 /*
599 * This specific transition between states needs to be reinterpreted:
600 * (1,1,1,1) -> (0,1,1,1) has to become
601 * (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1)
602 * where
603 * (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1)
604 */
605 if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
606 (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
607 && ((!sleep) && disable_gx && disable_gy && disable_gz)) {
608 result = MLDLPowerMgmtMPU(pdata, mlsl_handle, 0, 1, 0, 0, 0);
609 if (result)
610 return result;
611 b |= BIT_SLEEP;
612 b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
613 }
614
615 if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
616 if (sleep) {
617 result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
618 ERROR_CHECK(result);
619 b |= BIT_SLEEP;
620 result =
621 MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
622 MPUREG_PWR_MGM, b);
623 ERROR_CHECK(result);
624 pdata->gyro_is_suspended = TRUE;
625 } else {
626 b &= ~BIT_SLEEP;
627 result =
628 MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
629 MPUREG_PWR_MGM, b);
630 ERROR_CHECK(result);
631 pdata->gyro_is_suspended = FALSE;
632 MLOSSleep(5);
633 }
634 }
635 /*---
636 WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
637 1) put one axis at a time in stand-by
638 ---*/
639 if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) {
640 b ^= BIT_STBY_XG;
641 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
642 MPUREG_PWR_MGM, b);
643 ERROR_CHECK(result);
644 }
645 if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
646 b ^= BIT_STBY_YG;
647 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
648 MPUREG_PWR_MGM, b);
649 ERROR_CHECK(result);
650 }
651 if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
652 b ^= BIT_STBY_ZG;
653 result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
654 MPUREG_PWR_MGM, b);
655 ERROR_CHECK(result);
656 }
657
658 return ML_SUCCESS;
659}
660#endif /* M_HW */
661
662
663void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
664{
665 struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
666 struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
667 struct ext_slave_platform_data *compass =
668 &mldl_cfg->pdata->compass;
669 struct ext_slave_platform_data *pressure =
670 &mldl_cfg->pdata->pressure;
671
672 MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr);
673 MPL_LOGD("mldl_cfg.int_config = %02x\n",
674 mldl_cfg->int_config);
675 MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync);
676 MPL_LOGD("mldl_cfg.full_scale = %02x\n",
677 mldl_cfg->full_scale);
678 MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf);
679 MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src);
680 MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider);
681 MPL_LOGD("mldl_cfg.dmp_enable = %02x\n",
682 mldl_cfg->dmp_enable);
683 MPL_LOGD("mldl_cfg.fifo_enable = %02x\n",
684 mldl_cfg->fifo_enable);
685 MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1);
686 MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2);
687 MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n",
688 mldl_cfg->offset_tc[0]);
689 MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n",
690 mldl_cfg->offset_tc[1]);
691 MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n",
692 mldl_cfg->offset_tc[2]);
693 MPL_LOGD("mldl_cfg.silicon_revision = %02x\n",
694 mldl_cfg->silicon_revision);
695 MPL_LOGD("mldl_cfg.product_id = %02x\n",
696 mldl_cfg->product_id);
697 MPL_LOGD("mldl_cfg.trim = %02x\n", mldl_cfg->trim);
698 MPL_LOGD("mldl_cfg.requested_sensors= %04lx\n",
699 mldl_cfg->requested_sensors);
700
701 if (mldl_cfg->accel) {
702 MPL_LOGD("slave_accel->suspend = %02x\n",
703 (int) mldl_cfg->accel->suspend);
704 MPL_LOGD("slave_accel->resume = %02x\n",
705 (int) mldl_cfg->accel->resume);
706 MPL_LOGD("slave_accel->read = %02x\n",
707 (int) mldl_cfg->accel->read);
708 MPL_LOGD("slave_accel->type = %02x\n",
709 mldl_cfg->accel->type);
710 MPL_LOGD("slave_accel->reg = %02x\n",
711 mldl_cfg->accel->reg);
712 MPL_LOGD("slave_accel->len = %02x\n",
713 mldl_cfg->accel->len);
714 MPL_LOGD("slave_accel->endian = %02x\n",
715 mldl_cfg->accel->endian);
716 MPL_LOGD("slave_accel->range.mantissa= %02lx\n",
717 mldl_cfg->accel->range.mantissa);
718 MPL_LOGD("slave_accel->range.fraction= %02lx\n",
719 mldl_cfg->accel->range.fraction);
720 } else {
721 MPL_LOGD("slave_accel = NULL\n");
722 }
723
724 if (mldl_cfg->compass) {
725 MPL_LOGD("slave_compass->suspend = %02x\n",
726 (int) mldl_cfg->compass->suspend);
727 MPL_LOGD("slave_compass->resume = %02x\n",
728 (int) mldl_cfg->compass->resume);
729 MPL_LOGD("slave_compass->read = %02x\n",
730 (int) mldl_cfg->compass->read);
731 MPL_LOGD("slave_compass->type = %02x\n",
732 mldl_cfg->compass->type);
733 MPL_LOGD("slave_compass->reg = %02x\n",
734 mldl_cfg->compass->reg);
735 MPL_LOGD("slave_compass->len = %02x\n",
736 mldl_cfg->compass->len);
737 MPL_LOGD("slave_compass->endian = %02x\n",
738 mldl_cfg->compass->endian);
739 MPL_LOGD("slave_compass->range.mantissa= %02lx\n",
740 mldl_cfg->compass->range.mantissa);
741 MPL_LOGD("slave_compass->range.fraction= %02lx\n",
742 mldl_cfg->compass->range.fraction);
743
744 } else {
745 MPL_LOGD("slave_compass = NULL\n");
746 }
747
748 if (mldl_cfg->pressure) {
749 MPL_LOGD("slave_pressure->suspend = %02x\n",
750 (int) mldl_cfg->pressure->suspend);
751 MPL_LOGD("slave_pressure->resume = %02x\n",
752 (int) mldl_cfg->pressure->resume);
753 MPL_LOGD("slave_pressure->read = %02x\n",
754 (int) mldl_cfg->pressure->read);
755 MPL_LOGD("slave_pressure->type = %02x\n",
756 mldl_cfg->pressure->type);
757 MPL_LOGD("slave_pressure->reg = %02x\n",
758 mldl_cfg->pressure->reg);
759 MPL_LOGD("slave_pressure->len = %02x\n",
760 mldl_cfg->pressure->len);
761 MPL_LOGD("slave_pressure->endian = %02x\n",
762 mldl_cfg->pressure->endian);
763 MPL_LOGD("slave_pressure->range.mantissa= %02lx\n",
764 mldl_cfg->pressure->range.mantissa);
765 MPL_LOGD("slave_pressure->range.fraction= %02lx\n",
766 mldl_cfg->pressure->range.fraction);
767
768 } else {
769 MPL_LOGD("slave_pressure = NULL\n");
770 }
771 MPL_LOGD("accel->get_slave_descr = %x\n",
772 (unsigned int) accel->get_slave_descr);
773 MPL_LOGD("accel->irq = %02x\n", accel->irq);
774 MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num);
775 MPL_LOGD("accel->bus = %02x\n", accel->bus);
776 MPL_LOGD("accel->address = %02x\n", accel->address);
777 MPL_LOGD("accel->orientation =\n"
778 " %2d %2d %2d\n"
779 " %2d %2d %2d\n"
780 " %2d %2d %2d\n",
781 accel->orientation[0], accel->orientation[1],
782 accel->orientation[2], accel->orientation[3],
783 accel->orientation[4], accel->orientation[5],
784 accel->orientation[6], accel->orientation[7],
785 accel->orientation[8]);
786 MPL_LOGD("compass->get_slave_descr = %x\n",
787 (unsigned int) compass->get_slave_descr);
788 MPL_LOGD("compass->irq = %02x\n", compass->irq);
789 MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num);
790 MPL_LOGD("compass->bus = %02x\n", compass->bus);
791 MPL_LOGD("compass->address = %02x\n", compass->address);
792 MPL_LOGD("compass->orientation =\n"
793 " %2d %2d %2d\n"
794 " %2d %2d %2d\n"
795 " %2d %2d %2d\n",
796 compass->orientation[0], compass->orientation[1],
797 compass->orientation[2], compass->orientation[3],
798 compass->orientation[4], compass->orientation[5],
799 compass->orientation[6], compass->orientation[7],
800 compass->orientation[8]);
801 MPL_LOGD("pressure->get_slave_descr = %x\n",
802 (unsigned int) pressure->get_slave_descr);
803 MPL_LOGD("pressure->irq = %02x\n", pressure->irq);
804 MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num);
805 MPL_LOGD("pressure->bus = %02x\n", pressure->bus);
806 MPL_LOGD("pressure->address = %02x\n", pressure->address);
807 MPL_LOGD("pressure->orientation =\n"
808 " %2d %2d %2d\n"
809 " %2d %2d %2d\n"
810 " %2d %2d %2d\n",
811 pressure->orientation[0], pressure->orientation[1],
812 pressure->orientation[2], pressure->orientation[3],
813 pressure->orientation[4], pressure->orientation[5],
814 pressure->orientation[6], pressure->orientation[7],
815 pressure->orientation[8]);
816
817 MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config);
818 MPL_LOGD("pdata->level_shifter = %02x\n",
819 pdata->level_shifter);
820 MPL_LOGD("pdata->orientation =\n"
821 " %2d %2d %2d\n"
822 " %2d %2d %2d\n"
823 " %2d %2d %2d\n",
824 pdata->orientation[0], pdata->orientation[1],
825 pdata->orientation[2], pdata->orientation[3],
826 pdata->orientation[4], pdata->orientation[5],
827 pdata->orientation[6], pdata->orientation[7],
828 pdata->orientation[8]);
829
830 MPL_LOGD("Struct sizes: mldl_cfg: %d, "
831 "ext_slave_descr:%d, "
832 "mpu3050_platform_data:%d: RamOffset: %d\n",
833 sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
834 sizeof(struct mpu3050_platform_data),
835 offsetof(struct mldl_cfg, ram));
836}
837
838int mpu_set_slave(struct mldl_cfg *mldl_cfg,
839 void *gyro_handle,
840 struct ext_slave_descr *slave,
841 struct ext_slave_platform_data *slave_pdata)
842{
843 int result;
844 unsigned char reg;
845 unsigned char slave_reg;
846 unsigned char slave_len;
847 unsigned char slave_endian;
848 unsigned char slave_address;
849
850 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
851
852 if (NULL == slave || NULL == slave_pdata) {
853 slave_reg = 0;
854 slave_len = 0;
855 slave_endian = 0;
856 slave_address = 0;
857 } else {
858 slave_reg = slave->reg;
859 slave_len = slave->len;
860 slave_endian = slave->endian;
861 slave_address = slave_pdata->address;
862 }
863
864 /* Address */
865 result = MLSLSerialWriteSingle(gyro_handle,
866 mldl_cfg->addr,
867 MPUREG_AUX_SLV_ADDR,
868 slave_address);
869 ERROR_CHECK(result);
870 /* Register */
871 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
872 MPUREG_ACCEL_BURST_ADDR, 1,
873 &reg);
874 ERROR_CHECK(result);
875 reg = ((reg & 0x80) | slave_reg);
876 result = MLSLSerialWriteSingle(gyro_handle,
877 mldl_cfg->addr,
878 MPUREG_ACCEL_BURST_ADDR,
879 reg);
880 ERROR_CHECK(result);
881
882#ifdef M_HW
883 /* Length, byte swapping, grouping & enable */
884 if (slave_len > BITS_SLV_LENG) {
885 MPL_LOGW("Limiting slave burst read length to "
886 "the allowed maximum (15B, req. %d)\n",
887 slave_len);
888 slave_len = BITS_SLV_LENG;
889 }
890 reg = slave_len;
891 if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN)
892 reg |= BIT_SLV_BYTE_SW;
893 reg |= BIT_SLV_GRP;
894 reg |= BIT_SLV_ENABLE;
895
896 result = MLSLSerialWriteSingle(gyro_handle,
897 mldl_cfg->addr,
898 MPUREG_I2C_SLV0_CTRL,
899 reg);
900#else
901 /* Length */
902 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
903 MPUREG_USER_CTRL, 1, &reg);
904 ERROR_CHECK(result);
905 reg = (reg & ~BIT_AUX_RD_LENG);
906 result = MLSLSerialWriteSingle(gyro_handle,
907 mldl_cfg->addr,
908 MPUREG_USER_CTRL, reg);
909 ERROR_CHECK(result);
910#endif
911
912 if (slave_address) {
913 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, FALSE);
914 ERROR_CHECK(result);
915 }
916 return result;
917}
918
919/**
920 * Check to see if the gyro was reset by testing a couple of registers known
921 * to change on reset.
922 *
923 * @param mldl_cfg mldl configuration structure
924 * @param gyro_handle handle used to communicate with the gyro
925 *
926 * @return ML_SUCCESS or non-zero error code
927 */
928static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
929{
930 int result = ML_SUCCESS;
931 unsigned char reg;
932
933 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
934 MPUREG_DMP_CFG_2, 1, &reg);
935 ERROR_CHECK(result);
936
937 if (mldl_cfg->dmp_cfg2 != reg)
938 return TRUE;
939
940 if (0 != mldl_cfg->dmp_cfg1)
941 return FALSE;
942
943 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
944 MPUREG_SMPLRT_DIV, 1, &reg);
945 ERROR_CHECK(result);
946 if (reg != mldl_cfg->divider)
947 return TRUE;
948
949 if (0 != mldl_cfg->divider)
950 return FALSE;
951
952 /* Inconclusive assume it was reset */
953 return TRUE;
954}
955
956static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle)
957{
958 int result;
959 int ii;
960 int jj;
961 unsigned char reg;
962 unsigned char regs[7];
963
964 /* Wake up the part */
965#ifdef M_HW
966 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, RESET,
967 WAKE_UP);
968 ERROR_CHECK(result);
969
970 /* Configure the MPU */
971 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
972 ERROR_CHECK(result);
973 /* setting int_config with the propert flag BIT_BYPASS_EN
974 should be done by the setup functions */
975 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
976 MPUREG_INT_PIN_CFG,
977 (mldl_cfg->pdata->int_config |
978 BIT_BYPASS_EN));
979 ERROR_CHECK(result);
980 /* temporary: masking out higher bits to avoid switching
981 intelligence */
982 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
983 MPUREG_INT_ENABLE,
984 (mldl_cfg->int_config));
985 ERROR_CHECK(result);
986#else
987 result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 0, 0,
988 mldl_cfg->gyro_power & BIT_STBY_XG,
989 mldl_cfg->gyro_power & BIT_STBY_YG,
990 mldl_cfg->gyro_power & BIT_STBY_ZG);
991
992 if (!mldl_cfg->gyro_needs_reset &&
993 !mpu_was_reset(mldl_cfg, gyro_handle)) {
994 return ML_SUCCESS;
995 }
996
997 result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 1, 0,
998 mldl_cfg->gyro_power & BIT_STBY_XG,
999 mldl_cfg->gyro_power & BIT_STBY_YG,
1000 mldl_cfg->gyro_power & BIT_STBY_ZG);
1001 ERROR_CHECK(result);
1002 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1003 MPUREG_INT_CFG,
1004 (mldl_cfg->int_config |
1005 mldl_cfg->pdata->int_config));
1006 ERROR_CHECK(result);
1007#endif
1008
1009 result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
1010 MPUREG_PWR_MGM, 1, &reg);
1011 ERROR_CHECK(result);
1012 reg &= ~BITS_CLKSEL;
1013 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1014 MPUREG_PWR_MGM,
1015 mldl_cfg->clk_src | reg);
1016 ERROR_CHECK(result);
1017 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1018 MPUREG_SMPLRT_DIV,
1019 mldl_cfg->divider);
1020 ERROR_CHECK(result);
1021
1022#ifdef M_HW
1023 reg = DLPF_FS_SYNC_VALUE(0, mldl_cfg->full_scale, 0);
1024 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1025 MPUREG_GYRO_CONFIG, reg);
1026 reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync, 0, mldl_cfg->lpf);
1027 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1028 MPUREG_CONFIG, reg);
1029#else
1030 reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync,
1031 mldl_cfg->full_scale, mldl_cfg->lpf);
1032 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1033 MPUREG_DLPF_FS_SYNC, reg);
1034#endif
1035 ERROR_CHECK(result);
1036 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1037 MPUREG_DMP_CFG_1,
1038 mldl_cfg->dmp_cfg1);
1039 ERROR_CHECK(result);
1040 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1041 MPUREG_DMP_CFG_2,
1042 mldl_cfg->dmp_cfg2);
1043 ERROR_CHECK(result);
1044
1045 /* Write and verify memory */
1046 for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) {
1047 unsigned char read[MPU_MEM_BANK_SIZE];
1048
1049 result = MLSLSerialWriteMem(gyro_handle,
1050 mldl_cfg->addr,
1051 ((ii << 8) | 0x00),
1052 MPU_MEM_BANK_SIZE,
1053 mldl_cfg->ram[ii]);
1054 ERROR_CHECK(result);
1055 result = MLSLSerialReadMem(gyro_handle, mldl_cfg->addr,
1056 ((ii << 8) | 0x00),
1057 MPU_MEM_BANK_SIZE, read);
1058 ERROR_CHECK(result);
1059
1060#ifdef M_HW
1061#define ML_SKIP_CHECK 38
1062#else
1063#define ML_SKIP_CHECK 20
1064#endif
1065 for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) {
1066 /* skip the register memory locations */
1067 if (ii == 0 && jj < ML_SKIP_CHECK)
1068 continue;
1069 if (mldl_cfg->ram[ii][jj] != read[jj]) {
1070 result = ML_ERROR_SERIAL_WRITE;
1071 break;
1072 }
1073 }
1074 ERROR_CHECK(result);
1075 }
1076
1077 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1078 MPUREG_XG_OFFS_TC,
1079 mldl_cfg->offset_tc[0]);
1080 ERROR_CHECK(result);
1081 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1082 MPUREG_YG_OFFS_TC,
1083 mldl_cfg->offset_tc[1]);
1084 ERROR_CHECK(result);
1085 result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
1086 MPUREG_ZG_OFFS_TC,
1087 mldl_cfg->offset_tc[2]);
1088 ERROR_CHECK(result);
1089
1090 regs[0] = MPUREG_X_OFFS_USRH;
1091 for (ii = 0; ii < DIM(mldl_cfg->offset); ii++) {
1092 regs[1 + ii * 2] =
1093 (unsigned char)(mldl_cfg->offset[ii] >> 8)
1094 & 0xff;
1095 regs[1 + ii * 2 + 1] =
1096 (unsigned char)(mldl_cfg->offset[ii] & 0xff);
1097 }
1098 result = MLSLSerialWrite(gyro_handle, mldl_cfg->addr, 7, regs);
1099 ERROR_CHECK(result);
1100
1101 /* Configure slaves */
1102 result = MLDLSetLevelShifterBit(mldl_cfg, gyro_handle,
1103 mldl_cfg->pdata->level_shifter);
1104 ERROR_CHECK(result);
1105 return result;
1106}
1107/*******************************************************************************
1108 *******************************************************************************
1109 * Exported functions
1110 *******************************************************************************
1111 ******************************************************************************/
1112
1113/**
1114 * Initializes the pdata structure to defaults.
1115 *
1116 * Opens the device to read silicon revision, product id and whoami.
1117 *
1118 * @param mldl_cfg
1119 * The internal device configuration data structure.
1120 * @param mlsl_handle
1121 * The serial communication handle.
1122 *
1123 * @return ML_SUCCESS if silicon revision, product id and woami are supported
1124 * by this software.
1125 */
1126int mpu3050_open(struct mldl_cfg *mldl_cfg,
1127 void *mlsl_handle,
1128 void *accel_handle,
1129 void *compass_handle,
1130 void *pressure_handle)
1131{
1132 int result;
1133 /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
1134 mldl_cfg->ignore_system_suspend = FALSE;
1135 mldl_cfg->int_config = BIT_INT_ANYRD_2CLEAR | BIT_DMP_INT_EN;
1136 mldl_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
1137 mldl_cfg->lpf = MPU_FILTER_42HZ;
1138 mldl_cfg->full_scale = MPU_FS_2000DPS;
1139 mldl_cfg->divider = 4;
1140 mldl_cfg->dmp_enable = 1;
1141 mldl_cfg->fifo_enable = 1;
1142 mldl_cfg->ext_sync = 0;
1143 mldl_cfg->dmp_cfg1 = 0;
1144 mldl_cfg->dmp_cfg2 = 0;
1145 mldl_cfg->gyro_power = 0;
1146 mldl_cfg->gyro_is_bypassed = TRUE;
1147 mldl_cfg->dmp_is_running = FALSE;
1148 mldl_cfg->gyro_is_suspended = TRUE;
1149 mldl_cfg->accel_is_suspended = TRUE;
1150 mldl_cfg->compass_is_suspended = TRUE;
1151 mldl_cfg->pressure_is_suspended = TRUE;
1152 mldl_cfg->gyro_needs_reset = FALSE;
1153 if (mldl_cfg->addr == 0) {
1154#ifdef __KERNEL__
1155 return ML_ERROR_INVALID_PARAMETER;
1156#else
1157 mldl_cfg->addr = 0x68;
1158#endif
1159 }
1160
1161 /*
1162 * Reset,
1163 * Take the DMP out of sleep, and
1164 * read the product_id, sillicon rev and whoami
1165 */
1166#ifdef M_HW
1167 result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
1168 RESET, WAKE_UP);
1169#else
1170 result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
1171#endif
1172 ERROR_CHECK(result);
1173
1174 result = MLDLGetSiliconRev(mldl_cfg, mlsl_handle);
1175 ERROR_CHECK(result);
1176#ifndef M_HW
1177 result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
1178 MPUREG_PRODUCT_ID, 1,
1179 &mldl_cfg->product_id);
1180 ERROR_CHECK(result);
1181#endif
1182
1183 /* Get the factory temperature compensation offsets */
1184 result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
1185 MPUREG_XG_OFFS_TC, 1,
1186 &mldl_cfg->offset_tc[0]);
1187 ERROR_CHECK(result);
1188 result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
1189 MPUREG_YG_OFFS_TC, 1,
1190 &mldl_cfg->offset_tc[1]);
1191 ERROR_CHECK(result);
1192 result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
1193 MPUREG_ZG_OFFS_TC, 1,
1194 &mldl_cfg->offset_tc[2]);
1195 ERROR_CHECK(result);
1196
1197 /* Configure the MPU */
1198#ifdef M_HW
1199 result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
1200 FALSE, SLEEP);
1201#else
1202 result =
1203 MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
1204#endif
1205 ERROR_CHECK(result);
1206
1207 if (mldl_cfg->accel && mldl_cfg->accel->init) {
1208 result = mldl_cfg->accel->init(accel_handle,
1209 mldl_cfg->accel,
1210 &mldl_cfg->pdata->accel);
1211 ERROR_CHECK(result);
1212 }
1213
1214 if (mldl_cfg->compass && mldl_cfg->compass->init) {
1215 result = mldl_cfg->compass->init(compass_handle,
1216 mldl_cfg->compass,
1217 &mldl_cfg->pdata->compass);
1218 if (ML_SUCCESS != result) {
1219 MPL_LOGE("mldl_cfg->compass->init returned %d\n",
1220 result);
1221 goto out_accel;
1222 }
1223 }
1224 if (mldl_cfg->pressure && mldl_cfg->pressure->init) {
1225 result = mldl_cfg->pressure->init(pressure_handle,
1226 mldl_cfg->pressure,
1227 &mldl_cfg->pdata->pressure);
1228 if (ML_SUCCESS != result) {
1229 MPL_LOGE("mldl_cfg->pressure->init returned %d\n",
1230 result);
1231 goto out_compass;
1232 }
1233 }
1234
1235 mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO;
1236 if (mldl_cfg->accel && mldl_cfg->accel->resume)
1237 mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL;
1238
1239 if (mldl_cfg->compass && mldl_cfg->compass->resume)
1240 mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS;
1241
1242 if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
1243 mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE;
1244
1245 return result;
1246
1247out_compass:
1248 if (mldl_cfg->compass->init)
1249 mldl_cfg->compass->exit(compass_handle,
1250 mldl_cfg->compass,
1251 &mldl_cfg->pdata->compass);
1252out_accel:
1253 if (mldl_cfg->accel->init)
1254 mldl_cfg->accel->exit(accel_handle,
1255 mldl_cfg->accel,
1256 &mldl_cfg->pdata->accel);
1257 return result;
1258
1259}
1260
1261/**
1262 * Close the mpu3050 interface
1263 *
1264 * @param mldl_cfg pointer to the configuration structure
1265 * @param mlsl_handle pointer to the serial layer handle
1266 *
1267 * @return ML_SUCCESS or non-zero error code
1268 */
1269int mpu3050_close(struct mldl_cfg *mldl_cfg,
1270 void *mlsl_handle,
1271 void *accel_handle,
1272 void *compass_handle,
1273 void *pressure_handle)
1274{
1275 int result = ML_SUCCESS;
1276 int ret_result = ML_SUCCESS;
1277
1278 if (mldl_cfg->accel && mldl_cfg->accel->exit) {
1279 result = mldl_cfg->accel->exit(accel_handle,
1280 mldl_cfg->accel,
1281 &mldl_cfg->pdata->accel);
1282 if (ML_SUCCESS != result)
1283 MPL_LOGE("Accel exit failed %d\n", result);
1284 ret_result = result;
1285 }
1286 if (ML_SUCCESS == ret_result)
1287 ret_result = result;
1288
1289 if (mldl_cfg->compass && mldl_cfg->compass->exit) {
1290 result = mldl_cfg->compass->exit(compass_handle,
1291 mldl_cfg->compass,
1292 &mldl_cfg->pdata->compass);
1293 if (ML_SUCCESS != result)
1294 MPL_LOGE("Compass exit failed %d\n", result);
1295 }
1296 if (ML_SUCCESS == ret_result)
1297 ret_result = result;
1298
1299 if (mldl_cfg->pressure && mldl_cfg->pressure->exit) {
1300 result = mldl_cfg->pressure->exit(pressure_handle,
1301 mldl_cfg->pressure,
1302 &mldl_cfg->pdata->pressure);
1303 if (ML_SUCCESS != result)
1304 MPL_LOGE("Pressure exit failed %d\n", result);
1305 }
1306 if (ML_SUCCESS == ret_result)
1307 ret_result = result;
1308
1309 return ret_result;
1310}
1311
1312/**
1313 * @brief resume the MPU3050 device and all the other sensor
1314 * devices from their low power state.
1315 *
1316 * @param mldl_cfg
1317 * pointer to the configuration structure
1318 * @param gyro_handle
1319 * the main file handle to the MPU3050 device.
1320 * @param accel_handle
1321 * an handle to the accelerometer device, if sitting
1322 * onto a separate bus. Can match mlsl_handle if
1323 * the accelerometer device operates on the same
1324 * primary bus of MPU.
1325 * @param compass_handle
1326 * an handle to the compass device, if sitting
1327 * onto a separate bus. Can match mlsl_handle if
1328 * the compass device operates on the same
1329 * primary bus of MPU.
1330 * @param pressure_handle
1331 * an handle to the pressure sensor device, if sitting
1332 * onto a separate bus. Can match mlsl_handle if
1333 * the pressure sensor device operates on the same
1334 * primary bus of MPU.
1335 * @param resume_gyro
1336 * whether resuming the gyroscope device is
1337 * actually needed (if the device supports low power
1338 * mode of some sort).
1339 * @param resume_accel
1340 * whether resuming the accelerometer device is
1341 * actually needed (if the device supports low power
1342 * mode of some sort).
1343 * @param resume_compass
1344 * whether resuming the compass device is
1345 * actually needed (if the device supports low power
1346 * mode of some sort).
1347 * @param resume_pressure
1348 * whether resuming the pressure sensor device is
1349 * actually needed (if the device supports low power
1350 * mode of some sort).
1351 * @return ML_SUCCESS or a non-zero error code.
1352 */
1353int mpu3050_resume(struct mldl_cfg *mldl_cfg,
1354 void *gyro_handle,
1355 void *accel_handle,
1356 void *compass_handle,
1357 void *pressure_handle,
1358 bool resume_gyro,
1359 bool resume_accel,
1360 bool resume_compass,
1361 bool resume_pressure)
1362{
1363 int result = ML_SUCCESS;
1364
1365#ifdef CONFIG_MPU_SENSORS_DEBUG
1366 mpu_print_cfg(mldl_cfg);
1367#endif
1368
1369 if (resume_accel &&
1370 ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume)))
1371 return ML_ERROR_INVALID_PARAMETER;
1372 if (resume_compass &&
1373 ((!mldl_cfg->compass) || (!mldl_cfg->compass->resume)))
1374 return ML_ERROR_INVALID_PARAMETER;
1375 if (resume_pressure &&
1376 ((!mldl_cfg->pressure) || (!mldl_cfg->pressure->resume)))
1377 return ML_ERROR_INVALID_PARAMETER;
1378
1379 if (resume_gyro && mldl_cfg->gyro_is_suspended) {
1380 result = gyro_resume(mldl_cfg, gyro_handle);
1381 ERROR_CHECK(result);
1382 }
1383
1384 if (resume_accel && mldl_cfg->accel_is_suspended) {
1385 if (!mldl_cfg->gyro_is_suspended &&
1386 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
1387 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
1388 ERROR_CHECK(result);
1389 }
1390 result = mldl_cfg->accel->resume(accel_handle,
1391 mldl_cfg->accel,
1392 &mldl_cfg->pdata->accel);
1393 ERROR_CHECK(result);
1394 mldl_cfg->accel_is_suspended = FALSE;
1395 }
1396
1397 if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->accel_is_suspended &&
1398 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
1399 result = mpu_set_slave(mldl_cfg,
1400 gyro_handle,
1401 mldl_cfg->accel,
1402 &mldl_cfg->pdata->accel);
1403 ERROR_CHECK(result);
1404 }
1405
1406 if (resume_compass && mldl_cfg->compass_is_suspended) {
1407 if (!mldl_cfg->gyro_is_suspended &&
1408 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
1409 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
1410 ERROR_CHECK(result);
1411 }
1412 result = mldl_cfg->compass->resume(compass_handle,
1413 mldl_cfg->compass,
1414 &mldl_cfg->pdata->
1415 compass);
1416 ERROR_CHECK(result);
1417 mldl_cfg->compass_is_suspended = FALSE;
1418 }
1419
1420 if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->compass_is_suspended &&
1421 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
1422 result = mpu_set_slave(mldl_cfg,
1423 gyro_handle,
1424 mldl_cfg->compass,
1425 &mldl_cfg->pdata->compass);
1426 ERROR_CHECK(result);
1427 }
1428
1429 if (resume_pressure && mldl_cfg->pressure_is_suspended) {
1430 if (!mldl_cfg->gyro_is_suspended &&
1431 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
1432 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
1433 ERROR_CHECK(result);
1434 }
1435 result = mldl_cfg->pressure->resume(pressure_handle,
1436 mldl_cfg->pressure,
1437 &mldl_cfg->pdata->
1438 pressure);
1439 ERROR_CHECK(result);
1440 mldl_cfg->pressure_is_suspended = FALSE;
1441 }
1442
1443 if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->pressure_is_suspended &&
1444 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
1445 result = mpu_set_slave(mldl_cfg,
1446 gyro_handle,
1447 mldl_cfg->pressure,
1448 &mldl_cfg->pdata->pressure);
1449 ERROR_CHECK(result);
1450 }
1451
1452 /* Now start */
1453 if (resume_gyro) {
1454 result = dmp_start(mldl_cfg, gyro_handle);
1455 ERROR_CHECK(result);
1456 }
1457
1458 return result;
1459}
1460
1461/**
1462 * @brief suspend the MPU3050 device and all the other sensor
1463 * devices into their low power state.
1464 * @param gyro_handle
1465 * the main file handle to the MPU3050 device.
1466 * @param accel_handle
1467 * an handle to the accelerometer device, if sitting
1468 * onto a separate bus. Can match gyro_handle if
1469 * the accelerometer device operates on the same
1470 * primary bus of MPU.
1471 * @param compass_handle
1472 * an handle to the compass device, if sitting
1473 * onto a separate bus. Can match gyro_handle if
1474 * the compass device operates on the same
1475 * primary bus of MPU.
1476 * @param pressure_handle
1477 * an handle to the pressure sensor device, if sitting
1478 * onto a separate bus. Can match gyro_handle if
1479 * the pressure sensor device operates on the same
1480 * primary bus of MPU.
1481 * @param accel
1482 * whether suspending the accelerometer device is
1483 * actually needed (if the device supports low power
1484 * mode of some sort).
1485 * @param compass
1486 * whether suspending the compass device is
1487 * actually needed (if the device supports low power
1488 * mode of some sort).
1489 * @param pressure
1490 * whether suspending the pressure sensor device is
1491 * actually needed (if the device supports low power
1492 * mode of some sort).
1493 * @return ML_SUCCESS or a non-zero error code.
1494 */
1495int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
1496 void *gyro_handle,
1497 void *accel_handle,
1498 void *compass_handle,
1499 void *pressure_handle,
1500 bool suspend_gyro,
1501 bool suspend_accel,
1502 bool suspend_compass,
1503 bool suspend_pressure)
1504{
1505 int result = ML_SUCCESS;
1506
1507 if (suspend_gyro && !mldl_cfg->gyro_is_suspended) {
1508#ifdef M_HW
1509 return ML_SUCCESS;
1510 /* This puts the bus into bypass mode */
1511 result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
1512 ERROR_CHECK(result);
1513 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP);
1514#else
1515 result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle,
1516 0, SLEEP, 0, 0, 0);
1517#endif
1518 ERROR_CHECK(result);
1519 }
1520
1521 if (!mldl_cfg->accel_is_suspended && suspend_accel &&
1522 mldl_cfg->accel && mldl_cfg->accel->suspend) {
1523 if (!mldl_cfg->gyro_is_suspended &&
1524 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
1525 result = mpu_set_slave(mldl_cfg, gyro_handle,
1526 NULL, NULL);
1527 ERROR_CHECK(result);
1528 }
1529 result = mldl_cfg->accel->suspend(accel_handle,
1530 mldl_cfg->accel,
1531 &mldl_cfg->pdata->accel);
1532 ERROR_CHECK(result);
1533 mldl_cfg->accel_is_suspended = TRUE;
1534 }
1535
1536 if (!mldl_cfg->compass_is_suspended && suspend_compass &&
1537 mldl_cfg->compass && mldl_cfg->compass->suspend) {
1538 if (!mldl_cfg->gyro_is_suspended &&
1539 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
1540 result = mpu_set_slave(mldl_cfg, gyro_handle,
1541 NULL, NULL);
1542 ERROR_CHECK(result);
1543 }
1544 result = mldl_cfg->compass->suspend(compass_handle,
1545 mldl_cfg->compass,
1546 &mldl_cfg->
1547 pdata->compass);
1548 ERROR_CHECK(result);
1549 mldl_cfg->compass_is_suspended = TRUE;
1550 }
1551
1552 if (!mldl_cfg->pressure_is_suspended && suspend_pressure &&
1553 mldl_cfg->pressure && mldl_cfg->pressure->suspend) {
1554 if (!mldl_cfg->gyro_is_suspended &&
1555 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
1556 result = mpu_set_slave(mldl_cfg, gyro_handle,
1557 NULL, NULL);
1558 ERROR_CHECK(result);
1559 }
1560 result = mldl_cfg->pressure->suspend(pressure_handle,
1561 mldl_cfg->pressure,
1562 &mldl_cfg->
1563 pdata->pressure);
1564 ERROR_CHECK(result);
1565 mldl_cfg->pressure_is_suspended = TRUE;
1566 }
1567 return result;
1568}
1569
1570
1571/**
1572 * @brief read raw sensor data from the accelerometer device
1573 * in use.
1574 * @param mldl_cfg
1575 * A pointer to the struct mldl_cfg data structure.
1576 * @param accel_handle
1577 * The handle to the device the accelerometer is connected to.
1578 * @param data
1579 * a buffer to store the raw sensor data.
1580 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
1581 */
1582int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
1583 void *accel_handle, unsigned char *data)
1584{
1585 if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read)
1586 if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus)
1587 && (!mldl_cfg->gyro_is_bypassed))
1588 return ML_ERROR_FEATURE_NOT_ENABLED;
1589 else
1590 return mldl_cfg->accel->read(accel_handle,
1591 mldl_cfg->accel,
1592 &mldl_cfg->pdata->accel,
1593 data);
1594 else
1595 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1596}
1597
1598/**
1599 * @brief read raw sensor data from the compass device
1600 * in use.
1601 * @param mldl_cfg
1602 * A pointer to the struct mldl_cfg data structure.
1603 * @param compass_handle
1604 * The handle to the device the compass is connected to.
1605 * @param data
1606 * a buffer to store the raw sensor data.
1607 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
1608 */
1609int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
1610 void *compass_handle, unsigned char *data)
1611{
1612 if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read)
1613 if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus)
1614 && (!mldl_cfg->gyro_is_bypassed))
1615 return ML_ERROR_FEATURE_NOT_ENABLED;
1616 else
1617 return mldl_cfg->compass->read(compass_handle,
1618 mldl_cfg->compass,
1619 &mldl_cfg->pdata->compass,
1620 data);
1621 else
1622 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1623}
1624
1625/**
1626 * @brief read raw sensor data from the pressure device
1627 * in use.
1628 * @param mldl_cfg
1629 * A pointer to the struct mldl_cfg data structure.
1630 * @param pressure_handle
1631 * The handle to the device the pressure sensor is connected to.
1632 * @param data
1633 * a buffer to store the raw sensor data.
1634 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
1635 */
1636int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg,
1637 void *pressure_handle, unsigned char *data)
1638{
1639 if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read)
1640 if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus)
1641 && (!mldl_cfg->gyro_is_bypassed))
1642 return ML_ERROR_FEATURE_NOT_ENABLED;
1643 else
1644 return mldl_cfg->pressure->read(
1645 pressure_handle,
1646 mldl_cfg->pressure,
1647 &mldl_cfg->pdata->pressure,
1648 data);
1649 else
1650 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1651}
1652
1653int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
1654 void *accel_handle,
1655 struct ext_slave_config *data)
1656{
1657 if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->config)
1658 return mldl_cfg->accel->config(accel_handle,
1659 mldl_cfg->accel,
1660 &mldl_cfg->pdata->accel,
1661 data);
1662 else
1663 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1664
1665}
1666
1667int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
1668 void *compass_handle,
1669 struct ext_slave_config *data)
1670{
1671 if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->config)
1672 return mldl_cfg->compass->config(compass_handle,
1673 mldl_cfg->compass,
1674 &mldl_cfg->pdata->compass,
1675 data);
1676 else
1677 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1678
1679}
1680
1681int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
1682 void *pressure_handle,
1683 struct ext_slave_config *data)
1684{
1685 if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->config)
1686 return mldl_cfg->pressure->config(pressure_handle,
1687 mldl_cfg->pressure,
1688 &mldl_cfg->pdata->pressure,
1689 data);
1690 else
1691 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1692}
1693
1694int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg,
1695 void *accel_handle,
1696 struct ext_slave_config *data)
1697{
1698 if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->get_config)
1699 return mldl_cfg->accel->get_config(accel_handle,
1700 mldl_cfg->accel,
1701 &mldl_cfg->pdata->accel,
1702 data);
1703 else
1704 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1705
1706}
1707
1708int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg,
1709 void *compass_handle,
1710 struct ext_slave_config *data)
1711{
1712 if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->get_config)
1713 return mldl_cfg->compass->get_config(compass_handle,
1714 mldl_cfg->compass,
1715 &mldl_cfg->pdata->compass,
1716 data);
1717 else
1718 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1719
1720}
1721
1722int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg,
1723 void *pressure_handle,
1724 struct ext_slave_config *data)
1725{
1726 if (NULL != mldl_cfg->pressure &&
1727 NULL != mldl_cfg->pressure->get_config)
1728 return mldl_cfg->pressure->get_config(pressure_handle,
1729 mldl_cfg->pressure,
1730 &mldl_cfg->pdata->pressure,
1731 data);
1732 else
1733 return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
1734}
1735
1736
1737/**
1738 *@}
1739 */
generated by cgit v1.2.2 (git 2.25.0) at 2025-08-02 09:34:08 -0400