Revert "Merge commit 'main-jb-2012.08.03-B4' into t114-0806"
[linux-2.6.git] / drivers / misc / inv_mpu / mpu6050 / mldl_cfg.c
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  */
58 static 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  */
92 static 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  */
164 static 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  */
254 static 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 */
268 struct 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 */
276 static 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  */
341 short 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  */
369 static 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  */
459 static 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  */
500 static 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  */
589 static 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
665 static 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
926 static 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  */
944 static 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
967 int 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
1038 static 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
1173 int 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
1256 int 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  */
1359 int 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  */
1481 int 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  */
1531 int 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  */
1692 int 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
1794 int 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
1832 int 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
1873 int 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  */