aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/misc/inv_mpu/mpu6050/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/mpu6050/mldl_cfg.c
parent8dea78da5cee153b8af9c07a2745f6c55057fe12 (diff)
Added missing tegra files.HEADmaster
Diffstat (limited to 'drivers/misc/inv_mpu/mpu6050/mldl_cfg.c')
-rw-r--r--drivers/misc/inv_mpu/mpu6050/mldl_cfg.c1916
1 files changed, 1916 insertions, 0 deletions
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
new file mode 100644
index 00000000000..22af0c20098
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
@@ -0,0 +1,1916 @@
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 "mpu6050b1.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 0
47#define WAKE_UP 7
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 * @brief enables/disables the I2C bypass to an external device
159 * connected to MPU's secondary I2C bus.
160 * @param enable
161 * Non-zero to enable pass through.
162 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
163 */
164static int mpu6050b1_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
165 void *mlsl_handle, unsigned char enable)
166{
167 unsigned char reg;
168 int result;
169 unsigned char status = mldl_cfg->inv_mpu_state->status;
170 if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
171 (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
172 return INV_SUCCESS;
173
174 /*---- get current 'USER_CTRL' into b ----*/
175 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
176 MPUREG_USER_CTRL, 1, &reg);
177 if (result) {
178 LOG_RESULT_LOCATION(result);
179 return result;
180 }
181
182 if (!enable) {
183 /* setting int_config with the property flag BIT_BYPASS_EN
184 should be done by the setup functions */
185 result = inv_serial_single_write(
186 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
187 MPUREG_INT_PIN_CFG,
188 (mldl_cfg->pdata->int_config & ~(BIT_BYPASS_EN)));
189 if (!(reg & BIT_I2C_MST_EN)) {
190 result =
191 inv_serial_single_write(
192 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
193 MPUREG_USER_CTRL,
194 (reg | BIT_I2C_MST_EN));
195 if (result) {
196 LOG_RESULT_LOCATION(result);
197 return result;
198 }
199 }
200 } else if (enable) {
201 if (reg & BIT_AUX_IF_EN) {
202 result =
203 inv_serial_single_write(
204 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
205 MPUREG_USER_CTRL,
206 (reg & (~BIT_I2C_MST_EN)));
207 if (result) {
208 LOG_RESULT_LOCATION(result);
209 return result;
210 }
211 /*****************************************
212 * To avoid hanging the bus we must sleep until all
213 * slave transactions have been completed.
214 * 24 bytes max slave reads
215 * +1 byte possible extra write
216 * +4 max slave address
217 * ---
218 * 33 Maximum bytes
219 * x9 Approximate bits per byte
220 * ---
221 * 297 bits.
222 * 2.97 ms minimum @ 100kbps
223 * 0.75 ms minimum @ 400kbps.
224 *****************************************/
225 msleep(3);
226 }
227 result = inv_serial_single_write(
228 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
229 MPUREG_INT_PIN_CFG,
230 (mldl_cfg->pdata->int_config | BIT_BYPASS_EN));
231 if (result) {
232 LOG_RESULT_LOCATION(result);
233 return result;
234 }
235 }
236 if (enable)
237 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
238 else
239 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
240
241 return result;
242}
243
244
245
246
247/**
248 * @brief enables/disables the I2C bypass to an external device
249 * connected to MPU's secondary I2C bus.
250 * @param enable
251 * Non-zero to enable pass through.
252 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
253 */
254static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
255 unsigned char enable)
256{
257 return mpu6050b1_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
258}
259
260
261#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
262
263/* NOTE : when not indicated, product revision
264 is considered an 'npp'; non production part */
265
266/* produces an unique identifier for each device based on the
267 combination of product version and product revision */
268struct prod_rev_map_t {
269 unsigned short mpl_product_key;
270 unsigned char silicon_rev;
271 unsigned short gyro_trim;
272 unsigned short accel_trim;
273};
274
275/* NOTE: product entries are in chronological order */
276static struct prod_rev_map_t prod_rev_map[] = {
277 /* prod_ver = 0 */
278 {MPL_PROD_KEY(0, 1), MPU_SILICON_REV_A2, 131, 16384},
279 {MPL_PROD_KEY(0, 2), MPU_SILICON_REV_A2, 131, 16384},
280 {MPL_PROD_KEY(0, 3), MPU_SILICON_REV_A2, 131, 16384},
281 {MPL_PROD_KEY(0, 4), MPU_SILICON_REV_A2, 131, 16384},
282 {MPL_PROD_KEY(0, 5), MPU_SILICON_REV_A2, 131, 16384},
283 {MPL_PROD_KEY(0, 6), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/C2-1) */
284 /* prod_ver = 1, forced to 0 for MPU6050 A2 */
285 {MPL_PROD_KEY(0, 7), MPU_SILICON_REV_A2, 131, 16384},
286 {MPL_PROD_KEY(0, 8), MPU_SILICON_REV_A2, 131, 16384},
287 {MPL_PROD_KEY(0, 9), MPU_SILICON_REV_A2, 131, 16384},
288 {MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384},
289 {MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D2-1) */
290 {MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384},
291 {MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384},
292 {MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384},
293 {MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384},
294 {MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D4) */
295 /* prod_ver = 1 */
296 {MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-1) */
297 {MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-2) */
298 {MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-3) */
299 {MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-4) */
300 {MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-5) */
301 {MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D4) */
302 {MPL_PROD_KEY(1, 1), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-1) */
303 {MPL_PROD_KEY(1, 2), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-2) */
304 {MPL_PROD_KEY(1, 3), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-3) */
305 {MPL_PROD_KEY(1, 4), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-4) */
306 {MPL_PROD_KEY(1, 5), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-5) */
307 {MPL_PROD_KEY(1, 6), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-6) */
308 /* prod_ver = 2 */
309 {MPL_PROD_KEY(2, 7), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-1) */
310 {MPL_PROD_KEY(2, 8), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-2) */
311 {MPL_PROD_KEY(2, 9), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-3) */
312 {MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-4) */
313 {MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-5) */
314 {MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-6) */
315 {MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/D4) */
316 /* prod_ver = 3 */
317 {MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E2) */
318 /* prod_ver = 4 */
319 {MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131, 8192}, /* (B2/F1) */
320 {MPL_PROD_KEY(4, 1), MPU_SILICON_REV_B1, 131, 8192}, /* (B3/F1) */
321 {MPL_PROD_KEY(4, 3), MPU_SILICON_REV_B1, 131, 8192}, /* (B4/F1) */
322 /* prod_ver = 5 */
323 {MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
324 /* prod_ver = 7 */
325 {MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
326 /* prod_ver = 8 */
327 {MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
328 /* prod_ver = 9 */
329 {MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */
330 /* prod_ver = 10 */
331 {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384} /* (B5/E2) */
332};
333
334/**
335 * @internal
336 * @brief Inverse lookup of the index of an MPL product key .
337 * @param key
338 * the MPL product indentifier also referred to as 'key'.
339 * @return the index position of the key in the array, -1 if not found.
340 */
341short index_of_key(unsigned short key)
342{
343 int i;
344 for (i = 0; i < NUM_OF_PROD_REVS; i++)
345 if (prod_rev_map[i].mpl_product_key == key)
346 return (short)i;
347 return -1;
348}
349
350/**
351 * @internal
352 * @brief Get the product revision and version for MPU6050 and
353 * extract all per-part specific information.
354 * The product version number is read from the PRODUCT_ID register in
355 * user space register map.
356 * The product revision number is in read from OTP bank 0, ADDR6[7:2].
357 * These 2 numbers, combined, provide an unique key to be used to
358 * retrieve some per-device information such as the silicon revision
359 * and the gyro and accel sensitivity trim values.
360 *
361 * @param mldl_cfg
362 * a pointer to the mldl config data structure.
363 * @param mlsl_handle
364 * an file handle to the serial communication device the
365 * device is connected to.
366 *
367 * @return 0 on success, a non-zero error code otherwise.
368 */
369static int inv_get_silicon_rev_mpu6050(
370 struct mldl_cfg *mldl_cfg, void *mlsl_handle)
371{
372 int result;
373 unsigned char prod_ver = 0x00, prod_rev = 0x00;
374 unsigned char bank =
375 (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
376 unsigned short memAddr = ((bank << 8) | 0x06);
377 unsigned short key;
378 short index;
379 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
380
381 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
382 MPUREG_PRODUCT_ID, 1, &prod_ver);
383 if (result) {
384 LOG_RESULT_LOCATION(result);
385 return result;
386 }
387 prod_ver &= 0xF;
388
389 result = inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
390 memAddr, 1, &prod_rev);
391 if (result) {
392 LOG_RESULT_LOCATION(result);
393 return result;
394 }
395 prod_rev >>= 2;
396
397 /* clean the prefetch and cfg user bank bits */
398 result = inv_serial_single_write(
399 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
400 MPUREG_BANK_SEL, 0);
401 if (result) {
402 LOG_RESULT_LOCATION(result);
403 return result;
404 }
405
406 key = MPL_PROD_KEY(prod_ver, prod_rev);
407 if (key == 0) {
408 MPL_LOGE("Product id read as 0 "
409 "indicates device is either "
410 "incompatible or an MPU3050\n");
411 return INV_ERROR_INVALID_MODULE;
412 }
413 index = index_of_key(key);
414 if (index == -1 || index >= NUM_OF_PROD_REVS) {
415 MPL_LOGE("Unsupported product key %d in MPL\n", key);
416 return INV_ERROR_INVALID_MODULE;
417 }
418 /* check MPL is compiled for this device */
419 if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) {
420 MPL_LOGE("MPL compiled for MPU6050B1 support "
421 "but device is not MPU6050B1 (%d)\n", key);
422 return INV_ERROR_INVALID_MODULE;
423 }
424
425 mpu_chip_info->product_id = prod_ver;
426 mpu_chip_info->product_revision = prod_rev;
427 mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
428 mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
429 mpu_chip_info->accel_sens_trim = prod_rev_map[index].accel_trim;
430
431 return result;
432}
433#define inv_get_silicon_rev inv_get_silicon_rev_mpu6050
434
435
436/**
437 * @brief Enable / Disable the use MPU's secondary I2C interface level
438 * shifters.
439 * When enabled the secondary I2C interface to which the external
440 * device is connected runs at VDD voltage (main supply).
441 * When disabled the 2nd interface runs at VDDIO voltage.
442 * See the device specification for more details.
443 *
444 * @note using this API may produce unpredictable results, depending on how
445 * the MPU and slave device are setup on the target platform.
446 * Use of this API should entirely be restricted to system
447 * integrators. Once the correct value is found, there should be no
448 * need to change the level shifter at runtime.
449 *
450 * @pre Must be called after inv_serial_start().
451 * @note Typically called before inv_dmp_open().
452 *
453 * @param[in] enable:
454 * 0 to run at VDDIO (default),
455 * 1 to run at VDD.
456 *
457 * @return INV_SUCCESS if successfull, a non-zero error code otherwise.
458 */
459static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
460 void *mlsl_handle, unsigned char enable)
461{
462 int result;
463 unsigned char regval;
464
465 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
466 MPUREG_YG_OFFS_TC, 1, &regval);
467 if (result) {
468 LOG_RESULT_LOCATION(result);
469 return result;
470 }
471 if (enable)
472 regval |= BIT_I2C_MST_VDDIO;
473
474 result = inv_serial_single_write(
475 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
476 MPUREG_YG_OFFS_TC, regval);
477 if (result) {
478 LOG_RESULT_LOCATION(result);
479 return result;
480 }
481 return INV_SUCCESS;
482}
483
484
485/**
486 * @internal
487 * @brief MPU6050 B1 power management functions.
488 * @param mldl_cfg
489 * a pointer to the internal mldl_cfg data structure.
490 * @param mlsl_handle
491 * a file handle to the serial device used to communicate
492 * with the MPU6050 B1 device.
493 * @param reset
494 * 1 to reset hardware.
495 * @param sensors
496 * Bitfield of sensors to leave on
497 *
498 * @return 0 on success, a non-zero error code on error.
499 */
500static int mpu60xx_pwr_mgmt(struct mldl_cfg *mldl_cfg,
501 void *mlsl_handle,
502 unsigned int reset, unsigned long sensors)
503{
504 unsigned char pwr_mgmt[2];
505 unsigned char pwr_mgmt_prev[2];
506 int result;
507 int sleep = !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
508 | INV_DMP_PROCESSOR));
509
510 if (reset) {
511 MPL_LOGI("Reset MPU6050 B1\n");
512 result = inv_serial_single_write(
513 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
514 MPUREG_PWR_MGMT_1, BIT_H_RESET);
515 if (result) {
516 LOG_RESULT_LOCATION(result);
517 return result;
518 }
519 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
520 msleep(15);
521 }
522
523 /* NOTE : reading both PWR_MGMT_1 and PWR_MGMT_2 for efficiency because
524 they are accessible even when the device is powered off */
525 result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
526 MPUREG_PWR_MGMT_1, 2, pwr_mgmt_prev);
527 if (result) {
528 LOG_RESULT_LOCATION(result);
529 return result;
530 }
531
532 pwr_mgmt[0] = pwr_mgmt_prev[0];
533 pwr_mgmt[1] = pwr_mgmt_prev[1];
534
535 if (sleep) {
536 mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
537 pwr_mgmt[0] |= BIT_SLEEP;
538 } else {
539 mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
540 pwr_mgmt[0] &= ~BIT_SLEEP;
541 }
542 if (pwr_mgmt[0] != pwr_mgmt_prev[0]) {
543 result = inv_serial_single_write(
544 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
545 MPUREG_PWR_MGMT_1, pwr_mgmt[0]);
546 if (result) {
547 LOG_RESULT_LOCATION(result);
548 return result;
549 }
550 }
551
552 pwr_mgmt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
553 if (!(sensors & INV_X_GYRO))
554 pwr_mgmt[1] |= BIT_STBY_XG;
555 if (!(sensors & INV_Y_GYRO))
556 pwr_mgmt[1] |= BIT_STBY_YG;
557 if (!(sensors & INV_Z_GYRO))
558 pwr_mgmt[1] |= BIT_STBY_ZG;
559
560 if (pwr_mgmt[1] != pwr_mgmt_prev[1]) {
561 result = inv_serial_single_write(
562 mlsl_handle, mldl_cfg->mpu_chip_info->addr,
563 MPUREG_PWR_MGMT_2, pwr_mgmt[1]);
564 if (result) {
565 LOG_RESULT_LOCATION(result);
566 return result;
567 }
568 }
569
570 if ((pwr_mgmt[1] & (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
571 (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) {
572 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
573 } else {
574 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
575 }
576
577 return INV_SUCCESS;
578}
579
580
581/**
582 * @brief sets the clock source for the gyros.
583 * @param mldl_cfg
584 * a pointer to the struct mldl_cfg data structure.
585 * @param gyro_handle
586 * an handle to the serial device the gyro is assigned to.
587 * @return ML_SUCCESS if successful, a non-zero error code otherwise.
588 */
589static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
590{
591 int result;
592 unsigned char cur_clk_src;
593 unsigned char reg;
594
595 /* clock source selection */
596 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
597 MPUREG_PWR_MGM, 1, &reg);
598 if (result) {
599 LOG_RESULT_LOCATION(result);
600 return result;
601 }
602 cur_clk_src = reg & BITS_CLKSEL;
603 reg &= ~BITS_CLKSEL;
604
605
606 result = inv_serial_single_write(
607 gyro_handle, mldl_cfg->mpu_chip_info->addr,
608 MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
609 if (result) {
610 LOG_RESULT_LOCATION(result);
611 return result;
612 }
613
614 /* ERRATA:
615 workaroud to switch from any MPU_CLK_SEL_PLLGYROx to
616 MPU_CLK_SEL_INTERNAL and XGyro is powered up:
617 1) Select INT_OSC
618 2) PD XGyro
619 3) PU XGyro
620 */
621 if ((cur_clk_src == MPU_CLK_SEL_PLLGYROX
622 || cur_clk_src == MPU_CLK_SEL_PLLGYROY
623 || cur_clk_src == MPU_CLK_SEL_PLLGYROZ)
624 && mldl_cfg->mpu_gyro_cfg->clk_src == MPU_CLK_SEL_INTERNAL
625 && mldl_cfg->inv_mpu_cfg->requested_sensors & INV_X_GYRO) {
626 unsigned char first_result = INV_SUCCESS;
627 mldl_cfg->inv_mpu_cfg->requested_sensors &= ~INV_X_GYRO;
628 result = mpu60xx_pwr_mgmt(
629 mldl_cfg, gyro_handle,
630 false, mldl_cfg->inv_mpu_cfg->requested_sensors);
631 ERROR_CHECK_FIRST(first_result, result);
632 mldl_cfg->inv_mpu_cfg->requested_sensors |= INV_X_GYRO;
633 result = mpu60xx_pwr_mgmt(
634 mldl_cfg, gyro_handle,
635 false, mldl_cfg->inv_mpu_cfg->requested_sensors);
636 ERROR_CHECK_FIRST(first_result, result);
637 result = first_result;
638 }
639 return result;
640}
641
642/**
643 * Configures the MPU I2C Master
644 *
645 * @mldl_cfg Handle to the configuration data
646 * @gyro_handle handle to the gyro communictation interface
647 * @slave Can be Null if turning off the slave
648 * @slave_pdata Can be null if turning off the slave
649 * @slave_id enum ext_slave_type to determine which index to use
650 *
651 *
652 * This fucntion configures the slaves by:
653 * 1) Setting up the read
654 * a) Read Register
655 * b) Read Length
656 * 2) Set up the data trigger (MPU6050 only)
657 * a) Set trigger write register
658 * b) Set Trigger write value
659 * 3) Set up the divider (MPU6050 only)
660 * 4) Set the slave bypass mode depending on slave
661 *
662 * returns INV_SUCCESS or non-zero error code
663 */
664
665static int mpu_set_slave_mpu60xx(struct mldl_cfg *mldl_cfg,
666 void *gyro_handle,
667 struct ext_slave_descr *slave,
668 struct ext_slave_platform_data *slave_pdata,
669 int slave_id)
670{
671 int result;
672 unsigned char reg;
673 /* Slave values */
674 unsigned char slave_reg;
675 unsigned char slave_len;
676 unsigned char slave_endian;
677 unsigned char slave_address;
678 /* Which MPU6050 registers to use */
679 unsigned char addr_reg;
680 unsigned char reg_reg;
681 unsigned char ctrl_reg;
682 /* Which MPU6050 registers to use for the trigger */
683 unsigned char addr_trig_reg;
684 unsigned char reg_trig_reg;
685 unsigned char ctrl_trig_reg;
686
687 unsigned char bits_slave_delay = 0;
688 /* Divide down rate for the Slave, from the mpu rate */
689 unsigned char d0_trig_reg;
690 unsigned char delay_ctrl_orig;
691 unsigned char delay_ctrl;
692 long divider;
693
694 if (NULL == slave || NULL == slave_pdata) {
695 slave_reg = 0;
696 slave_len = 0;
697 slave_endian = 0;
698 slave_address = 0;
699 } else {
700 slave_reg = slave->read_reg;
701 slave_len = slave->read_len;
702 slave_endian = slave->endian;
703 slave_address = slave_pdata->address;
704 slave_address |= BIT_I2C_READ;
705 }
706
707 switch (slave_id) {
708 case EXT_SLAVE_TYPE_ACCEL:
709 addr_reg = MPUREG_I2C_SLV1_ADDR;
710 reg_reg = MPUREG_I2C_SLV1_REG;
711 ctrl_reg = MPUREG_I2C_SLV1_CTRL;
712 addr_trig_reg = 0;
713 reg_trig_reg = 0;
714 ctrl_trig_reg = 0;
715 bits_slave_delay = BIT_SLV1_DLY_EN;
716 break;
717 case EXT_SLAVE_TYPE_COMPASS:
718 addr_reg = MPUREG_I2C_SLV0_ADDR;
719 reg_reg = MPUREG_I2C_SLV0_REG;
720 ctrl_reg = MPUREG_I2C_SLV0_CTRL;
721 addr_trig_reg = MPUREG_I2C_SLV2_ADDR;
722 reg_trig_reg = MPUREG_I2C_SLV2_REG;
723 ctrl_trig_reg = MPUREG_I2C_SLV2_CTRL;
724 d0_trig_reg = MPUREG_I2C_SLV2_DO;
725 bits_slave_delay = BIT_SLV2_DLY_EN | BIT_SLV0_DLY_EN;
726 break;
727 case EXT_SLAVE_TYPE_PRESSURE:
728 addr_reg = MPUREG_I2C_SLV3_ADDR;
729 reg_reg = MPUREG_I2C_SLV3_REG;
730 ctrl_reg = MPUREG_I2C_SLV3_CTRL;
731 addr_trig_reg = MPUREG_I2C_SLV4_ADDR;
732 reg_trig_reg = MPUREG_I2C_SLV4_REG;
733 ctrl_trig_reg = MPUREG_I2C_SLV4_CTRL;
734 bits_slave_delay = BIT_SLV4_DLY_EN | BIT_SLV3_DLY_EN;
735 break;
736 default:
737 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
738 return INV_ERROR_INVALID_PARAMETER;
739 };
740
741 /* return if this slave has already been set */
742 if ((slave_address &&
743 ((mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay)
744 == bits_slave_delay)) ||
745 (!slave_address &&
746 (mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) ==
747 0))
748 return 0;
749
750 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
751
752 /* Address */
753 result = inv_serial_single_write(gyro_handle,
754 mldl_cfg->mpu_chip_info->addr,
755 addr_reg, slave_address);
756 if (result) {
757 LOG_RESULT_LOCATION(result);
758 return result;
759 }
760 /* Register */
761 result = inv_serial_single_write(gyro_handle,
762 mldl_cfg->mpu_chip_info->addr,
763 reg_reg, slave_reg);
764 if (result) {
765 LOG_RESULT_LOCATION(result);
766 return result;
767 }
768
769 /* Length, byte swapping, grouping & enable */
770 if (slave_len > BITS_SLV_LENG) {
771 MPL_LOGW("Limiting slave burst read length to "
772 "the allowed maximum (15B, req. %d)\n", slave_len);
773 slave_len = BITS_SLV_LENG;
774 return INV_ERROR_INVALID_CONFIGURATION;
775 }
776 reg = slave_len;
777 if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) {
778 reg |= BIT_SLV_BYTE_SW;
779 if (slave_reg & 1)
780 reg |= BIT_SLV_GRP;
781 }
782 if (slave_address)
783 reg |= BIT_SLV_ENABLE;
784
785 result = inv_serial_single_write(gyro_handle,
786 mldl_cfg->mpu_chip_info->addr,
787 ctrl_reg, reg);
788 if (result) {
789 LOG_RESULT_LOCATION(result);
790 return result;
791 }
792
793 /* Trigger */
794 if (addr_trig_reg) {
795 /* If slave address is 0 this clears the trigger */
796 result = inv_serial_single_write(gyro_handle,
797 mldl_cfg->mpu_chip_info->addr,
798 addr_trig_reg,
799 slave_address & ~BIT_I2C_READ);
800 if (result) {
801 LOG_RESULT_LOCATION(result);
802 return result;
803 }
804 }
805
806 if (slave && slave->trigger && reg_trig_reg) {
807 result = inv_serial_single_write(gyro_handle,
808 mldl_cfg->mpu_chip_info->addr,
809 reg_trig_reg,
810 slave->trigger->reg);
811 if (result) {
812 LOG_RESULT_LOCATION(result);
813 return result;
814 }
815 result = inv_serial_single_write(gyro_handle,
816 mldl_cfg->mpu_chip_info->addr,
817 ctrl_trig_reg,
818 BIT_SLV_ENABLE | 0x01);
819 if (result) {
820 LOG_RESULT_LOCATION(result);
821 return result;
822 }
823 result = inv_serial_single_write(gyro_handle,
824 mldl_cfg->mpu_chip_info->addr,
825 d0_trig_reg,
826 slave->trigger->value);
827 if (result) {
828 LOG_RESULT_LOCATION(result);
829 return result;
830 }
831 } else if (ctrl_trig_reg) {
832 result = inv_serial_single_write(gyro_handle,
833 mldl_cfg->mpu_chip_info->addr,
834 ctrl_trig_reg, 0x00);
835 if (result) {
836 LOG_RESULT_LOCATION(result);
837 return result;
838 }
839 }
840
841 /* Data rate */
842 if (slave) {
843 struct ext_slave_config config;
844 long data;
845 config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
846 config.len = sizeof(long);
847 config.apply = false;
848 config.data = &data;
849 if (!(slave->get_config))
850 return INV_ERROR_INVALID_CONFIGURATION;
851
852 result = slave->get_config(NULL, slave, slave_pdata, &config);
853 if (result) {
854 LOG_RESULT_LOCATION(result);
855 return result;
856 }
857 MPL_LOGI("Slave %d ODR: %ld Hz\n", slave_id, data / 1000);
858 divider = ((1000 * inv_mpu_get_sampling_rate_hz(
859 mldl_cfg->mpu_gyro_cfg))
860 / data) - 1;
861 } else {
862 divider = 0;
863 }
864
865 result = inv_serial_read(gyro_handle,
866 mldl_cfg->mpu_chip_info->addr,
867 MPUREG_I2C_MST_DELAY_CTRL,
868 1, &delay_ctrl_orig);
869 delay_ctrl = delay_ctrl_orig;
870 if (result) {
871 LOG_RESULT_LOCATION(result);
872 return result;
873 }
874
875 if (divider > 0 && divider <= MASK_I2C_MST_DLY) {
876 result = inv_serial_read(gyro_handle,
877 mldl_cfg->mpu_chip_info->addr,
878 MPUREG_I2C_SLV4_CTRL, 1, &reg);
879 if (result) {
880 LOG_RESULT_LOCATION(result);
881 return result;
882 }
883 if ((reg & MASK_I2C_MST_DLY) &&
884 ((long)(reg & MASK_I2C_MST_DLY) !=
885 (divider & MASK_I2C_MST_DLY))) {
886 MPL_LOGW("Changing slave divider: %ld to %ld\n",
887 (long)(reg & MASK_I2C_MST_DLY),
888 (divider & MASK_I2C_MST_DLY));
889
890 }
891 reg |= (unsigned char)(divider & MASK_I2C_MST_DLY);
892 result = inv_serial_single_write(gyro_handle,
893 mldl_cfg->mpu_chip_info->addr,
894 MPUREG_I2C_SLV4_CTRL,
895 reg);
896 if (result) {
897 LOG_RESULT_LOCATION(result);
898 return result;
899 }
900
901 delay_ctrl |= bits_slave_delay;
902 } else {
903 delay_ctrl &= ~(bits_slave_delay);
904 }
905 if (delay_ctrl != delay_ctrl_orig) {
906 result = inv_serial_single_write(
907 gyro_handle, mldl_cfg->mpu_chip_info->addr,
908 MPUREG_I2C_MST_DELAY_CTRL,
909 delay_ctrl);
910 if (result) {
911 LOG_RESULT_LOCATION(result);
912 return result;
913 }
914 }
915
916 if (slave_address)
917 mldl_cfg->inv_mpu_state->i2c_slaves_enabled |=
918 bits_slave_delay;
919 else
920 mldl_cfg->inv_mpu_state->i2c_slaves_enabled &=
921 ~bits_slave_delay;
922
923 return result;
924}
925
926static int mpu_set_slave(struct mldl_cfg *mldl_cfg,
927 void *gyro_handle,
928 struct ext_slave_descr *slave,
929 struct ext_slave_platform_data *slave_pdata,
930 int slave_id)
931{
932 return mpu_set_slave_mpu60xx(mldl_cfg, gyro_handle, slave,
933 slave_pdata, slave_id);
934}
935/**
936 * Check to see if the gyro was reset by testing a couple of registers known
937 * to change on reset.
938 *
939 * @mldl_cfg mldl configuration structure
940 * @gyro_handle handle used to communicate with the gyro
941 *
942 * @return INV_SUCCESS or non-zero error code
943 */
944static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
945{
946 int result = INV_SUCCESS;
947 unsigned char reg;
948
949 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
950 MPUREG_DMP_CFG_2, 1, &reg);
951 if (result) {
952 LOG_RESULT_LOCATION(result);
953 return result;
954 }
955
956 if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
957 return true;
958
959 if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
960 return false;
961
962 /* Inconclusive assume it was reset */
963 return true;
964}
965
966
967int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
968 const unsigned char *data, int size)
969{
970 int bank, offset, write_size;
971 int result;
972 unsigned char read[MPU_MEM_BANK_SIZE];
973
974 if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
975#if INV_CACHE_DMP == 1
976 memcpy(mldl_cfg->mpu_ram->ram, data, size);
977 return INV_SUCCESS;
978#else
979 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
980 return INV_ERROR_MEMORY_SET;
981#endif
982 }
983
984 if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
985 LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
986 return INV_ERROR_MEMORY_SET;
987 }
988 /* Write and verify memory */
989 for (bank = 0; size > 0; bank++,
990 size -= write_size,
991 data += write_size) {
992 if (size > MPU_MEM_BANK_SIZE)
993 write_size = MPU_MEM_BANK_SIZE;
994 else
995 write_size = size;
996
997 result = inv_serial_write_mem(mlsl_handle,
998 mldl_cfg->mpu_chip_info->addr,
999 ((bank << 8) | 0x00),
1000 write_size,
1001 data);
1002 if (result) {
1003 LOG_RESULT_LOCATION(result);
1004 MPL_LOGE("Write mem error in bank %d\n", bank);
1005 return result;
1006 }
1007 result = inv_serial_read_mem(mlsl_handle,
1008 mldl_cfg->mpu_chip_info->addr,
1009 ((bank << 8) | 0x00),
1010 write_size,
1011 read);
1012 if (result) {
1013 LOG_RESULT_LOCATION(result);
1014 MPL_LOGE("Read mem error in bank %d\n", bank);
1015 return result;
1016 }
1017
1018#define ML_SKIP_CHECK 38
1019 for (offset = 0; offset < write_size; offset++) {
1020 /* skip the register memory locations */
1021 if (bank == 0 && offset < ML_SKIP_CHECK)
1022 continue;
1023 if (data[offset] != read[offset]) {
1024 result = INV_ERROR_SERIAL_WRITE;
1025 break;
1026 }
1027 }
1028 if (result != INV_SUCCESS) {
1029 LOG_RESULT_LOCATION(result);
1030 MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
1031 bank, offset);
1032 return result;
1033 }
1034 }
1035 return INV_SUCCESS;
1036}
1037
1038static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
1039 unsigned long sensors)
1040{
1041 int result;
1042 int ii;
1043 unsigned char reg;
1044 unsigned char regs[7];
1045
1046 /* Wake up the part */
1047 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, sensors);
1048 if (result) {
1049 LOG_RESULT_LOCATION(result);
1050 return result;
1051 }
1052
1053 /* Always set the INT_ENABLE and DIVIDER as the Accel Only mode for 6050
1054 can set these too */
1055 result = inv_serial_single_write(
1056 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1057 MPUREG_INT_ENABLE, (mldl_cfg->mpu_gyro_cfg->int_config));
1058 if (result) {
1059 LOG_RESULT_LOCATION(result);
1060 return result;
1061 }
1062 result = inv_serial_single_write(
1063 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1064 MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
1065 if (result) {
1066 LOG_RESULT_LOCATION(result);
1067 return result;
1068 }
1069
1070 if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
1071 !mpu_was_reset(mldl_cfg, gyro_handle)) {
1072 return INV_SUCCESS;
1073 }
1074
1075 /* Configure the MPU */
1076 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1077 if (result) {
1078 LOG_RESULT_LOCATION(result);
1079 return result;
1080 }
1081 result = mpu_set_clock_source(gyro_handle, mldl_cfg);
1082 if (result) {
1083 LOG_RESULT_LOCATION(result);
1084 return result;
1085 }
1086
1087 reg = MPUREG_GYRO_CONFIG_VALUE(0, 0, 0,
1088 mldl_cfg->mpu_gyro_cfg->full_scale);
1089 result = inv_serial_single_write(
1090 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1091 MPUREG_GYRO_CONFIG, reg);
1092 reg = MPUREG_CONFIG_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
1093 mldl_cfg->mpu_gyro_cfg->lpf);
1094 result = inv_serial_single_write(
1095 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1096 MPUREG_CONFIG, reg);
1097 if (result) {
1098 LOG_RESULT_LOCATION(result);
1099 return result;
1100 }
1101 result = inv_serial_single_write(
1102 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1103 MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
1104 if (result) {
1105 LOG_RESULT_LOCATION(result);
1106 return result;
1107 }
1108 result = inv_serial_single_write(
1109 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1110 MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
1111 if (result) {
1112 LOG_RESULT_LOCATION(result);
1113 return result;
1114 }
1115
1116 /* Write and verify memory */
1117#if INV_CACHE_DMP != 0
1118 inv_mpu_set_firmware(mldl_cfg, gyro_handle,
1119 mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
1120#endif
1121
1122 result = inv_serial_single_write(
1123 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1124 MPUREG_XG_OFFS_TC,
1125 ((mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC));
1126 if (result) {
1127 LOG_RESULT_LOCATION(result);
1128 return result;
1129 }
1130 regs[0] = ((mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC);
1131 result = inv_serial_single_write(
1132 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1133 MPUREG_YG_OFFS_TC, regs[0]);
1134 if (result) {
1135 LOG_RESULT_LOCATION(result);
1136 return result;
1137 }
1138 result = inv_serial_single_write(
1139 gyro_handle, mldl_cfg->mpu_chip_info->addr,
1140 MPUREG_ZG_OFFS_TC,
1141 ((mldl_cfg->mpu_offsets->tc[2] << 1) & BITS_ZG_OFFS_TC));
1142 if (result) {
1143 LOG_RESULT_LOCATION(result);
1144 return result;
1145 }
1146 regs[0] = MPUREG_X_OFFS_USRH;
1147 for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
1148 regs[1 + ii * 2] =
1149 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
1150 & 0xff;
1151 regs[1 + ii * 2 + 1] =
1152 (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
1153 }
1154 result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1155 7, regs);
1156 if (result) {
1157 LOG_RESULT_LOCATION(result);
1158 return result;
1159 }
1160
1161 /* Configure slaves */
1162 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1163 mldl_cfg->pdata->level_shifter);
1164 if (result) {
1165 LOG_RESULT_LOCATION(result);
1166 return result;
1167 }
1168 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
1169
1170 return result;
1171}
1172
1173int gyro_config(void *mlsl_handle,
1174 struct mldl_cfg *mldl_cfg,
1175 struct ext_slave_config *data)
1176{
1177 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1178 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1179 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1180 int ii;
1181
1182 if (!data->data)
1183 return INV_ERROR_INVALID_PARAMETER;
1184
1185 switch (data->key) {
1186 case MPU_SLAVE_INT_CONFIG:
1187 mpu_gyro_cfg->int_config = *((__u8 *)data->data);
1188 break;
1189 case MPU_SLAVE_EXT_SYNC:
1190 mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
1191 break;
1192 case MPU_SLAVE_FULL_SCALE:
1193 mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
1194 break;
1195 case MPU_SLAVE_LPF:
1196 mpu_gyro_cfg->lpf = *((__u8 *)data->data);
1197 break;
1198 case MPU_SLAVE_CLK_SRC:
1199 mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
1200 break;
1201 case MPU_SLAVE_DIVIDER:
1202 mpu_gyro_cfg->divider = *((__u8 *)data->data);
1203 break;
1204 case MPU_SLAVE_DMP_ENABLE:
1205 mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
1206 break;
1207 case MPU_SLAVE_FIFO_ENABLE:
1208 mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
1209 break;
1210 case MPU_SLAVE_DMP_CFG1:
1211 mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
1212 break;
1213 case MPU_SLAVE_DMP_CFG2:
1214 mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
1215 break;
1216 case MPU_SLAVE_TC:
1217 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1218 mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
1219 break;
1220 case MPU_SLAVE_GYRO:
1221 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1222 mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
1223 break;
1224 case MPU_SLAVE_ADDR:
1225 mpu_chip_info->addr = *((__u8 *)data->data);
1226 break;
1227 case MPU_SLAVE_PRODUCT_REVISION:
1228 mpu_chip_info->product_revision = *((__u8 *)data->data);
1229 break;
1230 case MPU_SLAVE_SILICON_REVISION:
1231 mpu_chip_info->silicon_revision = *((__u8 *)data->data);
1232 break;
1233 case MPU_SLAVE_PRODUCT_ID:
1234 mpu_chip_info->product_id = *((__u8 *)data->data);
1235 break;
1236 case MPU_SLAVE_GYRO_SENS_TRIM:
1237 mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
1238 break;
1239 case MPU_SLAVE_ACCEL_SENS_TRIM:
1240 mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
1241 break;
1242 case MPU_SLAVE_RAM:
1243 if (data->len != mldl_cfg->mpu_ram->length)
1244 return INV_ERROR_INVALID_PARAMETER;
1245
1246 memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
1247 break;
1248 default:
1249 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1250 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1251 };
1252 mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
1253 return INV_SUCCESS;
1254}
1255
1256int gyro_get_config(void *mlsl_handle,
1257 struct mldl_cfg *mldl_cfg,
1258 struct ext_slave_config *data)
1259{
1260 struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
1261 struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
1262 struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
1263 int ii;
1264
1265 if (!data->data)
1266 return INV_ERROR_INVALID_PARAMETER;
1267
1268 switch (data->key) {
1269 case MPU_SLAVE_INT_CONFIG:
1270 *((__u8 *)data->data) = mpu_gyro_cfg->int_config;
1271 break;
1272 case MPU_SLAVE_EXT_SYNC:
1273 *((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
1274 break;
1275 case MPU_SLAVE_FULL_SCALE:
1276 *((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
1277 break;
1278 case MPU_SLAVE_LPF:
1279 *((__u8 *)data->data) = mpu_gyro_cfg->lpf;
1280 break;
1281 case MPU_SLAVE_CLK_SRC:
1282 *((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
1283 break;
1284 case MPU_SLAVE_DIVIDER:
1285 *((__u8 *)data->data) = mpu_gyro_cfg->divider;
1286 break;
1287 case MPU_SLAVE_DMP_ENABLE:
1288 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
1289 break;
1290 case MPU_SLAVE_FIFO_ENABLE:
1291 *((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
1292 break;
1293 case MPU_SLAVE_DMP_CFG1:
1294 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
1295 break;
1296 case MPU_SLAVE_DMP_CFG2:
1297 *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
1298 break;
1299 case MPU_SLAVE_TC:
1300 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1301 ((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
1302 break;
1303 case MPU_SLAVE_GYRO:
1304 for (ii = 0; ii < GYRO_NUM_AXES; ii++)
1305 ((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
1306 break;
1307 case MPU_SLAVE_ADDR:
1308 *((__u8 *)data->data) = mpu_chip_info->addr;
1309 break;
1310 case MPU_SLAVE_PRODUCT_REVISION:
1311 *((__u8 *)data->data) = mpu_chip_info->product_revision;
1312 break;
1313 case MPU_SLAVE_SILICON_REVISION:
1314 *((__u8 *)data->data) = mpu_chip_info->silicon_revision;
1315 break;
1316 case MPU_SLAVE_PRODUCT_ID:
1317 *((__u8 *)data->data) = mpu_chip_info->product_id;
1318 break;
1319 case MPU_SLAVE_GYRO_SENS_TRIM:
1320 *((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
1321 break;
1322 case MPU_SLAVE_ACCEL_SENS_TRIM:
1323 *((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
1324 break;
1325 case MPU_SLAVE_RAM:
1326 if (data->len != mldl_cfg->mpu_ram->length)
1327 return INV_ERROR_INVALID_PARAMETER;
1328
1329 memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
1330 break;
1331 default:
1332 LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
1333 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1334 };
1335
1336 return INV_SUCCESS;
1337}
1338
1339
1340/*******************************************************************************
1341 *******************************************************************************
1342 * Exported functions
1343 *******************************************************************************
1344 ******************************************************************************/
1345
1346/**
1347 * Initializes the pdata structure to defaults.
1348 *
1349 * Opens the device to read silicon revision, product id and whoami.
1350 *
1351 * @mldl_cfg
1352 * The internal device configuration data structure.
1353 * @mlsl_handle
1354 * The serial communication handle.
1355 *
1356 * @return INV_SUCCESS if silicon revision, product id and woami are supported
1357 * by this software.
1358 */
1359int inv_mpu_open(struct mldl_cfg *mldl_cfg,
1360 void *gyro_handle,
1361 void *accel_handle,
1362 void *compass_handle, void *pressure_handle)
1363{
1364 int result;
1365 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1366 int ii;
1367
1368 /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
1369 ii = 0;
1370 mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
1371 mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
1372 mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
1373 mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
1374 mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
1375 mldl_cfg->mpu_gyro_cfg->divider = 4;
1376 mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
1377 mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
1378 mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
1379 mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
1380 mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
1381 mldl_cfg->inv_mpu_state->status =
1382 MPU_DMP_IS_SUSPENDED |
1383 MPU_GYRO_IS_SUSPENDED |
1384 MPU_ACCEL_IS_SUSPENDED |
1385 MPU_COMPASS_IS_SUSPENDED |
1386 MPU_PRESSURE_IS_SUSPENDED |
1387 MPU_DEVICE_IS_SUSPENDED;
1388 mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
1389
1390 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1391 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1392 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1393 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1394
1395 if (mldl_cfg->mpu_chip_info->addr == 0) {
1396 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1397 return INV_ERROR_INVALID_PARAMETER;
1398 }
1399
1400 /*
1401 * Reset,
1402 * Take the DMP out of sleep, and
1403 * read the product_id, sillicon rev and whoami
1404 */
1405 mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
1406 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, true,
1407 INV_THREE_AXIS_GYRO);
1408 if (result) {
1409 LOG_RESULT_LOCATION(result);
1410 return result;
1411 }
1412
1413 result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
1414 if (result) {
1415 LOG_RESULT_LOCATION(result);
1416 return result;
1417 }
1418
1419 /* Get the factory temperature compensation offsets */
1420 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1421 MPUREG_XG_OFFS_TC, 1,
1422 &mldl_cfg->mpu_offsets->tc[0]);
1423 if (result) {
1424 LOG_RESULT_LOCATION(result);
1425 return result;
1426 }
1427 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1428 MPUREG_YG_OFFS_TC, 1,
1429 &mldl_cfg->mpu_offsets->tc[1]);
1430 if (result) {
1431 LOG_RESULT_LOCATION(result);
1432 return result;
1433 }
1434 result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
1435 MPUREG_ZG_OFFS_TC, 1,
1436 &mldl_cfg->mpu_offsets->tc[2]);
1437 if (result) {
1438 LOG_RESULT_LOCATION(result);
1439 return result;
1440 }
1441
1442 /* Into bypass mode before sleeping and calling the slaves init */
1443 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
1444 if (result) {
1445 LOG_RESULT_LOCATION(result);
1446 return result;
1447 }
1448 result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
1449 mldl_cfg->pdata->level_shifter);
1450 if (result) {
1451 LOG_RESULT_LOCATION(result);
1452 return result;
1453 }
1454
1455 for (ii = 0; ii < GYRO_NUM_AXES; ii++) {
1456 mldl_cfg->mpu_offsets->tc[ii] =
1457 (mldl_cfg->mpu_offsets->tc[ii] & BITS_XG_OFFS_TC) >> 1;
1458 }
1459
1460#if INV_CACHE_DMP != 0
1461 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, 0);
1462#endif
1463 if (result) {
1464 LOG_RESULT_LOCATION(result);
1465 return result;
1466 }
1467
1468
1469 return result;
1470
1471}
1472
1473/**
1474 * Close the mpu interface
1475 *
1476 * @mldl_cfg pointer to the configuration structure
1477 * @mlsl_handle pointer to the serial layer handle
1478 *
1479 * @return INV_SUCCESS or non-zero error code
1480 */
1481int inv_mpu_close(struct mldl_cfg *mldl_cfg,
1482 void *gyro_handle,
1483 void *accel_handle,
1484 void *compass_handle,
1485 void *pressure_handle)
1486{
1487 return 0;
1488}
1489
1490/**
1491 * @brief resume the MPU device and all the other sensor
1492 * devices from their low power state.
1493 *
1494 * @mldl_cfg
1495 * pointer to the configuration structure
1496 * @gyro_handle
1497 * the main file handle to the MPU device.
1498 * @accel_handle
1499 * an handle to the accelerometer device, if sitting
1500 * onto a separate bus. Can match mlsl_handle if
1501 * the accelerometer device operates on the same
1502 * primary bus of MPU.
1503 * @compass_handle
1504 * an handle to the compass device, if sitting
1505 * onto a separate bus. Can match mlsl_handle if
1506 * the compass device operates on the same
1507 * primary bus of MPU.
1508 * @pressure_handle
1509 * an handle to the pressure sensor device, if sitting
1510 * onto a separate bus. Can match mlsl_handle if
1511 * the pressure sensor device operates on the same
1512 * primary bus of MPU.
1513 * @resume_gyro
1514 * whether resuming the gyroscope device is
1515 * actually needed (if the device supports low power
1516 * mode of some sort).
1517 * @resume_accel
1518 * whether resuming the accelerometer device is
1519 * actually needed (if the device supports low power
1520 * mode of some sort).
1521 * @resume_compass
1522 * whether resuming the compass device is
1523 * actually needed (if the device supports low power
1524 * mode of some sort).
1525 * @resume_pressure
1526 * whether resuming the pressure sensor device is
1527 * actually needed (if the device supports low power
1528 * mode of some sort).
1529 * @return INV_SUCCESS or a non-zero error code.
1530 */
1531int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
1532 void *gyro_handle,
1533 void *accel_handle,
1534 void *compass_handle,
1535 void *pressure_handle,
1536 unsigned long sensors)
1537{
1538 int result = INV_SUCCESS;
1539 int ii;
1540 bool resume_slave[EXT_SLAVE_NUM_TYPES];
1541 bool resume_dmp = sensors & INV_DMP_PROCESSOR;
1542 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1543 resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1544 (sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1545 resume_slave[EXT_SLAVE_TYPE_ACCEL] =
1546 sensors & INV_THREE_AXIS_ACCEL;
1547 resume_slave[EXT_SLAVE_TYPE_COMPASS] =
1548 sensors & INV_THREE_AXIS_COMPASS;
1549 resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
1550 sensors & INV_THREE_AXIS_PRESSURE;
1551
1552 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1553 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1554 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1555 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1556
1557
1558 mldl_print_cfg(mldl_cfg);
1559
1560 /* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
1561 for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1562 if (resume_slave[ii] &&
1563 ((!mldl_cfg->slave[ii]) ||
1564 (!mldl_cfg->slave[ii]->resume))) {
1565 LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
1566 return INV_ERROR_INVALID_PARAMETER;
1567 }
1568 }
1569
1570 if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
1571 && ((mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED) ||
1572 (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
1573 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1574 if (result) {
1575 LOG_RESULT_LOCATION(result);
1576 return result;
1577 }
1578 result = dmp_stop(mldl_cfg, gyro_handle);
1579 if (result) {
1580 LOG_RESULT_LOCATION(result);
1581 return result;
1582 }
1583 result = gyro_resume(mldl_cfg, gyro_handle, sensors);
1584 if (result) {
1585 LOG_RESULT_LOCATION(result);
1586 return result;
1587 }
1588 }
1589
1590 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1591 if (!mldl_cfg->slave[ii] ||
1592 !mldl_cfg->pdata_slave[ii] ||
1593 !resume_slave[ii] ||
1594 !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
1595 continue;
1596
1597 if (EXT_SLAVE_BUS_SECONDARY ==
1598 mldl_cfg->pdata_slave[ii]->bus) {
1599 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
1600 true);
1601 if (result) {
1602 LOG_RESULT_LOCATION(result);
1603 return result;
1604 }
1605 }
1606 result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
1607 mldl_cfg->slave[ii],
1608 mldl_cfg->pdata_slave[ii]);
1609 if (result) {
1610 LOG_RESULT_LOCATION(result);
1611 return result;
1612 }
1613 mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
1614 }
1615
1616 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1617 if (resume_dmp &&
1618 !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
1619 mldl_cfg->pdata_slave[ii] &&
1620 EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
1621 result = mpu_set_slave(mldl_cfg,
1622 gyro_handle,
1623 mldl_cfg->slave[ii],
1624 mldl_cfg->pdata_slave[ii],
1625 mldl_cfg->slave[ii]->type);
1626 if (result) {
1627 LOG_RESULT_LOCATION(result);
1628 return result;
1629 }
1630 }
1631 }
1632
1633 /* Turn on the master i2c iterface if necessary */
1634 if (resume_dmp) {
1635 result = mpu_set_i2c_bypass(
1636 mldl_cfg, gyro_handle,
1637 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1638 if (result) {
1639 LOG_RESULT_LOCATION(result);
1640 return result;
1641 }
1642
1643 /* Now start */
1644 result = dmp_start(mldl_cfg, gyro_handle);
1645 if (result) {
1646 LOG_RESULT_LOCATION(result);
1647 return result;
1648 }
1649 }
1650 mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
1651
1652 return result;
1653}
1654
1655/**
1656 * @brief suspend the MPU device and all the other sensor
1657 * devices into their low power state.
1658 * @mldl_cfg
1659 * a pointer to the struct mldl_cfg internal data
1660 * structure.
1661 * @gyro_handle
1662 * the main file handle to the MPU device.
1663 * @accel_handle
1664 * an handle to the accelerometer device, if sitting
1665 * onto a separate bus. Can match gyro_handle if
1666 * the accelerometer device operates on the same
1667 * primary bus of MPU.
1668 * @compass_handle
1669 * an handle to the compass device, if sitting
1670 * onto a separate bus. Can match gyro_handle if
1671 * the compass device operates on the same
1672 * primary bus of MPU.
1673 * @pressure_handle
1674 * an handle to the pressure sensor device, if sitting
1675 * onto a separate bus. Can match gyro_handle if
1676 * the pressure sensor device operates on the same
1677 * primary bus of MPU.
1678 * @accel
1679 * whether suspending the accelerometer device is
1680 * actually needed (if the device supports low power
1681 * mode of some sort).
1682 * @compass
1683 * whether suspending the compass device is
1684 * actually needed (if the device supports low power
1685 * mode of some sort).
1686 * @pressure
1687 * whether suspending the pressure sensor device is
1688 * actually needed (if the device supports low power
1689 * mode of some sort).
1690 * @return INV_SUCCESS or a non-zero error code.
1691 */
1692int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
1693 void *gyro_handle,
1694 void *accel_handle,
1695 void *compass_handle,
1696 void *pressure_handle,
1697 unsigned long sensors)
1698{
1699 int result = INV_SUCCESS;
1700 int ii;
1701 struct ext_slave_descr **slave = mldl_cfg->slave;
1702 struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
1703 bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
1704 bool suspend_slave[EXT_SLAVE_NUM_TYPES];
1705 void *slave_handle[EXT_SLAVE_NUM_TYPES];
1706
1707 suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
1708 ((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
1709 == (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
1710 suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
1711 ((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
1712 suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
1713 ((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
1714 suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
1715 ((sensors & INV_THREE_AXIS_PRESSURE) ==
1716 INV_THREE_AXIS_PRESSURE);
1717
1718 slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
1719 slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
1720 slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
1721 slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
1722
1723 if (suspend_dmp) {
1724 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1725 if (result) {
1726 LOG_RESULT_LOCATION(result);
1727 return result;
1728 }
1729 result = dmp_stop(mldl_cfg, gyro_handle);
1730 if (result) {
1731 LOG_RESULT_LOCATION(result);
1732 return result;
1733 }
1734 }
1735
1736 /* Gyro */
1737 if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
1738 !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
1739 result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false,
1740 ((~sensors) & INV_ALL_SENSORS));
1741 if (result) {
1742 LOG_RESULT_LOCATION(result);
1743 return result;
1744 }
1745 }
1746
1747 for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
1748 bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
1749 if (!slave[ii] || !pdata_slave[ii] ||
1750 is_suspended || !suspend_slave[ii])
1751 continue;
1752
1753 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1754 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1755 if (result) {
1756 LOG_RESULT_LOCATION(result);
1757 return result;
1758 }
1759 }
1760 result = slave[ii]->suspend(slave_handle[ii],
1761 slave[ii],
1762 pdata_slave[ii]);
1763 if (result) {
1764 LOG_RESULT_LOCATION(result);
1765 return result;
1766 }
1767 if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
1768 result = mpu_set_slave(mldl_cfg, gyro_handle,
1769 NULL, NULL,
1770 slave[ii]->type);
1771 if (result) {
1772 LOG_RESULT_LOCATION(result);
1773 return result;
1774 }
1775 }
1776 mldl_cfg->inv_mpu_state->status |= (1 << ii);
1777 }
1778
1779 /* Re-enable the i2c master if there are configured slaves and DMP */
1780 if (!suspend_dmp) {
1781 result = mpu_set_i2c_bypass(
1782 mldl_cfg, gyro_handle,
1783 !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
1784 if (result) {
1785 LOG_RESULT_LOCATION(result);
1786 return result;
1787 }
1788 }
1789 mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
1790
1791 return result;
1792}
1793
1794int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
1795 void *gyro_handle,
1796 void *slave_handle,
1797 struct ext_slave_descr *slave,
1798 struct ext_slave_platform_data *pdata,
1799 unsigned char *data)
1800{
1801 int result;
1802 int bypass_result;
1803 int remain_bypassed = true;
1804
1805 if (NULL == slave || NULL == slave->read) {
1806 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1807 return INV_ERROR_INVALID_CONFIGURATION;
1808 }
1809
1810 if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1811 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1812 remain_bypassed = false;
1813 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1814 if (result) {
1815 LOG_RESULT_LOCATION(result);
1816 return result;
1817 }
1818 }
1819
1820 result = slave->read(slave_handle, slave, pdata, data);
1821
1822 if (!remain_bypassed) {
1823 bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1824 if (bypass_result) {
1825 LOG_RESULT_LOCATION(bypass_result);
1826 return bypass_result;
1827 }
1828 }
1829 return result;
1830}
1831
1832int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
1833 void *gyro_handle,
1834 void *slave_handle,
1835 struct ext_slave_config *data,
1836 struct ext_slave_descr *slave,
1837 struct ext_slave_platform_data *pdata)
1838{
1839 int result;
1840 int remain_bypassed = true;
1841
1842 if (NULL == slave || NULL == slave->config) {
1843 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1844 return INV_ERROR_INVALID_CONFIGURATION;
1845 }
1846
1847 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1848 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1849 remain_bypassed = false;
1850 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1851 if (result) {
1852 LOG_RESULT_LOCATION(result);
1853 return result;
1854 }
1855 }
1856
1857 result = slave->config(slave_handle, slave, pdata, data);
1858 if (result) {
1859 LOG_RESULT_LOCATION(result);
1860 return result;
1861 }
1862
1863 if (!remain_bypassed) {
1864 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1865 if (result) {
1866 LOG_RESULT_LOCATION(result);
1867 return result;
1868 }
1869 }
1870 return result;
1871}
1872
1873int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
1874 void *gyro_handle,
1875 void *slave_handle,
1876 struct ext_slave_config *data,
1877 struct ext_slave_descr *slave,
1878 struct ext_slave_platform_data *pdata)
1879{
1880 int result;
1881 int remain_bypassed = true;
1882
1883 if (NULL == slave || NULL == slave->get_config) {
1884 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
1885 return INV_ERROR_INVALID_CONFIGURATION;
1886 }
1887
1888 if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
1889 && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
1890 remain_bypassed = false;
1891 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
1892 if (result) {
1893 LOG_RESULT_LOCATION(result);
1894 return result;
1895 }
1896 }
1897
1898 result = slave->get_config(slave_handle, slave, pdata, data);
1899 if (result) {
1900 LOG_RESULT_LOCATION(result);
1901 return result;
1902 }
1903
1904 if (!remain_bypassed) {
1905 result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
1906 if (result) {
1907 LOG_RESULT_LOCATION(result);
1908 return result;
1909 }
1910 }
1911 return result;
1912}
1913
1914/**
1915 * @}
1916 */
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-10 03:01:24 -0400