From fcc9d2e5a6c89d22b8b773a64fb4ad21ac318446 Mon Sep 17 00:00:00 2001 From: Jonathan Herman Date: Tue, 22 Jan 2013 10:38:37 -0500 Subject: Added missing tegra files. --- drivers/misc/inv_mpu/mpu6050/Makefile | 18 + drivers/misc/inv_mpu/mpu6050/mldl_cfg.c | 1916 +++++++++++++++++++++++++ drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c | 138 ++ drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c | 420 ++++++ drivers/misc/inv_mpu/mpu6050/mpu-dev.c | 1309 +++++++++++++++++ 5 files changed, 3801 insertions(+) create mode 100644 drivers/misc/inv_mpu/mpu6050/Makefile create mode 100644 drivers/misc/inv_mpu/mpu6050/mldl_cfg.c create mode 100644 drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c create mode 100644 drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c create mode 100644 drivers/misc/inv_mpu/mpu6050/mpu-dev.c (limited to 'drivers/misc/inv_mpu/mpu6050') diff --git a/drivers/misc/inv_mpu/mpu6050/Makefile b/drivers/misc/inv_mpu/mpu6050/Makefile new file mode 100644 index 00000000000..a93aa97a699 --- /dev/null +++ b/drivers/misc/inv_mpu/mpu6050/Makefile @@ -0,0 +1,18 @@ + +# Kernel makefile for motions sensors +# +# + +obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050b1.o + +ccflags-y := -DMPU_CURRENT_BUILD_MPU6050B1 + +mpu6050b1-objs += mldl_cfg.o +mpu6050b1-objs += mldl_print_cfg.o +mpu6050b1-objs += mlsl-kernel.o +mpu6050b1-objs += mpu-dev.o +mpu6050b1-objs += ../accel/mpu6050.o + +EXTRA_CFLAGS += -Idrivers/misc/inv_mpu +EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER +EXTRA_CFLAGS += -DINV_CACHE_DMP=1 diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c new file mode 100644 index 00000000000..22af0c20098 --- /dev/null +++ b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c @@ -0,0 +1,1916 @@ +/* + $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 "mpu6050b1.h" + +#include "mlsl.h" +#include "mldl_print_cfg.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "mldl_cfg:" + +/* -------------------------------------------------------------------------- */ + +#define SLEEP 0 +#define WAKE_UP 7 +#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; +} + +/** + * @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 mpu6050b1_set_i2c_bypass(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, unsigned char enable) +{ + unsigned char reg; + 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, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (!enable) { + /* setting int_config with the property flag BIT_BYPASS_EN + should be done by the setup functions */ + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_INT_PIN_CFG, + (mldl_cfg->pdata->int_config & ~(BIT_BYPASS_EN))); + if (!(reg & BIT_I2C_MST_EN)) { + result = + inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, + (reg | BIT_I2C_MST_EN)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } else if (enable) { + if (reg & BIT_AUX_IF_EN) { + result = + inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_USER_CTRL, + (reg & (~BIT_I2C_MST_EN))); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /***************************************** + * To avoid hanging the bus we must sleep until all + * slave transactions have been completed. + * 24 bytes max slave reads + * +1 byte possible extra write + * +4 max slave address + * --- + * 33 Maximum bytes + * x9 Approximate bits per byte + * --- + * 297 bits. + * 2.97 ms minimum @ 100kbps + * 0.75 ms minimum @ 400kbps. + *****************************************/ + msleep(3); + } + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_INT_PIN_CFG, + (mldl_cfg->pdata->int_config | BIT_BYPASS_EN)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + 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 mpu6050b1_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 */ + +/* produces an unique identifier for each device based on the + combination of product version and product revision */ +struct prod_rev_map_t { + unsigned short mpl_product_key; + unsigned char silicon_rev; + unsigned short gyro_trim; + unsigned short accel_trim; +}; + +/* NOTE: product entries are in chronological order */ +static struct prod_rev_map_t prod_rev_map[] = { + /* prod_ver = 0 */ + {MPL_PROD_KEY(0, 1), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 2), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 3), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 4), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 5), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 6), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/C2-1) */ + /* prod_ver = 1, forced to 0 for MPU6050 A2 */ + {MPL_PROD_KEY(0, 7), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 8), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 9), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D2-1) */ + {MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384}, /* (A2/D4) */ + /* prod_ver = 1 */ + {MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-1) */ + {MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-2) */ + {MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-3) */ + {MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-4) */ + {MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D2-5) */ + {MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/D4) */ + {MPL_PROD_KEY(1, 1), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-1) */ + {MPL_PROD_KEY(1, 2), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-2) */ + {MPL_PROD_KEY(1, 3), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-3) */ + {MPL_PROD_KEY(1, 4), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-4) */ + {MPL_PROD_KEY(1, 5), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-5) */ + {MPL_PROD_KEY(1, 6), MPU_SILICON_REV_B1, 131, 16384}, /* (B1/E1-6) */ + /* prod_ver = 2 */ + {MPL_PROD_KEY(2, 7), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-1) */ + {MPL_PROD_KEY(2, 8), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-2) */ + {MPL_PROD_KEY(2, 9), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-3) */ + {MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-4) */ + {MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-5) */ + {MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E1-6) */ + {MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/D4) */ + /* prod_ver = 3 */ + {MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384}, /* (B2/E2) */ + /* prod_ver = 4 */ + {MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131, 8192}, /* (B2/F1) */ + {MPL_PROD_KEY(4, 1), MPU_SILICON_REV_B1, 131, 8192}, /* (B3/F1) */ + {MPL_PROD_KEY(4, 3), MPU_SILICON_REV_B1, 131, 8192}, /* (B4/F1) */ + /* prod_ver = 5 */ + {MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */ + /* prod_ver = 7 */ + {MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */ + /* prod_ver = 8 */ + {MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */ + /* prod_ver = 9 */ + {MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384}, /* (B5/E2) */ + /* prod_ver = 10 */ + {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384} /* (B5/E2) */ +}; + +/** + * @internal + * @brief Inverse lookup of the index of an MPL product key . + * @param key + * the MPL product indentifier also referred to as 'key'. + * @return the index position of the key in the array, -1 if not found. + */ +short index_of_key(unsigned short key) +{ + int i; + for (i = 0; i < NUM_OF_PROD_REVS; i++) + if (prod_rev_map[i].mpl_product_key == key) + return (short)i; + return -1; +} + +/** + * @internal + * @brief Get the product revision and version for MPU6050 and + * extract all per-part specific information. + * The product version number is read from the PRODUCT_ID register in + * user space register map. + * The product revision number is in read from OTP bank 0, ADDR6[7:2]. + * These 2 numbers, combined, provide an unique key to be used to + * retrieve some per-device information such as the silicon revision + * and the gyro and accel sensitivity trim values. + * + * @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_mpu6050( + struct mldl_cfg *mldl_cfg, void *mlsl_handle) +{ + int result; + unsigned char prod_ver = 0x00, prod_rev = 0x00; + unsigned char bank = + (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0); + unsigned short memAddr = ((bank << 8) | 0x06); + unsigned short key; + short index; + 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, &prod_ver); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + prod_ver &= 0xF; + + result = inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + memAddr, 1, &prod_rev); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + prod_rev >>= 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; + } + + key = MPL_PROD_KEY(prod_ver, prod_rev); + if (key == 0) { + MPL_LOGE("Product id read as 0 " + "indicates device is either " + "incompatible or an MPU3050\n"); + return INV_ERROR_INVALID_MODULE; + } + index = index_of_key(key); + if (index == -1 || index >= NUM_OF_PROD_REVS) { + MPL_LOGE("Unsupported product key %d in MPL\n", key); + return INV_ERROR_INVALID_MODULE; + } + /* check MPL is compiled for this device */ + if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) { + MPL_LOGE("MPL compiled for MPU6050B1 support " + "but device is not MPU6050B1 (%d)\n", key); + return INV_ERROR_INVALID_MODULE; + } + + mpu_chip_info->product_id = prod_ver; + mpu_chip_info->product_revision = prod_rev; + mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev; + mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim; + mpu_chip_info->accel_sens_trim = prod_rev_map[index].accel_trim; + + return result; +} +#define inv_get_silicon_rev inv_get_silicon_rev_mpu6050 + + +/** + * @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 regval; + + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_YG_OFFS_TC, 1, ®val); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if (enable) + regval |= BIT_I2C_MST_VDDIO; + + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_YG_OFFS_TC, regval); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return INV_SUCCESS; +} + + +/** + * @internal + * @brief MPU6050 B1 power management functions. + * @param mldl_cfg + * a pointer to the internal mldl_cfg data structure. + * @param mlsl_handle + * a file handle to the serial device used to communicate + * with the MPU6050 B1 device. + * @param reset + * 1 to reset hardware. + * @param sensors + * Bitfield of sensors to leave on + * + * @return 0 on success, a non-zero error code on error. + */ +static int mpu60xx_pwr_mgmt(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + unsigned int reset, unsigned long sensors) +{ + unsigned char pwr_mgmt[2]; + unsigned char pwr_mgmt_prev[2]; + int result; + int sleep = !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL + | INV_DMP_PROCESSOR)); + + if (reset) { + MPL_LOGI("Reset MPU6050 B1\n"); + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGMT_1, BIT_H_RESET); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED; + msleep(15); + } + + /* NOTE : reading both PWR_MGMT_1 and PWR_MGMT_2 for efficiency because + they are accessible even when the device is powered off */ + result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGMT_1, 2, pwr_mgmt_prev); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + pwr_mgmt[0] = pwr_mgmt_prev[0]; + pwr_mgmt[1] = pwr_mgmt_prev[1]; + + if (sleep) { + mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED; + pwr_mgmt[0] |= BIT_SLEEP; + } else { + mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED; + pwr_mgmt[0] &= ~BIT_SLEEP; + } + if (pwr_mgmt[0] != pwr_mgmt_prev[0]) { + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGMT_1, pwr_mgmt[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + pwr_mgmt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG); + if (!(sensors & INV_X_GYRO)) + pwr_mgmt[1] |= BIT_STBY_XG; + if (!(sensors & INV_Y_GYRO)) + pwr_mgmt[1] |= BIT_STBY_YG; + if (!(sensors & INV_Z_GYRO)) + pwr_mgmt[1] |= BIT_STBY_ZG; + + if (pwr_mgmt[1] != pwr_mgmt_prev[1]) { + result = inv_serial_single_write( + mlsl_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_PWR_MGMT_2, pwr_mgmt[1]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if ((pwr_mgmt[1] & (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) == + (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) { + mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED; + } else { + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED; + } + + 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; + } + + /* ERRATA: + workaroud to switch from any MPU_CLK_SEL_PLLGYROx to + MPU_CLK_SEL_INTERNAL and XGyro is powered up: + 1) Select INT_OSC + 2) PD XGyro + 3) PU XGyro + */ + if ((cur_clk_src == MPU_CLK_SEL_PLLGYROX + || cur_clk_src == MPU_CLK_SEL_PLLGYROY + || cur_clk_src == MPU_CLK_SEL_PLLGYROZ) + && mldl_cfg->mpu_gyro_cfg->clk_src == MPU_CLK_SEL_INTERNAL + && mldl_cfg->inv_mpu_cfg->requested_sensors & INV_X_GYRO) { + unsigned char first_result = INV_SUCCESS; + mldl_cfg->inv_mpu_cfg->requested_sensors &= ~INV_X_GYRO; + result = mpu60xx_pwr_mgmt( + mldl_cfg, gyro_handle, + false, mldl_cfg->inv_mpu_cfg->requested_sensors); + ERROR_CHECK_FIRST(first_result, result); + mldl_cfg->inv_mpu_cfg->requested_sensors |= INV_X_GYRO; + result = mpu60xx_pwr_mgmt( + mldl_cfg, gyro_handle, + false, mldl_cfg->inv_mpu_cfg->requested_sensors); + ERROR_CHECK_FIRST(first_result, result); + result = first_result; + } + 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_mpu60xx(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; + /* Slave values */ + unsigned char slave_reg; + unsigned char slave_len; + unsigned char slave_endian; + unsigned char slave_address; + /* Which MPU6050 registers to use */ + unsigned char addr_reg; + unsigned char reg_reg; + unsigned char ctrl_reg; + /* Which MPU6050 registers to use for the trigger */ + unsigned char addr_trig_reg; + unsigned char reg_trig_reg; + unsigned char ctrl_trig_reg; + + unsigned char bits_slave_delay = 0; + /* Divide down rate for the Slave, from the mpu rate */ + unsigned char d0_trig_reg; + unsigned char delay_ctrl_orig; + unsigned char delay_ctrl; + long divider; + + if (NULL == slave || NULL == slave_pdata) { + slave_reg = 0; + slave_len = 0; + slave_endian = 0; + slave_address = 0; + } else { + slave_reg = slave->read_reg; + slave_len = slave->read_len; + slave_endian = slave->endian; + slave_address = slave_pdata->address; + slave_address |= BIT_I2C_READ; + } + + switch (slave_id) { + case EXT_SLAVE_TYPE_ACCEL: + addr_reg = MPUREG_I2C_SLV1_ADDR; + reg_reg = MPUREG_I2C_SLV1_REG; + ctrl_reg = MPUREG_I2C_SLV1_CTRL; + addr_trig_reg = 0; + reg_trig_reg = 0; + ctrl_trig_reg = 0; + bits_slave_delay = BIT_SLV1_DLY_EN; + break; + case EXT_SLAVE_TYPE_COMPASS: + addr_reg = MPUREG_I2C_SLV0_ADDR; + reg_reg = MPUREG_I2C_SLV0_REG; + ctrl_reg = MPUREG_I2C_SLV0_CTRL; + addr_trig_reg = MPUREG_I2C_SLV2_ADDR; + reg_trig_reg = MPUREG_I2C_SLV2_REG; + ctrl_trig_reg = MPUREG_I2C_SLV2_CTRL; + d0_trig_reg = MPUREG_I2C_SLV2_DO; + bits_slave_delay = BIT_SLV2_DLY_EN | BIT_SLV0_DLY_EN; + break; + case EXT_SLAVE_TYPE_PRESSURE: + addr_reg = MPUREG_I2C_SLV3_ADDR; + reg_reg = MPUREG_I2C_SLV3_REG; + ctrl_reg = MPUREG_I2C_SLV3_CTRL; + addr_trig_reg = MPUREG_I2C_SLV4_ADDR; + reg_trig_reg = MPUREG_I2C_SLV4_REG; + ctrl_trig_reg = MPUREG_I2C_SLV4_CTRL; + bits_slave_delay = BIT_SLV4_DLY_EN | BIT_SLV3_DLY_EN; + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + }; + + /* return if this slave has already been set */ + if ((slave_address && + ((mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) + == bits_slave_delay)) || + (!slave_address && + (mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) == + 0)) + return 0; + + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true); + + /* Address */ + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + addr_reg, slave_address); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* Register */ + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + reg_reg, slave_reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Length, byte swapping, grouping & enable */ + if (slave_len > BITS_SLV_LENG) { + MPL_LOGW("Limiting slave burst read length to " + "the allowed maximum (15B, req. %d)\n", slave_len); + slave_len = BITS_SLV_LENG; + return INV_ERROR_INVALID_CONFIGURATION; + } + reg = slave_len; + if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) { + reg |= BIT_SLV_BYTE_SW; + if (slave_reg & 1) + reg |= BIT_SLV_GRP; + } + if (slave_address) + reg |= BIT_SLV_ENABLE; + + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + ctrl_reg, reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Trigger */ + if (addr_trig_reg) { + /* If slave address is 0 this clears the trigger */ + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + addr_trig_reg, + slave_address & ~BIT_I2C_READ); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (slave && slave->trigger && reg_trig_reg) { + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + reg_trig_reg, + slave->trigger->reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + ctrl_trig_reg, + BIT_SLV_ENABLE | 0x01); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + d0_trig_reg, + slave->trigger->value); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (ctrl_trig_reg) { + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + ctrl_trig_reg, 0x00); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + /* Data rate */ + if (slave) { + struct ext_slave_config config; + long data; + config.key = MPU_SLAVE_CONFIG_ODR_RESUME; + config.len = sizeof(long); + config.apply = false; + config.data = &data; + if (!(slave->get_config)) + return INV_ERROR_INVALID_CONFIGURATION; + + result = slave->get_config(NULL, slave, slave_pdata, &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + MPL_LOGI("Slave %d ODR: %ld Hz\n", slave_id, data / 1000); + divider = ((1000 * inv_mpu_get_sampling_rate_hz( + mldl_cfg->mpu_gyro_cfg)) + / data) - 1; + } else { + divider = 0; + } + + result = inv_serial_read(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_I2C_MST_DELAY_CTRL, + 1, &delay_ctrl_orig); + delay_ctrl = delay_ctrl_orig; + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (divider > 0 && divider <= MASK_I2C_MST_DLY) { + result = inv_serial_read(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_I2C_SLV4_CTRL, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if ((reg & MASK_I2C_MST_DLY) && + ((long)(reg & MASK_I2C_MST_DLY) != + (divider & MASK_I2C_MST_DLY))) { + MPL_LOGW("Changing slave divider: %ld to %ld\n", + (long)(reg & MASK_I2C_MST_DLY), + (divider & MASK_I2C_MST_DLY)); + + } + reg |= (unsigned char)(divider & MASK_I2C_MST_DLY); + result = inv_serial_single_write(gyro_handle, + mldl_cfg->mpu_chip_info->addr, + MPUREG_I2C_SLV4_CTRL, + reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + delay_ctrl |= bits_slave_delay; + } else { + delay_ctrl &= ~(bits_slave_delay); + } + if (delay_ctrl != delay_ctrl_orig) { + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_I2C_MST_DELAY_CTRL, + delay_ctrl); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (slave_address) + mldl_cfg->inv_mpu_state->i2c_slaves_enabled |= + bits_slave_delay; + else + mldl_cfg->inv_mpu_state->i2c_slaves_enabled &= + ~bits_slave_delay; + + 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_mpu60xx(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; + + /* Inconclusive assume it was reset */ + return true; +} + + +int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle, + const unsigned char *data, int size) +{ + int bank, offset, write_size; + int result; + unsigned char read[MPU_MEM_BANK_SIZE]; + + if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) { +#if INV_CACHE_DMP == 1 + memcpy(mldl_cfg->mpu_ram->ram, data, size); + return INV_SUCCESS; +#else + LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET); + return INV_ERROR_MEMORY_SET; +#endif + } + + if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) { + LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET); + return INV_ERROR_MEMORY_SET; + } + /* Write and verify memory */ + for (bank = 0; size > 0; bank++, + size -= write_size, + data += write_size) { + if (size > MPU_MEM_BANK_SIZE) + write_size = MPU_MEM_BANK_SIZE; + else + write_size = size; + + result = inv_serial_write_mem(mlsl_handle, + mldl_cfg->mpu_chip_info->addr, + ((bank << 8) | 0x00), + write_size, + data); + if (result) { + LOG_RESULT_LOCATION(result); + MPL_LOGE("Write mem error in bank %d\n", bank); + return result; + } + result = inv_serial_read_mem(mlsl_handle, + mldl_cfg->mpu_chip_info->addr, + ((bank << 8) | 0x00), + write_size, + read); + if (result) { + LOG_RESULT_LOCATION(result); + MPL_LOGE("Read mem error in bank %d\n", bank); + return result; + } + +#define ML_SKIP_CHECK 38 + for (offset = 0; offset < write_size; offset++) { + /* skip the register memory locations */ + if (bank == 0 && offset < ML_SKIP_CHECK) + continue; + if (data[offset] != read[offset]) { + result = INV_ERROR_SERIAL_WRITE; + break; + } + } + if (result != INV_SUCCESS) { + LOG_RESULT_LOCATION(result); + MPL_LOGE("Read data mismatch at bank %d, offset %d\n", + bank, offset); + return result; + } + } + return INV_SUCCESS; +} + +static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle, + unsigned long sensors) +{ + int result; + int ii; + unsigned char reg; + unsigned char regs[7]; + + /* Wake up the part */ + result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Always set the INT_ENABLE and DIVIDER as the Accel Only mode for 6050 + can set these too */ + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_INT_ENABLE, (mldl_cfg->mpu_gyro_cfg->int_config)); + 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; + } + + if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) && + !mpu_was_reset(mldl_cfg, gyro_handle)) { + return INV_SUCCESS; + } + + /* Configure the MPU */ + result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1); + 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; + } + + reg = MPUREG_GYRO_CONFIG_VALUE(0, 0, 0, + mldl_cfg->mpu_gyro_cfg->full_scale); + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_GYRO_CONFIG, reg); + reg = MPUREG_CONFIG_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync, + mldl_cfg->mpu_gyro_cfg->lpf); + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_CONFIG, 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 */ +#if INV_CACHE_DMP != 0 + inv_mpu_set_firmware(mldl_cfg, gyro_handle, + mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length); +#endif + + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_XG_OFFS_TC, + ((mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + regs[0] = ((mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC); + result = inv_serial_single_write( + gyro_handle, mldl_cfg->mpu_chip_info->addr, + MPUREG_YG_OFFS_TC, regs[0]); + 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] << 1) & BITS_ZG_OFFS_TC)); + 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; + } + mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG; + + 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_CONFIG; + 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 *gyro_handle, + void *accel_handle, + void *compass_handle, void *pressure_handle) +{ + int result; + void *slave_handle[EXT_SLAVE_NUM_TYPES]; + int ii; + + /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */ + ii = 0; + 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 | + MPU_DEVICE_IS_SUSPENDED; + mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0; + + 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; + + 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 = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, true, + INV_THREE_AXIS_GYRO); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_get_silicon_rev(mldl_cfg, gyro_handle); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Get the factory temperature compensation offsets */ + result = inv_serial_read(gyro_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(gyro_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(gyro_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, gyro_handle, true); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle, + mldl_cfg->pdata->level_shifter); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (ii = 0; ii < GYRO_NUM_AXES; ii++) { + mldl_cfg->mpu_offsets->tc[ii] = + (mldl_cfg->mpu_offsets->tc[ii] & BITS_XG_OFFS_TC) >> 1; + } + +#if INV_CACHE_DMP != 0 + result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, 0); +#endif + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + + 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 *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + return 0; +} + +/** + * @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) || + (mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) { + 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; + } + 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]; + void *slave_handle[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); + + 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; + + 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 = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, + ((~sensors) & INV_ALL_SENSORS)); + 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(slave_handle[ii], + 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; +} + +/** + * @} + */ diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c new file mode 100644 index 00000000000..7379a170761 --- /dev/null +++ b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c @@ -0,0 +1,138 @@ +/* + $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_print_cfg.c + * @brief The Motion Library Driver Layer. + */ + +#include +#include "mldl_cfg.h" +#include "mlsl.h" +#include "linux/mpu.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "mldl_print_cfg:" + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 1 + +void mldl_print_cfg(struct mldl_cfg *mldl_cfg) +{ + struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg; + struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets; + struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info; + struct inv_mpu_cfg *inv_mpu_cfg = mldl_cfg->inv_mpu_cfg; + struct inv_mpu_state *inv_mpu_state = mldl_cfg->inv_mpu_state; + struct ext_slave_descr **slave = mldl_cfg->slave; + struct mpu_platform_data *pdata = mldl_cfg->pdata; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + /* mpu_gyro_cfg */ + MPL_LOGV("int_config = %02x\n", mpu_gyro_cfg->int_config); + MPL_LOGV("ext_sync = %02x\n", mpu_gyro_cfg->ext_sync); + MPL_LOGV("full_scale = %02x\n", mpu_gyro_cfg->full_scale); + MPL_LOGV("lpf = %02x\n", mpu_gyro_cfg->lpf); + MPL_LOGV("clk_src = %02x\n", mpu_gyro_cfg->clk_src); + MPL_LOGV("divider = %02x\n", mpu_gyro_cfg->divider); + MPL_LOGV("dmp_enable = %02x\n", mpu_gyro_cfg->dmp_enable); + MPL_LOGV("fifo_enable = %02x\n", mpu_gyro_cfg->fifo_enable); + MPL_LOGV("dmp_cfg1 = %02x\n", mpu_gyro_cfg->dmp_cfg1); + MPL_LOGV("dmp_cfg2 = %02x\n", mpu_gyro_cfg->dmp_cfg2); + /* mpu_offsets */ + MPL_LOGV("tc[0] = %02x\n", mpu_offsets->tc[0]); + MPL_LOGV("tc[1] = %02x\n", mpu_offsets->tc[1]); + MPL_LOGV("tc[2] = %02x\n", mpu_offsets->tc[2]); + MPL_LOGV("gyro[0] = %04x\n", mpu_offsets->gyro[0]); + MPL_LOGV("gyro[1] = %04x\n", mpu_offsets->gyro[1]); + MPL_LOGV("gyro[2] = %04x\n", mpu_offsets->gyro[2]); + + /* mpu_chip_info */ + MPL_LOGV("addr = %02x\n", mldl_cfg->mpu_chip_info->addr); + + MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision); + MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision); + MPL_LOGV("product_id = %02x\n", mpu_chip_info->product_id); + MPL_LOGV("gyro_sens_trim = %02x\n", mpu_chip_info->gyro_sens_trim); + MPL_LOGV("accel_sens_trim = %02x\n", mpu_chip_info->accel_sens_trim); + + MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors); + MPL_LOGV("ignore_system_suspend= %04x\n", + inv_mpu_cfg->ignore_system_suspend); + MPL_LOGV("status = %04x\n", inv_mpu_state->status); + MPL_LOGV("i2c_slaves_enabled= %04x\n", + inv_mpu_state->i2c_slaves_enabled); + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!slave[ii]) + continue; + MPL_LOGV("SLAVE %d:\n", ii); + MPL_LOGV(" suspend = %02x\n", (int)slave[ii]->suspend); + MPL_LOGV(" resume = %02x\n", (int)slave[ii]->resume); + MPL_LOGV(" read = %02x\n", (int)slave[ii]->read); + MPL_LOGV(" type = %02x\n", slave[ii]->type); + MPL_LOGV(" reg = %02x\n", slave[ii]->read_reg); + MPL_LOGV(" len = %02x\n", slave[ii]->read_len); + MPL_LOGV(" endian = %02x\n", slave[ii]->endian); + MPL_LOGV(" range.mantissa= %02x\n", + slave[ii]->range.mantissa); + MPL_LOGV(" range.fraction= %02x\n", + slave[ii]->range.fraction); + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + continue; + MPL_LOGV("PDATA_SLAVE[%d]\n", ii); + MPL_LOGV(" irq = %02x\n", pdata_slave[ii]->irq); + MPL_LOGV(" adapt_num = %02x\n", pdata_slave[ii]->adapt_num); + MPL_LOGV(" bus = %02x\n", pdata_slave[ii]->bus); + MPL_LOGV(" address = %02x\n", pdata_slave[ii]->address); + MPL_LOGV(" orientation=\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + pdata_slave[ii]->orientation[0], + pdata_slave[ii]->orientation[1], + pdata_slave[ii]->orientation[2], + pdata_slave[ii]->orientation[3], + pdata_slave[ii]->orientation[4], + pdata_slave[ii]->orientation[5], + pdata_slave[ii]->orientation[6], + pdata_slave[ii]->orientation[7], + pdata_slave[ii]->orientation[8]); + } + + MPL_LOGV("pdata->int_config = %02x\n", pdata->int_config); + MPL_LOGV("pdata->level_shifter = %02x\n", pdata->level_shifter); + MPL_LOGV("pdata->orientation =\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + pdata->orientation[0], pdata->orientation[1], + pdata->orientation[2], pdata->orientation[3], + pdata->orientation[4], pdata->orientation[5], + pdata->orientation[6], pdata->orientation[7], + pdata->orientation[8]); +} diff --git a/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c new file mode 100644 index 00000000000..f1c228f48fe --- /dev/null +++ b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c @@ -0,0 +1,420 @@ +/* + $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 . + $ + */ + +#include "mlsl.h" +#include +#include "log.h" +#include "mpu6050b1.h" + +static int inv_i2c_write(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned int len, unsigned char const *data) +{ + struct i2c_msg msgs[1]; + int res; + + if (!data || !i2c_adap) { + LOG_RESULT_LOCATION(-EINVAL); + return -EINVAL; + } + + msgs[0].addr = address; + msgs[0].flags = 0; /* write */ + msgs[0].buf = (unsigned char *)data; + msgs[0].len = len; + + res = i2c_transfer(i2c_adap, msgs, 1); + if (res < 1) { + if (res == 0) + res = -EIO; + LOG_RESULT_LOCATION(res); + return res; + } else + return 0; +} + +static int inv_i2c_write_register(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned char reg, unsigned char value) +{ + unsigned char data[2]; + + data[0] = reg; + data[1] = value; + return inv_i2c_write(i2c_adap, address, 2, data); +} + +static int inv_i2c_read(struct i2c_adapter *i2c_adap, + unsigned char address, unsigned char reg, + unsigned int len, unsigned char *data) +{ + struct i2c_msg msgs[2]; + int res; + + if (!data || !i2c_adap) { + LOG_RESULT_LOCATION(-EINVAL); + return -EINVAL; + } + + msgs[0].addr = address; + msgs[0].flags = 0; /* write */ + msgs[0].buf = ® + msgs[0].len = 1; + + msgs[1].addr = address; + msgs[1].flags = I2C_M_RD; + msgs[1].buf = data; + msgs[1].len = len; + + res = i2c_transfer(i2c_adap, msgs, 2); + if (res < 2) { + if (res >= 0) + res = -EIO; + LOG_RESULT_LOCATION(res); + return res; + } else + return 0; +} + +static int mpu_memory_read(struct i2c_adapter *i2c_adap, + unsigned char mpu_addr, + unsigned short mem_addr, + unsigned int len, unsigned char *data) +{ + unsigned char bank[2]; + unsigned char addr[2]; + unsigned char buf; + + struct i2c_msg msgs[4]; + int res; + + if (!data || !i2c_adap) { + LOG_RESULT_LOCATION(-EINVAL); + return -EINVAL; + } + + bank[0] = MPUREG_BANK_SEL; + bank[1] = mem_addr >> 8; + + addr[0] = MPUREG_MEM_START_ADDR; + addr[1] = mem_addr & 0xFF; + + buf = MPUREG_MEM_R_W; + + /* write message */ + msgs[0].addr = mpu_addr; + msgs[0].flags = 0; + msgs[0].buf = bank; + msgs[0].len = sizeof(bank); + + msgs[1].addr = mpu_addr; + msgs[1].flags = 0; + msgs[1].buf = addr; + msgs[1].len = sizeof(addr); + + msgs[2].addr = mpu_addr; + msgs[2].flags = 0; + msgs[2].buf = &buf; + msgs[2].len = 1; + + msgs[3].addr = mpu_addr; + msgs[3].flags = I2C_M_RD; + msgs[3].buf = data; + msgs[3].len = len; + + res = i2c_transfer(i2c_adap, msgs, 4); + if (res != 4) { + if (res >= 0) + res = -EIO; + LOG_RESULT_LOCATION(res); + return res; + } else + return 0; +} + +static int mpu_memory_write(struct i2c_adapter *i2c_adap, + unsigned char mpu_addr, + unsigned short mem_addr, + unsigned int len, unsigned char const *data) +{ + unsigned char bank[2]; + unsigned char addr[2]; + unsigned char buf[513]; + + struct i2c_msg msgs[3]; + int res; + + if (!data || !i2c_adap) { + LOG_RESULT_LOCATION(-EINVAL); + return -EINVAL; + } + if (len >= (sizeof(buf) - 1)) { + LOG_RESULT_LOCATION(-ENOMEM); + return -ENOMEM; + } + + bank[0] = MPUREG_BANK_SEL; + bank[1] = mem_addr >> 8; + + addr[0] = MPUREG_MEM_START_ADDR; + addr[1] = mem_addr & 0xFF; + + buf[0] = MPUREG_MEM_R_W; + memcpy(buf + 1, data, len); + + /* write message */ + msgs[0].addr = mpu_addr; + msgs[0].flags = 0; + msgs[0].buf = bank; + msgs[0].len = sizeof(bank); + + msgs[1].addr = mpu_addr; + msgs[1].flags = 0; + msgs[1].buf = addr; + msgs[1].len = sizeof(addr); + + msgs[2].addr = mpu_addr; + msgs[2].flags = 0; + msgs[2].buf = (unsigned char *)buf; + msgs[2].len = len + 1; + + res = i2c_transfer(i2c_adap, msgs, 3); + if (res != 3) { + if (res >= 0) + res = -EIO; + LOG_RESULT_LOCATION(res); + return res; + } else + return 0; +} + +int inv_serial_single_write( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned char data) +{ + return inv_i2c_write_register((struct i2c_adapter *)sl_handle, + slave_addr, register_addr, data); +} +EXPORT_SYMBOL(inv_serial_single_write); + +int inv_serial_write( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data) +{ + int result; + const unsigned short data_length = length - 1; + const unsigned char start_reg_addr = data[0]; + unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1]; + unsigned short bytes_written = 0; + + while (bytes_written < data_length) { + unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE, + data_length - bytes_written); + if (bytes_written == 0) { + result = inv_i2c_write((struct i2c_adapter *) + sl_handle, slave_addr, + 1 + this_len, data); + } else { + /* manually increment register addr between chunks */ + i2c_write[0] = start_reg_addr + bytes_written; + memcpy(&i2c_write[1], &data[1 + bytes_written], + this_len); + result = inv_i2c_write((struct i2c_adapter *) + sl_handle, slave_addr, + 1 + this_len, i2c_write); + } + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_written += this_len; + } + return 0; +} +EXPORT_SYMBOL(inv_serial_write); + +int inv_serial_read( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned short length, + unsigned char *data) +{ + int result; + unsigned short bytes_read = 0; + + if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR + && (register_addr == MPUREG_FIFO_R_W || + register_addr == MPUREG_MEM_R_W)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + while (bytes_read < length) { + unsigned short this_len = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read); + result = inv_i2c_read((struct i2c_adapter *)sl_handle, + slave_addr, register_addr + bytes_read, + this_len, &data[bytes_read]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_read += this_len; + } + return 0; +} +EXPORT_SYMBOL(inv_serial_read); + +int inv_serial_write_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned short length, + unsigned char const *data) +{ + int result; + unsigned short bytes_written = 0; + + if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) { + pr_err("memory read length (%d B) extends beyond its" + " limits (%d) if started at location %d\n", length, + MPU_MEM_BANK_SIZE, mem_addr & 0xFF); + return INV_ERROR_INVALID_PARAMETER; + } + while (bytes_written < length) { + unsigned short this_len = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written); + result = mpu_memory_write((struct i2c_adapter *)sl_handle, + slave_addr, mem_addr + bytes_written, + this_len, &data[bytes_written]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_written += this_len; + } + return 0; +} +EXPORT_SYMBOL(inv_serial_write_mem); + +int inv_serial_read_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned short length, + unsigned char *data) +{ + int result; + unsigned short bytes_read = 0; + + if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) { + printk + ("memory read length (%d B) extends beyond its limits (%d) " + "if started at location %d\n", length, + MPU_MEM_BANK_SIZE, mem_addr & 0xFF); + return INV_ERROR_INVALID_PARAMETER; + } + while (bytes_read < length) { + unsigned short this_len = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read); + result = + mpu_memory_read((struct i2c_adapter *)sl_handle, + slave_addr, mem_addr + bytes_read, + this_len, &data[bytes_read]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_read += this_len; + } + return 0; +} +EXPORT_SYMBOL(inv_serial_read_mem); + +int inv_serial_write_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data) +{ + int result; + unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1]; + unsigned short bytes_written = 0; + + if (length > FIFO_HW_SIZE) { + printk(KERN_ERR + "maximum fifo write length is %d\n", FIFO_HW_SIZE); + return INV_ERROR_INVALID_PARAMETER; + } + while (bytes_written < length) { + unsigned short this_len = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written); + i2c_write[0] = MPUREG_FIFO_R_W; + memcpy(&i2c_write[1], &data[bytes_written], this_len); + result = inv_i2c_write((struct i2c_adapter *)sl_handle, + slave_addr, this_len + 1, i2c_write); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_written += this_len; + } + return 0; +} +EXPORT_SYMBOL(inv_serial_write_fifo); + +int inv_serial_read_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char *data) +{ + int result; + unsigned short bytes_read = 0; + + if (length > FIFO_HW_SIZE) { + printk(KERN_ERR + "maximum fifo read length is %d\n", FIFO_HW_SIZE); + return INV_ERROR_INVALID_PARAMETER; + } + while (bytes_read < length) { + unsigned short this_len = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read); + result = inv_i2c_read((struct i2c_adapter *)sl_handle, + slave_addr, MPUREG_FIFO_R_W, this_len, + &data[bytes_read]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + bytes_read += this_len; + } + + return 0; +} +EXPORT_SYMBOL(inv_serial_read_fifo); + +/** + * @} + */ diff --git a/drivers/misc/inv_mpu/mpu6050/mpu-dev.c b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c new file mode 100644 index 00000000000..e171dc2a7ef --- /dev/null +++ b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c @@ -0,0 +1,1309 @@ +/* + $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 . + $ + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "mpuirq.h" +#include "slaveirq.h" +#include "mlsl.h" +#include "mldl_cfg.h" +#include + +#include "accel/mpu6050.h" + +/* Platform data for the MPU */ +struct mpu_private_data { + struct miscdevice dev; + struct i2c_client *client; + + /* mldl_cfg data */ + struct mldl_cfg mldl_cfg; + struct mpu_ram mpu_ram; + struct mpu_gyro_cfg mpu_gyro_cfg; + struct mpu_offsets mpu_offsets; + struct mpu_chip_info mpu_chip_info; + struct inv_mpu_cfg inv_mpu_cfg; + struct inv_mpu_state inv_mpu_state; + + struct mutex mutex; + wait_queue_head_t mpu_event_wait; + struct completion completion; + struct timer_list timeout; + struct notifier_block nb; + struct mpuirq_data mpu_pm_event; + int response_timeout; /* In seconds */ + unsigned long event; + int pid; + struct module *slave_modules[EXT_SLAVE_NUM_TYPES]; +}; + +struct mpu_private_data *mpu_private_data; + +static void mpu_pm_timeout(u_long data) +{ + struct mpu_private_data *mpu = (struct mpu_private_data *)data; + struct i2c_client *client = mpu->client; + dev_dbg(&client->adapter->dev, "%s\n", __func__); + complete(&mpu->completion); +} + +static int mpu_pm_notifier_callback(struct notifier_block *nb, + unsigned long event, void *unused) +{ + struct mpu_private_data *mpu = + container_of(nb, struct mpu_private_data, nb); + struct i2c_client *client = mpu->client; + struct timeval event_time; + dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event); + + /* Prevent the file handle from being closed before we initialize + the completion event */ + mutex_lock(&mpu->mutex); + if (!(mpu->pid) || + (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) { + mutex_unlock(&mpu->mutex); + return NOTIFY_OK; + } + + if (event == PM_SUSPEND_PREPARE) + mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE; + if (event == PM_POST_SUSPEND) + mpu->event = MPU_PM_EVENT_POST_SUSPEND; + + do_gettimeofday(&event_time); + mpu->mpu_pm_event.interruptcount++; + mpu->mpu_pm_event.irqtime = + (((long long)event_time.tv_sec) << 32) + event_time.tv_usec; + mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT; + mpu->mpu_pm_event.data = mpu->event; + + if (mpu->response_timeout > 0) { + mpu->timeout.expires = jiffies + mpu->response_timeout * HZ; + add_timer(&mpu->timeout); + } + INIT_COMPLETION(mpu->completion); + mutex_unlock(&mpu->mutex); + + wake_up_interruptible(&mpu->mpu_event_wait); + wait_for_completion(&mpu->completion); + del_timer_sync(&mpu->timeout); + dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event); + return NOTIFY_OK; +} + +static int mpu_dev_open(struct inode *inode, struct file *file) +{ + struct mpu_private_data *mpu = + container_of(file->private_data, struct mpu_private_data, dev); + struct i2c_client *client = mpu->client; + int result; + int ii; + dev_dbg(&client->adapter->dev, "%s\n", __func__); + dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid); + + result = mutex_lock_interruptible(&mpu->mutex); + if (mpu->pid) { + mutex_unlock(&mpu->mutex); + return -EBUSY; + } + mpu->pid = current->pid; + + /* Reset the sensors to the default */ + if (result) { + dev_err(&client->adapter->dev, + "%s: mutex_lock_interruptible returned %d\n", + __func__, result); + return result; + } + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) + __module_get(mpu->slave_modules[ii]); + + mutex_unlock(&mpu->mutex); + return 0; +} + +/* close function - called when the "file" /dev/mpu is closed in userspace */ +static int mpu_release(struct inode *inode, struct file *file) +{ + struct mpu_private_data *mpu = + container_of(file->private_data, struct mpu_private_data, dev); + struct i2c_client *client = mpu->client; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + int result = 0; + int ii; + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + + mutex_lock(&mpu->mutex); + mldl_cfg->inv_mpu_cfg->requested_sensors = 0; + result = inv_mpu_suspend(mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + INV_ALL_SENSORS); + mpu->pid = 0; + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) + module_put(mpu->slave_modules[ii]); + + mutex_unlock(&mpu->mutex); + complete(&mpu->completion); + dev_dbg(&client->adapter->dev, "mpu_release\n"); + + return result; +} + +/* read function called when from /dev/mpu is read. Read from the FIFO */ +static ssize_t mpu_read(struct file *file, + char __user *buf, size_t count, loff_t *offset) +{ + struct mpu_private_data *mpu = + container_of(file->private_data, struct mpu_private_data, dev); + struct i2c_client *client = mpu->client; + size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long); + int err; + + if (!mpu->event && (!(file->f_flags & O_NONBLOCK))) + wait_event_interruptible(mpu->mpu_event_wait, mpu->event); + + if (!mpu->event || !buf + || count < sizeof(mpu->mpu_pm_event)) + return 0; + + err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event)); + if (err) { + dev_err(&client->adapter->dev, + "Copy to user returned %d\n", err); + return -EFAULT; + } + mpu->event = 0; + return len; +} + +static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll) +{ + struct mpu_private_data *mpu = + container_of(file->private_data, struct mpu_private_data, dev); + int mask = 0; + + poll_wait(file, &mpu->mpu_event_wait, poll); + if (mpu->event) + mask |= POLLIN | POLLRDNORM; + return mask; +} + +static int mpu_dev_ioctl_get_ext_slave_platform_data( + struct i2c_client *client, + struct ext_slave_platform_data __user *arg) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct ext_slave_platform_data *pdata_slave; + struct ext_slave_platform_data local_pdata_slave; + + if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave))) + return -EFAULT; + + if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES) + return -EINVAL; + + pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type]; + /* All but private data and irq_data */ + if (!pdata_slave) + return -ENODEV; + if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave))) + return -EFAULT; + return 0; +} + +static int mpu_dev_ioctl_get_mpu_platform_data( + struct i2c_client *client, + struct mpu_platform_data __user *arg) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata; + + if (copy_to_user(arg, pdata, sizeof(*pdata))) + return -EFAULT; + return 0; +} + +static int mpu_dev_ioctl_get_ext_slave_descr( + struct i2c_client *client, + struct ext_slave_descr __user *arg) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct ext_slave_descr *slave; + struct ext_slave_descr local_slave; + + if (copy_from_user(&local_slave, arg, sizeof(local_slave))) + return -EFAULT; + + if (local_slave.type >= EXT_SLAVE_NUM_TYPES) + return -EINVAL; + + slave = mpu->mldl_cfg.slave[local_slave.type]; + /* All but private data and irq_data */ + if (!slave) + return -ENODEV; + if (copy_to_user(arg, slave, sizeof(*slave))) + return -EFAULT; + return 0; +} + + +/** + * slave_config() - Pass a requested slave configuration to the slave sensor + * + * @adapter the adaptor to use to communicate with the slave + * @mldl_cfg the mldl configuration structuer + * @slave pointer to the slave descriptor + * @usr_config The configuration to pass to the slave sensor + * + * returns 0 or non-zero error code + */ +static int inv_mpu_config(struct mldl_cfg *mldl_cfg, + void *gyro_adapter, + struct ext_slave_config __user *usr_config) +{ + int retval = 0; + struct ext_slave_config config; + + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + if (config.len && config.data) { + void *data; + data = kmalloc(config.len, GFP_KERNEL); + if (!data) + return -ENOMEM; + + retval = copy_from_user(data, + (void __user *)config.data, config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = gyro_config(gyro_adapter, mldl_cfg, &config); + kfree(config.data); + return retval; +} + +static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg, + void *gyro_adapter, + struct ext_slave_config __user *usr_config) +{ + int retval = 0; + struct ext_slave_config config; + void *user_data; + + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + user_data = config.data; + if (config.len && config.data) { + void *data; + data = kmalloc(config.len, GFP_KERNEL); + if (!data) + return -ENOMEM; + + retval = copy_from_user(data, + (void __user *)config.data, config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = gyro_get_config(gyro_adapter, mldl_cfg, &config); + if (!retval) + retval = copy_to_user((unsigned char __user *)user_data, + config.data, config.len); + kfree(config.data); + return retval; +} + +static int slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_adapter, + void *slave_adapter, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config __user *usr_config) +{ + int retval = 0; + struct ext_slave_config config; + if ((!slave) || (!slave->config)) + return -ENODEV; + + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + if (config.len && config.data) { + void *data; + data = kmalloc(config.len, GFP_KERNEL); + if (!data) + return -ENOMEM; + + retval = copy_from_user(data, + (void __user *)config.data, config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter, + &config, slave, pdata); + kfree(config.data); + return retval; +} + +static int slave_get_config(struct mldl_cfg *mldl_cfg, + void *gyro_adapter, + void *slave_adapter, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config __user *usr_config) +{ + int retval = 0; + struct ext_slave_config config; + void *user_data; + if (!(slave) || !(slave->get_config)) + return -ENODEV; + + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + user_data = config.data; + if (config.len && config.data) { + void *data; + data = kmalloc(config.len, GFP_KERNEL); + if (!data) + return -ENOMEM; + + retval = copy_from_user(data, + (void __user *)config.data, config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter, + slave_adapter, &config, slave, pdata); + if (retval) { + kfree(config.data); + return retval; + } + retval = copy_to_user((unsigned char __user *)user_data, + config.data, config.len); + kfree(config.data); + return retval; +} + +static int inv_slave_read(struct mldl_cfg *mldl_cfg, + void *gyro_adapter, + void *slave_adapter, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + void __user *usr_data) +{ + int retval; + unsigned char *data; + data = kzalloc(slave->read_len, GFP_KERNEL); + if (!data) + return -EFAULT; + + retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter, + slave, pdata, data); + + if ((!retval) && + (copy_to_user((unsigned char __user *)usr_data, + data, slave->read_len))) + retval = -EFAULT; + + kfree(data); + return retval; +} + +static int mpu_handle_mlsl(void *sl_handle, + unsigned char addr, + unsigned int cmd, + struct mpu_read_write __user *usr_msg) +{ + int retval = 0; + struct mpu_read_write msg; + unsigned char *user_data; + retval = copy_from_user(&msg, usr_msg, sizeof(msg)); + if (retval) + return -EFAULT; + + user_data = msg.data; + if (msg.length && msg.data) { + unsigned char *data; + data = kmalloc(msg.length, GFP_KERNEL); + if (!data) + return -ENOMEM; + + retval = copy_from_user(data, + (void __user *)msg.data, msg.length); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + msg.data = data; + } else { + return -EPERM; + } + + switch (cmd) { + case MPU_READ: + retval = inv_serial_read(sl_handle, addr, + msg.address, msg.length, msg.data); + break; + case MPU_WRITE: + retval = inv_serial_write(sl_handle, addr, + msg.length, msg.data); + break; + case MPU_READ_MEM: + retval = inv_serial_read_mem(sl_handle, addr, + msg.address, msg.length, msg.data); + break; + case MPU_WRITE_MEM: + retval = inv_serial_write_mem(sl_handle, addr, + msg.address, msg.length, + msg.data); + break; + case MPU_READ_FIFO: + retval = inv_serial_read_fifo(sl_handle, addr, + msg.length, msg.data); + break; + case MPU_WRITE_FIFO: + retval = inv_serial_write_fifo(sl_handle, addr, + msg.length, msg.data); + break; + + }; + if (retval) { + dev_err(&((struct i2c_adapter *)sl_handle)->dev, + "%s: i2c %d error %d\n", + __func__, cmd, retval); + kfree(msg.data); + return retval; + } + retval = copy_to_user((unsigned char __user *)user_data, + msg.data, msg.length); + kfree(msg.data); + return retval; +} + +/* ioctl - I/O control */ +static long mpu_dev_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + struct mpu_private_data *mpu = + container_of(file->private_data, struct mpu_private_data, dev); + struct i2c_client *client = mpu->client; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + int retval = 0; + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_descr **slave = mldl_cfg->slave; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + + retval = mutex_lock_interruptible(&mpu->mutex); + if (retval) { + dev_err(&client->adapter->dev, + "%s: mutex_lock_interruptible returned %d\n", + __func__, retval); + return retval; + } + + switch (cmd) { + case MPU_GET_EXT_SLAVE_PLATFORM_DATA: + retval = mpu_dev_ioctl_get_ext_slave_platform_data( + client, + (struct ext_slave_platform_data __user *)arg); + break; + case MPU_GET_MPU_PLATFORM_DATA: + retval = mpu_dev_ioctl_get_mpu_platform_data( + client, + (struct mpu_platform_data __user *)arg); + break; + case MPU_GET_EXT_SLAVE_DESCR: + retval = mpu_dev_ioctl_get_ext_slave_descr( + client, + (struct ext_slave_descr __user *)arg); + break; + case MPU_READ: + case MPU_WRITE: + case MPU_READ_MEM: + case MPU_WRITE_MEM: + case MPU_READ_FIFO: + case MPU_WRITE_FIFO: + retval = mpu_handle_mlsl( + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + mldl_cfg->mpu_chip_info->addr, cmd, + (struct mpu_read_write __user *)arg); + break; + case MPU_CONFIG_GYRO: + retval = inv_mpu_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_ACCEL: + retval = slave_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave[EXT_SLAVE_TYPE_ACCEL], + pdata_slave[EXT_SLAVE_TYPE_ACCEL], + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_COMPASS: + retval = slave_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave[EXT_SLAVE_TYPE_COMPASS], + pdata_slave[EXT_SLAVE_TYPE_COMPASS], + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_PRESSURE: + retval = slave_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + slave[EXT_SLAVE_TYPE_PRESSURE], + pdata_slave[EXT_SLAVE_TYPE_PRESSURE], + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_GYRO: + retval = inv_mpu_get_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_ACCEL: + retval = slave_get_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave[EXT_SLAVE_TYPE_ACCEL], + pdata_slave[EXT_SLAVE_TYPE_ACCEL], + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_COMPASS: + retval = slave_get_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave[EXT_SLAVE_TYPE_COMPASS], + pdata_slave[EXT_SLAVE_TYPE_COMPASS], + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_PRESSURE: + retval = slave_get_config( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + slave[EXT_SLAVE_TYPE_PRESSURE], + pdata_slave[EXT_SLAVE_TYPE_PRESSURE], + (struct ext_slave_config __user *)arg); + break; + case MPU_SUSPEND: + retval = inv_mpu_suspend( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + arg); + break; + case MPU_RESUME: + retval = inv_mpu_resume( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + arg); + break; + case MPU_PM_EVENT_HANDLED: + dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd); + complete(&mpu->completion); + break; + case MPU_READ_ACCEL: + retval = inv_slave_read( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave[EXT_SLAVE_TYPE_ACCEL], + pdata_slave[EXT_SLAVE_TYPE_ACCEL], + (unsigned char __user *)arg); + break; + case MPU_READ_COMPASS: + retval = inv_slave_read( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave[EXT_SLAVE_TYPE_COMPASS], + pdata_slave[EXT_SLAVE_TYPE_COMPASS], + (unsigned char __user *)arg); + break; + case MPU_READ_PRESSURE: + retval = inv_slave_read( + mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + slave[EXT_SLAVE_TYPE_PRESSURE], + pdata_slave[EXT_SLAVE_TYPE_PRESSURE], + (unsigned char __user *)arg); + break; + case MPU_GET_REQUESTED_SENSORS: + if (copy_to_user( + (__u32 __user *)arg, + &mldl_cfg->inv_mpu_cfg->requested_sensors, + sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors))) + retval = -EFAULT; + break; + case MPU_SET_REQUESTED_SENSORS: + mldl_cfg->inv_mpu_cfg->requested_sensors = arg; + break; + case MPU_GET_IGNORE_SYSTEM_SUSPEND: + if (copy_to_user( + (unsigned char __user *)arg, + &mldl_cfg->inv_mpu_cfg->ignore_system_suspend, + sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend))) + retval = -EFAULT; + break; + case MPU_SET_IGNORE_SYSTEM_SUSPEND: + mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg; + break; + case MPU_GET_MLDL_STATUS: + if (copy_to_user( + (unsigned char __user *)arg, + &mldl_cfg->inv_mpu_state->status, + sizeof(mldl_cfg->inv_mpu_state->status))) + retval = -EFAULT; + break; + case MPU_GET_I2C_SLAVES_ENABLED: + if (copy_to_user( + (unsigned char __user *)arg, + &mldl_cfg->inv_mpu_state->i2c_slaves_enabled, + sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled))) + retval = -EFAULT; + break; + default: + dev_err(&client->adapter->dev, + "%s: Unknown cmd %x, arg %lu\n", + __func__, cmd, arg); + retval = -EINVAL; + }; + + mutex_unlock(&mpu->mutex); + dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n", + __func__, cmd, arg, retval); + + if (retval > 0) + retval = -retval; + + return retval; +} + +void mpu_shutdown(struct i2c_client *client) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + + mutex_lock(&mpu->mutex); + (void)inv_mpu_suspend(mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + INV_ALL_SENSORS); + mutex_unlock(&mpu->mutex); + dev_dbg(&client->adapter->dev, "%s\n", __func__); +} + +int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + + mutex_lock(&mpu->mutex); + if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) { + dev_dbg(&client->adapter->dev, + "%s: suspending on event %d\n", __func__, mesg.event); + (void)inv_mpu_suspend(mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + INV_ALL_SENSORS); + } else { + dev_dbg(&client->adapter->dev, + "%s: Already suspended %d\n", __func__, mesg.event); + } + mutex_unlock(&mpu->mutex); + return 0; +} + +int mpu_dev_resume(struct i2c_client *client) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + + mutex_lock(&mpu->mutex); + if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) { + (void)inv_mpu_resume(mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE], + mldl_cfg->inv_mpu_cfg->requested_sensors); + dev_dbg(&client->adapter->dev, + "%s for pid %d\n", __func__, mpu->pid); + } + mutex_unlock(&mpu->mutex); + return 0; +} + +/* define which file operations are supported */ +static const struct file_operations mpu_fops = { + .owner = THIS_MODULE, + .read = mpu_read, + .poll = mpu_poll, + .unlocked_ioctl = mpu_dev_ioctl, + .open = mpu_dev_open, + .release = mpu_release, +}; + +int inv_mpu_register_slave(struct module *slave_module, + struct i2c_client *slave_client, + struct ext_slave_platform_data *slave_pdata, + struct ext_slave_descr *(*get_slave_descr)(void)) +{ + struct mpu_private_data *mpu = mpu_private_data; + struct mldl_cfg *mldl_cfg; + struct ext_slave_descr *slave_descr; + struct ext_slave_platform_data **pdata_slave; + char *irq_name = NULL; + int result = 0; + + if (!slave_client || !slave_pdata || !get_slave_descr) + return -EINVAL; + + if (!mpu) { + dev_err(&slave_client->adapter->dev, + "%s: Null mpu_private_data\n", __func__); + return -EINVAL; + } + mldl_cfg = &mpu->mldl_cfg; + pdata_slave = mldl_cfg->pdata_slave; + slave_descr = get_slave_descr(); + + if (!slave_descr) { + dev_err(&slave_client->adapter->dev, + "%s: Null ext_slave_descr\n", __func__); + return -EINVAL; + } + + mutex_lock(&mpu->mutex); + if (mpu->pid) { + mutex_unlock(&mpu->mutex); + return -EBUSY; + } + + if (pdata_slave[slave_descr->type]) { + result = -EBUSY; + goto out_unlock_mutex; + } + + slave_pdata->address = slave_client->addr; + slave_pdata->irq = slave_client->irq; + slave_pdata->adapt_num = i2c_adapter_id(slave_client->adapter); + + dev_info(&slave_client->adapter->dev, + "%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n", + __func__, + slave_descr->name, + slave_descr->type, + slave_pdata->address, + slave_pdata->irq, + slave_pdata->adapt_num); + + switch (slave_descr->type) { + case EXT_SLAVE_TYPE_ACCEL: + irq_name = "accelirq"; + break; + case EXT_SLAVE_TYPE_COMPASS: + irq_name = "compassirq"; + break; + case EXT_SLAVE_TYPE_PRESSURE: + irq_name = "pressureirq"; + break; + default: + irq_name = "none"; + }; + if (slave_descr->init) { + result = slave_descr->init(slave_client->adapter, + slave_descr, + slave_pdata); + if (result) { + dev_err(&slave_client->adapter->dev, + "%s init failed %d\n", + slave_descr->name, result); + goto out_unlock_mutex; + } + } + + if (slave_descr->type == EXT_SLAVE_TYPE_ACCEL && + slave_descr->id == ACCEL_ID_MPU6050 && + slave_descr->config) { + /* pass a reference to the mldl_cfg data + structure to the mpu6050 accel "class" */ + struct ext_slave_config config; + config.key = MPU_SLAVE_CONFIG_INTERNAL_REFERENCE; + config.len = sizeof(struct mldl_cfg *); + config.apply = true; + config.data = mldl_cfg; + result = slave_descr->config( + slave_client->adapter, slave_descr, + slave_pdata, &config); + if (result) { + LOG_RESULT_LOCATION(result); + goto out_slavedescr_exit; + } + } + pdata_slave[slave_descr->type] = slave_pdata; + mpu->slave_modules[slave_descr->type] = slave_module; + mldl_cfg->slave[slave_descr->type] = slave_descr; + + goto out_unlock_mutex; + +out_slavedescr_exit: + if (slave_descr->exit) + slave_descr->exit(slave_client->adapter, + slave_descr, slave_pdata); +out_unlock_mutex: + mutex_unlock(&mpu->mutex); + + if (!result && irq_name && (slave_pdata->irq > 0)) { + int warn_result; + dev_info(&slave_client->adapter->dev, + "Installing %s irq using %d\n", + irq_name, + slave_pdata->irq); + warn_result = slaveirq_init(slave_client->adapter, + slave_pdata, irq_name); + if (result) + dev_WARN(&slave_client->adapter->dev, + "%s irq assigned error: %d\n", + slave_descr->name, warn_result); + } else { + dev_WARN(&slave_client->adapter->dev, + "%s irq not assigned: %d %d %d\n", + slave_descr->name, + result, (int)irq_name, slave_pdata->irq); + } + + return result; +} +EXPORT_SYMBOL(inv_mpu_register_slave); + +void inv_mpu_unregister_slave(struct i2c_client *slave_client, + struct ext_slave_platform_data *slave_pdata, + struct ext_slave_descr *(*get_slave_descr)(void)) +{ + struct mpu_private_data *mpu = mpu_private_data; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct ext_slave_descr *slave_descr; + int result; + + dev_info(&slave_client->adapter->dev, "%s\n", __func__); + + if (!slave_client || !slave_pdata || !get_slave_descr) + return; + + if (slave_pdata->irq) + slaveirq_exit(slave_pdata); + + slave_descr = get_slave_descr(); + if (!slave_descr) + return; + + mutex_lock(&mpu->mutex); + + if (slave_descr->exit) { + result = slave_descr->exit(slave_client->adapter, + slave_descr, + slave_pdata); + if (result) + dev_err(&slave_client->adapter->dev, + "Accel exit failed %d\n", result); + } + mldl_cfg->slave[slave_descr->type] = NULL; + mldl_cfg->pdata_slave[slave_descr->type] = NULL; + mpu->slave_modules[slave_descr->type] = NULL; + + mutex_unlock(&mpu->mutex); + +} +EXPORT_SYMBOL(inv_mpu_unregister_slave); + +static unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +static const struct i2c_device_id mpu_id[] = { + {"mpu3050", 0}, + {"mpu6050", 0}, + {"mpu6050_no_accel", 0}, + {} +}; +MODULE_DEVICE_TABLE(i2c, mpu_id); + +int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid) +{ + struct mpu_platform_data *pdata; + struct mpu_private_data *mpu; + struct mldl_cfg *mldl_cfg; + int res = 0; + int ii = 0; + unsigned long irq_flags; + + dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++); + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + res = -ENODEV; + goto out_check_functionality_failed; + } + + mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL); + if (!mpu) { + res = -ENOMEM; + goto out_alloc_data_failed; + } + mldl_cfg = &mpu->mldl_cfg; + mldl_cfg->mpu_ram = &mpu->mpu_ram; + mldl_cfg->mpu_gyro_cfg = &mpu->mpu_gyro_cfg; + mldl_cfg->mpu_offsets = &mpu->mpu_offsets; + mldl_cfg->mpu_chip_info = &mpu->mpu_chip_info; + mldl_cfg->inv_mpu_cfg = &mpu->inv_mpu_cfg; + mldl_cfg->inv_mpu_state = &mpu->inv_mpu_state; + + mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE; + mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL); + if (!mldl_cfg->mpu_ram->ram) { + res = -ENOMEM; + goto out_alloc_ram_failed; + } + mpu_private_data = mpu; + i2c_set_clientdata(client, mpu); + mpu->client = client; + + init_waitqueue_head(&mpu->mpu_event_wait); + mutex_init(&mpu->mutex); + init_completion(&mpu->completion); + + mpu->response_timeout = 60; /* Seconds */ + mpu->timeout.function = mpu_pm_timeout; + mpu->timeout.data = (u_long) mpu; + init_timer(&mpu->timeout); + + mpu->nb.notifier_call = mpu_pm_notifier_callback; + mpu->nb.priority = 0; + res = register_pm_notifier(&mpu->nb); + if (res) { + dev_err(&client->adapter->dev, + "Unable to register pm_notifier %d\n", res); + goto out_register_pm_notifier_failed; + } + + pdata = (struct mpu_platform_data *)client->dev.platform_data; + if (!pdata) { + dev_WARN(&client->adapter->dev, + "Missing platform data for mpu\n"); + } + mldl_cfg->pdata = pdata; + + mldl_cfg->mpu_chip_info->addr = client->addr; + res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL); + + if (res) { + dev_err(&client->adapter->dev, + "Unable to open %s %d\n", MPU_NAME, res); + res = -ENODEV; + goto out_whoami_failed; + } + + mpu->dev.minor = MISC_DYNAMIC_MINOR; + mpu->dev.name = "mpu"; + mpu->dev.fops = &mpu_fops; + res = misc_register(&mpu->dev); + if (res < 0) { + dev_err(&client->adapter->dev, + "ERROR: misc_register returned %d\n", res); + goto out_misc_register_failed; + } + + if (client->irq) { + dev_info(&client->adapter->dev, + "Installing irq using %d\n", client->irq); + if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL)) + irq_flags = IRQF_TRIGGER_FALLING; + else + irq_flags = IRQF_TRIGGER_RISING; + res = mpuirq_init(client, mldl_cfg, irq_flags); + + if (res) + goto out_mpuirq_failed; + } else { + dev_WARN(&client->adapter->dev, + "Missing %s IRQ\n", MPU_NAME); + } + if (!strcmp(mpu_id[1].name, devid->name)) { + /* Special case to re-use the inv_mpu_register_slave */ + struct ext_slave_platform_data *slave_pdata; + slave_pdata = kzalloc(sizeof(*slave_pdata), GFP_KERNEL); + if (!slave_pdata) { + res = -ENOMEM; + goto out_slave_pdata_kzalloc_failed; + } + slave_pdata->bus = EXT_SLAVE_BUS_PRIMARY; + for (ii = 0; ii < 9; ii++) + slave_pdata->orientation[ii] = pdata->orientation[ii]; + res = inv_mpu_register_slave( + NULL, client, + slave_pdata, + mpu6050_get_slave_descr); + if (res) { + /* if inv_mpu_register_slave fails there are no pointer + references to the memory allocated to slave_pdata */ + kfree(slave_pdata); + goto out_slave_pdata_kzalloc_failed; + } + } + return res; + +out_slave_pdata_kzalloc_failed: + if (client->irq) + mpuirq_exit(); +out_mpuirq_failed: + misc_deregister(&mpu->dev); +out_misc_register_failed: + inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL); +out_whoami_failed: + unregister_pm_notifier(&mpu->nb); +out_register_pm_notifier_failed: + kfree(mldl_cfg->mpu_ram->ram); + mpu_private_data = NULL; +out_alloc_ram_failed: + kfree(mpu); +out_alloc_data_failed: +out_check_functionality_failed: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res); + return res; + +} + +static int mpu_remove(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES]; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave; + int ii; + + for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) { + if (!pdata_slave[ii]) + slave_adapter[ii] = NULL; + else + slave_adapter[ii] = + i2c_get_adapter(pdata_slave[ii]->adapt_num); + } + + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter; + dev_dbg(&client->adapter->dev, "%s\n", __func__); + + inv_mpu_close(mldl_cfg, + slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE], + slave_adapter[EXT_SLAVE_TYPE_ACCEL], + slave_adapter[EXT_SLAVE_TYPE_COMPASS], + slave_adapter[EXT_SLAVE_TYPE_PRESSURE]); + + if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] && + (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id == + ACCEL_ID_MPU6050)) { + struct ext_slave_platform_data *slave_pdata = + mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]; + inv_mpu_unregister_slave( + client, + mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL], + mpu6050_get_slave_descr); + kfree(slave_pdata); + } + + if (client->irq) + mpuirq_exit(); + + misc_deregister(&mpu->dev); + + unregister_pm_notifier(&mpu->nb); + + kfree(mpu->mldl_cfg.mpu_ram->ram); + kfree(mpu); + + return 0; +} + +static struct i2c_driver mpu_driver = { + .class = I2C_CLASS_HWMON, + .probe = mpu_probe, + .remove = mpu_remove, + .id_table = mpu_id, + .driver = { + .owner = THIS_MODULE, + .name = MPU_NAME, + }, + .address_list = normal_i2c, + .shutdown = mpu_shutdown, /* optional */ + .suspend = mpu_dev_suspend, /* optional */ + .resume = mpu_dev_resume, /* optional */ + +}; + +static int __init mpu_init(void) +{ + int res = i2c_add_driver(&mpu_driver); + pr_info("%s: Probe name %s\n", __func__, MPU_NAME); + if (res) + pr_err("%s failed\n", __func__); + return res; +} + +static void __exit mpu_exit(void) +{ + pr_info("%s\n", __func__); + i2c_del_driver(&mpu_driver); +} + +module_init(mpu_init); +module_exit(mpu_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("User space character device interface for MPU"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS(MPU_NAME); -- cgit v1.2.2