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