From patchwork Fri Jul 1 02:18:21 2011 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Nathan Royer X-Patchwork-Id: 934162 Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by demeter1.kernel.org (8.14.4/8.14.4) with ESMTP id p612K2Fc013058 for ; Fri, 1 Jul 2011 02:20:02 GMT Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1754462Ab1GACT1 (ORCPT ); Thu, 30 Jun 2011 22:19:27 -0400 Received: from na3sys009aob106.obsmtp.com ([74.125.149.76]:39876 "HELO na3sys009aog106.obsmtp.com" rhost-flags-OK-OK-OK-FAIL) by vger.kernel.org with SMTP id S1754630Ab1GACTZ (ORCPT ); Thu, 30 Jun 2011 22:19:25 -0400 Received: from mail-iy0-f172.google.com ([209.85.210.172]) (using TLSv1) by na3sys009aob106.postini.com ([74.125.148.12]) with SMTP ID DSNKTg0uqtHupV+YbY606J/0IHc04Qiecya6@postini.com; Thu, 30 Jun 2011 19:19:25 PDT Received: by mail-iy0-f172.google.com with SMTP id 7so4276337iye.17 for ; Thu, 30 Jun 2011 19:19:22 -0700 (PDT) Received: by 10.43.54.146 with SMTP id vu18mr2702655icb.296.1309486762084; Thu, 30 Jun 2011 19:19:22 -0700 (PDT) Received: from localhost.localdomain ([209.119.66.34]) by mx.google.com with ESMTPS id j2sm2780359icu.22.2011.06.30.19.19.19 (version=TLSv1/SSLv3 cipher=OTHER); Thu, 30 Jun 2011 19:19:21 -0700 (PDT) From: Nathan Royer To: Nathan Royer Cc: Andrew Morton , Greg Kroah-Hartman , Jonathan Cameron , Jiri Kosina , Alan Cox , Jean Delvare , linux-kernel@vger.kernel.org, linux-input@vger.kernel.org Subject: [PATCH 05/11] misc: MPU3050 and slave device configuration. Date: Thu, 30 Jun 2011 19:18:21 -0700 Message-Id: <1309486707-1658-5-git-send-email-nroyer@invensense.com> X-Mailer: git-send-email 1.7.4.1 In-Reply-To: <1309486707-1658-1-git-send-email-nroyer@invensense.com> References: <1309486707-1658-1-git-send-email-nroyer@invensense.com> Sender: linux-input-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-input@vger.kernel.org X-Greylist: IP, sender and recipient auto-whitelisted, not delayed by milter-greylist-4.2.6 (demeter1.kernel.org [140.211.167.41]); Fri, 01 Jul 2011 02:20:02 +0000 (UTC) Signed-off-by: Nathan Royer --- drivers/misc/inv_mpu/mldl_cfg.c | 1737 +++++++++++++++++++++++++++++++++++++++ 1 files changed, 1737 insertions(+), 0 deletions(-) create mode 100644 drivers/misc/inv_mpu/mldl_cfg.c diff --git a/drivers/misc/inv_mpu/mldl_cfg.c b/drivers/misc/inv_mpu/mldl_cfg.c new file mode 100644 index 0000000..55a772d --- /dev/null +++ b/drivers/misc/inv_mpu/mldl_cfg.c @@ -0,0 +1,1737 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + $ + */ + +/** + * @addtogroup MLDL + * + * @{ + * @file mldl_cfg.c + * @brief The Motion Library Driver Layer. + */ + +/* -------------------------------------------------------------------------- */ +#include +#include + +#include + +#include "mldl_cfg.h" +#include +# include "mpu3050.h" + +#include "mlsl.h" +#include "mldl_print_cfg.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "mldl_cfg:" + +/* -------------------------------------------------------------------------- */ + +#define SLEEP 1 +#define WAKE_UP 0 +#define RESET 1 +#define STANDBY 1 + +/* -------------------------------------------------------------------------- */ + +/** + * @brief Stop the DMP running + * + * @return INV_SUCCESS or non-zero error code + */ +static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle) +{ + unsigned char user_ctrl_reg; + int result; + + if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) + return INV_SUCCESS; + + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, 1, &user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST; + user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST; + + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED; + + return result; +} + +/** + * @brief Starts the DMP running + * + * @return INV_SUCCESS or non-zero error code + */ +static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle) +{ + unsigned char user_ctrl_reg; + int result; + + if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) && + mldl_cfg->mpu_gyro_cfg->dmp_enable) + || + ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) && + !mldl_cfg->mpu_gyro_cfg->dmp_enable)) + return INV_SUCCESS; + + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, 1, &user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, + ((user_ctrl_reg & (~BIT_FIFO_EN)) + | BIT_FIFO_RST)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, 1, &user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + user_ctrl_reg |= BIT_DMP_EN; + + if (mldl_cfg->mpu_gyro_cfg->fifo_enable) + user_ctrl_reg |= BIT_FIFO_EN; + else + user_ctrl_reg &= ~BIT_FIFO_EN; + + user_ctrl_reg |= BIT_DMP_RST; + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, user_ctrl_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status &= ~MPU_DMP_IS_SUSPENDED; + + return result; +} + + + +static int mpu3050_set_i2c_bypass(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, unsigned char enable) +{ + unsigned char b; + int result; + unsigned char status = mldl_cfg->inv_mpu_state->status; + + if ((status & MPU_GYRO_IS_BYPASSED && enable) || + (!(status & MPU_GYRO_IS_BYPASSED) && !enable)) + return INV_SUCCESS; + + /*---- get current 'USER_CTRL' into b ----*/ + result = inv_serial_read( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, 1, &b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + b &= ~BIT_AUX_IF_EN; + + if (!enable) { + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, + (b | BIT_AUX_IF_EN)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + /* Coming out of I2C is tricky due to several erratta. Do not + * modify this algorithm + */ + /* + * 1) wait for the right time and send the command to change + * the aux i2c slave address to an invalid address that will + * get nack'ed + * + * 0x00 is broadcast. 0x7F is unlikely to be used by any aux. + */ + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_AUX_SLV_ADDR, 0x7F); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* + * 2) wait enough time for a nack to occur, then go into + * bypass mode: + */ + msleep(2); + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, (b)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* + * 3) wait for up to one MPU cycle then restore the slave + * address + */ + msleep(inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg) + / 1000); + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_AUX_SLV_ADDR, + mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL] + ->address); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* + * 4) reset the ime interface + */ + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, + (b | BIT_AUX_IF_RST)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + msleep(2); + } + if (enable) + mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED; + else + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED; + + return result; +} + + +/** + * @brief enables/disables the I2C bypass to an external device + * connected to MPU's secondary I2C bus. + * @param enable + * Non-zero to enable pass through. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, unsigned char enable) +{ + return mpu3050_set_i2c_bypass(mldl_cfg, mlsl_handle, enable); +} + + +#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map)) + +/* NOTE : when not indicated, product revision + is considered an 'npp'; non production part */ + +struct prod_rev_map_t { + unsigned char silicon_rev; + unsigned short gyro_trim; +}; + +#define OLDEST_PROD_REV_SUPPORTED 11 +static struct prod_rev_map_t prod_rev_map[] = { + {0, 0}, + {MPU_SILICON_REV_A4, 131}, /* 1 A? OBSOLETED */ + {MPU_SILICON_REV_A4, 131}, /* 2 | */ + {MPU_SILICON_REV_A4, 131}, /* 3 | */ + {MPU_SILICON_REV_A4, 131}, /* 4 | */ + {MPU_SILICON_REV_A4, 131}, /* 5 | */ + {MPU_SILICON_REV_A4, 131}, /* 6 | */ + {MPU_SILICON_REV_A4, 131}, /* 7 | */ + {MPU_SILICON_REV_A4, 131}, /* 8 | */ + {MPU_SILICON_REV_A4, 131}, /* 9 | */ + {MPU_SILICON_REV_A4, 131}, /* 10 V */ + {MPU_SILICON_REV_B1, 131}, /* 11 B1 */ + {MPU_SILICON_REV_B1, 131}, /* 12 | */ + {MPU_SILICON_REV_B1, 131}, /* 13 | */ + {MPU_SILICON_REV_B1, 131}, /* 14 V */ + {MPU_SILICON_REV_B4, 131}, /* 15 B4 */ + {MPU_SILICON_REV_B4, 131}, /* 16 | */ + {MPU_SILICON_REV_B4, 131}, /* 17 | */ + {MPU_SILICON_REV_B4, 131}, /* 18 | */ + {MPU_SILICON_REV_B4, 115}, /* 19 | */ + {MPU_SILICON_REV_B4, 115}, /* 20 V */ + {MPU_SILICON_REV_B6, 131}, /* 21 B6 (B6/A9) */ + {MPU_SILICON_REV_B4, 115}, /* 22 B4 (B7/A10) */ + {MPU_SILICON_REV_B6, 0}, /* 23 B6 */ + {MPU_SILICON_REV_B6, 0}, /* 24 | */ + {MPU_SILICON_REV_B6, 0}, /* 25 | */ + {MPU_SILICON_REV_B6, 131}, /* 26 V (B6/A11) */ +}; + +/** + * @internal + * @brief Get the silicon revision ID from OTP for MPU3050. + * The silicon revision number is in read from OTP bank 0, + * ADDR6[7:2]. The corresponding ID is retrieved by lookup + * in a map. + * + * @param mldl_cfg + * a pointer to the mldl config data structure. + * @param mlsl_handle + * an file handle to the serial communication device the + * device is connected to. + * + * @return 0 on success, a non-zero error code otherwise. + */ +static int inv_get_silicon_rev_mpu3050( + struct mldl_cfg *mldl_cfg, void *mlsl_handle) +{ + int result; + unsigned char index = 0x00; + unsigned char bank = + (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0); + unsigned short mem_addr = ((bank << 8) | 0x06); + struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info; + + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PRODUCT_ID, 1, + &mpu_chip_info->product_id); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_read_mem( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + mem_addr, 1, &index); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + index >>= 2; + + /* clean the prefetch and cfg user bank bits */ + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_BANK_SEL, 0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (index < OLDEST_PROD_REV_SUPPORTED || index >= NUM_OF_PROD_REVS) { + mpu_chip_info->silicon_revision = 0; + mpu_chip_info->gyro_sens_trim = 0; + MPL_LOGE("Unsupported Product Revision Detected : %d\n", index); + return INV_ERROR_INVALID_MODULE; + } + + mpu_chip_info->product_revision = index; + mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev; + mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim; + if (mpu_chip_info->gyro_sens_trim == 0) { + MPL_LOGE("gyro sensitivity trim is 0" + " - unsupported non production part.\n"); + return INV_ERROR_INVALID_MODULE; + } + + return result; +} +#define inv_get_silicon_rev inv_get_silicon_rev_mpu3050 + + +/** + * @brief Enable / Disable the use MPU's secondary I2C interface level + * shifters. + * When enabled the secondary I2C interface to which the external + * device is connected runs at VDD voltage (main supply). + * When disabled the 2nd interface runs at VDDIO voltage. + * See the device specification for more details. + * + * @note using this API may produce unpredictable results, depending on how + * the MPU and slave device are setup on the target platform. + * Use of this API should entirely be restricted to system + * integrators. Once the correct value is found, there should be no + * need to change the level shifter at runtime. + * + * @pre Must be called after inv_serial_start(). + * @note Typically called before inv_dmp_open(). + * + * @param[in] enable: + * 0 to run at VDDIO (default), + * 1 to run at VDD. + * + * @return INV_SUCCESS if successfull, a non-zero error code otherwise. + */ +static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, unsigned char enable) +{ + int result; + unsigned char reg; + unsigned char mask; + unsigned char regval; + + if (0 == mldl_cfg->mpu_chip_info->silicon_revision) + return INV_ERROR_INVALID_PARAMETER; + + /*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR -- + NOTE: this is incompatible with ST accelerometers where the VDDIO + bit MUST be set to enable ST's internal logic to autoincrement + the register address on burst reads --*/ + if ((mldl_cfg->mpu_chip_info->silicon_revision & 0xf) + < MPU_SILICON_REV_B6) { + reg = MPUREG_ACCEL_BURST_ADDR; + mask = 0x80; + } else { + /*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 => + the mask is always 0x04 --*/ + reg = MPUREG_FIFO_EN2; + mask = 0x04; + } + + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + reg, 1, ®val); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (enable) + regval |= mask; + else + regval &= ~mask; + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, reg, regval); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; + return INV_SUCCESS; +} + + +/** + * @internal + * @brief This function controls the power management on the MPU device. + * The entire chip can be put to low power sleep mode, or individual + * gyros can be turned on/off. + * + * Putting the device into sleep mode depending upon the changing needs + * of the associated applications is a recommended method for reducing + * power consuption. It is a safe opearation in that sleep/wake up of + * gyros while running will not result in any interruption of data. + * + * Although it is entirely allowed to put the device into full sleep + * while running the DMP, it is not recomended because it will disrupt + * the ongoing calculations carried on inside the DMP and consequently + * the sensor fusion algorithm. Furthermore, while in sleep mode + * read & write operation from the app processor on both registers and + * memory are disabled and can only regained by restoring the MPU in + * normal power mode. + * Disabling any of the gyro axis will reduce the associated power + * consuption from the PLL but will not stop the DMP from running + * state. + * + * @param reset + * Non-zero to reset the device. Note that this setting + * is volatile and the corresponding register bit will + * clear itself right after being applied. + * @param sleep + * Non-zero to put device into full sleep. + * @param disable_gx + * Non-zero to disable gyro X. + * @param disable_gy + * Non-zero to disable gyro Y. + * @param disable_gz + * Non-zero to disable gyro Z. + * + * @return INV_SUCCESS if successfull; a non-zero error code otherwise. + */ +static int mpu3050_pwr_mgmt(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + unsigned char reset, + unsigned char sleep, + unsigned char disable_gx, + unsigned char disable_gy, + unsigned char disable_gz) +{ + unsigned char b; + int result; + + result = + inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, 1, &b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* If we are awake, we need to put it in bypass before resetting */ + if ((!(b & BIT_SLEEP)) && reset) + result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1); + + /* Reset if requested */ + if (reset) { + MPL_LOGV("Reset MPU3050\n"); + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b | BIT_H_RESET); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + msleep(5); + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_RESET; + /* Some chips are awake after reset and some are asleep, + * check the status */ + result = inv_serial_read( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, 1, &b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + /* Update the suspended state just in case we return early */ + if (b & BIT_SLEEP) + mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED; + else + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED; + + /* if power status match requested, nothing else's left to do */ + if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) == + (((sleep != 0) * BIT_SLEEP) | + ((disable_gx != 0) * BIT_STBY_XG) | + ((disable_gy != 0) * BIT_STBY_YG) | + ((disable_gz != 0) * BIT_STBY_ZG))) { + return INV_SUCCESS; + } + + /* + * This specific transition between states needs to be reinterpreted: + * (1,1,1,1) -> (0,1,1,1) has to become + * (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1) + * where + * (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1) + */ + if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) == + (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) + && ((!sleep) && disable_gx && disable_gy && disable_gz)) { + result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, 1, 0, 0, 0); + if (result) + return result; + b |= BIT_SLEEP; + b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG); + } + + if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) { + if (sleep) { + result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + b |= BIT_SLEEP; + result = + inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status |= + MPU_GYRO_IS_SUSPENDED; + } else { + b &= ~BIT_SLEEP; + result = + inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status &= + ~MPU_GYRO_IS_SUSPENDED; + msleep(5); + } + } + /*--- + WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE + 1) put one axis at a time in stand-by + ---*/ + if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) { + b ^= BIT_STBY_XG; + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) { + b ^= BIT_STBY_YG; + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) { + b ^= BIT_STBY_ZG; + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + return INV_SUCCESS; +} + + +/** + * @brief sets the clock source for the gyros. + * @param mldl_cfg + * a pointer to the struct mldl_cfg data structure. + * @param gyro_handle + * an handle to the serial device the gyro is assigned to. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg) +{ + int result; + unsigned char cur_clk_src; + unsigned char reg; + + /* clock source selection */ + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cur_clk_src = reg & BITS_CLKSEL; + reg &= ~BITS_CLKSEL; + + + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* TODO : workarounds to be determined and implemented */ + + return result; +} + +/** + * Configures the MPU I2C Master + * + * @mldl_cfg Handle to the configuration data + * @gyro_handle handle to the gyro communictation interface + * @slave Can be Null if turning off the slave + * @slave_pdata Can be null if turning off the slave + * @slave_id enum ext_slave_type to determine which index to use + * + * + * This fucntion configures the slaves by: + * 1) Setting up the read + * a) Read Register + * b) Read Length + * 2) Set up the data trigger (MPU6050 only) + * a) Set trigger write register + * b) Set Trigger write value + * 3) Set up the divider (MPU6050 only) + * 4) Set the slave bypass mode depending on slave + * + * returns INV_SUCCESS or non-zero error code + */ +static int mpu_set_slave_mpu3050(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *slave_pdata, + int slave_id) +{ + int result; + unsigned char reg; + unsigned char slave_reg; + unsigned char slave_len; + unsigned char slave_endian; + unsigned char slave_address; + + if (slave_id != EXT_SLAVE_TYPE_ACCEL) + return 0; + + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true); + + if (NULL == slave || NULL == slave_pdata) { + slave_reg = 0; + slave_len = 0; + slave_endian = 0; + slave_address = 0; + mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0; + } else { + slave_reg = slave->read_reg; + slave_len = slave->read_len; + slave_endian = slave->endian; + slave_address = slave_pdata->address; + mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 1; + } + + /* Address */ + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_AUX_SLV_ADDR, slave_address); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* Register */ + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_ACCEL_BURST_ADDR, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + reg = ((reg & 0x80) | slave_reg); + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_ACCEL_BURST_ADDR, reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Length */ + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + reg = (reg & ~BIT_AUX_RD_LENG); + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + + +static int mpu_set_slave(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *slave_pdata, + int slave_id) +{ + return mpu_set_slave_mpu3050(mldl_cfg, gyro_handle, slave, + slave_pdata, slave_id); +} +/** + * Check to see if the gyro was reset by testing a couple of registers known + * to change on reset. + * + * @mldl_cfg mldl configuration structure + * @gyro_handle handle used to communicate with the gyro + * + * @return INV_SUCCESS or non-zero error code + */ +static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle) +{ + int result = INV_SUCCESS; + unsigned char reg; + + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_DMP_CFG_2, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg) + return true; + + if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1) + return false; + + result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_SMPLRT_DIV, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if (reg != mldl_cfg->mpu_gyro_cfg->divider) + return true; + + if (0 != mldl_cfg->mpu_gyro_cfg->divider) + return false; + + /* Inconclusive assume it was reset */ + return true; +} + +static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle, + unsigned long sensors) +{ + int result; + int ii; + int jj; + unsigned char reg; + unsigned char regs[7]; + + /* Wake up the part */ + result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, false, false, + !(sensors & INV_X_GYRO), + !(sensors & INV_Y_GYRO), + !(sensors & INV_Z_GYRO)); + + if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_RESET) && + !mpu_was_reset(mldl_cfg, gyro_handle)) { + return INV_SUCCESS; + } + + result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, true, false, + !(sensors & INV_X_GYRO), + !(sensors & INV_Y_GYRO), + !(sensors & INV_Z_GYRO)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_INT_CFG, + (mldl_cfg->mpu_gyro_cfg->int_config | + mldl_cfg->pdata->int_config)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = mpu_set_clock_source(gyro_handle, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + reg = DLPF_FS_SYNC_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync, + mldl_cfg->mpu_gyro_cfg->full_scale, + mldl_cfg->mpu_gyro_cfg->lpf); + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_DLPF_FS_SYNC, reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Write and verify memory */ + for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) { + unsigned char read[MPU_MEM_BANK_SIZE]; + + result = inv_serial_write_mem( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + ((ii << 8) | 0x00), MPU_MEM_BANK_SIZE, + &mldl_cfg->mpu_ram->ram[ii * MPU_MEM_BANK_SIZE]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_read_mem( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + ((ii << 8) | 0x00), MPU_MEM_BANK_SIZE, read); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + +#define ML_SKIP_CHECK 20 + for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) { + /* skip the register memory locations */ + if (ii == 0 && jj < ML_SKIP_CHECK) + continue; + if (mldl_cfg->mpu_ram->ram[ii * MPU_MEM_BANK_SIZE + jj] + != read[jj]) { + result = INV_ERROR_SERIAL_WRITE; + break; + } + } + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_XG_OFFS_TC, mldl_cfg->mpu_offsets->tc[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_YG_OFFS_TC, mldl_cfg->mpu_offsets->tc[1]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_ZG_OFFS_TC, mldl_cfg->mpu_offsets->tc[2]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + regs[0] = MPUREG_X_OFFS_USRH; + for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) { + regs[1 + ii * 2] = + (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8) + & 0xff; + regs[1 + ii * 2 + 1] = + (unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff); + } + result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr, + 7, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Configure slaves */ + result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle, + mldl_cfg->pdata->level_shifter); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +int gyro_config(void *mlsl_handle, + struct mldl_cfg *mldl_cfg, + struct ext_slave_config *data) +{ + struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg; + struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info; + struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets; + int ii; + + if (!data->data) + return INV_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_INT_CONFIG: + mpu_gyro_cfg->int_config = *((__u8 *)data->data); + break; + case MPU_SLAVE_EXT_SYNC: + mpu_gyro_cfg->ext_sync = *((__u8 *)data->data); + break; + case MPU_SLAVE_FULL_SCALE: + mpu_gyro_cfg->full_scale = *((__u8 *)data->data); + break; + case MPU_SLAVE_LPF: + mpu_gyro_cfg->lpf = *((__u8 *)data->data); + break; + case MPU_SLAVE_CLK_SRC: + mpu_gyro_cfg->clk_src = *((__u8 *)data->data); + break; + case MPU_SLAVE_DIVIDER: + mpu_gyro_cfg->divider = *((__u8 *)data->data); + break; + case MPU_SLAVE_DMP_ENABLE: + mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data); + break; + case MPU_SLAVE_FIFO_ENABLE: + mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data); + break; + case MPU_SLAVE_DMP_CFG1: + mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data); + break; + case MPU_SLAVE_DMP_CFG2: + mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data); + break; + case MPU_SLAVE_TC: + for (ii = 0; ii < GYRO_NUM_AXES; ii++) + mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii]; + break; + case MPU_SLAVE_GYRO: + for (ii = 0; ii < GYRO_NUM_AXES; ii++) + mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii]; + break; + case MPU_SLAVE_ADDR: + mpu_chip_info->addr = *((__u8 *)data->data); + break; + case MPU_SLAVE_PRODUCT_REVISION: + mpu_chip_info->product_revision = *((__u8 *)data->data); + break; + case MPU_SLAVE_SILICON_REVISION: + mpu_chip_info->silicon_revision = *((__u8 *)data->data); + break; + case MPU_SLAVE_PRODUCT_ID: + mpu_chip_info->product_id = *((__u8 *)data->data); + break; + case MPU_SLAVE_GYRO_SENS_TRIM: + mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data); + break; + case MPU_SLAVE_ACCEL_SENS_TRIM: + mpu_chip_info->accel_sens_trim = *((__u16 *)data->data); + break; + case MPU_SLAVE_RAM: + if (data->len != mldl_cfg->mpu_ram->length) + return INV_ERROR_INVALID_PARAMETER; + + memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len); + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_RESET; + return INV_SUCCESS; +} + +int gyro_get_config(void *mlsl_handle, + struct mldl_cfg *mldl_cfg, + struct ext_slave_config *data) +{ + struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg; + struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info; + struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets; + int ii; + + if (!data->data) + return INV_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_INT_CONFIG: + *((__u8 *)data->data) = mpu_gyro_cfg->int_config; + break; + case MPU_SLAVE_EXT_SYNC: + *((__u8 *)data->data) = mpu_gyro_cfg->ext_sync; + break; + case MPU_SLAVE_FULL_SCALE: + *((__u8 *)data->data) = mpu_gyro_cfg->full_scale; + break; + case MPU_SLAVE_LPF: + *((__u8 *)data->data) = mpu_gyro_cfg->lpf; + break; + case MPU_SLAVE_CLK_SRC: + *((__u8 *)data->data) = mpu_gyro_cfg->clk_src; + break; + case MPU_SLAVE_DIVIDER: + *((__u8 *)data->data) = mpu_gyro_cfg->divider; + break; + case MPU_SLAVE_DMP_ENABLE: + *((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable; + break; + case MPU_SLAVE_FIFO_ENABLE: + *((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable; + break; + case MPU_SLAVE_DMP_CFG1: + *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1; + break; + case MPU_SLAVE_DMP_CFG2: + *((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2; + break; + case MPU_SLAVE_TC: + for (ii = 0; ii < GYRO_NUM_AXES; ii++) + ((__u8 *)data->data)[ii] = mpu_offsets->tc[ii]; + break; + case MPU_SLAVE_GYRO: + for (ii = 0; ii < GYRO_NUM_AXES; ii++) + ((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii]; + break; + case MPU_SLAVE_ADDR: + *((__u8 *)data->data) = mpu_chip_info->addr; + break; + case MPU_SLAVE_PRODUCT_REVISION: + *((__u8 *)data->data) = mpu_chip_info->product_revision; + break; + case MPU_SLAVE_SILICON_REVISION: + *((__u8 *)data->data) = mpu_chip_info->silicon_revision; + break; + case MPU_SLAVE_PRODUCT_ID: + *((__u8 *)data->data) = mpu_chip_info->product_id; + break; + case MPU_SLAVE_GYRO_SENS_TRIM: + *((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim; + break; + case MPU_SLAVE_ACCEL_SENS_TRIM: + *((__u16 *)data->data) = mpu_chip_info->accel_sens_trim; + break; + case MPU_SLAVE_RAM: + if (data->len != mldl_cfg->mpu_ram->length) + return INV_ERROR_INVALID_PARAMETER; + + memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len); + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return INV_SUCCESS; +} + + +/******************************************************************************* + ******************************************************************************* + * Exported functions + ******************************************************************************* + ******************************************************************************/ + +/** + * Initializes the pdata structure to defaults. + * + * Opens the device to read silicon revision, product id and whoami. + * + * @mldl_cfg + * The internal device configuration data structure. + * @mlsl_handle + * The serial communication handle. + * + * @return INV_SUCCESS if silicon revision, product id and woami are supported + * by this software. + */ +int inv_mpu_open(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, void *pressure_handle) +{ + int result; + int ii; + + /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */ + mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false; + mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN; + mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ; + mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ; + mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS; + mldl_cfg->mpu_gyro_cfg->divider = 4; + mldl_cfg->mpu_gyro_cfg->dmp_enable = 1; + mldl_cfg->mpu_gyro_cfg->fifo_enable = 1; + mldl_cfg->mpu_gyro_cfg->ext_sync = 0; + mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0; + mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0; + mldl_cfg->inv_mpu_state->status = + MPU_DMP_IS_SUSPENDED | + MPU_GYRO_IS_SUSPENDED | + MPU_ACCEL_IS_SUSPENDED | + MPU_COMPASS_IS_SUSPENDED | + MPU_PRESSURE_IS_SUSPENDED; + mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0; + + if (mldl_cfg->mpu_chip_info->addr == 0) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + /* + * Reset, + * Take the DMP out of sleep, and + * read the product_id, sillicon rev and whoami + */ + mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED; + result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_get_silicon_rev(mldl_cfg, mlsl_handle); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Get the factory temperature compensation offsets */ + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_XG_OFFS_TC, 1, + &mldl_cfg->mpu_offsets->tc[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_YG_OFFS_TC, 1, + &mldl_cfg->mpu_offsets->tc[1]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_ZG_OFFS_TC, 1, + &mldl_cfg->mpu_offsets->tc[2]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Into bypass mode before sleeping and calling the slaves init */ + result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, true); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + + if (!mldl_cfg->slave[ii] || !mldl_cfg->slave[ii]->init) + continue; + + result = mldl_cfg->slave[ii]->init(accel_handle, + mldl_cfg->slave[ii], + mldl_cfg->pdata_slave[ii]); + if (result) { + LOG_RESULT_LOCATION(result); + goto out_slave_failure; + } + + } + + + return result; + +out_slave_failure: + for (ii = ii - 1; ii >= 0; ii--) + if (mldl_cfg->slave[ii] && mldl_cfg->slave[ii]->exit) + mldl_cfg->slave[ii]->exit(compass_handle, + mldl_cfg->slave[ii], + mldl_cfg->pdata_slave[ii]); + return result; +} + +/** + * Close the mpu interface + * + * @mldl_cfg pointer to the configuration structure + * @mlsl_handle pointer to the serial layer handle + * + * @return INV_SUCCESS or non-zero error code + */ +int inv_mpu_close(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + int result = INV_SUCCESS; + int ret_result = INV_SUCCESS; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!mldl_cfg->slave[ii] || !mldl_cfg->slave[ii]->exit) + continue; + result = mldl_cfg->slave[ii]->exit(accel_handle, + mldl_cfg->slave[ii], + mldl_cfg->pdata_slave[ii]); + if (INV_SUCCESS != result) + MPL_LOGE("Slave %d exit failed %d\n", ii, result); + + if (INV_SUCCESS == ret_result) + ret_result = result; + } + + return ret_result; +} + +/** + * @brief resume the MPU device and all the other sensor + * devices from their low power state. + * + * @mldl_cfg + * pointer to the configuration structure + * @gyro_handle + * the main file handle to the MPU device. + * @accel_handle + * an handle to the accelerometer device, if sitting + * onto a separate bus. Can match mlsl_handle if + * the accelerometer device operates on the same + * primary bus of MPU. + * @compass_handle + * an handle to the compass device, if sitting + * onto a separate bus. Can match mlsl_handle if + * the compass device operates on the same + * primary bus of MPU. + * @pressure_handle + * an handle to the pressure sensor device, if sitting + * onto a separate bus. Can match mlsl_handle if + * the pressure sensor device operates on the same + * primary bus of MPU. + * @resume_gyro + * whether resuming the gyroscope device is + * actually needed (if the device supports low power + * mode of some sort). + * @resume_accel + * whether resuming the accelerometer device is + * actually needed (if the device supports low power + * mode of some sort). + * @resume_compass + * whether resuming the compass device is + * actually needed (if the device supports low power + * mode of some sort). + * @resume_pressure + * whether resuming the pressure sensor device is + * actually needed (if the device supports low power + * mode of some sort). + * @return INV_SUCCESS or a non-zero error code. + */ +int inv_mpu_resume(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors) +{ + int result = INV_SUCCESS; + int ii; + bool resume_slave[EXT_SLAVE_NUM_TYPES]; + bool resume_dmp = sensors & INV_DMP_PROCESSOR; + void *slave_handle[EXT_SLAVE_NUM_TYPES]; + resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] = + (sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)); + resume_slave[EXT_SLAVE_TYPE_ACCEL] = + sensors & INV_THREE_AXIS_ACCEL; + resume_slave[EXT_SLAVE_TYPE_COMPASS] = + sensors & INV_THREE_AXIS_COMPASS; + resume_slave[EXT_SLAVE_TYPE_PRESSURE] = + sensors & INV_THREE_AXIS_PRESSURE; + + slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle; + slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle; + slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle; + slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle; + + + mldl_print_cfg(mldl_cfg); + + /* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */ + for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (resume_slave[ii] && + ((!mldl_cfg->slave[ii]) || + (!mldl_cfg->slave[ii]->resume))) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + } + + if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp) + && (mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) { + result = gyro_resume(mldl_cfg, gyro_handle, sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!mldl_cfg->slave[ii] || + !mldl_cfg->pdata_slave[ii] || + !resume_slave[ii] || + !(mldl_cfg->inv_mpu_state->status & (1 << ii))) + continue; + + if (EXT_SLAVE_BUS_SECONDARY == + mldl_cfg->pdata_slave[ii]->bus) { + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, + true); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + result = mldl_cfg->slave[ii]->resume(slave_handle[ii], + mldl_cfg->slave[ii], + mldl_cfg->pdata_slave[ii]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status &= ~(1 << ii); + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (resume_dmp && + !(mldl_cfg->inv_mpu_state->status & (1 << ii)) && + mldl_cfg->pdata_slave[ii] && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) { + result = mpu_set_slave(mldl_cfg, + gyro_handle, + mldl_cfg->slave[ii], + mldl_cfg->pdata_slave[ii], + mldl_cfg->slave[ii]->type); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + /* Turn on the master i2c iterface if necessary */ + if (resume_dmp) { + result = mpu_set_i2c_bypass( + mldl_cfg, gyro_handle, + !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Now start */ + result = dmp_start(mldl_cfg, gyro_handle); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + mldl_cfg->inv_mpu_cfg->requested_sensors = sensors; + + return result; +} + +/** + * @brief suspend the MPU device and all the other sensor + * devices into their low power state. + * @mldl_cfg + * a pointer to the struct mldl_cfg internal data + * structure. + * @gyro_handle + * the main file handle to the MPU device. + * @accel_handle + * an handle to the accelerometer device, if sitting + * onto a separate bus. Can match gyro_handle if + * the accelerometer device operates on the same + * primary bus of MPU. + * @compass_handle + * an handle to the compass device, if sitting + * onto a separate bus. Can match gyro_handle if + * the compass device operates on the same + * primary bus of MPU. + * @pressure_handle + * an handle to the pressure sensor device, if sitting + * onto a separate bus. Can match gyro_handle if + * the pressure sensor device operates on the same + * primary bus of MPU. + * @accel + * whether suspending the accelerometer device is + * actually needed (if the device supports low power + * mode of some sort). + * @compass + * whether suspending the compass device is + * actually needed (if the device supports low power + * mode of some sort). + * @pressure + * whether suspending the pressure sensor device is + * actually needed (if the device supports low power + * mode of some sort). + * @return INV_SUCCESS or a non-zero error code. + */ +int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors) +{ + int result = INV_SUCCESS; + int ii; + struct ext_slave_descr **slave = mldl_cfg->slave; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR); + bool suspend_slave[EXT_SLAVE_NUM_TYPES]; + + suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] = + ((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)) + == (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)); + suspend_slave[EXT_SLAVE_TYPE_ACCEL] = + ((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL); + suspend_slave[EXT_SLAVE_TYPE_COMPASS] = + ((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS); + suspend_slave[EXT_SLAVE_TYPE_PRESSURE] = + ((sensors & INV_THREE_AXIS_PRESSURE) == + INV_THREE_AXIS_PRESSURE); + + if (suspend_dmp) { + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = dmp_stop(mldl_cfg, gyro_handle); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + /* Gyro */ + if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] && + !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) { + result = mpu3050_pwr_mgmt( + mldl_cfg, gyro_handle, 0, + suspend_dmp && suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE], + (unsigned char)(sensors & INV_X_GYRO), + (unsigned char)(sensors & INV_Y_GYRO), + (unsigned char)(sensors & INV_Z_GYRO)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii); + if (!slave[ii] || !pdata_slave[ii] || + is_suspended || !suspend_slave[ii]) + continue; + + if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) { + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + result = slave[ii]->suspend(accel_handle, + slave[ii], + pdata_slave[ii]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) { + result = mpu_set_slave(mldl_cfg, gyro_handle, + NULL, NULL, + slave[ii]->type); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + mldl_cfg->inv_mpu_state->status |= (1 << ii); + } + + /* Re-enable the i2c master if there are configured slaves and DMP */ + if (!suspend_dmp) { + result = mpu_set_i2c_bypass( + mldl_cfg, gyro_handle, + !(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS; + + return result; +} + +int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + int bypass_result; + int remain_bypassed = true; + + if (NULL == slave || NULL == slave->read) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); + return INV_ERROR_INVALID_CONFIGURATION; + } + + if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus) + && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) { + remain_bypassed = false; + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = slave->read(slave_handle, slave, pdata, data); + + if (!remain_bypassed) { + bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0); + if (bypass_result) { + LOG_RESULT_LOCATION(bypass_result); + return bypass_result; + } + } + return result; +} + +int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + int remain_bypassed = true; + + if (NULL == slave || NULL == slave->config) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); + return INV_ERROR_INVALID_CONFIGURATION; + } + + if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus) + && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) { + remain_bypassed = false; + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = slave->config(slave_handle, slave, pdata, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (!remain_bypassed) { + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + return result; +} + +int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + int remain_bypassed = true; + + if (NULL == slave || NULL == slave->get_config) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); + return INV_ERROR_INVALID_CONFIGURATION; + } + + if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus) + && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) { + remain_bypassed = false; + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = slave->get_config(slave_handle, slave, pdata, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (!remain_bypassed) { + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + return result; +} + +/** + * @} + */