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