diff options
Diffstat (limited to 'drivers/misc/mpu3050')
46 files changed, 13805 insertions, 0 deletions
diff --git a/drivers/misc/mpu3050/Kconfig b/drivers/misc/mpu3050/Kconfig new file mode 100755 index 00000000000..933aa3399f7 --- /dev/null +++ b/drivers/misc/mpu3050/Kconfig @@ -0,0 +1,147 @@ + +menu "Motion Sensors Support" + +config MPU_NONE + bool "None" + +config MPU_SENSORS_MPU6000 + tristate "MPU6000" + depends on I2C + +choice + prompt "Accelerometer Type" + depends on MPU_SENSORS_MPU3050 + default MPU_SENSORS_ACCELEROMETER_NONE + +config MPU_SENSORS_ACCELEROMETER_NONE + bool "NONE" + depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 + +config MPU_SENSORS_ADXL346 + bool "ADI adxl346" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_BMA150 + bool "Bosch BMA150" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_BMA222 + bool "Bosch BMA222" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_KXSD9 + bool "Kionix KXSD9" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_KXUD9 + bool "Kionix KXUD9" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_KXTF9 + bool "Kionix KXTF9" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_LIS331DLH + bool "ST lis331dlh" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_LIS3DH + bool "ST lis3dh" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_LSM303DLHA + bool "ST lsm303dlh" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_MMA8450 + bool "Freescale mma8450" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_MMA845X + bool "Freescale mma8451/8452/8453" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_KXTF9_LIS3DH + bool "Kionix KXTF9+ ST LIS3DH" + depends on MPU_SENSORS_MPU3050 + +endchoice + +choice + prompt "Compass Type" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + default MPU_SENSORS_COMPASS_NONE + +config MPU_SENSORS_COMPASS_NONE + bool "NONE" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + +config MPU_SENSORS_AK8975 + bool "AKM ak8975" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + +config MPU_SENSORS_MMC314X + bool "MEMSIC mmc314x" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_MMC328X + bool "MEMSIC mmc328x" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_AMI30X + bool "Aichi Steel ami30X" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_HMC5883 + bool "Honeywell hmc5883" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_LSM303DLHM + bool "ST lsm303dlh" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_MMC314X + bool "MEMSIC mmc314xMS" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_YAS529 + bool "Yamaha yas529" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_HSCDTD002B + bool "Alps hscdtd002b" + depends on MPU_SENSORS_MPU3050 + +config MPU_SENSORS_HSCDTD004A + bool "Alps hscdtd004a" + depends on MPU_SENSORS_MPU3050 + +endchoice + +choice + prompt "Pressure Type" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + default MPU_SENSORS_PRESSURE_NONE + +config MPU_SENSORS_PRESSURE_NONE + bool "NONE" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + +config MPU_SENSORS_BMA085 + bool "Bosch BMA085" + depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050 + +endchoice + + +config MPU_SENSORS_CORE + tristate "Sensors core" + +config MPU_SENSORS_TIMERIRQ + tristate "Timer IRQ" + +config MPU_SENSORS_DEBUG + bool "MPU debug" + depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 || MPU_SENSORS_TIMERIRQ + +endmenu diff --git a/drivers/misc/mpu3050/Makefile b/drivers/misc/mpu3050/Makefile new file mode 100755 index 00000000000..74321229323 --- /dev/null +++ b/drivers/misc/mpu3050/Makefile @@ -0,0 +1,145 @@ +#
+# Kernel makefile for motions sensors
+#
+#
+
+# MPU
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050.o
+mpu3050-objs += mpuirq.o \
+ slaveirq.o \
+ mpu-dev.o \
+ mpu-i2c.o \
+ mlsl-kernel.o \
+ mlos-kernel.o \
+ mpu-accel.o \
+ $(MLLITE_DIR)mldl_cfg.o
+
+#
+# Accel options
+#
+ifdef CONFIG_MPU_SENSORS_ADXL346
+mpu3050-objs += $(MLLITE_DIR)accel/adxl346.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA150
+mpu3050-objs += $(MLLITE_DIR)accel/bma150.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA222
+mpu3050-objs += $(MLLITE_DIR)accel/bma222.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXSD9
+mpu3050-objs += $(MLLITE_DIR)accel/kxsd9.o
+endif
+
+ifdef CONFIG_MACH_BOSE_ATT
+ mpu3050-objs += $(MLLITE_DIR)accel/kxud9.o
+ mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+else
+ifdef CONFIG_MPU_SENSORS_KXUD9
+mpu3050-objs += $(MLLITE_DIR)accel/kxud9.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXTF9
+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+endif
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS331DLH
+mpu3050-objs += $(MLLITE_DIR)accel/lis331.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS3DH
+mpu3050-objs += $(MLLITE_DIR)accel/lis3dh.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHA
+mpu3050-objs += $(MLLITE_DIR)accel/lsm303a.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA8450
+mpu3050-objs += $(MLLITE_DIR)accel/mma8450.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA845X
+mpu3050-objs += $(MLLITE_DIR)accel/mma845x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXTF9_LIS3DH
+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+mpu3050-objs += $(MLLITE_DIR)accel/lis3dh.o
+endif
+
+#
+# Compass options
+#
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu3050-objs += $(MLLITE_DIR)compass/mpuak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_AMI30X
+mpu3050-objs += $(MLLITE_DIR)compass/ami30x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HMC5883
+mpu3050-objs += $(MLLITE_DIR)compass/hmc5883.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHM
+mpu3050-objs += $(MLLITE_DIR)compass/lsm303m.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMC314X
+mpu3050-objs += $(MLLITE_DIR)compass/mmc314x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMC328X
+mpu3050-objs += $(MLLITE_DIR)compass/mmc328x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_YAS529
+mpu3050-objs += $(MLLITE_DIR)compass/yas529-kernel.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD002B
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd002b.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD004A
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd004a.o
+endif
+
+#
+# Pressure options
+#
+ifdef CONFIG_MPU_SENSORS_BMA085
+mpu3050-objs += $(MLLITE_DIR)pressure/bma085.o
+endif
+
+ccflags-y += -I$(M)/$(MLLITE_DIR) \
+ -I$(M)/../../include \
+ -Idrivers/misc/mpu3050 \
+ -Iinclude/linux
+
+obj-$(CONFIG_MPU_SENSORS_MPU6000)+= mpu6000.o
+mpu6000-objs += mpuirq.o \
+ slaveirq.o \
+ mpu-dev.o \
+ mpu-i2c.o \
+ mlsl-kernel.o \
+ mlos-kernel.o \
+ $(MLLITE_DIR)mldl_cfg.o \
+ $(MLLITE_DIR)accel/mantis.o
+
+ifdef CONFIG_MPU_SENSORS_MPU6000
+ccflags-y += -DM_HW
+endif
+
+obj-$(CONFIG_MPU_SENSORS_CORE) += sensors_core.o
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+
+ifdef CONFIG_MPU_SENSORS_DEBUG
+ccflags-y += -DDEBUG
+endif
+
diff --git a/drivers/misc/mpu3050/README b/drivers/misc/mpu3050/README new file mode 100755 index 00000000000..2734dc14163 --- /dev/null +++ b/drivers/misc/mpu3050/README @@ -0,0 +1,250 @@ +Kernel driver mpu
+=====================
+
+Supported chips:
+ * InvenSense IMU3050
+ Prefix: 'mpu3050'
+ Datasheet:
+ PS-MPU-3000A-00.2.4b.pdf
+
+ * InvenSense IMU6000
+ Prefix: 'mpu6000'
+ Datasheet:
+ MPU-6000A-00 v1.0.pdf
+
+Author: InvenSense <http://invensense.com>
+
+Description
+-----------
+The mpu is a motion processor unit that controls the mpu3050 gyroscope, a slave
+accelerometer, a compass and a pressure sensor, or the mpu6000 and slave
+compass. This document describes how to install the driver into a Linux kernel
+and a small note about how to set up the file permissions in an android file
+system.
+
+Sysfs entries
+-------------
+/dev/mpu
+/dev/mpuirq
+/dev/accelirq
+/dev/compassirq
+/dev/pressureirq
+
+General Remarks MPU3050
+-----------------------
+* Valid addresses for the MPU3050 is 0x68.
+* Accelerometer must be on the secondary I2C bus for MPU3050, the
+ magnetometer must be on the primary bus and pressure sensor must
+ be on the primary bus.
+
+General Remarks MPU6000
+-----------------------
+* Valid addresses for the MPU6000 is 0x68.
+* Magnetometer must be on the secondary I2C bus for the MPU6000.
+* Accelerometer slave address must be set to 0x68
+* Gyro and Accel orientation matrices should be the same
+
+Programming the chip using /dev/mpu
+----------------------------------
+Programming of MPU3050 or MPU6000 is done by first opening the /dev/mpu file and
+then performing a series of IOCTLS on the handle returned. The IOCTL codes can
+be found in mpu.h. Typically this is done by the mllite library in user
+space.
+
+Adding to a Kernel
+==================
+
+The mpu driver is designed to be inserted in the drivers/misc part of the
+kernel. Extracting the tarball from the root kernel dir will place the
+contents of the tarball here:
+
+ <kernel root dir>/drivers/misc/mpu3050
+ <kernel root dir>/include/linux/mpu.h
+ <kernel root dir>/include/linux/mpu3050.h
+ <kernel root dir>/include/linux/mpu6000.h
+
+After this is done the drivers/misc/Kconfig must be edited to add the line:
+
+ source "drivers/misc/mpu3050/Kconfig"
+
+Similarly drivers/misc/Makefile must be edited to add the line:
+
+ obj-y += mpu3050/
+
+Configuration can then be done as normal.
+
+NOTE: This driver depends on a kernel patch to drivers/char/char.c. This patch
+started to be included in most 2.6.35 based kernels.
+drivers: misc: pass miscdevice pointer via file private data
+https://patchwork.kernel.org/patch/96412/
+
+---
+ drivers/char/misc.c | 1 +
+ 1 files changed, 1 insertions(+), 0 deletions(-)
+
+
+diff --git a/drivers/char/misc.c b/drivers/char/misc.c
+index 92ab03d..cd650ca 100644
+--- a/drivers/char/misc.c
++++ b/drivers/char/misc.c
+@@ -144,6 +144,7 @@ static int misc_open(struct inode * inode, struct file * file)
+ old_fops = file->f_op;
+ file->f_op = new_fops;
+ if (file->f_op->open) {
++ file->private_data = c;
+ err=file->f_op->open(inode,file);
+ if (err) {
+ fops_put(file->f_op);
+---
+
+Board and Platform Data
+-----------------------
+
+In order for the driver to work, board and platform data specific to the device
+needs to be added to the board file. A mpu3050_platform_data structure must
+be created and populated and set in the i2c_board_info_structure. For details
+of each structure member see mpu.h. All values below are simply an example and
+should be modified for your platform.
+
+#include <linux/mpu.h>
+
+#if defined(CONFIG_SENSORS_MPU3050) || defined(CONFIG_SENSORS_MPU3050_MODULE)
+
+#define SENSOR_MPU_NAME "mpu3050"
+
+static struct mpu3050_platform_data mpu_data = {
+ .int_config = 0x10,
+ .orientation = { -1, 0, 0,
+ 0, 1, 0,
+ 0, 0, -1 },
+ /* accel */
+ .accel = {
+#ifdef CONFIG_SENSORS_MPU3050_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_accel_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_SECONDARY,
+ .address = 0x0F,
+ .orientation = { -1, 0, 0,
+ 0, 1, 0,
+ 0, 0, -1 },
+ },
+ /* compass */
+ .compass = {
+#ifdef CONFIG_SENSORS_MPU3050_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_compass_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_PRIMARY,
+ .address = 0x0E,
+ .orientation = { 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1 },
+ },
+ /* pressure */
+ .pressure = {
+#ifdef CONFIG_SENSORS_MPU3050_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_pressure_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_PRIMARY,
+ .address = 0x77,
+ .orientation = { 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1 },
+ },
+};
+#endif
+
+#if defined(CONFIG_SENSORS_MPU6000) || defined(CONFIG_SENSORS_MPU6000_MODULE)
+
+#define SENSOR_MPU_NAME "mpu6000"
+
+static struct mpu3050_platform_data mpu_data = {
+ .int_config = 0x10,
+ .orientation = { -1, 0, 0,
+ 0, 1, 0,
+ 0, 0, -1 },
+ /* accel */
+ .accel = {
+#ifdef CONFIG_SENSORS_MPU6000_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_accel_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_PRIMARY,
+ .address = 0x68,
+ .orientation = { -1, 0, 0,
+ 0, 1, 0,
+ 0, 0, -1 },
+ },
+ /* compass */
+ .compass = {
+#ifdef CONFIG_SENSORS_MPU6000_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_compass_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_SECONDARY,
+ .address = 0x0E,
+ .orientation = { 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1 },
+ },
+ /* pressure */
+ .pressure = {
+#ifdef CONFIG_SENSORS_MPU6000_MODULE
+ .get_slave_descr = NULL,
+#else
+ .get_slave_descr = get_pressure_slave_descr,
+#endif
+ .adapt_num = 2,
+ .bus = EXT_SLAVE_BUS_PRIMARY,
+ .address = 0x77,
+ .orientation = { 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1 },
+ },
+
+};
+#endif
+
+static struct i2c_board_info __initdata beagle_i2c_2_boardinfo[] = {
+ {
+ I2C_BOARD_INFO(SENSOR_MPU_NAME, 0x68),
+ .irq = (IH_GPIO_BASE + MPU_GPIO_IRQ),
+ .platform_data = &mpu_data,
+ },
+};
+
+Typically the IRQ is a GPIO input pin and needs to be configured properly. If
+in the above example GPIO 168 corresponds to IRQ 299, the following should be
+done as well:
+
+#define MPU_GPIO_IRQ 168
+
+ gpio_request(MPU_GPIO_IRQ,"MPUIRQ");
+ gpio_direction_input(MPU_GPIO_IRQ)
+
+
+Android File Permissions
+========================
+
+To set up the file permissions on an android system, the /dev/mpu and
+/dev/mpuirq files needs to be added to the system/core/init/devices.c file
+inside the perms_ structure.
+
+static struct perms_ devperms[] = {
+ { "/dev/mpu" ,0640, AID_SYSTEM, AID_SYSTEM, 1 },
+};
+
+Sufficient file permissions need to be give to read and write it by the system.
+
diff --git a/drivers/misc/mpu3050/accel/adxl346.c b/drivers/misc/mpu3050/accel/adxl346.c new file mode 100755 index 00000000000..14cb38a829e --- /dev/null +++ b/drivers/misc/mpu3050/accel/adxl346.c @@ -0,0 +1,163 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file adxl346.c + * @brief Accelerometer setup and handling methods for AD adxl346. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +#define ACCEL_ADI346_SLEEP_REG (0x2D) +#define ACCEL_ADI346_SLEEP_MASK (0x04) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int adxl346_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_ADI346_SLEEP_REG, 1, ®); + ERROR_CHECK(result); + reg |= ACCEL_ADI346_SLEEP_MASK; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_ADI346_SLEEP_REG, reg); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register & mask */ +#define ACCEL_ADI346_CTRL_REG (0x31) +#define ACCEL_ADI346_CTRL_MASK (0x03) + +int adxl346_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_ADI346_SLEEP_REG, 1, ®); + ERROR_CHECK(result); + reg &= ~ACCEL_ADI346_SLEEP_MASK; + /*wake up if sleeping */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_ADI346_SLEEP_REG, reg); + ERROR_CHECK(result); + /*MLOSSleep(10) */ + + /* Full Scale */ + reg = 0x04; + reg &= ~ACCEL_ADI346_CTRL_MASK; + if (slave->range.mantissa == 4) + reg |= 0x1; + else if (slave->range.mantissa == 8) + reg |= 0x2; + else if (slave->range.mantissa == 16) + reg |= 0x3; + else { + slave->range.mantissa = 2; + reg |= 0x0; + } + slave->range.fraction = 0; + + /* DATA_FORMAT: full resolution of +/-2g; data is left justified */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x31, reg); + ERROR_CHECK(result); + /* BW_RATE: normal power operation with output data rate of 200Hz */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2C, 0x0B); + ERROR_CHECK(result); + /* POWER_CTL: power on in measurement mode */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2D, 0x28); + ERROR_CHECK(result); + /*--- after wake up, it takes at least [1/(data rate) + 1.1]ms ==> + 6.1ms to get valid sensor data ---*/ + MLOSSleep(10); + + return result; +} + +int adxl346_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +struct ext_slave_descr adxl346_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ adxl346_suspend, + /*.resume = */ adxl346_resume, + /*.read = */ adxl346_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "adx1346", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_ADI346, + /*.reg = */ 0x32, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *adxl346_get_slave_descr(void) +{ + return &adxl346_descr; +} +EXPORT_SYMBOL(adxl346_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/bma150.c b/drivers/misc/mpu3050/accel/bma150.c new file mode 100755 index 00000000000..30fed156485 --- /dev/null +++ b/drivers/misc/mpu3050/accel/bma150.c @@ -0,0 +1,149 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file bma150.c + * @brief Accelerometer setup and handling methods. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlos.h" +#include "mlsl.h" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/********************************************* + Accelerometer Initialization Functions +**********************************************/ + +static int bma150_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x01); + MLOSSleep(3); /* 3 ms powerup time maximum */ + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register and mask */ +#define ACCEL_BOSCH_CTRL_REG (0x14) +#define ACCEL_BOSCH_CTRL_MASK (0x18) + +static int bma150_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg = 0; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x02); + ERROR_CHECK(result); + MLOSSleep(3); + + result = + MLSLSerialRead(mlsl_handle, pdata->address, 0x14, 1, ®); + ERROR_CHECK(result); + + /* Bandwidth */ + reg &= 0xc0; + reg |= 3; /* 3=190 Hz */ + + /* Full Scale */ + reg &= ~ACCEL_BOSCH_CTRL_MASK; + if (slave->range.mantissa == 4) + reg |= 0x08; + else if (slave->range.mantissa == 8) + reg |= 0x10; + else { + slave->range.mantissa = 2; + reg |= 0x00; + } + slave->range.fraction = 0; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, reg); + ERROR_CHECK(result); + + return result; +} + +static int bma150_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +static struct ext_slave_descr bma150_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ bma150_suspend, + /*.resume = */ bma150_resume, + /*.read = */ bma150_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "bma150", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_BMA150, + /*.reg = */ 0x02, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *bma150_get_slave_descr(void) +{ + return &bma150_descr; +} +EXPORT_SYMBOL(bma150_get_slave_descr); + +#ifdef __KERNEL__ +MODULE_AUTHOR("Invensense"); +MODULE_DESCRIPTION("User space IRQ handler for MPU3xxx devices"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bma"); +#endif + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/accel/bma222.c b/drivers/misc/mpu3050/accel/bma222.c new file mode 100755 index 00000000000..534a1e5fa64 --- /dev/null +++ b/drivers/misc/mpu3050/accel/bma222.c @@ -0,0 +1,142 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/* + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file bma222.c + * @brief Accelerometer setup and handling methods. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlos.h" +#include "mlsl.h" + +#define ACCEL_BMA222_RANGE_REG (0x0F) +#define ACCEL_BMA222_BW_REG (0x10) +#define ACCEL_BMA222_SUSPEND_REG (0x11) +#define ACCEL_BMA222_SFT_RST_REG (0x14) + +/********************************************* + Accelerometer Initialization Functions +**********************************************/ + +static int bma222_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_BMA222_SUSPEND_REG, 0x80); + ERROR_CHECK(result); + + return result; +} + +static int bma222_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg = 0; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_BMA222_SFT_RST_REG, 0xB6); + ERROR_CHECK(result); + MLOSSleep(10); + + /*Bandwidth */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_BMA222_BW_REG, 0x0C); + ERROR_CHECK(result); + + /* Full Scale */ + if (slave->range.mantissa == 4) + reg |= 0x05; + else if (slave->range.mantissa == 8) + reg |= 0x08; + else if (slave->range.mantissa == 16) + reg |= 0x0C; + else { + slave->range.mantissa = 2; + reg |= 0x03; + } + slave->range.fraction = 0; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_BMA222_RANGE_REG, reg); + ERROR_CHECK(result); + + return result; +} + +static int bma222_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +static struct ext_slave_descr bma222_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ bma222_suspend, + /*.resume = */ bma222_resume, + /*.read = */ bma222_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "bma222", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_BMA222, + /*.reg = */ 0x02, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *bma222_get_slave_descr(void) +{ + return &bma222_descr; +} +EXPORT_SYMBOL(bma222_get_slave_descr); + +/* + * @} + */ diff --git a/drivers/misc/mpu3050/accel/cma3000.c b/drivers/misc/mpu3050/accel/cma3000.c new file mode 100755 index 00000000000..05925951c62 --- /dev/null +++ b/drivers/misc/mpu3050/accel/cma3000.c @@ -0,0 +1,109 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file cma3000.c + * @brief Accelerometer setup and handling methods for VTI CMA3000 + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" +#include "accel.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int cma3000_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + /* RAM reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1d, 0xcd); + return result; +} + +int cma3000_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + + + return ML_SUCCESS; +} + +int cma3000_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +struct ext_slave_descr cma3000_descr = { + /*.suspend = */ cma3000_suspend, + /*.resume = */ cma3000_resume, + /*.read = */ cma3000_read, + /*.name = */ "cma3000", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ID_INVALID, + /* fixme - id to added when support becomes available */ + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ 65536, +}; + +struct ext_slave_descr *cma3000_get_slave_descr(void) +{ + return &cma3000_descr; +} +EXPORT_SYMBOL(cma3000_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/kxsd9.c b/drivers/misc/mpu3050/accel/kxsd9.c new file mode 100755 index 00000000000..77bc52c8e58 --- /dev/null +++ b/drivers/misc/mpu3050/accel/kxsd9.c @@ -0,0 +1,145 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file kxsd9.c + * @brief Accelerometer setup and handling methods. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/kernel.h> +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +static int kxsd9_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + /* CTRL_REGB: low-power standby mode */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x0); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register and mask */ +#define ACCEL_KIONIX_CTRL_REG (0x0C) +#define ACCEL_KIONIX_CTRL_MASK (0x3) + +static int kxsd9_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + /* Full Scale */ + reg = 0x0; + reg &= ~ACCEL_KIONIX_CTRL_MASK; + reg |= 0x00; + if (slave->range.mantissa == 4) { /* 4g scale = 4.9951 */ + reg |= 0x2; + slave->range.fraction = 9951; + } else if (slave->range.mantissa == 7) { /* 6g scale = 7.5018 */ + reg |= 0x1; + slave->range.fraction = 5018; + } else if (slave->range.mantissa == 9) { /* 8g scale = 9.9902 */ + reg |= 0x0; + slave->range.fraction = 9902; + } else { + slave->range.mantissa = 2; /* 2g scale = 2.5006 */ + slave->range.fraction = 5006; + reg |= 0x3; + } + reg |= 0xC0; /* 100Hz LPF */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_KIONIX_CTRL_REG, reg); + ERROR_CHECK(result); + /* normal operation */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x40); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +static int kxsd9_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +static struct ext_slave_descr kxsd9_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ kxsd9_suspend, + /*.resume = */ kxsd9_resume, + /*.read = */ kxsd9_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "kxsd9", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_KXSD9, + /*.reg = */ 0x00, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 5006}, +}; + +struct ext_slave_descr *kxsd9_get_slave_descr(void) +{ + return &kxsd9_descr; +} +EXPORT_SYMBOL(kxsd9_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/kxtf9.c b/drivers/misc/mpu3050/accel/kxtf9.c new file mode 100755 index 00000000000..f4382590191 --- /dev/null +++ b/drivers/misc/mpu3050/accel/kxtf9.c @@ -0,0 +1,617 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file kxtf9.c + * @brief Accelerometer setup and handling methods. +*/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 1 + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +#define KXTF9_XOUT_HPF_L (0x00) /* 0000 0000 */ +#define KXTF9_XOUT_HPF_H (0x01) /* 0000 0001 */ +#define KXTF9_YOUT_HPF_L (0x02) /* 0000 0010 */ +#define KXTF9_YOUT_HPF_H (0x03) /* 0000 0011 */ +#define KXTF9_ZOUT_HPF_L (0x04) /* 0001 0100 */ +#define KXTF9_ZOUT_HPF_H (0x05) /* 0001 0101 */ +#define KXTF9_XOUT_L (0x06) /* 0000 0110 */ +#define KXTF9_XOUT_H (0x07) /* 0000 0111 */ +#define KXTF9_YOUT_L (0x08) /* 0000 1000 */ +#define KXTF9_YOUT_H (0x09) /* 0000 1001 */ +#define KXTF9_ZOUT_L (0x0A) /* 0001 1010 */ +#define KXTF9_ZOUT_H (0x0B) /* 0001 1011 */ +#define KXTF9_ST_RESP (0x0C) /* 0000 1100 */ +#define KXTF9_WHO_AM_I (0x0F) /* 0000 1111 */ +#define KXTF9_TILT_POS_CUR (0x10) /* 0001 0000 */ +#define KXTF9_TILT_POS_PRE (0x11) /* 0001 0001 */ +#define KXTF9_INT_SRC_REG1 (0x15) /* 0001 0101 */ +#define KXTF9_INT_SRC_REG2 (0x16) /* 0001 0110 */ +#define KXTF9_STATUS_REG (0x18) /* 0001 1000 */ +#define KXTF9_INT_REL (0x1A) /* 0001 1010 */ +#define KXTF9_CTRL_REG1 (0x1B) /* 0001 1011 */ +#define KXTF9_CTRL_REG2 (0x1C) /* 0001 1100 */ +#define KXTF9_CTRL_REG3 (0x1D) /* 0001 1101 */ +#define KXTF9_INT_CTRL_REG1 (0x1E) /* 0001 1110 */ +#define KXTF9_INT_CTRL_REG2 (0x1F) /* 0001 1111 */ +#define KXTF9_INT_CTRL_REG3 (0x20) /* 0010 0000 */ +#define KXTF9_DATA_CTRL_REG (0x21) /* 0010 0001 */ +#define KXTF9_TILT_TIMER (0x28) /* 0010 1000 */ +#define KXTF9_WUF_TIMER (0x29) /* 0010 1001 */ +#define KXTF9_TDT_TIMER (0x2B) /* 0010 1011 */ +#define KXTF9_TDT_H_THRESH (0x2C) /* 0010 1100 */ +#define KXTF9_TDT_L_THRESH (0x2D) /* 0010 1101 */ +#define KXTF9_TDT_TAP_TIMER (0x2E) /* 0010 1110 */ +#define KXTF9_TDT_TOTAL_TIMER (0x2F) /* 0010 1111 */ +#define KXTF9_TDT_LATENCY_TIMER (0x30) /* 0011 0000 */ +#define KXTF9_TDT_WINDOW_TIMER (0x31) /* 0011 0001 */ +#define KXTF9_WUF_THRESH (0x5A) /* 0101 1010 */ +#define KXTF9_TILT_ANGLE (0x5C) /* 0101 1100 */ +#define KXTF9_HYST_SET (0x5F) /* 0101 1111 */ + +#define KXTF9_MAX_DUR (0xFF) +#define KXTF9_MAX_THS (0xFF) +#define KXTF9_THS_COUNTS_P_G (32) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +struct kxtf9_config { + unsigned int odr; /* Output data rate mHz */ + unsigned int fsr; /* full scale range mg */ + unsigned int ths; /* Motion no-motion thseshold mg */ + unsigned int dur; /* Motion no-motion duration ms */ + unsigned int irq_type; + unsigned char reg_ths; + unsigned char reg_dur; + unsigned char reg_odr; + unsigned char reg_int_cfg1; + unsigned char reg_int_cfg2; + unsigned char ctrl_reg1; +}; + +struct kxtf9_private_data { + struct kxtf9_config suspend; + struct kxtf9_config resume; +}; + +extern struct acc_data cal_data; +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +static int kxtf9_set_ths(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct kxtf9_config *config, int apply, long ths) +{ + int result = ML_SUCCESS; + if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS) + ths = (KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G; + + if (ths < 0) + ths = 0; + + config->ths = ths; + config->reg_ths = (unsigned char) + ((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000); + MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_WUF_THRESH, + config->reg_ths); + return result; +} + +static int kxtf9_set_dur(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct kxtf9_config *config, int apply, long dur) +{ + int result = ML_SUCCESS; + long reg_dur = (dur * config->odr) / 1000000; + config->dur = dur; + + if (reg_dur > KXTF9_MAX_DUR) + reg_dur = KXTF9_MAX_DUR; + + config->reg_dur = (unsigned char)reg_dur; + MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_WUF_TIMER, + (unsigned char)reg_dur); + return result; +} + +/** + * Sets the IRQ to fire when one of the IRQ events occur. Threshold and + * duration will not be used uless the type is MOT or NMOT. + * + * @param config configuration to apply to, suspend or resume + * @param irq_type The type of IRQ. Valid values are + * - MPU_SLAVE_IRQ_TYPE_NONE + * - MPU_SLAVE_IRQ_TYPE_MOTION + * - MPU_SLAVE_IRQ_TYPE_DATA_READY + */ +static int kxtf9_set_irq(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct kxtf9_config *config, int apply, long irq_type) +{ + int result = ML_SUCCESS; + struct kxtf9_private_data *private_data = pdata->private_data; + + config->irq_type = (unsigned char)irq_type; + config->ctrl_reg1 &= ~0x22; + if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + config->ctrl_reg1 |= 0x20; + config->reg_int_cfg1 = 0x38; + config->reg_int_cfg2 = 0x00; + } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { + config->ctrl_reg1 |= 0x02; + if ((unsigned long)config == + (unsigned long)&private_data->suspend) + config->reg_int_cfg1 = 0x34; + else + config->reg_int_cfg1 = 0x24; + config->reg_int_cfg2 = 0xE0; + } else { + config->reg_int_cfg1 = 0x00; + config->reg_int_cfg2 = 0x00; + } + + if (apply) { + /* Must clear bit 7 before writing new configuration */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, 0x40); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_INT_CTRL_REG1, + config->reg_int_cfg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_INT_CTRL_REG2, + config->reg_int_cfg2); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, + config->ctrl_reg1); + } + MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n", + (unsigned long)config->ctrl_reg1, + (unsigned long)config->reg_int_cfg1, + (unsigned long)config->reg_int_cfg2); + + return result; +} + +/** + * Set the Output data rate for the particular configuration + * + * @param config Config to modify with new ODR + * @param odr Output data rate in units of 1/1000Hz + */ +static int kxtf9_set_odr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct kxtf9_config *config, int apply, long odr) +{ + unsigned char bits; + int result = ML_SUCCESS; + + /* Data sheet says there is 12.5 hz, but that seems to produce a single + * correct data value, thus we remove it from the table */ + if (odr > 400000) { + config->odr = 800000; + bits = 0x06; + } else if (odr > 200000) { + config->odr = 400000; + bits = 0x05; + } else if (odr > 100000) { + config->odr = 200000; + bits = 0x04; + } else if (odr > 50000) { + config->odr = 100000; + bits = 0x03; + } else if (odr > 25000) { + config->odr = 50000; + bits = 0x02; + } else if (odr != 0) { + config->odr = 25000; + bits = 0x01; + } else { + config->odr = 0; + bits = 0; + } + + if (odr != 0) + config->ctrl_reg1 |= 0x80; + + config->reg_odr = bits; + kxtf9_set_dur(mlsl_handle, pdata, config, apply, config->dur); + MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1); + if (apply) { + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_DATA_CTRL_REG, + config->reg_odr); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, 0x40); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, + config->ctrl_reg1); + } + return result; +} + +/** + * Set the full scale range of the accels + * + * @param config pointer to configuration + * @param fsr requested full scale range + */ +static int kxtf9_set_fsr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct kxtf9_config *config, int apply, long fsr) +{ + int result = ML_SUCCESS; + + config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7); + if (fsr <= 2000) { + config->fsr = 2000; + config->ctrl_reg1 |= 0x00; + } else if (fsr <= 4000) { + config->fsr = 4000; + config->ctrl_reg1 |= 0x08; + } else { + config->fsr = 8000; + config->ctrl_reg1 |= 0x10; + } + + MPL_LOGV("FSR: %d\n", config->fsr); + if (apply) { + /* Must clear bit 7 before writing new configuration */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, 0x40); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, + config->ctrl_reg1); + } + return result; +} + +static int kxtf9_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, 0x0); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register and mask */ +#define ACCEL_KIONIX_CTRL_REG (0x1b) +#define ACCEL_KIONIX_CTRL_MASK (0x18) + +static int kxtf9_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char data; + struct kxtf9_private_data *private_data = pdata->private_data; + + /* Wake up */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, 0x40); + ERROR_CHECK(result); + /* INT_CTRL_REG1: */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_INT_CTRL_REG1, + private_data->resume.reg_int_cfg1); + ERROR_CHECK(result); + /* WUF_THRESH: */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_WUF_THRESH, + private_data->resume.reg_ths); + ERROR_CHECK(result); + /* DATA_CTRL_REG */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_DATA_CTRL_REG, + private_data->resume.reg_odr); + ERROR_CHECK(result); + /* WUF_TIMER */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_WUF_TIMER, + private_data->resume.reg_dur); + ERROR_CHECK(result); + + /* Normal operation */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + KXTF9_CTRL_REG1, + private_data->resume.ctrl_reg1); + ERROR_CHECK(result); + result = MLSLSerialRead(mlsl_handle, pdata->address, + KXTF9_INT_REL, 1, &data); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +static int kxtf9_init(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + + struct kxtf9_private_data *private_data; + int result = ML_SUCCESS; + + private_data = (struct kxtf9_private_data *) + MLOSMalloc(sizeof(struct kxtf9_private_data)); + + if (!private_data) + return ML_ERROR_MEMORY_EXAUSTED; + + /* RAM reset */ + result = MLSLSerialWriteSingle(mlsl_handle, + pdata->address, KXTF9_CTRL_REG1, 0x40); + /* Fastest Reset */ + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, + pdata->address, KXTF9_DATA_CTRL_REG, 0x36); + /* Fastest Reset */ + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, + pdata->address, KXTF9_CTRL_REG3, 0xcd); + /* Reset */ + ERROR_CHECK(result); + MLOSSleep(2); + + pdata->private_data = private_data; + + private_data->resume.ctrl_reg1 = 0xC0; + private_data->suspend.ctrl_reg1 = 0x40; + + result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend, + FALSE, 1000); + ERROR_CHECK(result); + result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume, + FALSE, 2540); + ERROR_CHECK(result); + + result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 50000); + ERROR_CHECK(result); + result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume, + FALSE, 200000); + + result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 2000); + ERROR_CHECK(result); + result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume, + FALSE, 2000); + ERROR_CHECK(result); + + result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend, + FALSE, 80); + ERROR_CHECK(result); + result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume, + FALSE, 40); + ERROR_CHECK(result); + + result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend, + FALSE, MPU_SLAVE_IRQ_TYPE_NONE); + ERROR_CHECK(result); + result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume, + FALSE, MPU_SLAVE_IRQ_TYPE_NONE); + ERROR_CHECK(result); + return result; +} + +static int kxtf9_exit(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + if (pdata->private_data) + return MLOSFree(pdata->private_data); + else + return ML_SUCCESS; +} + +static int kxtf9_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct kxtf9_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + return kxtf9_set_odr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_ODR_RESUME: + return kxtf9_set_odr(mlsl_handle, pdata, + &private_data->resume, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + return kxtf9_set_fsr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_RESUME: + return kxtf9_set_fsr(mlsl_handle, pdata, + &private_data->resume, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_THS: + return kxtf9_set_ths(mlsl_handle, pdata, + &private_data->suspend, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_THS: + return kxtf9_set_ths(mlsl_handle, pdata, + &private_data->resume, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_DUR: + return kxtf9_set_dur(mlsl_handle, pdata, + &private_data->suspend, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_DUR: + return kxtf9_set_dur(mlsl_handle, pdata, + &private_data->resume, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + return kxtf9_set_irq(mlsl_handle, pdata, + &private_data->suspend, + data->apply, *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_RESUME: + return kxtf9_set_irq(mlsl_handle, pdata, + &private_data->resume, + data->apply, *((long *)data->data)); + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +static int kxtf9_get_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct kxtf9_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long)private_data->suspend.odr; + break; + case MPU_SLAVE_CONFIG_ODR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long)private_data->resume.odr; + break; + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long)private_data->suspend.fsr; + break; + case MPU_SLAVE_CONFIG_FSR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long)private_data->resume.fsr; + break; + case MPU_SLAVE_CONFIG_MOT_THS: + (*(unsigned long *)data->data) = + (unsigned long)private_data->suspend.ths; + break; + case MPU_SLAVE_CONFIG_NMOT_THS: + (*(unsigned long *)data->data) = + (unsigned long)private_data->resume.ths; + break; + case MPU_SLAVE_CONFIG_MOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long)private_data->suspend.dur; + break; + case MPU_SLAVE_CONFIG_NMOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long)private_data->resume.dur; + break; + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long)private_data->suspend.irq_type; + break; + case MPU_SLAVE_CONFIG_IRQ_RESUME: + (*(unsigned long *)data->data) = + (unsigned long)private_data->resume.irq_type; + break; + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +static int kxtf9_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + int x, y, z; + + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + + if (slave->len == 6) { + x = (s16) ((data[1] << 4) | (data[0] >> 4)) + cal_data.x; + y = (s16) ((data[3] << 4) | (data[2] >> 4)) + cal_data.y; + z = (s16) ((data[5] << 4) | (data[4] >> 4)) + cal_data.z; + + data[0] = (x & 0xf) << 4; + data[1] = (x & 0xff0) >> 4; + data[2] = (y & 0xf) << 4; + data[3] = (y & 0xff0) >> 4; + data[4] = (z & 0xf) << 4; + data[5] = (z & 0xff0) >> 4; + } + + ERROR_CHECK(result); + return result; +} + +static struct ext_slave_descr kxtf9_descr = { + /*.init = */ kxtf9_init, + /*.exit = */ kxtf9_exit, + /*.suspend = */ kxtf9_suspend, + /*.resume = */ kxtf9_resume, + /*.read = */ kxtf9_read, + /*.config = */ kxtf9_config, + /*.get_config = */ kxtf9_get_config, + /*.name = */ "kxtf9", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_KXTF9, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *kxtf9_get_slave_descr(void) +{ + return &kxtf9_descr; +} +EXPORT_SYMBOL(kxtf9_get_slave_descr); diff --git a/drivers/misc/mpu3050/accel/kxud9.c b/drivers/misc/mpu3050/accel/kxud9.c new file mode 100755 index 00000000000..651219e2c97 --- /dev/null +++ b/drivers/misc/mpu3050/accel/kxud9.c @@ -0,0 +1,145 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file kxud9.c + * @brief Accelerometer setup and handling methods. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/kernel.h> +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions + *****************************************/ + +static int kxud9_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + /* CTRL_REGB: low-power standby mode */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x0); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register and mask */ +#define ACCEL_KIONIX_CTRL_REG (0x0C) +#define ACCEL_KIONIX_CTRL_MASK (0x3) + +static int kxud9_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + /* Full Scale */ + reg = 0x0; + reg &= ~ACCEL_KIONIX_CTRL_MASK; + reg |= 0x00; + if (slave->range.mantissa == 4) { /* 4g scale = 4.9951 */ + reg |= 0x2; + slave->range.fraction = 9951; + } else if (slave->range.mantissa == 7) { /* 6g scale = 7.5018 */ + reg |= 0x1; + slave->range.fraction = 5018; + } else if (slave->range.mantissa == 9) { /* 8g scale = 9.9902 */ + reg |= 0x0; + slave->range.fraction = 9902; + } else { + slave->range.mantissa = 2; /* 2g scale = 2.5006 */ + slave->range.fraction = 5006; + reg |= 0x3; + } + reg |= 0xC0; /* 100Hz LPF */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_KIONIX_CTRL_REG, reg); + ERROR_CHECK(result); + /* normal operation */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x40); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +static int kxud9_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +static struct ext_slave_descr kxud9_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ kxud9_suspend, + /*.resume = */ kxud9_resume, + /*.read = */ kxud9_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "kxud9", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ /* ACCEL_ID_KXUD9, */ ACCEL_ID_KXSD9, + /*.reg = */ 0x00, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 5006}, +}; + +struct ext_slave_descr *kxud9_get_slave_descr(void) +{ + return &kxud9_descr; +} +EXPORT_SYMBOL(kxud9_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/lis331.c b/drivers/misc/mpu3050/accel/lis331.c new file mode 100755 index 00000000000..53c599b2ef2 --- /dev/null +++ b/drivers/misc/mpu3050/accel/lis331.c @@ -0,0 +1,617 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file lis331.c + * @brief Accelerometer setup and handling methods for ST LIS331 + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 1 + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* full scale setting - register & mask */ +#define LIS331_CTRL_REG1 (0x20) +#define LIS331_CTRL_REG2 (0x21) +#define LIS331_CTRL_REG3 (0x22) +#define LIS331_CTRL_REG4 (0x23) +#define LIS331_CTRL_REG5 (0x24) +#define LIS331_HP_FILTER_RESET (0x25) +#define LIS331_REFERENCE (0x26) +#define LIS331_STATUS_REG (0x27) +#define LIS331_OUT_X_L (0x28) +#define LIS331_OUT_X_H (0x29) +#define LIS331_OUT_Y_L (0x2a) +#define LIS331_OUT_Y_H (0x2b) +#define LIS331_OUT_Z_L (0x2b) +#define LIS331_OUT_Z_H (0x2d) + +#define LIS331_INT1_CFG (0x30) +#define LIS331_INT1_SRC (0x31) +#define LIS331_INT1_THS (0x32) +#define LIS331_INT1_DURATION (0x33) + +#define LIS331_INT2_CFG (0x34) +#define LIS331_INT2_SRC (0x35) +#define LIS331_INT2_THS (0x36) +#define LIS331_INT2_DURATION (0x37) + +#define LIS331_CTRL_MASK (0x30) +#define LIS331_SLEEP_MASK (0x20) + +#define LIS331_MAX_DUR (0x7F) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +struct lis331dlh_config { + unsigned int odr; + unsigned int fsr; /* full scale range mg */ + unsigned int ths; /* Motion no-motion thseshold mg */ + unsigned int dur; /* Motion no-motion duration ms */ + unsigned char reg_ths; + unsigned char reg_dur; + unsigned char ctrl_reg1; + unsigned char irq_type; + unsigned char mot_int1_cfg; +}; + +struct lis331dlh_private_data { + struct lis331dlh_config suspend; + struct lis331dlh_config resume; +}; + + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +static int lis331dlh_set_ths(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis331dlh_config *config, + int apply, + long ths) +{ + int result = ML_SUCCESS; + if ((unsigned int) ths >= config->fsr) + ths = (long) config->fsr - 1; + + if (ths < 0) + ths = 0; + + config->ths = ths; + config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr)); + MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_THS, + config->reg_ths); + return result; +} + +static int lis331dlh_set_dur(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis331dlh_config *config, + int apply, + long dur) +{ + int result = ML_SUCCESS; + long reg_dur = (dur * config->odr) / 1000000L; + config->dur = dur; + + if (reg_dur > LIS331_MAX_DUR) + reg_dur = LIS331_MAX_DUR; + + config->reg_dur = (unsigned char) reg_dur; + MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_DURATION, + (unsigned char)reg_dur); + return result; +} + +/** + * Sets the IRQ to fire when one of the IRQ events occur. Threshold and + * duration will not be used uless the type is MOT or NMOT. + * + * @param config configuration to apply to, suspend or resume + * @param irq_type The type of IRQ. Valid values are + * - MPU_SLAVE_IRQ_TYPE_NONE + * - MPU_SLAVE_IRQ_TYPE_MOTION + * - MPU_SLAVE_IRQ_TYPE_DATA_READY + */ +static int lis331dlh_set_irq(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis331dlh_config *config, + int apply, + long irq_type) +{ + int result = ML_SUCCESS; + unsigned char reg1; + unsigned char reg2; + + config->irq_type = (unsigned char)irq_type; + if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x02; + reg2 = 0x00; + } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x00; + reg2 = config->mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + + if (apply) { + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG3, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_CFG, reg2); + } + + return result; +} + +/** + * Set the Output data rate for the particular configuration + * + * @param config Config to modify with new ODR + * @param odr Output data rate in units of 1/1000Hz + */ +static int lis331dlh_set_odr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis331dlh_config *config, + int apply, + long odr) +{ + unsigned char bits; + int result = ML_SUCCESS; + + if (odr > 400000) { + config->odr = 1000000; + bits = 0x38; + } else if (odr > 100000) { + config->odr = 400000; + bits = 0x30; + } else if (odr > 50000) { + config->odr = 100000; + bits = 0x28; + } else if (odr > 10000) { + config->odr = 50000; + bits = 0x20; + } else if (odr > 5000) { + config->odr = 10000; + bits = 0xC0; + } else if (odr > 2000) { + config->odr = 5000; + bits = 0xB0; + } else if (odr > 1000) { + config->odr = 2000; + bits = 0x80; + } else if (odr > 500) { + config->odr = 1000; + bits = 0x60; + } else if (odr > 0) { + config->odr = 500; + bits = 0x40; + } else { + config->odr = 0; + bits = 0; + } + + config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7); + lis331dlh_set_dur(mlsl_handle, pdata, + config, apply, config->dur); + MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG1, + config->ctrl_reg1); + return result; +} + +/** + * Set the full scale range of the accels + * + * @param config pointer to configuration + * @param fsr requested full scale range + */ +static int lis331dlh_set_fsr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis331dlh_config *config, + int apply, + long fsr) +{ + unsigned char reg1 = 0x40; + int result = ML_SUCCESS; + + if (fsr <= 2048) { + config->fsr = 2048; + } else if (fsr <= 4096) { + reg1 |= 0x30; + config->fsr = 4096; + } else { + reg1 |= 0x10; + config->fsr = 8192; + } + + lis331dlh_set_ths(mlsl_handle, pdata, + config, apply, config->ths); + MPL_LOGV("FSR: %d\n", config->fsr); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG4, reg1); + + return result; +} + +static int lis331dlh_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg1; + unsigned char reg2; + struct lis331dlh_private_data *private_data = pdata->private_data; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG1, + private_data->suspend.ctrl_reg1); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG2, 0x0f); + reg1 = 0x40; + if (private_data->suspend.fsr == 8192) + reg1 |= 0x30; + else if (private_data->suspend.fsr == 4096) + reg1 |= 0x10; + /* else bits [4..5] are already zero */ + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG4, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_THS, + private_data->suspend.reg_ths); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_DURATION, + private_data->suspend.reg_dur); + + if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x02; + reg2 = 0x00; + } else if (private_data->suspend.irq_type == + MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x00; + reg2 = private_data->suspend.mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG3, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_CFG, reg2); + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS331_HP_FILTER_RESET, 1, ®1); + return result; +} + +static int lis331dlh_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg1; + unsigned char reg2; + struct lis331dlh_private_data *private_data = pdata->private_data; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG1, + private_data->resume.ctrl_reg1); + ERROR_CHECK(result); + MLOSSleep(6); + + /* Full Scale */ + reg1 = 0x40; + if (private_data->resume.fsr == 8192) + reg1 |= 0x30; + else if (private_data->resume.fsr == 4096) + reg1 |= 0x10; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG4, reg1); + ERROR_CHECK(result); + + /* Configure high pass filter */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG2, 0x0F); + ERROR_CHECK(result); + + if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x02; + reg2 = 0x00; + } else if (private_data->resume.irq_type == + MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x00; + reg2 = private_data->resume.mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_CTRL_REG3, reg1); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_THS, + private_data->resume.reg_ths); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_DURATION, + private_data->resume.reg_dur); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS331_INT1_CFG, reg2); + ERROR_CHECK(result); + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS331_HP_FILTER_RESET, 1, ®1); + ERROR_CHECK(result); + return result; +} + +static int lis331dlh_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result = ML_SUCCESS; + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS331_STATUS_REG, 1, data); + if (data[0] & 0x0F) { + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; + } else + return ML_ERROR_ACCEL_DATA_NOT_READY; +} + +static int lis331dlh_init(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + struct lis331dlh_private_data *private_data; + private_data = (struct lis331dlh_private_data *) + MLOSMalloc(sizeof(struct lis331dlh_private_data)); + + if (!private_data) + return ML_ERROR_MEMORY_EXAUSTED; + + pdata->private_data = private_data; + + private_data->resume.ctrl_reg1 = 0x37; + private_data->suspend.ctrl_reg1 = 0x47; + private_data->resume.mot_int1_cfg = 0x95; + private_data->suspend.mot_int1_cfg = 0x2a; + + lis331dlh_set_odr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 0); + lis331dlh_set_odr(mlsl_handle, pdata, &private_data->resume, + FALSE, 200000); + lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 2048); + lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->resume, + FALSE, 2048); + lis331dlh_set_ths(mlsl_handle, pdata, &private_data->suspend, + FALSE, 80); + lis331dlh_set_ths(mlsl_handle, pdata, &private_data->resume, + FALSE, 40); + lis331dlh_set_dur(mlsl_handle, pdata, &private_data->suspend, + FALSE, 1000); + lis331dlh_set_dur(mlsl_handle, pdata, &private_data->resume, + FALSE, 2540); + lis331dlh_set_irq(mlsl_handle, pdata, &private_data->suspend, + FALSE, + MPU_SLAVE_IRQ_TYPE_NONE); + lis331dlh_set_irq(mlsl_handle, pdata, &private_data->resume, + FALSE, + MPU_SLAVE_IRQ_TYPE_NONE); + return ML_SUCCESS; +} + +static int lis331dlh_exit(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + if (pdata->private_data) + return MLOSFree(pdata->private_data); + else + return ML_SUCCESS; +} + +static int lis331dlh_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct lis331dlh_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + return lis331dlh_set_odr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_ODR_RESUME: + return lis331dlh_set_odr(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + return lis331dlh_set_fsr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_RESUME: + return lis331dlh_set_fsr(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_THS: + return lis331dlh_set_ths(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_THS: + return lis331dlh_set_ths(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_DUR: + return lis331dlh_set_dur(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_DUR: + return lis331dlh_set_dur(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + return lis331dlh_set_irq(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_RESUME: + return lis331dlh_set_irq(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +static int lis331dlh_get_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct lis331dlh_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.odr; + break; + case MPU_SLAVE_CONFIG_ODR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.odr; + break; + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.fsr; + break; + case MPU_SLAVE_CONFIG_FSR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.fsr; + break; + case MPU_SLAVE_CONFIG_MOT_THS: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.ths; + break; + case MPU_SLAVE_CONFIG_NMOT_THS: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.ths; + break; + case MPU_SLAVE_CONFIG_MOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.dur; + break; + case MPU_SLAVE_CONFIG_NMOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.dur; + break; + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.irq_type; + break; + case MPU_SLAVE_CONFIG_IRQ_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.irq_type; + break; + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +static struct ext_slave_descr lis331dlh_descr = { + /*.init = */ lis331dlh_init, + /*.exit = */ lis331dlh_exit, + /*.suspend = */ lis331dlh_suspend, + /*.resume = */ lis331dlh_resume, + /*.read = */ lis331dlh_read, + /*.config = */ lis331dlh_config, + /*.get_config = */ lis331dlh_get_config, + /*.name = */ "lis331dlh", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_LIS331, + /*.reg = */ (0x28 | 0x80), /* 0x80 for burst reads */ + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 480}, +}; + +struct ext_slave_descr *lis331dlh_get_slave_descr(void) +{ + return &lis331dlh_descr; +} +EXPORT_SYMBOL(lis331dlh_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/lis3dh.c b/drivers/misc/mpu3050/accel/lis3dh.c new file mode 100755 index 00000000000..594cd422ac4 --- /dev/null +++ b/drivers/misc/mpu3050/accel/lis3dh.c @@ -0,0 +1,625 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file lis3dh.c + * @brief Accelerometer setup and handling methods for ST LIS3DH + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* full scale setting - register & mask */ +#define LIS3DH_CTRL_REG1 (0x20) +#define LIS3DH_CTRL_REG2 (0x21) +#define LIS3DH_CTRL_REG3 (0x22) +#define LIS3DH_CTRL_REG4 (0x23) +#define LIS3DH_CTRL_REG5 (0x24) +#define LIS3DH_CTRL_REG6 (0x25) +#define LIS3DH_REFERENCE (0x26) +#define LIS3DH_STATUS_REG (0x27) +#define LIS3DH_OUT_X_L (0x28) +#define LIS3DH_OUT_X_H (0x29) +#define LIS3DH_OUT_Y_L (0x2a) +#define LIS3DH_OUT_Y_H (0x2b) +#define LIS3DH_OUT_Z_L (0x2b) +#define LIS3DH_OUT_Z_H (0x2d) + +#define LIS3DH_INT1_CFG (0x30) +#define LIS3DH_INT1_SRC (0x31) +#define LIS3DH_INT1_THS (0x32) +#define LIS3DH_INT1_DURATION (0x33) + +#define LIS3DH_MAX_DUR (0x7F) + + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +struct lis3dh_config { + unsigned int odr; + unsigned int fsr; /* full scale range mg */ + unsigned int ths; /* Motion no-motion thseshold mg */ + unsigned int dur; /* Motion no-motion duration ms */ + unsigned char reg_ths; + unsigned char reg_dur; + unsigned char ctrl_reg1; + unsigned char irq_type; + unsigned char mot_int1_cfg; +}; + +struct lis3dh_private_data { + struct lis3dh_config suspend; + struct lis3dh_config resume; +}; + + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +static int lis3dh_set_ths(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis3dh_config *config, + int apply, + long ths) +{ + int result = ML_SUCCESS; + if ((unsigned int) ths > 1000 * config->fsr) + ths = (long) 1000 * config->fsr; + + if (ths < 0) + ths = 0; + + config->ths = ths; + config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr)); + MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_THS, + config->reg_ths); + return result; +} + +static int lis3dh_set_dur(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis3dh_config *config, + int apply, + long dur) +{ + int result = ML_SUCCESS; + long reg_dur = (dur * config->odr) / 1000000L; + config->dur = dur; + + if (reg_dur > LIS3DH_MAX_DUR) + reg_dur = LIS3DH_MAX_DUR; + + config->reg_dur = (unsigned char) reg_dur; + MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_DURATION, + (unsigned char)reg_dur); + return result; +} + +/** + * Sets the IRQ to fire when one of the IRQ events occur. Threshold and + * duration will not be used uless the type is MOT or NMOT. + * + * @param config configuration to apply to, suspend or resume + * @param irq_type The type of IRQ. Valid values are + * - MPU_SLAVE_IRQ_TYPE_NONE + * - MPU_SLAVE_IRQ_TYPE_MOTION + * - MPU_SLAVE_IRQ_TYPE_DATA_READY + */ +static int lis3dh_set_irq(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis3dh_config *config, + int apply, + long irq_type) +{ + int result = ML_SUCCESS; + unsigned char reg1; + unsigned char reg2; + + config->irq_type = (unsigned char)irq_type; + if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x10; + reg2 = 0x00; + } else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x40; + reg2 = config->mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + + if (apply) { + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG3, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_CFG, reg2); + } + + return result; +} + +/** + * Set the Output data rate for the particular configuration + * + * @param config Config to modify with new ODR + * @param odr Output data rate in units of 1/1000Hz + */ +static int lis3dh_set_odr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis3dh_config *config, + int apply, + long odr) +{ + unsigned char bits; + int result = ML_SUCCESS; + + if (odr > 400000) { + config->odr = 1250000; + bits = 0x90; + } else if (odr > 200000) { + config->odr = 400000; + bits = 0x70; + } else if (odr > 100000) { + config->odr = 200000; + bits = 0x60; + } else if (odr > 50000) { + config->odr = 100000; + bits = 0x50; + } else if (odr > 25000) { + config->odr = 50000; + bits = 0x40; + } else if (odr > 10000) { + config->odr = 25000; + bits = 0x30; + } else if (odr > 1000) { + config->odr = 10000; + bits = 0x20; + } else if (odr > 500) { + config->odr = 1000; + bits = 0x10; + } else { + config->odr = 0; + bits = 0; + } + + config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xf); + lis3dh_set_dur(mlsl_handle, pdata, + config, apply, config->dur); + MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG1, + config->ctrl_reg1); + return result; +} + +/** + * Set the full scale range of the accels + * + * @param config pointer to configuration + * @param fsr requested full scale range + */ +static int lis3dh_set_fsr(void *mlsl_handle, + struct ext_slave_platform_data *pdata, + struct lis3dh_config *config, + int apply, + long fsr) +{ + int result = ML_SUCCESS; + unsigned char reg1 = 0x48; + + if (fsr <= 2048) { + config->fsr = 2048; + } else if (fsr <= 4096) { + reg1 |= 0x10; + config->fsr = 4096; + } else if (fsr <= 8192) { + reg1 |= 0x20; + config->fsr = 8192; + } else { + reg1 |= 0x30; + config->fsr = 16348; + } + + lis3dh_set_ths(mlsl_handle, pdata, + config, apply, config->ths); + MPL_LOGV("FSR: %d\n", config->fsr); + if (apply) + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG4, reg1); + + return result; +} + +static int lis3dh_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg1; + unsigned char reg2; + struct lis3dh_private_data *private_data = pdata->private_data; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG1, + private_data->suspend.ctrl_reg1); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG2, 0x31); + reg1 = 0x48; + if (private_data->suspend.fsr == 16384) + reg1 |= 0x30; + else if (private_data->suspend.fsr == 8192) + reg1 |= 0x20; + else if (private_data->suspend.fsr == 4096) + reg1 |= 0x10; + else if (private_data->suspend.fsr == 2048) + reg1 |= 0x00; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG4, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_THS, + private_data->suspend.reg_ths); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_DURATION, + private_data->suspend.reg_dur); + + if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x10; + reg2 = 0x00; + } else if (private_data->suspend.irq_type == + MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x40; + reg2 = private_data->suspend.mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG3, reg1); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_CFG, reg2); + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG6, 1, ®1); + + return result; + +} + +static int lis3dh_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + tMLError result; + unsigned char reg1; + unsigned char reg2; + struct lis3dh_private_data *private_data = pdata->private_data; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG1, + private_data->resume.ctrl_reg1); + ERROR_CHECK(result); + MLOSSleep(6); + + /* Full Scale */ + reg1 = 0x48; + if (private_data->suspend.fsr == 16384) + reg1 |= 0x30; + else if (private_data->suspend.fsr == 8192) + reg1 |= 0x20; + else if (private_data->suspend.fsr == 4096) + reg1 |= 0x10; + else if (private_data->suspend.fsr == 2048) + reg1 |= 0x00; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG4, reg1); + ERROR_CHECK(result); + + /* Configure high pass filter */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG2, 0x31); + ERROR_CHECK(result); + + if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { + reg1 = 0x10; + reg2 = 0x00; + } else if (private_data->resume.irq_type == + MPU_SLAVE_IRQ_TYPE_MOTION) { + reg1 = 0x40; + reg2 = private_data->resume.mot_int1_cfg; + } else { + reg1 = 0x00; + reg2 = 0x00; + } + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG3, reg1); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_THS, + private_data->resume.reg_ths); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_DURATION, + private_data->resume.reg_dur); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_INT1_CFG, reg2); + ERROR_CHECK(result); + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG6, 1, ®1); + ERROR_CHECK(result); + return result; +} + +static int lis3dh_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result = ML_SUCCESS; + result = MLSLSerialRead(mlsl_handle, pdata->address, + LIS3DH_STATUS_REG, 1, data); + if (data[0] & 0x0F) { + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; + } else + return ML_ERROR_ACCEL_DATA_NOT_READY; +} + +static int lis3dh_init(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + tMLError result; + + struct lis3dh_private_data *private_data; + private_data = (struct lis3dh_private_data *) + MLOSMalloc(sizeof(struct lis3dh_private_data)); + + if (!private_data) + return ML_ERROR_MEMORY_EXAUSTED; + + pdata->private_data = private_data; + + private_data->resume.ctrl_reg1 = 0x67; + private_data->suspend.ctrl_reg1 = 0x18; + private_data->resume.mot_int1_cfg = 0x95; + private_data->suspend.mot_int1_cfg = 0x2a; + + lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 0); + lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume, + FALSE, 200000); + lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend, + FALSE, 2048); + lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume, + FALSE, 2048); + lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend, + FALSE, 80); + lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume, + FALSE, 40); + lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend, + FALSE, 1000); + lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume, + FALSE, 2540); + lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend, + FALSE, + MPU_SLAVE_IRQ_TYPE_NONE); + lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume, + FALSE, + MPU_SLAVE_IRQ_TYPE_NONE); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LIS3DH_CTRL_REG1, 0x07); + MLOSSleep(6); + + return ML_SUCCESS; +} + +static int lis3dh_exit(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + if (pdata->private_data) + return MLOSFree(pdata->private_data); + else + return ML_SUCCESS; +} + +static int lis3dh_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct lis3dh_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + return lis3dh_set_odr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_ODR_RESUME: + return lis3dh_set_odr(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + return lis3dh_set_fsr(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_FSR_RESUME: + return lis3dh_set_fsr(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_THS: + return lis3dh_set_ths(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_THS: + return lis3dh_set_ths(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_MOT_DUR: + return lis3dh_set_dur(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_NMOT_DUR: + return lis3dh_set_dur(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + return lis3dh_set_irq(mlsl_handle, pdata, + &private_data->suspend, + data->apply, + *((long *)data->data)); + case MPU_SLAVE_CONFIG_IRQ_RESUME: + return lis3dh_set_irq(mlsl_handle, pdata, + &private_data->resume, + data->apply, + *((long *)data->data)); + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + return ML_SUCCESS; +} + +static int lis3dh_get_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct lis3dh_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.odr; + break; + case MPU_SLAVE_CONFIG_ODR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.odr; + break; + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.fsr; + break; + case MPU_SLAVE_CONFIG_FSR_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.fsr; + break; + case MPU_SLAVE_CONFIG_MOT_THS: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.ths; + break; + case MPU_SLAVE_CONFIG_NMOT_THS: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.ths; + break; + case MPU_SLAVE_CONFIG_MOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.dur; + break; + case MPU_SLAVE_CONFIG_NMOT_DUR: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.dur; + break; + case MPU_SLAVE_CONFIG_IRQ_SUSPEND: + (*(unsigned long *)data->data) = + (unsigned long) private_data->suspend.irq_type; + break; + case MPU_SLAVE_CONFIG_IRQ_RESUME: + (*(unsigned long *)data->data) = + (unsigned long) private_data->resume.irq_type; + break; + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +static struct ext_slave_descr lis3dh_descr = { + /*.init = */ lis3dh_init, + /*.exit = */ lis3dh_exit, + /*.suspend = */ lis3dh_suspend, + /*.resume = */ lis3dh_resume, + /*.read = */ lis3dh_read, + /*.config = */ lis3dh_config, + /*.get_config = */ lis3dh_get_config, + /*.name = */ "lis3dh", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_LIS3DH, + /*.reg = */ 0x28 | 0x80, /* 0x80 for burst reads */ + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 480}, +}; + +struct ext_slave_descr *lis3dh_get_slave_descr(void) +{ + return &lis3dh_descr; +} +EXPORT_SYMBOL(lis3dh_get_slave_descr); + +/* + * @} +*/ diff --git a/drivers/misc/mpu3050/accel/lsm303a.c b/drivers/misc/mpu3050/accel/lsm303a.c new file mode 100755 index 00000000000..b8494962d3a --- /dev/null +++ b/drivers/misc/mpu3050/accel/lsm303a.c @@ -0,0 +1,178 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file lsm303a.c + * @brief Accelerometer setup and handling methods for ST LSM303 + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +#define ACCEL_ST_SLEEP_REG (0x20) +#define ACCEL_ST_SLEEP_MASK (0x20) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int lsm303dlha_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_ST_SLEEP_REG, + 1, ®); + ERROR_CHECK(result); + reg &= ~(0x27); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_ST_SLEEP_REG, reg); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register & mask */ +#define ACCEL_ST_CTRL_REG (0x23) +#define ACCEL_ST_CTRL_MASK (0x30) + +int lsm303dlha_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_ST_SLEEP_REG, + 1, ®); + ERROR_CHECK(result); + reg |= 0x27; + /*wake up if sleeping */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_ST_SLEEP_REG, reg); + ERROR_CHECK(result); + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x20, 0x37); + ERROR_CHECK(result); + MLOSSleep(500); + + reg = 0x40; + + /* Full Scale */ + reg &= ~ACCEL_ST_CTRL_MASK; + if (slave->range.mantissa == 4) { + slave->range.fraction = 960; + reg |= 0x10; + } else if (slave->range.mantissa == 8) { + slave->range.fraction = 1920; + reg |= 0x30; + } else { + slave->range.mantissa = 2; + slave->range.fraction = 480; + reg |= 0x00; + } + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x23, reg); + ERROR_CHECK(result); + + /* Configure high pass filter */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x21, 0x0F); + ERROR_CHECK(result); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x32, 0x00); + ERROR_CHECK(result); + /* Configure INT1_DURATION */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x33, 0x7F); + ERROR_CHECK(result); + /* Configure INT1_CFG */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x30, 0x95); + ERROR_CHECK(result); + MLOSSleep(50); + return result; +} + +int lsm303dlha_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +struct ext_slave_descr lsm303dlha_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ lsm303dlha_suspend, + /*.resume = */ lsm303dlha_resume, + /*.read = */ lsm303dlha_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "lsm303dlha", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_LSM303, + /*.reg = */ (0x28 | 0x80), /* 0x80 for burst reads */ + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 480}, +}; + +struct ext_slave_descr *lsm303dlha_get_slave_descr(void) +{ + return &lsm303dlha_descr; +} +EXPORT_SYMBOL(lsm303dlha_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/mantis.c b/drivers/misc/mpu3050/accel/mantis.c new file mode 100755 index 00000000000..1cb9847fee0 --- /dev/null +++ b/drivers/misc/mpu3050/accel/mantis.c @@ -0,0 +1,306 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file lis331.c + * @brief Accelerometer setup and handling methods for Invensense MANTIS + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +struct mantis_config { + unsigned int odr; /* output data rate 1/1000 Hz*/ + unsigned int fsr; /* full scale range mg */ + unsigned int ths; /* Motion no-motion thseshold mg */ + unsigned int dur; /* Motion no-motion duration ms */ +}; + +struct mantis_private_data { + struct mantis_config suspend; + struct mantis_config resume; +}; + + +/***************************************** + *Accelerometer Initialization Functions + *****************************************/ +/** + * Record the odr for use in computing duration values. + * + * @param config Config to set, suspend or resume structure + * @param odr output data rate in 1/1000 hz + */ +void mantis_set_odr(struct mantis_config *config, + long odr) +{ + config->odr = odr; +} + +void mantis_set_ths(struct mantis_config *config, + long ths) +{ + if (ths < 0) + ths = 0; + + config->ths = ths; + MPL_LOGV("THS: %d\n", config->ths); +} + +void mantis_set_dur(struct mantis_config *config, + long dur) +{ + if (dur < 0) + dur = 0; + + config->dur = dur; + MPL_LOGV("DUR: %d\n", config->dur); +} + +static void mantis_set_fsr( + struct mantis_config *config, + long fsr) +{ + if (fsr <= 2000) + config->fsr = 2000; + else if (fsr <= 4000) + config->fsr = 4000; + else + config->fsr = 8000; + + MPL_LOGV("FSR: %d\n", config->fsr); +} + +static int mantis_init(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + struct mantis_private_data *private_data; + private_data = (struct mantis_private_data *) + MLOSMalloc(sizeof(struct mantis_private_data)); + + if (!private_data) + return ML_ERROR_MEMORY_EXAUSTED; + + pdata->private_data = private_data; + + mantis_set_odr(&private_data->suspend, 0); + mantis_set_odr(&private_data->resume, 200000); + mantis_set_fsr(&private_data->suspend, 2000); + mantis_set_fsr(&private_data->resume, 2000); + mantis_set_ths(&private_data->suspend, 80); + mantis_set_ths(&private_data->resume, 40); + mantis_set_dur(&private_data->suspend, 1000); + mantis_set_dur(&private_data->resume, 2540); + return ML_SUCCESS; +} + +static int mantis_exit(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + if (pdata->private_data) + return MLOSFree(pdata->private_data); + else + return ML_SUCCESS; +} + +int mantis_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + unsigned char reg; + int result; + + result = MLSLSerialRead(mlsl_handle, pdata->address, + MPUREG_PWR_MGMT_2, 1, ®); + ERROR_CHECK(result); + reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_PWR_MGMT_2, reg); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +int mantis_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + struct mantis_private_data *private_data; + + private_data = (struct mantis_private_data *) pdata->private_data; + + MLSLSerialRead(mlsl_handle, pdata->address, + MPUREG_PWR_MGMT_2, 1, ®); + + reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_PWR_MGMT_2, reg); + ERROR_CHECK(result); + + if (slave->range.mantissa == 2) + reg = 0; + else if (slave->range.mantissa == 4) + reg = 1 << 3; + else if (slave->range.mantissa == 8) + reg = 2 << 3; + else if (slave->range.mantissa == 16) + reg = 3 << 3; + else + return ML_ERROR; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_ACCEL_CONFIG, reg); + ERROR_CHECK(result); + + reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_THR_LSB; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_ACCEL_MOT_THR, reg); + ERROR_CHECK(result); + + reg = (unsigned char) + ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_ACCEL_ZRMOT_THR, reg); + ERROR_CHECK(result); + + reg = (unsigned char) private_data->suspend.ths / ACCEL_MOT_DUR_LSB; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_ACCEL_MOT_DUR, reg); + ERROR_CHECK(result); + + reg = (unsigned char) private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MPUREG_ACCEL_ZRMOT_DUR, reg); + ERROR_CHECK(result); + return result; +} + +int mantis_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + int result; + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, slave->len, data); + return result; +} + +static int mantis_config(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *data) +{ + struct mantis_private_data *private_data = pdata->private_data; + if (!data->data) + return ML_ERROR_INVALID_PARAMETER; + + switch (data->key) { + case MPU_SLAVE_CONFIG_ODR_SUSPEND: + mantis_set_odr(&private_data->suspend, + *((long *)data->data)); + break; + case MPU_SLAVE_CONFIG_ODR_RESUME: + mantis_set_odr(&private_data->resume, + *((long *)data->data)); + break; + case MPU_SLAVE_CONFIG_FSR_SUSPEND: + mantis_set_fsr(&private_data->suspend, + *((long *)data->data)); + break; + case MPU_SLAVE_CONFIG_FSR_RESUME: + mantis_set_fsr(&private_data->resume, + *((long *)data->data)); + break; + case MPU_SLAVE_CONFIG_MOT_THS: + mantis_set_ths(&private_data->suspend, + (*((long *)data->data))); + break; + case MPU_SLAVE_CONFIG_NMOT_THS: + mantis_set_ths(&private_data->resume, + (*((long *)data->data))); + break; + case MPU_SLAVE_CONFIG_MOT_DUR: + mantis_set_dur(&private_data->suspend, + (*((long *)data->data))); + break; + case MPU_SLAVE_CONFIG_NMOT_DUR: + mantis_set_dur(&private_data->resume, + (*((long *)data->data))); + break; + default: + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + }; + + return ML_SUCCESS; +} + +struct ext_slave_descr mantis_descr = { + /*.init = */ mantis_init, + /*.exit = */ mantis_exit, + /*.suspend = */ mantis_suspend, + /*.resume = */ mantis_resume, + /*.read = */ mantis_read, + /*.config = */ mantis_config, + /*.get_config = */ NULL, + /*.name = */ "mantis", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_MPU6000, + /*.reg = */ 0xA8, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *mantis_get_slave_descr(void) +{ + return &mantis_descr; +} +EXPORT_SYMBOL(mantis_get_slave_descr); + +/** + * @} + */ + diff --git a/drivers/misc/mpu3050/accel/mma8450.c b/drivers/misc/mpu3050/accel/mma8450.c new file mode 100755 index 00000000000..b5b3728eea3 --- /dev/null +++ b/drivers/misc/mpu3050/accel/mma8450.c @@ -0,0 +1,156 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file mma8450.c + * @brief Accelerometer setup and handling methods for Freescale MMA8450 + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include <stdlib.h> +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" +#include <string.h> + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +#define ACCEL_MMA8450_SLEEP_REG (0x38) +#define ACCEL_MMA8450_SLEEP_MASK (0x3) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int mma8450_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_MMA8450_SLEEP_REG, 1, ®); + ERROR_CHECK(result); + reg &= ~ACCEL_MMA8450_SLEEP_MASK; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_MMA8450_SLEEP_REG, reg); + ERROR_CHECK(result); + return result; +} + +/* full scale setting - register & mask */ +#define ACCEL_MMA8450_CTRL_REG (0x38) +#define ACCEL_MMA8450_CTRL_MASK (0x3) + +int mma8450_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_MMA8450_CTRL_REG, 1, ®); + ERROR_CHECK(result); + + /* data rate = 200Hz */ + reg &= 0xE3; + reg |= 0x4; + + /* Full Scale */ + reg &= ~ACCEL_MMA8450_CTRL_MASK; + if (slave->range.mantissa == 4) + reg |= 0x2; + else if (slave->range.mantissa == 8) + reg |= 0x3; + else { + slave->range.mantissa = 2; + reg |= 0x1; + } + slave->range.fraction = 0; + + /* XYZ_DATA_CFG: event flag enabled on all axis */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x16, 0x05); + ERROR_CHECK(result); + /* CTRL_REG1: rate + scale config + wakeup */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_MMA8450_CTRL_REG, reg); + ERROR_CHECK(result); + + return result; +} + +int mma8450_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + int result; + unsigned char local_data[4]; /* Status register + 3 bytes data */ + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, sizeof(local_data), local_data); + ERROR_CHECK(result); + memcpy(data, &local_data[1], (slave->len) - 1); + return result; +} + +struct ext_slave_descr mma8450_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mma8450_suspend, + /*.resume = */ mma8450_resume, + /*.read = */ mma8450_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mma8450", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_MMA8450, + /*.reg = */ 0x00, + /*.len = */ 4, + /*.endian = */ EXT_SLAVE_FS8_BIG_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *mma8450_get_slave_descr(void) +{ + return &mma8450_descr; +} + +EXPORT_SYMBOL(mma8450_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/accel/mma845x.c b/drivers/misc/mpu3050/accel/mma845x.c new file mode 100755 index 00000000000..27150ad8699 --- /dev/null +++ b/drivers/misc/mpu3050/accel/mma845x.c @@ -0,0 +1,158 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file mma845x.c + * @brief Accelerometer setup and handling methods for Freescale MMA845X + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include <stdlib.h> +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" +#include <string.h> + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +#define ACCEL_MMA845X_CTRL_REG1 (0x2A) +#define ACCEL_MMA845X_SLEEP_MASK (0x01) + +/* full scale setting - register & mask */ +#define ACCEL_MMA845X_CFG_REG (0x0E) +#define ACCEL_MMA845X_CTRL_MASK (0x03) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int mma845x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_MMA845X_CTRL_REG1, 1, ®); + ERROR_CHECK(result); + reg &= ~ACCEL_MMA845X_SLEEP_MASK; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_MMA845X_CTRL_REG1, reg); + ERROR_CHECK(result); + return result; +} + + +int mma845x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + unsigned char reg; + + result = MLSLSerialRead(mlsl_handle, pdata->address, + ACCEL_MMA845X_CFG_REG, 1, ®); + ERROR_CHECK(result); + + /* data rate = 200Hz */ + + /* Full Scale */ + reg &= ~ACCEL_MMA845X_CTRL_MASK; + if (slave->range.mantissa == 4) + reg |= 0x1; + else if (slave->range.mantissa == 8) + reg |= 0x2; + else { + slave->range.mantissa = 2; + reg |= 0x0; + } + slave->range.fraction = 0; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_MMA845X_CFG_REG, reg); + ERROR_CHECK(result); + /* 200Hz + active mode */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + ACCEL_MMA845X_CTRL_REG1, 0x11); + ERROR_CHECK(result); + + return result; +} + +int mma845x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + unsigned char local_data[7]; /* Status register + 6 bytes data */ + result = MLSLSerialRead(mlsl_handle, pdata->address, + slave->reg, sizeof(local_data), local_data); + ERROR_CHECK(result); + memcpy(data, &local_data[1], slave->len); + return result; +} + +struct ext_slave_descr mma845x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mma845x_suspend, + /*.resume = */ mma845x_resume, + /*.read = */ mma845x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mma845x", + /*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER, + /*.id = */ ACCEL_ID_MMA845X, + /*.reg = */ 0x00, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_FS16_BIG_ENDIAN, + /*.range = */ {2, 0}, +}; + +struct ext_slave_descr *mma845x_get_slave_descr(void) +{ + return &mma845x_descr; +} +EXPORT_SYMBOL(mma845x_get_slave_descr); + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/compass/ami304.c b/drivers/misc/mpu3050/compass/ami304.c new file mode 100755 index 00000000000..5c33861fc8b --- /dev/null +++ b/drivers/misc/mpu3050/compass/ami304.c @@ -0,0 +1,164 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file ami304.c + * @brief Magnetometer setup and handling methods for Aichi ami304 compass. +*/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +#define AMI304_REG_DATAX (0x10) +#define AMI304_REG_STAT1 (0x18) +#define AMI304_REG_CNTL1 (0x1B) +#define AMI304_REG_CNTL2 (0x1C) +#define AMI304_REG_CNTL3 (0x1D) + +#define AMI304_BIT_CNTL1_PC1 (0x80) +#define AMI304_BIT_CNTL1_ODR1 (0x10) +#define AMI304_BIT_CNTL1_FS1 (0x02) + +#define AMI304_BIT_CNTL2_IEN (0x10) +#define AMI304_BIT_CNTL2_DREN (0x08) +#define AMI304_BIT_CNTL2_DRP (0x04) +#define AMI304_BIT_CNTL3_F0RCE (0x40) + +int ami304_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_CNTL1, + 1, ®); + ERROR_CHECK(result); + + reg &= ~(AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL1, reg); + ERROR_CHECK(result); + + return result; +} + +int ami304_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Set CNTL1 reg to power model active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL1, + AMI304_BIT_CNTL1_PC1|AMI304_BIT_CNTL1_FS1); + ERROR_CHECK(result); + /* Set CNTL2 reg to DRDY active high and enabled */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL2, + AMI304_BIT_CNTL2_DREN | + AMI304_BIT_CNTL2_DRP); + ERROR_CHECK(result); + /* Set CNTL3 reg to forced measurement period */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL3, AMI304_BIT_CNTL3_F0RCE); + + return result; +} + +int ami304_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char stat; + int result = ML_SUCCESS; + + /* Read status reg and check if data ready (DRDY) */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI304_REG_STAT1, + 1, &stat); + ERROR_CHECK(result); + + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + AMI304_REG_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + /* start another measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI304_REG_CNTL3, + AMI304_BIT_CNTL3_F0RCE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } + + return ML_ERROR_COMPASS_DATA_NOT_READY; +} + +struct ext_slave_descr ami304_descr = { + /*.suspend = */ ami304_suspend, + /*.resume = */ ami304_resume, + /*.read = */ ami304_read, + /*.name = */ "ami304", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AICHI, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {5461, 3333} +}; + +struct ext_slave_descr *ami304_get_slave_descr(void) +{ + return &ami304_descr; +} + +#ifdef __KERNEL__ +EXPORT_SYMBOL(ami304_get_slave_descr); +#endif + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/ami30x.c b/drivers/misc/mpu3050/compass/ami30x.c new file mode 100755 index 00000000000..5e4a33efc7f --- /dev/null +++ b/drivers/misc/mpu3050/compass/ami30x.c @@ -0,0 +1,167 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file ami30x.c + * @brief Magnetometer setup and handling methods for Aichi AMI304/AMI305 + * compass. +*/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +#define AMI30X_REG_DATAX (0x10) +#define AMI30X_REG_STAT1 (0x18) +#define AMI30X_REG_CNTL1 (0x1B) +#define AMI30X_REG_CNTL2 (0x1C) +#define AMI30X_REG_CNTL3 (0x1D) + +#define AMI30X_BIT_CNTL1_PC1 (0x80) +#define AMI30X_BIT_CNTL1_ODR1 (0x10) +#define AMI30X_BIT_CNTL1_FS1 (0x02) + +#define AMI30X_BIT_CNTL2_IEN (0x10) +#define AMI30X_BIT_CNTL2_DREN (0x08) +#define AMI30X_BIT_CNTL2_DRP (0x04) +#define AMI30X_BIT_CNTL3_F0RCE (0x40) + +int ami30x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + unsigned char reg; + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_CNTL1, + 1, ®); + ERROR_CHECK(result); + + reg &= ~(AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL1, reg); + ERROR_CHECK(result); + + return result; +} + +int ami30x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Set CNTL1 reg to power model active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL1, + AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1); + ERROR_CHECK(result); + /* Set CNTL2 reg to DRDY active high and enabled */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL2, + AMI30X_BIT_CNTL2_DREN | + AMI30X_BIT_CNTL2_DRP); + ERROR_CHECK(result); + /* Set CNTL3 reg to forced measurement period */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE); + + return result; +} + +int ami30x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char stat; + int result = ML_SUCCESS; + + /* Read status reg and check if data ready (DRDY) */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1, + 1, &stat); + ERROR_CHECK(result); + + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + AMI30X_REG_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + /* start another measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AMI30X_REG_CNTL3, + AMI30X_BIT_CNTL3_F0RCE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } + + return ML_ERROR_COMPASS_DATA_NOT_READY; +} + +struct ext_slave_descr ami30x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ ami30x_suspend, + /*.resume = */ ami30x_resume, + /*.read = */ ami30x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "ami30x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AMI30X, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {5461, 3333} + /* For AMI305,the range field needs to be modified to {9830.4f}*/ +}; + +struct ext_slave_descr *ami30x_get_slave_descr(void) +{ + return &ami30x_descr; +} +EXPORT_SYMBOL(ami30x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hmc5883.c b/drivers/misc/mpu3050/compass/hmc5883.c new file mode 100755 index 00000000000..051b071fe7f --- /dev/null +++ b/drivers/misc/mpu3050/compass/hmc5883.c @@ -0,0 +1,254 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hmc5883.c + * @brief Magnetometer setup and handling methods for honeywell hmc5883 + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*-----HONEYWELL HMC5883 Registers ------*/ +enum HMC_REG { + HMC_REG_CONF_A = 0x0, + HMC_REG_CONF_B = 0x1, + HMC_REG_MODE = 0x2, + HMC_REG_X_M = 0x3, + HMC_REG_X_L = 0x4, + HMC_REG_Z_M = 0x5, + HMC_REG_Z_L = 0x6, + HMC_REG_Y_M = 0x7, + HMC_REG_Y_L = 0x8, + HMC_REG_STATUS = 0x9, + HMC_REG_ID_A = 0xA, + HMC_REG_ID_B = 0xB, + HMC_REG_ID_C = 0xC +}; + +enum HMC_CONF_A { + HMC_CONF_A_DRATE_MASK = 0x1C, + HMC_CONF_A_DRATE_0_75 = 0x00, + HMC_CONF_A_DRATE_1_5 = 0x04, + HMC_CONF_A_DRATE_3 = 0x08, + HMC_CONF_A_DRATE_7_5 = 0x0C, + HMC_CONF_A_DRATE_15 = 0x10, + HMC_CONF_A_DRATE_30 = 0x14, + HMC_CONF_A_DRATE_75 = 0x18, + HMC_CONF_A_MEAS_MASK = 0x3, + HMC_CONF_A_MEAS_NORM = 0x0, + HMC_CONF_A_MEAS_POS = 0x1, + HMC_CONF_A_MEAS_NEG = 0x2 +}; + +enum HMC_CONF_B { + HMC_CONF_B_GAIN_MASK = 0xE0, + HMC_CONF_B_GAIN_0_9 = 0x00, + HMC_CONF_B_GAIN_1_2 = 0x20, + HMC_CONF_B_GAIN_1_9 = 0x40, + HMC_CONF_B_GAIN_2_5 = 0x60, + HMC_CONF_B_GAIN_4_0 = 0x80, + HMC_CONF_B_GAIN_4_6 = 0xA0, + HMC_CONF_B_GAIN_5_5 = 0xC0, + HMC_CONF_B_GAIN_7_9 = 0xE0 +}; + +enum HMC_MODE { + HMC_MODE_MASK = 0x3, + HMC_MODE_CONT = 0x0, + HMC_MODE_SINGLE = 0x1, + HMC_MODE_IDLE = 0x2, + HMC_MODE_SLEEP = 0x3 +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int hmc5883_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SLEEP); + ERROR_CHECK(result); + MLOSSleep(3); + + return result; +} + +int hmc5883_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Use single measurement mode. Start at sleep state. */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SLEEP); + ERROR_CHECK(result); + /* Config normal measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_CONF_A, 0); + ERROR_CHECK(result); + /* Adjust gain to 307 LSB/Gauss */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_CONF_B, HMC_CONF_B_GAIN_5_5); + ERROR_CHECK(result); + + return result; +} + +int hmc5883_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + unsigned char tmp; + short axisFixed; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, HMC_REG_STATUS, 1, + &stat); + ERROR_CHECK(result); + if (stat & 0x01) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + HMC_REG_X_M, 6, (unsigned char *) data); + ERROR_CHECK(result); + + /* switch YZ axis to proper position */ + tmp = data[2]; + data[2] = data[4]; + data[4] = tmp; + tmp = data[3]; + data[3] = data[5]; + data[5] = tmp; + + /*drop data if overflows */ + if ((data[0] == 0xf0) || (data[2] == 0xf0) + || (data[4] == 0xf0)) { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + HMC_REG_MODE, + HMC_MODE_SINGLE); + ERROR_CHECK(result); + return ML_ERROR_COMPASS_DATA_OVERFLOW; + } + /* convert to fixed point and apply sensitivity correction for + Z-axis */ + axisFixed = + (short) ((unsigned short) data[5] + + (unsigned short) data[4] * 256); + /* scale up by 1.125 (36/32) */ + axisFixed = (short) (axisFixed * 36); + data[4] = axisFixed >> 8; + data[5] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[3] + + (unsigned short) data[2] * 256); + axisFixed = (short) (axisFixed * 32); + data[2] = axisFixed >> 8; + data[3] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[1] + + (unsigned short) data[0] * 256); + axisFixed = (short) (axisFixed * 32); + data[0] = axisFixed >> 8; + data[1] = axisFixed & 0xFF; + + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } else { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + HMC_REG_MODE, HMC_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +struct ext_slave_descr hmc5883_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hmc5883_suspend, + /*.resume = */ hmc5883_resume, + /*.read = */ hmc5883_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hmc5883", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HMC5883, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {10673, 6156}, +}; + +struct ext_slave_descr *hmc5883_get_slave_descr(void) +{ + return &hmc5883_descr; +} +EXPORT_SYMBOL(hmc5883_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hscdtd002b.c b/drivers/misc/mpu3050/compass/hscdtd002b.c new file mode 100755 index 00000000000..bf26cae06d2 --- /dev/null +++ b/drivers/misc/mpu3050/compass/hscdtd002b.c @@ -0,0 +1,163 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hscdtd002b.c + * @brief Magnetometer setup and handling methods for Alps hscdtd002b + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ALPS HSCDTD002B Registers ------*/ +#define COMPASS_HSCDTD002B_STAT (0x18) +#define COMPASS_HSCDTD002B_CTRL1 (0x1B) +#define COMPASS_HSCDTD002B_CTRL2 (0x1C) +#define COMPASS_HSCDTD002B_CTRL3 (0x1D) +#define COMPASS_HSCDTD002B_DATAX (0x10) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Compass Initialization Functions +*****************************************/ + +int hscdtd002b_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Power mode: stand-by */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL1, 0x00); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-off time */ + + return result; +} + +int hscdtd002b_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL3, 0x80); + ERROR_CHECK(result); + /* Force state; Power mode: active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL1, 0x82); + ERROR_CHECK(result); + /* Data ready enable */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL2, 0x08); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-on time */ + + return result; +} + +int hscdtd002b_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + int status = ML_SUCCESS; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_STAT, 1, &stat); + ERROR_CHECK(result); + if (stat & 0x40) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + status = ML_SUCCESS; + } else if (stat & 0x20) { + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + } else { + status = ML_ERROR_COMPASS_DATA_NOT_READY; + } + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD002B_CTRL3, 0x40); + ERROR_CHECK(result); + + return status; +} + +struct ext_slave_descr hscdtd002b_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hscdtd002b_suspend, + /*.resume = */ hscdtd002b_resume, + /*.read = */ hscdtd002b_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hscdtd002b", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HSCDTD002B, + /*.reg = */ 0x10, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000}, +}; + +struct ext_slave_descr *hscdtd002b_get_slave_descr(void) +{ + return &hscdtd002b_descr; +} +EXPORT_SYMBOL(hscdtd002b_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/hscdtd004a.c b/drivers/misc/mpu3050/compass/hscdtd004a.c new file mode 100755 index 00000000000..43fc14a23fc --- /dev/null +++ b/drivers/misc/mpu3050/compass/hscdtd004a.c @@ -0,0 +1,162 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file hscdtd004a.c + * @brief Magnetometer setup and handling methods for Alps hscdtd004a + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ALPS HSCDTD004A Registers ------*/ +#define COMPASS_HSCDTD004A_STAT (0x18) +#define COMPASS_HSCDTD004A_CTRL1 (0x1B) +#define COMPASS_HSCDTD004A_CTRL2 (0x1C) +#define COMPASS_HSCDTD004A_CTRL3 (0x1D) +#define COMPASS_HSCDTD004A_DATAX (0x10) + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Compass Initialization Functions +*****************************************/ + +int hscdtd004a_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Power mode: stand-by */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL1, 0x00); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-off time */ + + return result; +} + +int hscdtd004a_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Soft reset */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL3, 0x80); + ERROR_CHECK(result); + /* Normal state; Power mode: active */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL1, 0x82); + ERROR_CHECK(result); + /* Data ready enable */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL2, 0x7C); + ERROR_CHECK(result); + MLOSSleep(1); /* turn-on time */ + return result; +} + +int hscdtd004a_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + int status = ML_SUCCESS; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_STAT, 1, &stat); + ERROR_CHECK(result); + if (stat & 0x48) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_DATAX, 6, + (unsigned char *) data); + ERROR_CHECK(result); + status = ML_SUCCESS; + } else if (stat & 0x68) { + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + } else { + status = ML_ERROR_COMPASS_DATA_NOT_READY; + } + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + COMPASS_HSCDTD004A_CTRL3, 0x40); + ERROR_CHECK(result); + return status; + +} + +struct ext_slave_descr hscdtd004a_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ hscdtd004a_suspend, + /*.resume = */ hscdtd004a_resume, + /*.read = */ hscdtd004a_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "hscdtd004a", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_HSCDTD004A, + /*.reg = */ 0x10, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000}, +}; + +struct ext_slave_descr *hscdtd004a_get_slave_descr(void) +{ + return &hscdtd004a_descr; +} +EXPORT_SYMBOL(hscdtd004a_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/lsm303m.c b/drivers/misc/mpu3050/compass/lsm303m.c new file mode 100755 index 00000000000..871d002fd36 --- /dev/null +++ b/drivers/misc/mpu3050/compass/lsm303m.c @@ -0,0 +1,244 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @brief Provides the interface to setup and handle a compass + * connected to the primary I2C interface of the gyroscope. + * + * @{ + * @file lsm303m.c + * @brief Magnetometer setup and handling methods for ST LSM303. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/*----- ST LSM303 Registers ------*/ +enum LSM_REG { + LSM_REG_CONF_A = 0x0, + LSM_REG_CONF_B = 0x1, + LSM_REG_MODE = 0x2, + LSM_REG_X_M = 0x3, + LSM_REG_X_L = 0x4, + LSM_REG_Z_M = 0x5, + LSM_REG_Z_L = 0x6, + LSM_REG_Y_M = 0x7, + LSM_REG_Y_L = 0x8, + LSM_REG_STATUS = 0x9, + LSM_REG_ID_A = 0xA, + LSM_REG_ID_B = 0xB, + LSM_REG_ID_C = 0xC +}; + +enum LSM_CONF_A { + LSM_CONF_A_DRATE_MASK = 0x1C, + LSM_CONF_A_DRATE_0_75 = 0x00, + LSM_CONF_A_DRATE_1_5 = 0x04, + LSM_CONF_A_DRATE_3 = 0x08, + LSM_CONF_A_DRATE_7_5 = 0x0C, + LSM_CONF_A_DRATE_15 = 0x10, + LSM_CONF_A_DRATE_30 = 0x14, + LSM_CONF_A_DRATE_75 = 0x18, + LSM_CONF_A_MEAS_MASK = 0x3, + LSM_CONF_A_MEAS_NORM = 0x0, + LSM_CONF_A_MEAS_POS = 0x1, + LSM_CONF_A_MEAS_NEG = 0x2 +}; + +enum LSM_CONF_B { + LSM_CONF_B_GAIN_MASK = 0xE0, + LSM_CONF_B_GAIN_0_9 = 0x00, + LSM_CONF_B_GAIN_1_2 = 0x20, + LSM_CONF_B_GAIN_1_9 = 0x40, + LSM_CONF_B_GAIN_2_5 = 0x60, + LSM_CONF_B_GAIN_4_0 = 0x80, + LSM_CONF_B_GAIN_4_6 = 0xA0, + LSM_CONF_B_GAIN_5_5 = 0xC0, + LSM_CONF_B_GAIN_7_9 = 0xE0 +}; + +enum LSM_MODE { + LSM_MODE_MASK = 0x3, + LSM_MODE_CONT = 0x0, + LSM_MODE_SINGLE = 0x1, + LSM_MODE_IDLE = 0x2, + LSM_MODE_SLEEP = 0x3 +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int lsm303dlhm_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SLEEP); + ERROR_CHECK(result); + MLOSSleep(3); + + return result; +} + +int lsm303dlhm_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + /* Use single measurement mode. Start at sleep state. */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SLEEP); + ERROR_CHECK(result); + /* Config normal measurement */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_CONF_A, 0); + ERROR_CHECK(result); + /* Adjust gain to 320 LSB/Gauss */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5); + ERROR_CHECK(result); + + return result; +} + +int lsm303dlhm_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + unsigned char stat; + tMLError result = ML_SUCCESS; + short axisFixed; + + /* Read status reg. to check if data is ready */ + result = + MLSLSerialRead(mlsl_handle, pdata->address, LSM_REG_STATUS, 1, + &stat); + ERROR_CHECK(result); + if (stat & 0x01) { + result = + MLSLSerialRead(mlsl_handle, pdata->address, + LSM_REG_X_M, 6, (unsigned char *) data); + ERROR_CHECK(result); + + /*drop data if overflows */ + if ((data[0] == 0xf0) || (data[2] == 0xf0) + || (data[4] == 0xf0)) { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + LSM_REG_MODE, + LSM_MODE_SINGLE); + ERROR_CHECK(result); + return ML_ERROR_COMPASS_DATA_OVERFLOW; + } + /* convert to fixed point and apply sensitivity correction for + Z-axis */ + axisFixed = + (short) ((unsigned short) data[5] + + (unsigned short) data[4] * 256); + /* scale up by 1.125 (36/32) approximate of 1.122 (320/285) */ + axisFixed = (short) (axisFixed * 36); + data[4] = axisFixed >> 8; + data[5] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[3] + + (unsigned short) data[2] * 256); + axisFixed = (short) (axisFixed * 32); + data[2] = axisFixed >> 8; + data[3] = axisFixed & 0xFF; + + axisFixed = + (short) ((unsigned short) data[1] + + (unsigned short) data[0] * 256); + axisFixed = (short) (axisFixed * 32); + data[0] = axisFixed >> 8; + data[1] = axisFixed & 0xFF; + + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_SUCCESS; + } else { + /* trigger next measurement read */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + LSM_REG_MODE, LSM_MODE_SINGLE); + ERROR_CHECK(result); + + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +struct ext_slave_descr lsm303dlhm_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ lsm303dlhm_suspend, + /*.resume = */ lsm303dlhm_resume, + /*.read = */ lsm303dlhm_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "lsm303dlhm", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_LSM303, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {10240, 0}, +}; + +struct ext_slave_descr *lsm303dlhm_get_slave_descr(void) +{ + return &lsm303dlhm_descr; +} +EXPORT_SYMBOL(lsm303dlhm_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mmc314x.c b/drivers/misc/mpu3050/compass/mmc314x.c new file mode 100755 index 00000000000..010d7a78d14 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mmc314x.c @@ -0,0 +1,184 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file mmc314x.c + * @brief Magnetometer setup and handling methods for ???? compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +static int reset_int = 1000; +static int read_count = 1; +static char reset_mode; /* in Z-init section */ + +#define MMC314X_REG_ST (0x00) +#define MMC314X_REG_X_MSB (0x01) + +#define MMC314X_CNTL_MODE_WAKE_UP (0x01) +#define MMC314X_CNTL_MODE_SET (0x02) +#define MMC314X_CNTL_MODE_RESET (0x04) + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int mmc314x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + return result; +} + +int mmc314x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + + int result; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, MMC314X_CNTL_MODE_RESET); + ERROR_CHECK(result); + MLOSSleep(10); + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, MMC314X_CNTL_MODE_SET); + ERROR_CHECK(result); + MLOSSleep(10); + read_count = 1; + return ML_SUCCESS; +} + +int mmc314x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result, ii; + short tmp[3]; + unsigned char tmpdata[6]; + + + if (read_count > 1000) + read_count = 1; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, MMC314X_REG_X_MSB, + 6, (unsigned char *) data); + ERROR_CHECK(result); + + for (ii = 0; ii < 6; ii++) + tmpdata[ii] = data[ii]; + + for (ii = 0; ii < 3; ii++) { + tmp[ii] = + (short) ((tmpdata[2 * ii] << 8) + tmpdata[2 * ii + 1]); + tmp[ii] = tmp[ii] - 4096; + tmp[ii] = tmp[ii] * 16; + } + + for (ii = 0; ii < 3; ii++) { + data[2 * ii] = (unsigned char) (tmp[ii] >> 8); + data[2 * ii + 1] = (unsigned char) (tmp[ii]); + } + + if (read_count % reset_int == 0) { + if (reset_mode) { + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_RESET); + ERROR_CHECK(result); + reset_mode = 0; + return ML_ERROR_COMPASS_DATA_NOT_READY; + } else { + result = + MLSLSerialWriteSingle(mlsl_handle, + pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_SET); + ERROR_CHECK(result); + reset_mode = 1; + read_count++; + return ML_ERROR_COMPASS_DATA_NOT_READY; + } + } + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + MMC314X_REG_ST, + MMC314X_CNTL_MODE_WAKE_UP); + ERROR_CHECK(result); + read_count++; + + return ML_SUCCESS; +} + +struct ext_slave_descr mmc314x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mmc314x_suspend, + /*.resume = */ mmc314x_resume, + /*.read = */ mmc314x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mmc314x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_MMC314X, + /*.reg = */ 0x01, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {400, 0}, +}; + +struct ext_slave_descr *mmc314x_get_slave_descr(void) +{ + return &mmc314x_descr; +} +EXPORT_SYMBOL(mmc314x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mmc328x.c b/drivers/misc/mpu3050/compass/mmc328x.c new file mode 100755 index 00000000000..d2914f3ef62 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mmc328x.c @@ -0,0 +1,220 @@ +/* + * Copyright (C) 2010 MEMSIC, Inc. + * + * 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, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include <linux/i2c.h> + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> + +#define MMC328X_I2C_ADDR 0x30 + +#define MMC328X_REG_CTRL 0x07 +#define MMC328X_REG_DATA 0x00 +#define MMC328X_REG_DS 0x06 + +#define MMC328X_CTRL_TM 0x01 +#define MMC328X_CTRL_RM 0x20 + +#define MMC328X_DELAY_TM 10 /* ms */ +#define MMC328X_DELAY_RM 10 /* ms */ +#define MMC328X_DELAY_STDN 1 /* ms */ + +static int mmc328x_i2c_rx_data(void *mlsl_handle, + struct ext_slave_platform_data *pdata, char *buf, int len) +{ + uint8_t i; + struct i2c_msg msgs[2] = { 0, }; + + if (NULL == buf || NULL == mlsl_handle) + return -EINVAL; + + msgs[0].addr = pdata->address; + msgs[0].flags = 0; + msgs[0].len = 1; + msgs[0].buf = buf; + + msgs[1].addr = pdata->address; + msgs[1].flags = I2C_M_RD; + msgs[1].len = len; + msgs[1].buf = buf; + + if (i2c_transfer(mlsl_handle, msgs, 2) >= 0) + return -1; + + return 0; +} + +static int mmc328x_i2c_tx_data(void *mlsl_handle, + struct ext_slave_platform_data *pdata, char *buf, int len) +{ + struct i2c_msg msgs[1]; + int res; + + if (NULL == buf || NULL == mlsl_handle) + return -EINVAL; + + msgs[0].addr = pdata->address; + msgs[0].flags = 0; /* write */ + msgs[0].buf = (unsigned char *)buf; + msgs[0].len = len; + + res = i2c_transfer(mlsl_handle, msgs, 1); + if (res < 1) + return res; + else + return 0; +} + +int memsic_api_resume(void *mlsl_handle, + struct ext_slave_platform_data *pdata) +{ +#if 0 + unsigned char data[2] = { 0 }; + + data[0] = MMC328X_REG_CTRL; + data[1] = MMC328X_CTRL_RM; + if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0) + return -1; + + msleep(MMC328X_DELAY_RM); +#endif + return 0; +} + +int memsic_api_suspend(void *mlsl_handle, + struct ext_slave_platform_data *pdata) +{ +#if 0 + unsigned char data[2] = { 0 }; + + data[0] = MMC328X_REG_CTRL; + data[1] = 0; + if (mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2) < 0) + return -1; + msleep(MMC328X_DELAY_RM); + + data[0] = MMC328X_REG_CTRL; + data[1] = 0; + mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2); + msleep(MMC328X_DELAY_TM); +#endif + return 0; +} + +int memsic_api_read(void *mlsl_handle, struct ext_slave_platform_data *pdata, + unsigned char *raw_data, int *accuracy) +{ + unsigned char data[6] = { 0 }; + int MD_times = 0; + + data[0] = MMC328X_REG_CTRL; + data[1] = MMC328X_CTRL_TM; + mmc328x_i2c_tx_data(mlsl_handle, pdata, data, 2); + msleep(MMC328X_DELAY_TM); + + data[0] = MMC328X_REG_DS; + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0) + return -EFAULT; + + while (!(data[0] & 0x01)) { + msleep(20); + data[0] = MMC328X_REG_DS; + + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 1) < 0) + return -EFAULT; + + if (data[0] & 0x01) + break; + + MD_times++; + + if (MD_times > 2) + return -EFAULT; + } + + data[0] = MMC328X_REG_DATA; + if (mmc328x_i2c_rx_data(mlsl_handle, pdata, data, 6) < 0) + return -EFAULT; + + memcpy(raw_data, data, sizeof(unsigned char) * 6); + + *accuracy = 0; + + return 0; +} + +int mmc328x_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + int xyz[3] = { 0, }; + int accuracy = 0; + + memsic_api_read(mlsl_handle, pdata, data, &accuracy); + + return ML_SUCCESS; +} + +int mmc328x_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + memsic_api_suspend(mlsl_handle, pdata); + return result; +} + +int mmc328x_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + + memsic_api_resume(mlsl_handle, pdata); + return ML_SUCCESS; +} + +struct ext_slave_descr mmc328x_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ mmc328x_suspend, + /*.resume = */ mmc328x_resume, + /*.read = */ mmc328x_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "mmc328x", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_MMC328X, + /*.reg = */ 0x01, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {400, 0}, +}; + +struct ext_slave_descr *mmc328x_get_slave_descr(void) +{ + return &mmc328x_descr; +} +EXPORT_SYMBOL(mmc328x_get_slave_descr); + +/** + * @} +**/ diff --git a/drivers/misc/mpu3050/compass/mpuak8975.c b/drivers/misc/mpu3050/compass/mpuak8975.c new file mode 100755 index 00000000000..991de77dbd0 --- /dev/null +++ b/drivers/misc/mpu3050/compass/mpuak8975.c @@ -0,0 +1,188 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup COMPASSDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file AK8975.c + * @brief Magnetometer setup and handling methods for AKM 8975 compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include <string.h> + +#ifdef __KERNEL__ +#include <linux/module.h> +#endif + +#include "mpu.h" +#include "mlsl.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + + +#define AK8975_REG_ST1 (0x02) +#define AK8975_REG_HXL (0x03) +#define AK8975_REG_ST2 (0x09) + +#define AK8975_REG_CNTL (0x0A) + +#define AK8975_CNTL_MODE_POWER_DOWN (0x00) +#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01) + +int ak8975_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_POWER_DOWN); + MLOSSleep(1); /* wait at least 100us */ + ERROR_CHECK(result); + return result; +} + +int ak8975_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_SINGLE_MEASUREMENT); + ERROR_CHECK(result); + return result; +} + +int ak8975_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char regs[8]; + unsigned char *stat = ®s[0]; + unsigned char *stat2 = ®s[7]; + int result = ML_SUCCESS; + int status = ML_SUCCESS; + + result = + MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1, + 8, regs); + ERROR_CHECK(result); + + /* + * ST : data ready - + * Measurement has been completed and data is ready to be read. + */ + if (*stat & 0x01) { + memcpy(data, ®s[1], 6); + status = ML_SUCCESS; + } + + /* + * ST2 : data error - + * occurs when data read is started outside of a readable period; + * data read would not be correct. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour but we + * stil account for it and return an error, since the data would be + * corrupted. + * DERR bit is self-clearing when ST2 register is read. + */ + if (*stat2 & 0x04) + status = ML_ERROR_COMPASS_DATA_ERROR; + /* + * ST2 : overflow - + * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. + * This is likely to happen in presence of an external magnetic + * disturbance; it indicates, the sensor data is incorrect and should + * be ignored. + * An error is returned. + * HOFL bit clears when a new measurement starts. + */ + if (*stat2 & 0x08) + status = ML_ERROR_COMPASS_DATA_OVERFLOW; + /* + * ST : overrun - + * the previous sample was not fetched and lost. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour and we + * don't consider this condition an error. + * DOR bit is self-clearing when ST2 or any meas. data register is + * read. + */ + if (*stat & 0x02) { + /* status = ML_ERROR_COMPASS_DATA_UNDERFLOW; */ + status = ML_SUCCESS; + } + + /* + * trigger next measurement if: + * - stat is non zero; + * - if stat is zero and stat2 is non zero. + * Won't trigger if data is not ready and there was no error. + */ + if (*stat != 0x00 || *stat2 != 0x00) { + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->address, + AK8975_REG_CNTL, + AK8975_CNTL_MODE_SINGLE_MEASUREMENT); + ERROR_CHECK(result); + } + + return status; +} + +struct ext_slave_descr ak8975_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ ak8975_suspend, + /*.resume = */ ak8975_resume, + /*.read = */ ak8975_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "ak8975", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_AKM, + /*.reg = */ 0x01, + /*.len = */ 9, + /*.endian = */ EXT_SLAVE_LITTLE_ENDIAN, + /*.range = */ {9830, 4000} +}; + +struct ext_slave_descr *ak8975_get_slave_descr(void) +{ + return &ak8975_descr; +} +EXPORT_SYMBOL(ak8975_get_slave_descr); + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/compass/yas529-kernel.c b/drivers/misc/mpu3050/compass/yas529-kernel.c new file mode 100755 index 00000000000..239ab669cc1 --- /dev/null +++ b/drivers/misc/mpu3050/compass/yas529-kernel.c @@ -0,0 +1,477 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +/** + * @defgroup ACCELDL (Motion Library - Accelerometer Driver Layer) + * @brief Provides the interface to setup and handle an accelerometers + * connected to the secondary I2C interface of the gyroscope. + * + * @{ + * @file yas529.c + * @brief Magnetometer setup and handling methods for Yamaha yas529 + * compass. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#ifdef __KERNEL__ +#include <linux/module.h> +#include <linux/i2c.h> +#endif + +#include "mpu.h" +#include "mlos.h" + +#include <log.h> +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-acc" + +/*----- YAMAHA YAS529 Registers ------*/ +enum YAS_REG { + YAS_REG_CMDR = 0x00, /* 000 < 5 */ + YAS_REG_XOFFSETR = 0x20, /* 001 < 5 */ + YAS_REG_Y1OFFSETR = 0x40, /* 010 < 5 */ + YAS_REG_Y2OFFSETR = 0x60, /* 011 < 5 */ + YAS_REG_ICOILR = 0x80, /* 100 < 5 */ + YAS_REG_CAL = 0xA0, /* 101 < 5 */ + YAS_REG_CONFR = 0xC0, /* 110 < 5 */ + YAS_REG_DOUTR = 0xE0 /* 111 < 5 */ +}; + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +static long a1; +static long a2; +static long a3; +static long a4; +static long a5; +static long a6; +static long a7; +static long a8; +static long a9; + +/***************************************** + Yamaha I2C access functions +*****************************************/ + +static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned int len, unsigned char *data) +{ + struct i2c_msg msgs[1]; + int res; + + if (NULL == data || NULL == i2c_adap) + 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) + return res; + else + return 0; +} + +static int yas529_sensor_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 (NULL == data || NULL == i2c_adap) + return -EINVAL; + + msgs[0].addr = address; + msgs[0].flags = I2C_M_RD; + msgs[0].buf = data; + msgs[0].len = len; + + res = i2c_transfer(i2c_adap, msgs, 1); + if (res < 1) + return res; + else + return 0; +} + +/***************************************** + Accelerometer Initialization Functions +*****************************************/ + +int yas529_suspend(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + return result; +} + +int yas529_resume(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result = ML_SUCCESS; + + unsigned char dummyData[1] = { 0 }; + unsigned char dummyRegister = 0; + unsigned char rawData[6]; + unsigned char calData[9]; + + short xoffset, y1offset, y2offset; + short d2, d3, d4, d5, d6, d7, d8, d9; + + /* YAS529 Application Manual MS-3C - Section 4.4.5 */ + /* =============================================== */ + /* Step 1 - register initialization */ + /* zero initialization coil register - "100 00 000" */ + dummyData[0] = YAS_REG_ICOILR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + /* zero config register - "110 00 000" */ + dummyData[0] = YAS_REG_CONFR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + + /* Step 2 - initialization coil operation */ + dummyData[0] = YAS_REG_ICOILR | 0x11; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x01; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x12; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x02; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x13; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x03; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x14; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x04; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x15; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x05; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x16; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x06; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x17; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x07; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x10; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_ICOILR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + + /* Step 3 - rough offset measurement */ + /* Config register - Measurements results - "110 00 000" */ + dummyData[0] = YAS_REG_CONFR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + /* Measurements command register - Rough offset measurement - + "000 00001" */ + dummyData[0] = YAS_REG_CMDR | 0x01; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + MLOSSleep(2); /* wait at least 1.5ms */ + + /* Measurement data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 6, rawData); + ERROR_CHECK(result); + xoffset = + (short) ((unsigned short) rawData[5] + + ((unsigned short) rawData[4] & 0x7) * 256) - 5; + if (xoffset < 0) + xoffset = 0; + y1offset = + (short) ((unsigned short) rawData[3] + + ((unsigned short) rawData[2] & 0x7) * 256) - 5; + if (y1offset < 0) + y1offset = 0; + y2offset = + (short) ((unsigned short) rawData[1] + + ((unsigned short) rawData[0] & 0x7) * 256) - 5; + if (y2offset < 0) + y2offset = 0; + + /* Step 4 - rough offset setting */ + /* Set rough offset register values */ + dummyData[0] = YAS_REG_XOFFSETR | xoffset; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_Y1OFFSETR | y1offset; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + dummyData[0] = YAS_REG_Y2OFFSETR | y2offset; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + + /* CAL matrix read (first read is invalid) */ + /* Config register - CAL register read - "110 01 000" */ + dummyData[0] = YAS_REG_CONFR | 0x08; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + /* CAL data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 9, calData); + ERROR_CHECK(result); + /* Config register - CAL register read - "110 01 000" */ + dummyData[0] = YAS_REG_CONFR | 0x08; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + /* CAL data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 9, calData); + ERROR_CHECK(result); + + /* Calculate coefficients of the sensitivity corrcetion matrix */ +#if 1 /* production sensor */ + a1 = 100; + d2 = (calData[0] & 0xFC) >> 2; /* [71..66] 6bit */ + a2 = (short) (d2 - 32); + /* [65..62] 4bit */ + d3 = ((calData[0] & 0x03) << 2) | ((calData[1] & 0xC0) >> 6); + a3 = (short) (d3 - 8); + d4 = (calData[1] & 0x3F); /* [61..56] 6bit */ + a4 = (short) (d4 - 32); + d5 = (calData[2] & 0xFC) >> 2; /* [55..50] 6bit */ + a5 = (short) (d5 - 32) + 70; + /* [49..44] 6bit */ + d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4); + a6 = (short) (d6 - 32); + /* [43..38] 6bit */ + d7 = ((calData[3] & 0x0F) << 2) | ((calData[4] & 0xC0) >> 6); + a7 = (short) (d7 - 32); + d8 = (calData[4] & 0x3F); /* [37..32] 6bit */ + a8 = (short) (d8 - 32); + d9 = (calData[5] & 0xFE) >> 1; /* [31..25] 7bit */ + a9 = (short) (d9 - 64) + 130; +#else /* evaluation sensor */ + a1 = 1.0f; + /* [71..66] 6bit */ + d2 = (calData[0] & 0xFC) >> 2; + a2 = (short) d2; + /* [65..60] 6bit */ + d3 = ((calData[0] & 0x03) << 4) | ((calData[1] & 0xF0) >> 4); + a3 = (short) d3; + /* [59..54] 6bit */ + d4 = ((calData[1] & 0x0F) << 2) | ((calData[2] & 0xC0) >> 6); + a4 = (short) d4; + /* [53..48] 6bit */ + d5 = (calData[2] & 0x3F); + a5 = (short) (d5 + 70); + /* [47..42] 6bit */ + d6 = ((calData[3] & 0xFC) >> 2); + a6 = (short) d6; + /* [41..36] 6bit */ + d7 = ((calData[3] & 0x03) << 4) | ((calData[4] & 0xF0) >> 4); + a7 = (short) d7; + /* [35..30] 6bit */ + d8 = ((calData[4] & 0x0F) << 2) | ((calData[5] & 0xC0) >> 6); + a8 = (short) d8; + /* [29..24] 6bit */ + d9 = (calData[5] & 0x3F); + a9 = (short) (d9 + 150); +#endif + + return result; +} + +int yas529_read(void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, unsigned char *data) +{ + unsigned char stat; + unsigned char rawData[6]; + unsigned char dummyData[1] = { 0 }; + unsigned char dummyRegister = 0; + tMLError result = ML_SUCCESS; + short SX, SY1, SY2, SY, SZ; + short row1fixed, row2fixed, row3fixed; + + /* Config register - Measurements results - "110 00 000" */ + dummyData[0] = YAS_REG_CONFR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + /* Measurements command register - Normal magnetic field measurement - + "000 00000" */ + dummyData[0] = YAS_REG_CMDR | 0x00; + result = + yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, + dummyData); + ERROR_CHECK(result); + MLOSSleep(10); + /* Measurement data read */ + result = + yas529_sensor_i2c_read(mlsl_handle, pdata->address, + dummyRegister, 6, + (unsigned char *) &rawData); + ERROR_CHECK(result); + + stat = rawData[0] & 0x80; + if (stat == 0x00) { + /* Extract raw data */ + SX = (short) ((unsigned short) rawData[5] + + ((unsigned short) rawData[4] & 0x7) * 256); + SY1 = + (short) ((unsigned short) rawData[3] + + ((unsigned short) rawData[2] & 0x7) * 256); + SY2 = + (short) ((unsigned short) rawData[1] + + ((unsigned short) rawData[0] & 0x7) * 256); + if ((SX <= 1) || (SY1 <= 1) || (SY2 <= 1)) + return ML_ERROR_COMPASS_DATA_UNDERFLOW; + if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024)) + return ML_ERROR_COMPASS_DATA_OVERFLOW; + /* Convert to XYZ axis */ + SX = -1 * SX; + SY = SY2 - SY1; + SZ = SY1 + SY2; + + /* Apply sensitivity correction matrix */ + row1fixed = + (short) ((a1 * SX + a2 * SY + a3 * SZ) >> 7) * 41; + row2fixed = + (short) ((a4 * SX + a5 * SY + a6 * SZ) >> 7) * 41; + row3fixed = + (short) ((a7 * SX + a8 * SY + a9 * SZ) >> 7) * 41; + + data[0] = row1fixed >> 8; + data[1] = row1fixed & 0xFF; + data[2] = row2fixed >> 8; + data[3] = row2fixed & 0xFF; + data[4] = row3fixed >> 8; + data[5] = row3fixed & 0xFF; + + return ML_SUCCESS; + } else { + return ML_ERROR_COMPASS_DATA_NOT_READY; + } +} + +struct ext_slave_descr yas529_descr = { + /*.init = */ NULL, + /*.exit = */ NULL, + /*.suspend = */ yas529_suspend, + /*.resume = */ yas529_resume, + /*.read = */ yas529_read, + /*.config = */ NULL, + /*.get_config = */ NULL, + /*.name = */ "yas529", + /*.type = */ EXT_SLAVE_TYPE_COMPASS, + /*.id = */ COMPASS_ID_YAS529, + /*.reg = */ 0x06, + /*.len = */ 6, + /*.endian = */ EXT_SLAVE_BIG_ENDIAN, + /*.range = */ {19660, 8000}, +}; + +struct ext_slave_descr *yas529_get_slave_descr(void) +{ + return &yas529_descr; +} +EXPORT_SYMBOL(yas529_get_slave_descr); + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/log.h b/drivers/misc/mpu3050/log.h new file mode 100755 index 00000000000..197b7745fa0 --- /dev/null +++ b/drivers/misc/mpu3050/log.h @@ -0,0 +1,290 @@ +/* + Copyright (C) 1995-97 Simon G. Vogl + Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl> + Copyright (C) 2003 Greg Kroah-Hartman <greg@kroah.com> + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. +*/ + +/* + * C/C++ logging functions. See the logging documentation for API details. + * + * We'd like these to be available from C code (in case we import some from + * somewhere), so this has a C interface. + * + * The output will be correct when the log file is shared between multiple + * threads and/or multiple processes so long as the operating system + * supports O_APPEND. These calls have mutex-protected data structures + * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. + */ +#ifndef _LIBS_CUTILS_MPL_LOG_H +#define _LIBS_CUTILS_MPL_LOG_H + +#include <stdarg.h> + +#ifdef ANDROID +#include <utils/Log.h> /* For the LOG macro */ +#endif + +#ifdef __KERNEL__ +#include <linux/kernel.h> +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. + * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" + * at the top of your source file) to change that behavior. + */ +#ifndef MPL_LOG_NDEBUG +#ifdef NDEBUG +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif +#endif + +#ifdef __KERNEL__ +#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE +#define MPL_LOG_DEFAULT KERN_DEFAULT +#define MPL_LOG_VERBOSE KERN_CONT +#define MPL_LOG_DEBUG KERN_NOTICE +#define MPL_LOG_INFO KERN_INFO +#define MPL_LOG_WARN KERN_WARNING +#define MPL_LOG_ERROR KERN_ERR +#define MPL_LOG_SILENT MPL_LOG_VERBOSE + +#else + /* Based off the log priorities in android + /system/core/include/android/log.h */ +#define MPL_LOG_UNKNOWN (0) +#define MPL_LOG_DEFAULT (1) +#define MPL_LOG_VERBOSE (2) +#define MPL_LOG_DEBUG (3) +#define MPL_LOG_INFO (4) +#define MPL_LOG_WARN (5) +#define MPL_LOG_ERROR (6) +#define MPL_LOG_SILENT (8) +#endif + + +/* + * This is the local tag used for the following simplified + * logging macros. You can change this preprocessor definition + * before using the other macros to change the tag. + */ +#ifndef MPL_LOG_TAG +#ifdef __KERNEL__ +#define MPL_LOG_TAG +#else +#define MPL_LOG_TAG NULL +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGV +#if MPL_LOG_NDEBUG +#define MPL_LOGV(...) ((void)0) +#else +#define MPL_LOGV(...) ((void)MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, __VA_ARGS__)) +#endif +#endif + +#ifndef CONDITION +#define CONDITION(cond) ((cond) != 0) +#endif + +#ifndef MPL_LOGV_IF +#if MPL_LOG_NDEBUG +#define MPL_LOGV_IF(cond, ...) ((void)0) +#else +#define MPL_LOGV_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) +#endif +#endif + +/* + * Simplified macro to send a debug log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGD +#define MPL_LOGD(...) ((void)MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, __VA_ARGS__)) +#endif + +#ifndef MPL_LOGD_IF +#define MPL_LOGD_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) +#endif + +/* + * Simplified macro to send an info log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGI +#define MPL_LOGI(...) ((void)MPL_LOG(LOG_INFO, MPL_LOG_TAG, __VA_ARGS__)) +#endif + +#ifndef MPL_LOGI_IF +#define MPL_LOGI_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)MPL_LOG(LOG_INFO, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) +#endif + +/* + * Simplified macro to send a warning log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGW +#define MPL_LOGW(...) ((void)MPL_LOG(LOG_WARN, MPL_LOG_TAG, __VA_ARGS__)) +#endif + +#ifndef MPL_LOGW_IF +#define MPL_LOGW_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)MPL_LOG(LOG_WARN, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) +#endif + +/* + * Simplified macro to send an error log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGE +#define MPL_LOGE(...) ((void)MPL_LOG(LOG_ERROR, MPL_LOG_TAG, __VA_ARGS__)) +#endif + +#ifndef MPL_LOGE_IF +#define MPL_LOGE_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)MPL_LOG(LOG_ERROR, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Log a fatal error. If the given condition fails, this stops program + * execution like a normal assertion, but also generating the given message. + * It is NOT stripped from release builds. Note that the condition test + * is -inverted- from the normal assert() semantics. + */ +#define MPL_LOG_ALWAYS_FATAL_IF(cond, ...) \ + ((CONDITION(cond)) \ + ? ((void)android_printAssert(#cond, MPL_LOG_TAG, __VA_ARGS__)) \ + : (void)0) + +#define MPL_LOG_ALWAYS_FATAL(...) \ + (((void)android_printAssert(NULL, MPL_LOG_TAG, __VA_ARGS__))) + +/* + * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that + * are stripped out of release builds. + */ +#if MPL_LOG_NDEBUG + +#define MPL_LOG_FATAL_IF(cond, ...) ((void)0) +#define MPL_LOG_FATAL(...) ((void)0) + +#else + +#define MPL_LOG_FATAL_IF(cond, ...) MPL_LOG_ALWAYS_FATAL_IF(cond, __VA_ARGS__) +#define MPL_LOG_FATAL(...) MPL_LOG_ALWAYS_FATAL(__VA_ARGS__) + +#endif + +/* + * Assertion that generates a log message when the assertion fails. + * Stripped out of release builds. Uses the current MPL_LOG_TAG. + */ +#define MPL_LOG_ASSERT(cond, ...) MPL_LOG_FATAL_IF(!(cond), __VA_ARGS__) + +/* --------------------------------------------------------------------- */ + +/* + * Basic log message macro. + * + * Example: + * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); + * + * The second argument may be NULL or "" to indicate the "global" tag. + */ +#ifndef MPL_LOG +#define MPL_LOG(priority, tag, ...) \ + MPL_LOG_PRI(priority, tag, __VA_ARGS__) +#endif + +/* + * Log macro that allows you to specify a number for the priority. + */ +#ifndef MPL_LOG_PRI +#ifdef ANDROID +#define MPL_LOG_PRI(priority, tag, ...) \ + LOG(priority, tag, __VA_ARGS__) +#elif defined __KERNEL__ +#define MPL_LOG_PRI(priority, tag, ...) \ + printk(MPL_##priority tag __VA_ARGS__) +#else +#define MPL_LOG_PRI(priority, tag, ...) \ + _MLPrintLog(MPL_##priority, tag, __VA_ARGS__) +#endif +#endif + +/* + * Log macro that allows you to pass in a varargs ("args" is a va_list). + */ +#ifndef MPL_LOG_PRI_VA +#ifdef ANDROID +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + android_vprintLog(priority, NULL, tag, fmt, args) +#elif defined __KERNEL__ +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + vprintk(MPL_##priority tag fmt, args) +#else +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + _MLPrintVaLog(priority, NULL, tag, fmt, args) +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * =========================================================================== + * + * The stuff in the rest of this file should not be used directly. + */ + +#ifndef ANDROID + int _MLPrintLog(int priority, const char *tag, const char *fmt, + ...); + int _MLPrintVaLog(int priority, const char *tag, const char *fmt, + va_list args); +/* Final implementation of actual writing to a character device */ + int _MLWriteLog(const char *buf, int buflen); +#endif + +#ifdef __cplusplus +} +#endif +#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c new file mode 100755 index 00000000000..6ce2c98d05f --- /dev/null +++ b/drivers/misc/mpu3050/mldl_cfg.c @@ -0,0 +1,1742 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @addtogroup MLDL + * + * @{ + * @file mldl_cfg.c + * @brief The Motion Library Driver Layer. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include <stddef.h> + +#include "mldl_cfg.h" +#include "mpu.h" + +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#include "mpu-accel.h" + +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "mldl_cfg:" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ +#ifdef M_HW +#define SLEEP 0 +#define WAKE_UP 7 +#define RESET 1 +#define STANDBY 1 +#else +/* licteral significance of all parameters used in MLDLPowerMgmtMPU */ +#define SLEEP 1 +#define WAKE_UP 0 +#define RESET 1 +#define STANDBY 1 +#endif + +/*---------------------*/ +/*- Prototypes. -*/ +/*---------------------*/ + +/*----------------------*/ +/*- Static Functions. -*/ +/*----------------------*/ + +static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle) +{ + unsigned char userCtrlReg; + int result; + + if (!mldl_cfg->dmp_is_running) + return ML_SUCCESS; + + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, 1, &userCtrlReg); + ERROR_CHECK(result); + userCtrlReg = (userCtrlReg & (~BIT_FIFO_EN)) | BIT_FIFO_RST; + userCtrlReg = (userCtrlReg & (~BIT_DMP_EN)) | BIT_DMP_RST; + + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, userCtrlReg); + ERROR_CHECK(result); + mldl_cfg->dmp_is_running = 0; + + return result; + +} +/** + * @brief Starts the DMP running + * + * @return ML_SUCCESS or non-zero error code + */ +static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle) +{ + unsigned char userCtrlReg; + int result; + + if (pdata->dmp_is_running == pdata->dmp_enable) + return ML_SUCCESS; + + result = MLSLSerialRead(mlsl_handle, pdata->addr, + MPUREG_USER_CTRL, 1, &userCtrlReg); + ERROR_CHECK(result); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_USER_CTRL, + ((userCtrlReg & (~BIT_FIFO_EN)) + | BIT_FIFO_RST)); + ERROR_CHECK(result); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_USER_CTRL, userCtrlReg); + ERROR_CHECK(result); + + result = MLSLSerialRead(mlsl_handle, pdata->addr, + MPUREG_USER_CTRL, 1, &userCtrlReg); + ERROR_CHECK(result); + + if (pdata->dmp_enable) + userCtrlReg |= BIT_DMP_EN; + else + userCtrlReg &= ~BIT_DMP_EN; + + if (pdata->fifo_enable) + userCtrlReg |= BIT_FIFO_EN; + else + userCtrlReg &= ~BIT_FIFO_EN; + + userCtrlReg |= BIT_DMP_RST; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_USER_CTRL, userCtrlReg); + ERROR_CHECK(result); + pdata->dmp_is_running = pdata->dmp_enable; + + 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 ML_SUCCESS if successful, a non-zero error code otherwise. + */ +static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + unsigned char enable) +{ + unsigned char b; + int result; + + if ((mldl_cfg->gyro_is_bypassed && enable) || + (!mldl_cfg->gyro_is_bypassed && !enable)) + return ML_SUCCESS; + + /*---- get current 'USER_CTRL' into b ----*/ + result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, 1, &b); + ERROR_CHECK(result); + + b &= ~BIT_AUX_IF_EN; + + if (!enable) { + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, + (b | BIT_AUX_IF_EN)); + ERROR_CHECK(result); + } else { + /* Coming out of I2C is tricky due to several erratta. Do not + * modify this algorithm + */ + /* + * 1) wait for the right time and send the command to change + * the aux i2c slave address to an invalid address that will + * get nack'ed + * + * 0x00 is broadcast. 0x7F is unlikely to be used by any aux. + */ + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_AUX_SLV_ADDR, 0x7F); + ERROR_CHECK(result); + /* + * 2) wait enough time for a nack to occur, then go into + * bypass mode: + */ + MLOSSleep(2); + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, (b)); + ERROR_CHECK(result); + /* + * 3) wait for up to one MPU cycle then restore the slave + * address + */ + MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000); + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_AUX_SLV_ADDR, + mldl_cfg->pdata-> + accel.address); + ERROR_CHECK(result); + + /* + * 4) reset the ime interface + */ +#ifdef M_HW + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, + (b | BIT_I2C_MST_RST)); + +#else + result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, + (b | BIT_AUX_IF_RST)); +#endif + ERROR_CHECK(result); + MLOSSleep(2); + } + mldl_cfg->gyro_is_bypassed = enable; + + return result; +} + +struct tsProdRevMap { + unsigned char siliconRev; + unsigned short sensTrim; +}; + +#define NUM_OF_PROD_REVS (DIM(prodRevsMap)) + +/* NOTE : 'npp' is a non production part */ +#ifdef M_HW +#define OLDEST_PROD_REV_SUPPORTED 1 +static struct tsProdRevMap prodRevsMap[] = { + {0, 0}, + {MPU_SILICON_REV_A1, 131}, /* 1 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 2 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 3 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 4 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 5 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 6 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 7 A1 (npp) */ + {MPU_SILICON_REV_A1, 131}, /* 8 A1 (npp) */ +}; + +#else /* !M_HW */ +#define OLDEST_PROD_REV_SUPPORTED 11 + +static struct tsProdRevMap prodRevsMap[] = { + {0, 0}, + {MPU_SILICON_REV_A4, 131}, /* 1 A? OBSOLETED */ + {MPU_SILICON_REV_A4, 131}, /* 2 | */ + {MPU_SILICON_REV_A4, 131}, /* 3 V */ + {MPU_SILICON_REV_A4, 131}, /* 4 */ + {MPU_SILICON_REV_A4, 131}, /* 5 */ + {MPU_SILICON_REV_A4, 131}, /* 6 */ + {MPU_SILICON_REV_A4, 131}, /* 7 */ + {MPU_SILICON_REV_A4, 131}, /* 8 */ + {MPU_SILICON_REV_A4, 131}, /* 9 */ + {MPU_SILICON_REV_A4, 131}, /* 10 */ + {MPU_SILICON_REV_B1, 131}, /* 11 B1 */ + {MPU_SILICON_REV_B1, 131}, /* 12 | */ + {MPU_SILICON_REV_B1, 131}, /* 13 V */ + {MPU_SILICON_REV_B1, 131}, /* 14 B4 */ + {MPU_SILICON_REV_B4, 131}, /* 15 | */ + {MPU_SILICON_REV_B4, 131}, /* 16 V */ + {MPU_SILICON_REV_B4, 131}, /* 17 */ + {MPU_SILICON_REV_B4, 131}, /* 18 */ + {MPU_SILICON_REV_B4, 115}, /* 19 */ + {MPU_SILICON_REV_B4, 115}, /* 20 */ + {MPU_SILICON_REV_B6, 131}, /* 21 B6 (B6/A9) */ + {MPU_SILICON_REV_B4, 115}, /* 22 B4 (B7/A10) */ + {MPU_SILICON_REV_B6, 0}, /* 23 B6 (npp) */ + {MPU_SILICON_REV_B6, 0}, /* 24 | (npp) */ + {MPU_SILICON_REV_B6, 0}, /* 25 V (npp) */ + {MPU_SILICON_REV_B6, 131}, /* 26 (B6/A11) */ +}; +#endif /* !M_HW */ + +/** + * @internal + * @brief Get the silicon revision ID from OTP. + * The silicon revision number is in read from OTP bank 0, + * ADDR6[7:2]. The corresponding ID is retrieved by lookup + * in a map. + * @return The silicon revision ID (0 on error). + */ +static int MLDLGetSiliconRev(struct mldl_cfg *pdata, + void *mlsl_handle) +{ + int result; + unsigned char index = 0x00; + unsigned char bank = + (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0); + unsigned short memAddr = ((bank << 8) | 0x06); + + result = MLSLSerialReadMem(mlsl_handle, pdata->addr, + memAddr, 1, &index); + ERROR_CHECK(result) + if (result) + return result; + index >>= 2; + + /* clean the prefetch and cfg user bank bits */ + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_BANK_SEL, 0); + ERROR_CHECK(result) + if (result) + return result; + + if (index < OLDEST_PROD_REV_SUPPORTED || NUM_OF_PROD_REVS <= index) { + pdata->silicon_revision = 0; + pdata->trim = 0; + MPL_LOGE("Unsupported Product Revision Detected : %d\n", index); + return ML_ERROR_INVALID_MODULE; + } + + pdata->silicon_revision = prodRevsMap[index].siliconRev; + pdata->trim = prodRevsMap[index].sensTrim; + + if (pdata->trim == 0) { + MPL_LOGE("sensitivity trim is 0" + " - unsupported non production part.\n"); + return ML_ERROR_INVALID_MODULE; + } + + return result; +} + +/** + * @brief Enable/Disable the use MPU's VDDIO level shifters. + * When enabled the voltage interface with AUX or other external + * accelerometer is using Vlogic instead of VDD (supply). + * + * @note Must be called after MLSerialOpen(). + * @note Typically be called before MLDmpOpen(). + * If called after MLDmpOpen(), must be followed by a call to + * MLDLApplyLevelShifterBit() to write the setting on the hw. + * + * @param[in] enable + * 1 to enable, 0 to disable + * + * @return ML_SUCCESS if successfull, a non-zero error code otherwise. +**/ +static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata, + void *mlsl_handle, + unsigned char enable) +{ +#ifndef M_HW + int result; + unsigned char reg; + unsigned char mask; + unsigned char regval; + + if (0 == pdata->silicon_revision) + return ML_ERROR_INVALID_PARAMETER; + + /*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR -- + NOTE: this is incompatible with ST accelerometers where the VDDIO + bit MUST be set to enable ST's internal logic to autoincrement + the register address on burst reads --*/ + if ((pdata->silicon_revision & 0xf) < MPU_SILICON_REV_B6) { + reg = MPUREG_ACCEL_BURST_ADDR; + mask = 0x80; + } else { + /*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 => + the mask is always 0x04 --*/ + reg = MPUREG_FIFO_EN2; + mask = 0x04; + } + + result = MLSLSerialRead(mlsl_handle, pdata->addr, reg, 1, ®val); + if (result) + return result; + + if (enable) + regval |= mask; + else + regval &= ~mask; + + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->addr, reg, regval); + + return result; +#else + return ML_SUCCESS; +#endif +} + + +#ifdef M_HW +/** + * @internal + * @param reset 1 to reset hardware + */ +static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata, + void *mlsl_handle, + unsigned char reset, + unsigned char powerselection) +{ + unsigned char b; + tMLError result; + + if (powerselection < 0 || powerselection > 7) + return ML_ERROR_INVALID_PARAMETER; + + result = + MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_1, 1, + &b); + ERROR_CHECK(result); + + b &= ~(BITS_PWRSEL); + + if (reset) { + /* Current sillicon has an errata where the reset will get + * nacked. Ignore the error code for now. */ + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b | BIT_H_RESET); +#define M_HW_RESET_ERRATTA +#ifndef M_HW_RESET_ERRATTA + ERROR_CHECK(result); +#else + MLOSSleep(50); +#endif + } + + b |= (powerselection << 4); + + if (b & BITS_PWRSEL) + pdata->gyro_is_suspended = FALSE; + else + pdata->gyro_is_suspended = TRUE; + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +/** + * @internal + */ +static tMLError MLDLStandByGyros(struct mldl_cfg *pdata, + void *mlsl_handle, + unsigned char disable_gx, + unsigned char disable_gy, + unsigned char disable_gz) +{ + unsigned char b; + tMLError result; + + result = + MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1, + &b); + ERROR_CHECK(result); + + b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG); + b |= (disable_gx << 2 | disable_gy << 1 | disable_gz); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGMT_2, b); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +/** + * @internal + */ +static tMLError MLDLStandByAccels(struct mldl_cfg *pdata, + void *mlsl_handle, + unsigned char disable_ax, + unsigned char disable_ay, + unsigned char disable_az) +{ + unsigned char b; + tMLError result; + + result = + MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1, + &b); + ERROR_CHECK(result); + + b &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); + b |= (disable_ax << 2 | disable_ay << 1 | disable_az); + + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGMT_2, b); + ERROR_CHECK(result); + + return ML_SUCCESS; +} + +#else /* ! M_HW */ + +/** + * @internal + * @brief This function controls the power management on the MPU device. + * The entire chip can be put to low power sleep mode, or individual + * gyros can be turned on/off. + * + * Putting the device into sleep mode depending upon the changing needs + * of the associated applications is a recommended method for reducing + * power consuption. It is a safe opearation in that sleep/wake up of + * gyros while running will not result in any interruption of data. + * + * Although it is entirely allowed to put the device into full sleep + * while running the DMP, it is not recomended because it will disrupt + * the ongoing calculations carried on inside the DMP and consequently + * the sensor fusion algorithm. Furthermore, while in sleep mode + * read & write operation from the app processor on both registers and + * memory are disabled and can only regained by restoring the MPU in + * normal power mode. + * Disabling any of the gyro axis will reduce the associated power + * consuption from the PLL but will not stop the DMP from running + * state. + * + * @param reset + * Non-zero to reset the device. Note that this setting + * is volatile and the corresponding register bit will + * clear itself right after being applied. + * @param sleep + * Non-zero to put device into full sleep. + * @param disable_gx + * Non-zero to disable gyro X. + * @param disable_gy + * Non-zero to disable gyro Y. + * @param disable_gz + * Non-zero to disable gyro Z. + * + * @return ML_SUCCESS if successfull; a non-zero error code otherwise. + */ +static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata, + void *mlsl_handle, + unsigned char reset, + unsigned char sleep, + unsigned char disable_gx, + unsigned char disable_gy, + unsigned char disable_gz) +{ + unsigned char b; + int result; + + result = + MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGM, 1, + &b); + ERROR_CHECK(result); + + /* If we are awake, we need to put it in bypass before resetting */ + if ((!(b & BIT_SLEEP)) && reset) + result = MLDLSetI2CBypass(pdata, mlsl_handle, 1); + + /* If we are awake, we need stop the dmp sleeping */ + if ((!(b & BIT_SLEEP)) && sleep) + dmp_stop(pdata, mlsl_handle); + + /* Reset if requested */ + if (reset) { + MPL_LOGV("Reset MPU3050\n"); + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b | BIT_H_RESET); + ERROR_CHECK(result); + MLOSSleep(5); + pdata->gyro_needs_reset = FALSE; + /* Some chips are awake after reset and some are asleep, + * check the status */ + result = MLSLSerialRead(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, 1, &b); + ERROR_CHECK(result); + } + + /* Update the suspended state just in case we return early */ + if (b & BIT_SLEEP) + pdata->gyro_is_suspended = TRUE; + else + pdata->gyro_is_suspended = FALSE; + + /* if power status match requested, nothing else's left to do */ + if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) == + (((sleep != 0) * BIT_SLEEP) | + ((disable_gx != 0) * BIT_STBY_XG) | + ((disable_gy != 0) * BIT_STBY_YG) | + ((disable_gz != 0) * BIT_STBY_ZG))) { + return ML_SUCCESS; + } + + /* + * This specific transition between states needs to be reinterpreted: + * (1,1,1,1) -> (0,1,1,1) has to become + * (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1) + * where + * (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1) + */ + if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) == + (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) + && ((!sleep) && disable_gx && disable_gy && disable_gz)) { + result = MLDLPowerMgmtMPU(pdata, mlsl_handle, 0, 1, 0, 0, 0); + if (result) + return result; + b |= BIT_SLEEP; + b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG); + } + + if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) { + if (sleep) { + result = MLDLSetI2CBypass(pdata, mlsl_handle, 1); + ERROR_CHECK(result); + b |= BIT_SLEEP; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + pdata->gyro_is_suspended = TRUE; + } else { + b &= ~BIT_SLEEP; + result = + MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + pdata->gyro_is_suspended = FALSE; + MLOSSleep(5); + } + } + /*--- + WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE + 1) put one axis at a time in stand-by + ---*/ + if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) { + b ^= BIT_STBY_XG; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + } + if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) { + b ^= BIT_STBY_YG; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + } + if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) { + b ^= BIT_STBY_ZG; + result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr, + MPUREG_PWR_MGM, b); + ERROR_CHECK(result); + } + + return ML_SUCCESS; +} +#endif /* M_HW */ + + +void mpu_print_cfg(struct mldl_cfg *mldl_cfg) +{ + struct mpu3050_platform_data *pdata = mldl_cfg->pdata; + struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel; + struct ext_slave_platform_data *compass = + &mldl_cfg->pdata->compass; + struct ext_slave_platform_data *pressure = + &mldl_cfg->pdata->pressure; + + MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr); + MPL_LOGD("mldl_cfg.int_config = %02x\n", + mldl_cfg->int_config); + MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync); + MPL_LOGD("mldl_cfg.full_scale = %02x\n", + mldl_cfg->full_scale); + MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf); + MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src); + MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider); + MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", + mldl_cfg->dmp_enable); + MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", + mldl_cfg->fifo_enable); + MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1); + MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2); + MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", + mldl_cfg->offset_tc[0]); + MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", + mldl_cfg->offset_tc[1]); + MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", + mldl_cfg->offset_tc[2]); + MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", + mldl_cfg->silicon_revision); + MPL_LOGD("mldl_cfg.product_id = %02x\n", + mldl_cfg->product_id); + MPL_LOGD("mldl_cfg.trim = %02x\n", mldl_cfg->trim); + MPL_LOGD("mldl_cfg.requested_sensors= %04lx\n", + mldl_cfg->requested_sensors); + + if (mldl_cfg->accel) { + MPL_LOGD("slave_accel->suspend = %02x\n", + (int) mldl_cfg->accel->suspend); + MPL_LOGD("slave_accel->resume = %02x\n", + (int) mldl_cfg->accel->resume); + MPL_LOGD("slave_accel->read = %02x\n", + (int) mldl_cfg->accel->read); + MPL_LOGD("slave_accel->type = %02x\n", + mldl_cfg->accel->type); + MPL_LOGD("slave_accel->reg = %02x\n", + mldl_cfg->accel->reg); + MPL_LOGD("slave_accel->len = %02x\n", + mldl_cfg->accel->len); + MPL_LOGD("slave_accel->endian = %02x\n", + mldl_cfg->accel->endian); + MPL_LOGD("slave_accel->range.mantissa= %02lx\n", + mldl_cfg->accel->range.mantissa); + MPL_LOGD("slave_accel->range.fraction= %02lx\n", + mldl_cfg->accel->range.fraction); + } else { + MPL_LOGD("slave_accel = NULL\n"); + } + + if (mldl_cfg->compass) { + MPL_LOGD("slave_compass->suspend = %02x\n", + (int) mldl_cfg->compass->suspend); + MPL_LOGD("slave_compass->resume = %02x\n", + (int) mldl_cfg->compass->resume); + MPL_LOGD("slave_compass->read = %02x\n", + (int) mldl_cfg->compass->read); + MPL_LOGD("slave_compass->type = %02x\n", + mldl_cfg->compass->type); + MPL_LOGD("slave_compass->reg = %02x\n", + mldl_cfg->compass->reg); + MPL_LOGD("slave_compass->len = %02x\n", + mldl_cfg->compass->len); + MPL_LOGD("slave_compass->endian = %02x\n", + mldl_cfg->compass->endian); + MPL_LOGD("slave_compass->range.mantissa= %02lx\n", + mldl_cfg->compass->range.mantissa); + MPL_LOGD("slave_compass->range.fraction= %02lx\n", + mldl_cfg->compass->range.fraction); + + } else { + MPL_LOGD("slave_compass = NULL\n"); + } + + if (mldl_cfg->pressure) { + MPL_LOGD("slave_pressure->suspend = %02x\n", + (int) mldl_cfg->pressure->suspend); + MPL_LOGD("slave_pressure->resume = %02x\n", + (int) mldl_cfg->pressure->resume); + MPL_LOGD("slave_pressure->read = %02x\n", + (int) mldl_cfg->pressure->read); + MPL_LOGD("slave_pressure->type = %02x\n", + mldl_cfg->pressure->type); + MPL_LOGD("slave_pressure->reg = %02x\n", + mldl_cfg->pressure->reg); + MPL_LOGD("slave_pressure->len = %02x\n", + mldl_cfg->pressure->len); + MPL_LOGD("slave_pressure->endian = %02x\n", + mldl_cfg->pressure->endian); + MPL_LOGD("slave_pressure->range.mantissa= %02lx\n", + mldl_cfg->pressure->range.mantissa); + MPL_LOGD("slave_pressure->range.fraction= %02lx\n", + mldl_cfg->pressure->range.fraction); + + } else { + MPL_LOGD("slave_pressure = NULL\n"); + } + MPL_LOGD("accel->get_slave_descr = %x\n", + (unsigned int) accel->get_slave_descr); + MPL_LOGD("accel->irq = %02x\n", accel->irq); + MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num); + MPL_LOGD("accel->bus = %02x\n", accel->bus); + MPL_LOGD("accel->address = %02x\n", accel->address); + MPL_LOGD("accel->orientation =\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + accel->orientation[0], accel->orientation[1], + accel->orientation[2], accel->orientation[3], + accel->orientation[4], accel->orientation[5], + accel->orientation[6], accel->orientation[7], + accel->orientation[8]); + MPL_LOGD("compass->get_slave_descr = %x\n", + (unsigned int) compass->get_slave_descr); + MPL_LOGD("compass->irq = %02x\n", compass->irq); + MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num); + MPL_LOGD("compass->bus = %02x\n", compass->bus); + MPL_LOGD("compass->address = %02x\n", compass->address); + MPL_LOGD("compass->orientation =\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + compass->orientation[0], compass->orientation[1], + compass->orientation[2], compass->orientation[3], + compass->orientation[4], compass->orientation[5], + compass->orientation[6], compass->orientation[7], + compass->orientation[8]); + MPL_LOGD("pressure->get_slave_descr = %x\n", + (unsigned int) pressure->get_slave_descr); + MPL_LOGD("pressure->irq = %02x\n", pressure->irq); + MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num); + MPL_LOGD("pressure->bus = %02x\n", pressure->bus); + MPL_LOGD("pressure->address = %02x\n", pressure->address); + MPL_LOGD("pressure->orientation =\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + pressure->orientation[0], pressure->orientation[1], + pressure->orientation[2], pressure->orientation[3], + pressure->orientation[4], pressure->orientation[5], + pressure->orientation[6], pressure->orientation[7], + pressure->orientation[8]); + + MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config); + MPL_LOGD("pdata->level_shifter = %02x\n", + pdata->level_shifter); + MPL_LOGD("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]); + + MPL_LOGD("Struct sizes: mldl_cfg: %d, " + "ext_slave_descr:%d, " + "mpu3050_platform_data:%d: RamOffset: %d\n", + sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr), + sizeof(struct mpu3050_platform_data), + offsetof(struct mldl_cfg, ram)); +} + +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 result; + unsigned char reg; + unsigned char slave_reg; + unsigned char slave_len; + unsigned char slave_endian; + unsigned char slave_address; + + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE); + + if (NULL == slave || NULL == slave_pdata) { + slave_reg = 0; + slave_len = 0; + slave_endian = 0; + slave_address = 0; + } else { + slave_reg = slave->reg; + slave_len = slave->len; + slave_endian = slave->endian; + slave_address = slave_pdata->address; + } + + /* Address */ + result = MLSLSerialWriteSingle(gyro_handle, + mldl_cfg->addr, + MPUREG_AUX_SLV_ADDR, + slave_address); + ERROR_CHECK(result); + /* Register */ + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_ACCEL_BURST_ADDR, 1, + ®); + ERROR_CHECK(result); + reg = ((reg & 0x80) | slave_reg); + result = MLSLSerialWriteSingle(gyro_handle, + mldl_cfg->addr, + MPUREG_ACCEL_BURST_ADDR, + reg); + ERROR_CHECK(result); + +#ifdef M_HW + /* 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; + } + reg = slave_len; + if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) + reg |= BIT_SLV_BYTE_SW; + reg |= BIT_SLV_GRP; + reg |= BIT_SLV_ENABLE; + + result = MLSLSerialWriteSingle(gyro_handle, + mldl_cfg->addr, + MPUREG_I2C_SLV0_CTRL, + reg); +#else + /* Length */ + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_USER_CTRL, 1, ®); + ERROR_CHECK(result); + reg = (reg & ~BIT_AUX_RD_LENG); + result = MLSLSerialWriteSingle(gyro_handle, + mldl_cfg->addr, + MPUREG_USER_CTRL, reg); + ERROR_CHECK(result); +#endif + + if (slave_address) { + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, FALSE); + ERROR_CHECK(result); + } + return result; +} + +/** + * Check to see if the gyro was reset by testing a couple of registers known + * to change on reset. + * + * @param mldl_cfg mldl configuration structure + * @param gyro_handle handle used to communicate with the gyro + * + * @return ML_SUCCESS or non-zero error code + */ +static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle) +{ + int result = ML_SUCCESS; + unsigned char reg; + + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_DMP_CFG_2, 1, ®); + ERROR_CHECK(result); + + if (mldl_cfg->dmp_cfg2 != reg) + return TRUE; + + if (0 != mldl_cfg->dmp_cfg1) + return FALSE; + + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_SMPLRT_DIV, 1, ®); + ERROR_CHECK(result); + if (reg != mldl_cfg->divider) + return TRUE; + + if (0 != mldl_cfg->divider) + return FALSE; + + /* Inconclusive assume it was reset */ + return TRUE; +} + +static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle) +{ + int result; + int ii; + int jj; + unsigned char reg; + unsigned char regs[7]; + + /* Wake up the part */ +#ifdef M_HW + result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, RESET, + WAKE_UP); + ERROR_CHECK(result); + + /* Configure the MPU */ + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1); + ERROR_CHECK(result); + /* setting int_config with the propert flag BIT_BYPASS_EN + should be done by the setup functions */ + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_INT_PIN_CFG, + (mldl_cfg->pdata->int_config | + BIT_BYPASS_EN)); + ERROR_CHECK(result); + /* temporary: masking out higher bits to avoid switching + intelligence */ + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_INT_ENABLE, + (mldl_cfg->int_config)); + ERROR_CHECK(result); +#else + result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 0, 0, + mldl_cfg->gyro_power & BIT_STBY_XG, + mldl_cfg->gyro_power & BIT_STBY_YG, + mldl_cfg->gyro_power & BIT_STBY_ZG); + + if (!mldl_cfg->gyro_needs_reset && + !mpu_was_reset(mldl_cfg, gyro_handle)) { + return ML_SUCCESS; + } + + result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 1, 0, + mldl_cfg->gyro_power & BIT_STBY_XG, + mldl_cfg->gyro_power & BIT_STBY_YG, + mldl_cfg->gyro_power & BIT_STBY_ZG); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_INT_CFG, + (mldl_cfg->int_config | + mldl_cfg->pdata->int_config)); + ERROR_CHECK(result); +#endif + + result = MLSLSerialRead(gyro_handle, mldl_cfg->addr, + MPUREG_PWR_MGM, 1, ®); + ERROR_CHECK(result); + reg &= ~BITS_CLKSEL; + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_PWR_MGM, + mldl_cfg->clk_src | reg); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_SMPLRT_DIV, + mldl_cfg->divider); + ERROR_CHECK(result); + +#ifdef M_HW + reg = DLPF_FS_SYNC_VALUE(0, mldl_cfg->full_scale, 0); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_GYRO_CONFIG, reg); + reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync, 0, mldl_cfg->lpf); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_CONFIG, reg); +#else + reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync, + mldl_cfg->full_scale, mldl_cfg->lpf); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_DLPF_FS_SYNC, reg); +#endif + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_DMP_CFG_1, + mldl_cfg->dmp_cfg1); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_DMP_CFG_2, + mldl_cfg->dmp_cfg2); + ERROR_CHECK(result); + + /* Write and verify memory */ + for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) { + unsigned char read[MPU_MEM_BANK_SIZE]; + + result = MLSLSerialWriteMem(gyro_handle, + mldl_cfg->addr, + ((ii << 8) | 0x00), + MPU_MEM_BANK_SIZE, + mldl_cfg->ram[ii]); + ERROR_CHECK(result); + result = MLSLSerialReadMem(gyro_handle, mldl_cfg->addr, + ((ii << 8) | 0x00), + MPU_MEM_BANK_SIZE, read); + ERROR_CHECK(result); + +#ifdef M_HW +#define ML_SKIP_CHECK 38 +#else +#define ML_SKIP_CHECK 20 +#endif + for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) { + /* skip the register memory locations */ + if (ii == 0 && jj < ML_SKIP_CHECK) + continue; + if (mldl_cfg->ram[ii][jj] != read[jj]) { + result = ML_ERROR_SERIAL_WRITE; + break; + } + } + ERROR_CHECK(result); + } + + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_XG_OFFS_TC, + mldl_cfg->offset_tc[0]); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_YG_OFFS_TC, + mldl_cfg->offset_tc[1]); + ERROR_CHECK(result); + result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr, + MPUREG_ZG_OFFS_TC, + mldl_cfg->offset_tc[2]); + ERROR_CHECK(result); + + regs[0] = MPUREG_X_OFFS_USRH; + for (ii = 0; ii < DIM(mldl_cfg->offset); ii++) { + regs[1 + ii * 2] = + (unsigned char)(mldl_cfg->offset[ii] >> 8) + & 0xff; + regs[1 + ii * 2 + 1] = + (unsigned char)(mldl_cfg->offset[ii] & 0xff); + } + result = MLSLSerialWrite(gyro_handle, mldl_cfg->addr, 7, regs); + ERROR_CHECK(result); + + /* Configure slaves */ + result = MLDLSetLevelShifterBit(mldl_cfg, gyro_handle, + mldl_cfg->pdata->level_shifter); + ERROR_CHECK(result); + return result; +} +/******************************************************************************* + ******************************************************************************* + * Exported functions + ******************************************************************************* + ******************************************************************************/ + +/** + * Initializes the pdata structure to defaults. + * + * Opens the device to read silicon revision, product id and whoami. + * + * @param mldl_cfg + * The internal device configuration data structure. + * @param mlsl_handle + * The serial communication handle. + * + * @return ML_SUCCESS if silicon revision, product id and woami are supported + * by this software. + */ +int mpu3050_open(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + int result; + /* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */ + mldl_cfg->int_config = BIT_INT_ANYRD_2CLEAR | BIT_DMP_INT_EN; + mldl_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ; + mldl_cfg->lpf = MPU_FILTER_42HZ; + mldl_cfg->full_scale = MPU_FS_2000DPS; + mldl_cfg->divider = 4; + mldl_cfg->dmp_enable = 1; + mldl_cfg->fifo_enable = 1; + mldl_cfg->ext_sync = 0; + mldl_cfg->dmp_cfg1 = 0; + mldl_cfg->dmp_cfg2 = 0; + mldl_cfg->gyro_power = 0; + mldl_cfg->gyro_is_bypassed = TRUE; + mldl_cfg->dmp_is_running = FALSE; + mldl_cfg->gyro_is_suspended = TRUE; + mldl_cfg->accel_is_suspended = TRUE; + mldl_cfg->compass_is_suspended = TRUE; + mldl_cfg->pressure_is_suspended = TRUE; + mldl_cfg->gyro_needs_reset = FALSE; + if (mldl_cfg->addr == 0) { +#ifdef __KERNEL__ + return ML_ERROR_INVALID_PARAMETER; +#else + mldl_cfg->addr = 0x68; +#endif + } + + /* + * Reset, + * Take the DMP out of sleep, and + * read the product_id, sillicon rev and whoami + */ +#ifdef M_HW + result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle, + RESET, WAKE_UP); +#else + result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0); +#endif + ERROR_CHECK(result); + + result = MLDLGetSiliconRev(mldl_cfg, mlsl_handle); + ERROR_CHECK(result); +#ifndef M_HW + result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr, + MPUREG_PRODUCT_ID, 1, + &mldl_cfg->product_id); + ERROR_CHECK(result); +#endif + + /* Get the factory temperature compensation offsets */ + result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr, + MPUREG_XG_OFFS_TC, 1, + &mldl_cfg->offset_tc[0]); + ERROR_CHECK(result); + result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr, + MPUREG_YG_OFFS_TC, 1, + &mldl_cfg->offset_tc[1]); + ERROR_CHECK(result); + result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr, + MPUREG_ZG_OFFS_TC, 1, + &mldl_cfg->offset_tc[2]); + ERROR_CHECK(result); + + /* Configure the MPU */ +#ifdef M_HW + result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle, + FALSE, SLEEP); +#else + result = + MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0); +#endif + ERROR_CHECK(result); + + if (mldl_cfg->accel && mldl_cfg->accel->init) { + result = mldl_cfg->accel->init(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); + ERROR_CHECK(result); + } + + if (mldl_cfg->compass && mldl_cfg->compass->init) { + result = mldl_cfg->compass->init(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass); + if (ML_SUCCESS != result) { + MPL_LOGE("mldl_cfg->compass->init returned %d\n", + result); + goto out_accel; + } + } + if (mldl_cfg->pressure && mldl_cfg->pressure->init) { + result = mldl_cfg->pressure->init(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure); + if (ML_SUCCESS != result) { + MPL_LOGE("mldl_cfg->pressure->init returned %d\n", + result); + goto out_compass; + } + } + + mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO; + if (mldl_cfg->accel && mldl_cfg->accel->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL; + + if (mldl_cfg->compass && mldl_cfg->compass->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS; + + if (mldl_cfg->pressure && mldl_cfg->pressure->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE; + + return result; + +out_compass: + if (mldl_cfg->compass->init) + mldl_cfg->compass->exit(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass); +out_accel: + if (mldl_cfg->accel->init) + mldl_cfg->accel->exit(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); + return result; + +} + +/** + * Close the mpu3050 interface + * + * @param mldl_cfg pointer to the configuration structure + * @param mlsl_handle pointer to the serial layer handle + * + * @return ML_SUCCESS or non-zero error code + */ +int mpu3050_close(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + int result = ML_SUCCESS; + int ret_result = ML_SUCCESS; + + if (mldl_cfg->accel && mldl_cfg->accel->exit) { + result = mldl_cfg->accel->exit(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); + if (ML_SUCCESS != result) + MPL_LOGE("Accel exit failed %d\n", result); + ret_result = result; + } + if (ML_SUCCESS == ret_result) + ret_result = result; + + if (mldl_cfg->compass && mldl_cfg->compass->exit) { + result = mldl_cfg->compass->exit(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass); + if (ML_SUCCESS != result) + MPL_LOGE("Compass exit failed %d\n", result); + } + if (ML_SUCCESS == ret_result) + ret_result = result; + + if (mldl_cfg->pressure && mldl_cfg->pressure->exit) { + result = mldl_cfg->pressure->exit(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure); + if (ML_SUCCESS != result) + MPL_LOGE("Pressure exit failed %d\n", result); + } + if (ML_SUCCESS == ret_result) + ret_result = result; + + return ret_result; +} + +/** + * @brief resume the MPU3050 device and all the other sensor + * devices from their low power state. + * + * @param mldl_cfg + * pointer to the configuration structure + * @param gyro_handle + * the main file handle to the MPU3050 device. + * @param 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. + * @param 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. + * @param 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. + * @param resume_gyro + * whether resuming the gyroscope device is + * actually needed (if the device supports low power + * mode of some sort). + * @param resume_accel + * whether resuming the accelerometer device is + * actually needed (if the device supports low power + * mode of some sort). + * @param resume_compass + * whether resuming the compass device is + * actually needed (if the device supports low power + * mode of some sort). + * @param resume_pressure + * whether resuming the pressure sensor device is + * actually needed (if the device supports low power + * mode of some sort). + * @return ML_SUCCESS or a non-zero error code. + */ +int mpu3050_resume(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + bool resume_gyro, + bool resume_accel, + bool resume_compass, + bool resume_pressure) +{ + int result = ML_SUCCESS; + +#ifdef CONFIG_MPU_SENSORS_DEBUG + mpu_print_cfg(mldl_cfg); +#endif + + if (resume_accel && + ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume))) + return ML_ERROR_INVALID_PARAMETER; + if (resume_compass && + ((!mldl_cfg->compass) || (!mldl_cfg->compass->resume))) + return ML_ERROR_INVALID_PARAMETER; + if (resume_pressure && + ((!mldl_cfg->pressure) || (!mldl_cfg->pressure->resume))) + return ML_ERROR_INVALID_PARAMETER; + + if (resume_gyro && mldl_cfg->gyro_is_suspended) { + result = gyro_resume(mldl_cfg, gyro_handle); + ERROR_CHECK(result); + } + + if (resume_accel && mldl_cfg->accel_is_suspended) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) { + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE); + ERROR_CHECK(result); + } + +#if 0 + result = mldl_cfg->accel->resume(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); +#else + result = mpu_accel_resume(mldl_cfg); +#endif + ERROR_CHECK(result); + mldl_cfg->accel_is_suspended = FALSE; + } + + if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->accel_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) { + result = mpu_set_slave(mldl_cfg, + gyro_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); + ERROR_CHECK(result); + } + + if (resume_compass && mldl_cfg->compass_is_suspended) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) { + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE); + ERROR_CHECK(result); + } + result = mldl_cfg->compass->resume(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata-> + compass); + ERROR_CHECK(result); + mldl_cfg->compass_is_suspended = FALSE; + } + + if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->compass_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) { + result = mpu_set_slave(mldl_cfg, + gyro_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass); + ERROR_CHECK(result); + } + + if (resume_pressure && mldl_cfg->pressure_is_suspended) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) { + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE); + ERROR_CHECK(result); + } + result = mldl_cfg->pressure->resume(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata-> + pressure); + ERROR_CHECK(result); + mldl_cfg->pressure_is_suspended = FALSE; + } + + if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->pressure_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) { + result = mpu_set_slave(mldl_cfg, + gyro_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure); + ERROR_CHECK(result); + } + + /* Now start */ + if (resume_gyro) { + result = dmp_start(mldl_cfg, gyro_handle); + ERROR_CHECK(result); + } + + return result; +} + +/** + * @brief suspend the MPU3050 device and all the other sensor + * devices into their low power state. + * @param gyro_handle + * the main file handle to the MPU3050 device. + * @param 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. + * @param 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. + * @param 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. + * @param accel + * whether suspending the accelerometer device is + * actually needed (if the device supports low power + * mode of some sort). + * @param compass + * whether suspending the compass device is + * actually needed (if the device supports low power + * mode of some sort). + * @param pressure + * whether suspending the pressure sensor device is + * actually needed (if the device supports low power + * mode of some sort). + * @return ML_SUCCESS or a non-zero error code. + */ +int mpu3050_suspend(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + bool suspend_gyro, + bool suspend_accel, + bool suspend_compass, + bool suspend_pressure) +{ + int result = ML_SUCCESS; + + if (suspend_gyro && !mldl_cfg->gyro_is_suspended) { +#ifdef M_HW + return ML_SUCCESS; + /* This puts the bus into bypass mode */ + result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1); + ERROR_CHECK(result); + result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP); +#else + result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, + 0, SLEEP, 0, 0, 0); +#endif + ERROR_CHECK(result); + } + + if (!mldl_cfg->accel_is_suspended && suspend_accel && + mldl_cfg->accel && mldl_cfg->accel->suspend) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) { + result = mpu_set_slave(mldl_cfg, gyro_handle, + NULL, NULL); + ERROR_CHECK(result); + } + +#if 0 + result = mldl_cfg->accel->suspend(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel); +#else + result = mpu_accel_suspend(mldl_cfg); +#endif + ERROR_CHECK(result); + mldl_cfg->accel_is_suspended = TRUE; + } + + if (!mldl_cfg->compass_is_suspended && suspend_compass && + mldl_cfg->compass && mldl_cfg->compass->suspend) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) { + result = mpu_set_slave(mldl_cfg, gyro_handle, + NULL, NULL); + ERROR_CHECK(result); + } + result = mldl_cfg->compass->suspend(compass_handle, + mldl_cfg->compass, + &mldl_cfg-> + pdata->compass); + ERROR_CHECK(result); + mldl_cfg->compass_is_suspended = TRUE; + } + + if (!mldl_cfg->pressure_is_suspended && suspend_pressure && + mldl_cfg->pressure && mldl_cfg->pressure->suspend) { + if (!mldl_cfg->gyro_is_suspended && + EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) { + result = mpu_set_slave(mldl_cfg, gyro_handle, + NULL, NULL); + ERROR_CHECK(result); + } + result = mldl_cfg->pressure->suspend(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg-> + pdata->pressure); + ERROR_CHECK(result); + mldl_cfg->pressure_is_suspended = TRUE; + } + return result; +} + + +/** + * @brief read raw sensor data from the accelerometer device + * in use. + * @param mldl_cfg + * A pointer to the struct mldl_cfg data structure. + * @param accel_handle + * The handle to the device the accelerometer is connected to. + * @param data + * a buffer to store the raw sensor data. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +int mpu3050_read_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, unsigned char *data) +{ + if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read) + if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) + && (!mldl_cfg->gyro_is_bypassed)) + return ML_ERROR_FEATURE_NOT_ENABLED; + else + return mldl_cfg->accel->read(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +/** + * @brief read raw sensor data from the compass device + * in use. + * @param mldl_cfg + * A pointer to the struct mldl_cfg data structure. + * @param compass_handle + * The handle to the device the compass is connected to. + * @param data + * a buffer to store the raw sensor data. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +int mpu3050_read_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, unsigned char *data) +{ + if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read) + if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) + && (!mldl_cfg->gyro_is_bypassed)) + return ML_ERROR_FEATURE_NOT_ENABLED; + else + return mldl_cfg->compass->read(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +/** + * @brief read raw sensor data from the pressure device + * in use. + * @param mldl_cfg + * A pointer to the struct mldl_cfg data structure. + * @param pressure_handle + * The handle to the device the pressure sensor is connected to. + * @param data + * a buffer to store the raw sensor data. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg, + void *pressure_handle, unsigned char *data) +{ + if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read) + if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) + && (!mldl_cfg->gyro_is_bypassed)) + return ML_ERROR_FEATURE_NOT_ENABLED; + else + return mldl_cfg->pressure->read( + pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +int mpu3050_config_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->config) + return mldl_cfg->accel->config(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + +} + +int mpu3050_config_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->config) + return mldl_cfg->compass->config(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + +} + +int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg, + void *pressure_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->config) + return mldl_cfg->pressure->config(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->get_config) + return mldl_cfg->accel->get_config(accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + +} + +int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->get_config) + return mldl_cfg->compass->get_config(compass_handle, + mldl_cfg->compass, + &mldl_cfg->pdata->compass, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; + +} + +int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg, + void *pressure_handle, + struct ext_slave_config *data) +{ + if (NULL != mldl_cfg->pressure && + NULL != mldl_cfg->pressure->get_config) + return mldl_cfg->pressure->get_config(pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure, + data); + else + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + + +/** + *@} + */ diff --git a/drivers/misc/mpu3050/mldl_cfg.h b/drivers/misc/mpu3050/mldl_cfg.h new file mode 100755 index 00000000000..b893946a1da --- /dev/null +++ b/drivers/misc/mpu3050/mldl_cfg.h @@ -0,0 +1,209 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @addtogroup MLDL + * + * @{ + * @file mldl_cfg.h + * @brief The Motion Library Driver Layer Configuration header file. + */ + +#ifndef __MLDL_CFG_H__ +#define __MLDL_CFG_H__ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include "mlsl.h" +#include "mpu.h" + +/* --------------------- */ +/* - Defines. - */ +/* --------------------- */ + +/*************************************************************************/ +/* Sensors */ +/*************************************************************************/ + +#define ML_X_GYRO (0x0001) +#define ML_Y_GYRO (0x0002) +#define ML_Z_GYRO (0x0004) +#define ML_DMP_PROCESSOR (0x0008) + +#define ML_X_ACCEL (0x0010) +#define ML_Y_ACCEL (0x0020) +#define ML_Z_ACCEL (0x0040) + +#define ML_X_COMPASS (0x0080) +#define ML_Y_COMPASS (0x0100) +#define ML_Z_COMPASS (0x0200) + +#define ML_X_PRESSURE (0x0300) +#define ML_Y_PRESSURE (0x0800) +#define ML_Z_PRESSURE (0x1000) + +#define ML_TEMPERATURE (0x2000) +#define ML_TIME (0x4000) + +#define ML_THREE_AXIS_GYRO (0x000F) +#define ML_THREE_AXIS_ACCEL (0x0070) +#define ML_THREE_AXIS_COMPASS (0x0380) +#define ML_THREE_AXIS_PRESSURE (0x1C00) + +#define ML_FIVE_AXIS (0x007B) +#define ML_SIX_AXIS_GYRO_ACCEL (0x007F) +#define ML_SIX_AXIS_ACCEL_COMPASS (0x03F0) +#define ML_NINE_AXIS (0x03FF) +#define ML_ALL_SENSORS (0x7FFF) + +#define SAMPLING_RATE_HZ(mldl_cfg) \ + ((((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7)) \ + ? (8000) \ + : (1000)) \ + / ((mldl_cfg)->divider + 1)) + +#define SAMPLING_PERIOD_US(mldl_cfg) \ + ((1000000L * ((mldl_cfg)->divider + 1)) / \ + (((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7)) \ + ? (8000) \ + : (1000))) +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +/*exteded variables for only kernel*/ +struct mldl_ext_cfg { + void *mpuacc_data; /*Mpu-Accel Data*/ +}; + + +/* Platform data for the MPU */ +struct mldl_cfg { + /* MPU related configuration */ + unsigned long requested_sensors; + unsigned char addr; + unsigned char int_config; + unsigned char ext_sync; + unsigned char full_scale; + unsigned char lpf; + unsigned char clk_src; + unsigned char divider; + unsigned char dmp_enable; + unsigned char fifo_enable; + unsigned char dmp_cfg1; + unsigned char dmp_cfg2; + unsigned char gyro_power; + unsigned char offset_tc[MPU_NUM_AXES]; + unsigned short offset[MPU_NUM_AXES]; + unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE]; + + /* MPU Related stored status and info */ + unsigned char silicon_revision; + unsigned char product_id; + unsigned short trim; + + /* Driver/Kernel related state information */ + int gyro_is_bypassed; + int dmp_is_running; + int gyro_is_suspended; + int accel_is_suspended; + int compass_is_suspended; + int pressure_is_suspended; + int gyro_needs_reset; + + /* Slave related information */ + struct ext_slave_descr *accel; + struct ext_slave_descr *compass; + struct ext_slave_descr *pressure; + + /* Platform Data */ + struct mpu3050_platform_data *pdata; + + /*---------------------------------------------------*/ + /*KERNEL ONLY VARIABLES */ + /*---------------------------------------------------*/ + struct mldl_ext_cfg ext; +}; + + +int mpu3050_open(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle); +int mpu3050_close(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle); +int mpu3050_resume(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + bool resume_gyro, + bool resume_accel, + bool resume_compass, + bool resume_pressure); +int mpu3050_suspend(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + bool suspend_gyro, + bool suspend_accel, + bool suspend_compass, + bool suspend_pressure); +int mpu3050_read_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, + unsigned char *data); +int mpu3050_read_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, + unsigned char *data); +int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg, void *mlsl_handle, + unsigned char *data); + +int mpu3050_config_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, + struct ext_slave_config *data); +int mpu3050_config_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, + struct ext_slave_config *data); +int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg, + void *pressure_handle, + struct ext_slave_config *data); + +int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg, + void *accel_handle, + struct ext_slave_config *data); +int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg, + void *compass_handle, + struct ext_slave_config *data); +int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg, + void *pressure_handle, + struct ext_slave_config *data); + + +#endif /* __MLDL_CFG_H__ */ + +/** + *@} + */ diff --git a/drivers/misc/mpu3050/mlos-kernel.c b/drivers/misc/mpu3050/mlos-kernel.c new file mode 100755 index 00000000000..02f18433978 --- /dev/null +++ b/drivers/misc/mpu3050/mlos-kernel.c @@ -0,0 +1,93 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +/** + * @defgroup + * @brief + * + * @{ + * @file mlos-kernel.c + * @brief + * + * + */ + +#include "mlos.h" +#include <linux/delay.h> +#include <linux/slab.h> +#include <linux/time.h> + +void *MLOSMalloc(unsigned int numBytes) +{ + return kmalloc(numBytes, GFP_KERNEL); +} + +tMLError MLOSFree(void *ptr) +{ + kfree(ptr); + return ML_SUCCESS; +} + +tMLError MLOSCreateMutex(HANDLE *mutex) +{ + /* @todo implement if needed */ + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +tMLError MLOSLockMutex(HANDLE mutex) +{ + /* @todo implement if needed */ + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +tMLError MLOSUnlockMutex(HANDLE mutex) +{ + /* @todo implement if needed */ + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +tMLError MLOSDestroyMutex(HANDLE handle) +{ + /* @todo implement if needed */ + return ML_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +FILE *MLOSFOpen(char *filename) +{ + /* @todo implement if needed */ + return NULL; +} + +void MLOSFClose(FILE *fp) +{ + /* @todo implement if needed */ +} + +void MLOSSleep(int mSecs) +{ + msleep(mSecs); +} + +unsigned long MLOSGetTickCount(void) +{ + struct timespec now; + + getnstimeofday(&now); + + return (long)(now.tv_sec * 1000L + now.tv_nsec / 1000000L); +} diff --git a/drivers/misc/mpu3050/mlos.h b/drivers/misc/mpu3050/mlos.h new file mode 100755 index 00000000000..4ebb86c9fa5 --- /dev/null +++ b/drivers/misc/mpu3050/mlos.h @@ -0,0 +1,73 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include <stdio.h> +#endif + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + void *MLOSMalloc(unsigned int numBytes); + tMLError MLOSFree(void *ptr); + tMLError MLOSCreateMutex(HANDLE *mutex); + tMLError MLOSLockMutex(HANDLE mutex); + tMLError MLOSUnlockMutex(HANDLE mutex); + FILE *MLOSFOpen(char *filename); + void MLOSFClose(FILE *fp); + + tMLError MLOSDestroyMutex(HANDLE handle); + + void MLOSSleep(int mSecs); + unsigned long MLOSGetTickCount(void); + +#ifdef __cplusplus +} +#endif +#endif /* _MLOS_H */ diff --git a/drivers/misc/mpu3050/mlsl-kernel.c b/drivers/misc/mpu3050/mlsl-kernel.c new file mode 100755 index 00000000000..908b16f16b2 --- /dev/null +++ b/drivers/misc/mpu3050/mlsl-kernel.c @@ -0,0 +1,331 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#include "mlsl.h" +#include "mpu-i2c.h" + +/* ------------ */ +/* - Defines. - */ +/* ------------ */ + +/* ---------------------- */ +/* - Types definitions. - */ +/* ---------------------- */ + +/* --------------------- */ +/* - Function p-types. - */ +/* --------------------- */ + +/** + * @brief used to open the I2C or SPI serial port. + * This port is used to send and receive data to the MPU device. + * @param portNum + * The COM port number associated with the device in use. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +tMLError MLSLSerialOpen(char const *port, void **sl_handle) +{ + return ML_SUCCESS; +} + +/** + * @brief used to reset any buffering the driver may be doing + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +tMLError MLSLSerialReset(void *sl_handle) +{ + return ML_SUCCESS; +} + +/** + * @brief used to close the I2C or SPI serial port. + * This port is used to send and receive data to the MPU device. + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +tMLError MLSLSerialClose(void *sl_handle) +{ + return ML_SUCCESS; +} + +/** + * @brief used to read a single byte of data. + * This should be sent by I2C or SPI. + * + * @param slaveAddr I2C slave address of device. + * @param registerAddr Register address to read. + * @param data Single byte of data to read. + * + * @return ML_SUCCESS if the command is successful, an error code otherwise. + */ +tMLError MLSLSerialWriteSingle(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned char data) +{ + return sensor_i2c_write_register((struct i2c_adapter *) sl_handle, + slaveAddr, registerAddr, data); +} + + +/** + * @brief used to write multiple bytes of data from registers. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param registerAddr Register address to write. + * @param length Length of burst of data. + * @param data Pointer to block of data. + * + * @return ML_SUCCESS if successful, a non-zero error code otherwise. + */ +tMLError MLSLSerialWrite(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, unsigned char const *data) +{ + tMLError result; + const unsigned short dataLength = length - 1; + const unsigned char startRegAddr = data[0]; + unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1]; + unsigned short bytesWritten = 0; + + while (bytesWritten < dataLength) { + unsigned short thisLen = min(SERIAL_MAX_TRANSFER_SIZE, + dataLength - bytesWritten); + if (bytesWritten == 0) { + result = sensor_i2c_write((struct i2c_adapter *) + sl_handle, slaveAddr, + 1 + thisLen, data); + } else { + /* manually increment register addr between chunks */ + i2cWrite[0] = startRegAddr + bytesWritten; + memcpy(&i2cWrite[1], &data[1 + bytesWritten], + thisLen); + result = sensor_i2c_write((struct i2c_adapter *) + sl_handle, slaveAddr, + 1 + thisLen, i2cWrite); + } + if (ML_SUCCESS != result) + return result; + bytesWritten += thisLen; + } + return ML_SUCCESS; +} + + +/** + * @brief used to read multiple bytes of data from registers. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param registerAddr Register address to read. + * @param length Length of burst of data. + * @param data Pointer to block of data. + * + * @return Zero if successful; an error code otherwise + */ +tMLError MLSLSerialRead(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned short length, unsigned char *data) +{ + tMLError result; + unsigned short bytesRead = 0; + + if (registerAddr == MPUREG_FIFO_R_W + || registerAddr == MPUREG_MEM_R_W) { + return ML_ERROR_INVALID_PARAMETER; + } + while (bytesRead < length) { + unsigned short thisLen = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead); + result = + sensor_i2c_read((struct i2c_adapter *) sl_handle, + slaveAddr, registerAddr + bytesRead, + thisLen, &data[bytesRead]); + if (ML_SUCCESS != result) + return result; + bytesRead += thisLen; + } + return ML_SUCCESS; +} + + +/** + * @brief used to write multiple bytes of data to the memory. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param memAddr The location in the memory to write to. + * @param length Length of burst data. + * @param data Pointer to block of data. + * + * @return Zero if successful; an error code otherwise + */ +tMLError MLSLSerialWriteMem(void *sl_handle, + unsigned char slaveAddr, + unsigned short memAddr, + unsigned short length, + unsigned char const *data) +{ + tMLError result; + unsigned short bytesWritten = 0; + + if ((memAddr & 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, memAddr & 0xFF); + return ML_ERROR_INVALID_PARAMETER; + } + while (bytesWritten < length) { + unsigned short thisLen = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten); + result = + mpu_memory_write((struct i2c_adapter *) sl_handle, + slaveAddr, memAddr + bytesWritten, + thisLen, &data[bytesWritten]); + if (ML_SUCCESS != result) + return result; + bytesWritten += thisLen; + } + return ML_SUCCESS; +} + + +/** + * @brief used to read multiple bytes of data from the memory. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param memAddr The location in the memory to read from. + * @param length Length of burst data. + * @param data Pointer to block of data. + * + * @return Zero if successful; an error code otherwise + */ +tMLError MLSLSerialReadMem(void *sl_handle, + unsigned char slaveAddr, + unsigned short memAddr, + unsigned short length, unsigned char *data) +{ + tMLError result; + unsigned short bytesRead = 0; + + if ((memAddr & 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, memAddr & 0xFF); + return ML_ERROR_INVALID_PARAMETER; + } + while (bytesRead < length) { + unsigned short thisLen = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead); + result = + mpu_memory_read((struct i2c_adapter *) sl_handle, + slaveAddr, memAddr + bytesRead, + thisLen, &data[bytesRead]); + if (ML_SUCCESS != result) + return result; + bytesRead += thisLen; + } + return ML_SUCCESS; +} + + +/** + * @brief used to write multiple bytes of data to the fifo. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param length Length of burst of data. + * @param data Pointer to block of data. + * + * @return Zero if successful; an error code otherwise + */ +tMLError MLSLSerialWriteFifo(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, + unsigned char const *data) +{ + tMLError result; + unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1]; + unsigned short bytesWritten = 0; + + if (length > FIFO_HW_SIZE) { + printk(KERN_ERR + "maximum fifo write length is %d\n", FIFO_HW_SIZE); + return ML_ERROR_INVALID_PARAMETER; + } + while (bytesWritten < length) { + unsigned short thisLen = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten); + i2cWrite[0] = MPUREG_FIFO_R_W; + memcpy(&i2cWrite[1], &data[bytesWritten], thisLen); + result = sensor_i2c_write((struct i2c_adapter *) sl_handle, + slaveAddr, thisLen + 1, + i2cWrite); + if (ML_SUCCESS != result) + return result; + bytesWritten += thisLen; + } + return ML_SUCCESS; +} + + +/** + * @brief used to read multiple bytes of data from the fifo. + * This should be sent by I2C. + * + * @param slaveAddr I2C slave address of device. + * @param length Length of burst of data. + * @param data Pointer to block of data. + * + * @return Zero if successful; an error code otherwise + */ +tMLError MLSLSerialReadFifo(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, unsigned char *data) +{ + tMLError result; + unsigned short bytesRead = 0; + + if (length > FIFO_HW_SIZE) { + printk(KERN_ERR + "maximum fifo read length is %d\n", FIFO_HW_SIZE); + return ML_ERROR_INVALID_PARAMETER; + } + while (bytesRead < length) { + unsigned short thisLen = + min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead); + result = + sensor_i2c_read((struct i2c_adapter *) sl_handle, + slaveAddr, MPUREG_FIFO_R_W, thisLen, + &data[bytesRead]); + if (ML_SUCCESS != result) + return result; + bytesRead += thisLen; + } + + return ML_SUCCESS; +} + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/mlsl.h b/drivers/misc/mpu3050/mlsl.h new file mode 100755 index 00000000000..51fe401b8e5 --- /dev/null +++ b/drivers/misc/mpu3050/mlsl.h @@ -0,0 +1,110 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#ifndef __MSSL_H__ +#define __MSSL_H__ + +#include "mltypes.h" +#include "mpu.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ------------ */ +/* - Defines. - */ +/* ------------ */ +/* acceleration data */ +struct acc_data { + s16 x; + s16 y; + s16 z; +}; + + +/* + * NOTE : to properly support Yamaha compass reads, + * the max transfer size should be at least 9 B. + * Length in bytes, typically a power of 2 >= 2 + */ +#define SERIAL_MAX_TRANSFER_SIZE 128 + +/* ---------------------- */ +/* - Types definitions. - */ +/* ---------------------- */ + +/* --------------------- */ +/* - Function p-types. - */ +/* --------------------- */ + + tMLError MLSLSerialOpen(char const *port, + void **sl_handle); + tMLError MLSLSerialReset(void *sl_handle); + tMLError MLSLSerialClose(void *sl_handle); + + tMLError MLSLSerialWriteSingle(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned char data); + + tMLError MLSLSerialRead(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned short length, + unsigned char *data); + + tMLError MLSLSerialWrite(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, + unsigned char const *data); + + tMLError MLSLSerialReadMem(void *sl_handle, + unsigned char slaveAddr, + unsigned short memAddr, + unsigned short length, + unsigned char *data); + + tMLError MLSLSerialWriteMem(void *sl_handle, + unsigned char slaveAddr, + unsigned short memAddr, + unsigned short length, + unsigned char const *data); + + tMLError MLSLSerialReadFifo(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, + unsigned char *data); + + tMLError MLSLSerialWriteFifo(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, + unsigned char const *data); + + tMLError MLSLWriteCal(unsigned char *cal, unsigned int len); + tMLError MLSLReadCal(unsigned char *cal, unsigned int len); + tMLError MLSLGetCalLength(unsigned int *len); + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ +#endif /* MLSL_H */ diff --git a/drivers/misc/mpu3050/mltypes.h b/drivers/misc/mpu3050/mltypes.h new file mode 100755 index 00000000000..5c1b684e5b5 --- /dev/null +++ b/drivers/misc/mpu3050/mltypes.h @@ -0,0 +1,227 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup MLERROR + * @brief Motion Library - Error definitions. + * Definition of the error codes used within the MPL and returned + * to the user. + * Every function tries to return a meaningful error code basing + * on the occuring error condition. The error code is numeric. + * + * The available error codes and their associated values are: + * - (0) ML_SUCCESS + * - (1) ML_ERROR + * - (2) ML_ERROR_INVALID_PARAMETER + * - (3) ML_ERROR_FEATURE_NOT_ENABLED + * - (4) ML_ERROR_FEATURE_NOT_IMPLEMENTED + * - (6) ML_ERROR_DMP_NOT_STARTED + * - (7) ML_ERROR_DMP_STARTED + * - (8) ML_ERROR_NOT_OPENED + * - (9) ML_ERROR_OPENED + * - (10) ML_ERROR_INVALID_MODULE + * - (11) ML_ERROR_MEMORY_EXAUSTED + * - (12) ML_ERROR_DIVIDE_BY_ZERO + * - (13) ML_ERROR_ASSERTION_FAILURE + * - (14) ML_ERROR_FILE_OPEN + * - (15) ML_ERROR_FILE_READ + * - (16) ML_ERROR_FILE_WRITE + * - (20) ML_ERROR_SERIAL_CLOSED + * - (21) ML_ERROR_SERIAL_OPEN_ERROR + * - (22) ML_ERROR_SERIAL_READ + * - (23) ML_ERROR_SERIAL_WRITE + * - (24) ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED + * - (25) ML_ERROR_SM_TRANSITION + * - (26) ML_ERROR_SM_IMPROPER_STATE + * - (30) ML_ERROR_FIFO_OVERFLOW + * - (31) ML_ERROR_FIFO_FOOTER + * - (32) ML_ERROR_FIFO_READ_COUNT + * - (33) ML_ERROR_FIFO_READ_DATA + * - (40) ML_ERROR_MEMORY_SET + * - (50) ML_ERROR_LOG_MEMORY_ERROR + * - (51) ML_ERROR_LOG_OUTPUT_ERROR + * - (60) ML_ERROR_OS_BAD_PTR + * - (61) ML_ERROR_OS_BAD_HANDLE + * - (62) ML_ERROR_OS_CREATE_FAILED + * - (63) ML_ERROR_OS_LOCK_FAILED + * - (70) ML_ERROR_COMPASS_DATA_OVERFLOW + * - (71) ML_ERROR_COMPASS_DATA_UNDERFLOW + * - (72) ML_ERROR_COMPASS_DATA_NOT_READY + * - (73) ML_ERROR_COMPASS_DATA_ERROR + * - (75) ML_ERROR_CALIBRATION_LOAD + * - (76) ML_ERROR_CALIBRATION_STORE + * - (77) ML_ERROR_CALIBRATION_LEN + * - (78) ML_ERROR_CALIBRATION_CHECKSUM + * + * @{ + * @file mltypes.h + * @} + */ + +#ifndef MLTYPES_H +#define MLTYPES_H + +#ifdef __KERNEL__ +#include <linux/types.h> +#else +#include "stdint_invensense.h" +#endif +#include "log.h" + +/*--------------------------- + ML Types +---------------------------*/ + +/** + * @struct tMLError mltypes.h "mltypes" + * @brief The MPL Error Code return type. + * + * @code + * typedef unsigned char tMLError; + * @endcode + */ +typedef unsigned char tMLError; + +#if defined(LINUX) || defined(__KERNEL__) +typedef unsigned int HANDLE; +#endif + +#ifdef __KERNEL__ +typedef HANDLE FILE; +#endif + +#ifndef __cplusplus +#ifndef __KERNEL__ +typedef int_fast8_t bool; +#endif +#endif + +/*--------------------------- + ML Defines +---------------------------*/ + +#ifndef NULL +#define NULL 0 +#endif + +#ifndef TRUE +#define TRUE 1 +#endif + +#ifndef FALSE +#define FALSE 0 +#endif + +/* Dimension of an array */ +#ifndef DIM +#define DIM(array) (sizeof(array)/sizeof((array)[0])) +#endif + +/* - ML Errors. - */ +#define ERROR_NAME(x) (#x) +#define ERROR_CHECK(x) \ + { \ + if (ML_SUCCESS != x) { \ + MPL_LOGE("%s|%s|%d returning %d\n", \ + __FILE__, __func__, __LINE__, x); \ + return x; \ + } \ + } + +#define ERROR_CHECK_FIRST(first, x) \ + { if (ML_SUCCESS == first) first = x; } + +#define ML_SUCCESS (0) +/* Generic Error code. Proprietary Error Codes only */ +#define ML_ERROR (1) + +/* Compatibility and other generic error codes */ +#define ML_ERROR_INVALID_PARAMETER (2) +#define ML_ERROR_FEATURE_NOT_ENABLED (3) +#define ML_ERROR_FEATURE_NOT_IMPLEMENTED (4) +#define ML_ERROR_DMP_NOT_STARTED (6) +#define ML_ERROR_DMP_STARTED (7) +#define ML_ERROR_NOT_OPENED (8) +#define ML_ERROR_OPENED (9) +#define ML_ERROR_INVALID_MODULE (10) +#define ML_ERROR_MEMORY_EXAUSTED (11) +#define ML_ERROR_DIVIDE_BY_ZERO (12) +#define ML_ERROR_ASSERTION_FAILURE (13) +#define ML_ERROR_FILE_OPEN (14) +#define ML_ERROR_FILE_READ (15) +#define ML_ERROR_FILE_WRITE (16) +#define ML_ERROR_INVALID_CONFIGURATION (17) + +/* Serial Communication */ +#define ML_ERROR_SERIAL_CLOSED (20) +#define ML_ERROR_SERIAL_OPEN_ERROR (21) +#define ML_ERROR_SERIAL_READ (22) +#define ML_ERROR_SERIAL_WRITE (23) +#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (24) + +/* SM = State Machine */ +#define ML_ERROR_SM_TRANSITION (25) +#define ML_ERROR_SM_IMPROPER_STATE (26) + +/* Fifo */ +#define ML_ERROR_FIFO_OVERFLOW (30) +#define ML_ERROR_FIFO_FOOTER (31) +#define ML_ERROR_FIFO_READ_COUNT (32) +#define ML_ERROR_FIFO_READ_DATA (33) + +/* Memory & Registers, Set & Get */ +#define ML_ERROR_MEMORY_SET (40) + +#define ML_ERROR_LOG_MEMORY_ERROR (50) +#define ML_ERROR_LOG_OUTPUT_ERROR (51) + +/* OS interface errors */ +#define ML_ERROR_OS_BAD_PTR (60) +#define ML_ERROR_OS_BAD_HANDLE (61) +#define ML_ERROR_OS_CREATE_FAILED (62) +#define ML_ERROR_OS_LOCK_FAILED (63) + +/* Compass errors */ +#define ML_ERROR_COMPASS_DATA_OVERFLOW (70) +#define ML_ERROR_COMPASS_DATA_UNDERFLOW (71) +#define ML_ERROR_COMPASS_DATA_NOT_READY (72) +#define ML_ERROR_COMPASS_DATA_ERROR (73) + +/* Load/Store calibration */ +#define ML_ERROR_CALIBRATION_LOAD (75) +#define ML_ERROR_CALIBRATION_STORE (76) +#define ML_ERROR_CALIBRATION_LEN (77) +#define ML_ERROR_CALIBRATION_CHECKSUM (78) + +/* Accel errors */ +#define ML_ERROR_ACCEL_DATA_OVERFLOW (79) +#define ML_ERROR_ACCEL_DATA_UNDERFLOW (80) +#define ML_ERROR_ACCEL_DATA_NOT_READY (81) +#define ML_ERROR_ACCEL_DATA_ERROR (82) + +/* For Linux coding compliance */ +#ifndef __KERNEL__ +#define EXPORT_SYMBOL(x) +#endif + +/*--------------------------- + p-Types +---------------------------*/ + +#endif /* MLTYPES_H */ diff --git a/drivers/misc/mpu3050/mpu-accel.c b/drivers/misc/mpu3050/mpu-accel.c new file mode 100755 index 00000000000..4b5c64187c6 --- /dev/null +++ b/drivers/misc/mpu3050/mpu-accel.c @@ -0,0 +1,679 @@ +/* + mpu-accel.c - mpu3050 input device interface + + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. +*/ + +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/irq.h> +#include <linux/gpio.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/slab.h> +#include <linux/version.h> +#include <linux/pm.h> +#include <linux/mutex.h> +#include <linux/suspend.h> + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> +#include <linux/input.h> + +#include "mpuirq.h" +#include "slaveirq.h" +#include "mlsl.h" +#include "mpu-i2c.h" +#include "mldl_cfg.h" +#include "mpu.h" +#include "mpu-accel.h" + +#define MPUACC_DEBUG 0 +#define MPUACC_DEBUG_CFG 0 + +#define MPUACCEL_INPUT_NAME "mpu-accel" + +struct mpuaccel_data { + struct input_dev *input_data; + struct delayed_work work; + struct mutex data_mutex; + + struct mldl_cfg *mldl_cfg; + void *accel_handle; + + atomic_t enable; + atomic_t poll_delay; + int device_is_on; +#ifdef MPUACC_USES_CACHED_DATA + unsigned char cached_data[6]; +#endif /* MPUACC_USES_CACHED_DATA */ +}; + +static struct mpuaccel_data *pThisData; +extern struct acc_data cal_data; + +static void mpu_accel_print_mldl_cfg(struct mldl_cfg *mldl_cfg) +{ + if (MPUACC_DEBUG_CFG) { + pr_info("requested_sensors:%ld\n", mldl_cfg->requested_sensors); +/* pr_info("ignore_system_suspend:%d\n", mldl_cfg->ignore_system_suspend); */ + pr_info("addr:%d\n", mldl_cfg->addr); + pr_info("int_config:%d\n", mldl_cfg->int_config); + pr_info("ext_sync:%d\n", mldl_cfg->ext_sync); + pr_info("full_scale:%d\n", mldl_cfg->full_scale); + pr_info("dmp_enable:%d\n", mldl_cfg->dmp_enable); + pr_info("fifo_enable:%d\n", mldl_cfg->fifo_enable); + pr_info("dmp_cfg1:%d\n", mldl_cfg->dmp_cfg1); + pr_info("dmp_cfg2:%d\n", mldl_cfg->dmp_cfg2); + pr_info("gyro_power:%d\n", mldl_cfg->gyro_power); + pr_info("gyro_is_bypassed:%d\n", mldl_cfg->gyro_is_bypassed); + pr_info("dmp_is_running:%d\n", mldl_cfg->dmp_is_running); + pr_info("gyro_is_suspended:%d\n", mldl_cfg->gyro_is_suspended); + pr_info("accel_is_suspended:%d\n", + mldl_cfg->accel_is_suspended); + pr_info("compass_is_suspended:%d\n", + mldl_cfg->compass_is_suspended); + pr_info("pressure_is_suspended:%d\n", + mldl_cfg->pressure_is_suspended); + pr_info("gyro_needs_reset:%d\n", mldl_cfg->gyro_needs_reset); + } +} + +static int mpu_accel_mutex_lock(struct mpuaccel_data *data) +{ + mutex_lock(&data->data_mutex); + + return ML_SUCCESS; +} + +static int mpu_accel_mutex_unlock(struct mpuaccel_data *data) +{ + mutex_unlock(&data->data_mutex); + + return ML_SUCCESS; +} + +static int mpu_accel_activate_device(struct mpuaccel_data *data, int enable) +{ + int result = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + if (enable) { + /*turn on accel */ + if (NULL != mldl_cfg->accel + && NULL != mldl_cfg->accel->resume) { + result = mldl_cfg->accel->resume(data->accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata-> + accel); + } + } else { + /*turn off accel */ + if (NULL != mldl_cfg->accel + && NULL != mldl_cfg->accel->suspend) { + result = mldl_cfg->accel->suspend(data->accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata-> + accel); + } + } + + if (result == ML_SUCCESS) + data->device_is_on = enable; + + if (MPUACC_DEBUG) + pr_info("activate device:%d, result=%d\n", enable, result); + + return result; +} + +static int mpu_accel_get_data_from_device(struct mpuaccel_data *data, + unsigned char *buffer) +{ + int result = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read) { + result = mldl_cfg->accel->read(data->accel_handle, + mldl_cfg->accel, + &mldl_cfg->pdata->accel, buffer); + } + + return result; +} + +static int mpu_accel_get_data_from_mpu(struct mpuaccel_data *data, unsigned char *buffer) +{ + int result = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + result = + MLSLSerialRead(data->accel_handle, mldl_cfg->addr, 0x23, 6, buffer); + return result; +} + +static int mpu_accel_get_data(struct mpuaccel_data *data, unsigned char *buffer, + int *from_mpu) +{ + int res = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + if (mldl_cfg->accel_is_suspended == 1 || + (mldl_cfg->dmp_is_running == 0 + && mldl_cfg->accel_is_suspended == 0)) { + + if (from_mpu != NULL) + *from_mpu = 0; + + /* + Retrieve accel data from accel device driver directly. + */ + res = mpu_accel_get_data_from_device(data, buffer); + } else if (mldl_cfg->dmp_is_running && + mldl_cfg->accel_is_suspended == 0) { + + if (from_mpu != NULL) + *from_mpu = 1; + + /* + Retrieve accel data from MPU registers(0x23 to 0x28). + */ + res = mpu_accel_get_data_from_mpu(data, buffer); + } + + return res; +} + +static int mpu_accel_build_data(struct mpuaccel_data *data, + const unsigned char *buffer, s16 *val) +{ + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + int endian = mldl_cfg->accel->endian; + int dev_id = mldl_cfg->accel->id; + + if (endian == EXT_SLAVE_LITTLE_ENDIAN) { + if (dev_id == ACCEL_ID_BMA150) + *val = (*(s16 *)&buffer[0]) >> 6; + else if (dev_id == ACCEL_ID_KXTF9) { + *val = + (short)(((signed char)buffer[1] << 4) | + (buffer[0] >> 4)); + } else + *val = (buffer[1] << 8) | buffer[0]; + } else if (endian == EXT_SLAVE_BIG_ENDIAN) { + *val = (buffer[0] << 8) | buffer[1]; + } + + return ML_SUCCESS; +} + +static void mpu_accel_input_work_func(struct work_struct *work) +{ + int res = 0; + int poll_time = 0; + int enable = 0; + int i = 0; + + struct mpuaccel_data *data = container_of((struct delayed_work *)work, + struct mpuaccel_data, work); + + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + poll_time = atomic_read(&data->poll_delay); + + if (MPUACC_DEBUG) + pr_info("________________START____________________\n"); + if (MPUACC_DEBUG_CFG) + mpu_accel_print_mldl_cfg(mldl_cfg); + + mpu_accel_mutex_lock(data); + enable = atomic_read(&data->enable); + mpu_accel_mutex_unlock(data); + + if (enable) { + unsigned char buffer[6] = { 0, }; + s16 raw[3] = { 0, }; + int data_is_avail = 0; + int data_is_from_mpu = 0; + + mpu_accel_mutex_lock(data); + mpu_accel_get_data(data, buffer, &data_is_from_mpu); + mpu_accel_mutex_unlock(data); + + if (res == ML_SUCCESS) + data_is_avail = 1; + + if (data_is_avail) { + int data_is_valid = 0; + + for (i = 0; i < 3; i++) { + mpu_accel_build_data(data, &buffer[i * 2], + &raw[i]); + } + raw[0] += cal_data.x; + raw[1] += cal_data.y; + raw[2] += cal_data.z; + + if (raw[0] && raw[1] && raw[2]) + data_is_valid = 1; + + if (data_is_valid) { + int accel[3] = { 0, }; + + /*apply mounting matrix */ + for (i = 0; i < 3; i++) { +#ifdef MPUACC_USES_MOUNTING_MATRIX + int j = 0; + for (j = 0; j < 3; j++) { + accel[i] += + mldl_cfg->pdata->accel. + orientation[i * 3 + + j] * raw[j]; + } +#else + accel[i] = raw[i]; +#endif + } + + if (MPUACC_DEBUG) { + if (data_is_from_mpu == 1) + pr_info + ("MPU_ACCEL:[%d][%d][%d]\n", + accel[0], accel[1], + accel[2]); + else + pr_info("ACCEL:[%d][%d][%d]\n", + accel[0], accel[1], + accel[2]); + } +#ifdef MPUACC_USES_CACHED_DATA + memcpy(data->cached_data, buffer, + sizeof(unsigned char) * 6); +#endif /* #ifdef MPUACC_USES_CACHED_DATA */ + input_report_rel(data->input_data, REL_X, + accel[0]); + input_report_rel(data->input_data, REL_Y, + accel[1]); + input_report_rel(data->input_data, REL_Z, + accel[2]); + input_sync(data->input_data); + + if (MPUACC_DEBUG) + pr_info("input device is updated\n"); + } + } + } + + if (MPUACC_DEBUG) + pr_info("________________END____________________\n"); + + mpu_accel_mutex_lock(data); + enable = atomic_read(&data->enable); + mpu_accel_mutex_unlock(data); + + if (enable) { + if (poll_time > 0) { + schedule_delayed_work(&data->work, + msecs_to_jiffies(poll_time) + /*+ 1 */); + } else { + schedule_delayed_work(&data->work, 0); + } + + } + +} + +static int mpu_accel_enable(struct mpuaccel_data *data) +{ + int res = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + if (MPUACC_DEBUG) + pr_info("mpu_accel_enable : %d\n", atomic_read(&data->enable)); + + if (atomic_read(&data->enable) != 1) { + + if (MPUACC_DEBUG) + pr_info("mpu_accel_enable : enabled\n"); + + if (mldl_cfg->accel_is_suspended == 1) { + if (MPUACC_DEBUG) + pr_info("mpu_accel_enable : turn on accel\n"); + mpu_accel_activate_device(data, 1); + } + + atomic_set(&data->enable, 1); + schedule_delayed_work(&data->work, 0); + + } + + return res; +} + +static int mpu_accel_disable(struct mpuaccel_data *data) +{ + int res = ML_SUCCESS; + struct mldl_cfg *mldl_cfg = data->mldl_cfg; + + if (MPUACC_DEBUG) + pr_info("mpu_accel_disable : %d\n", atomic_read(&data->enable)); + + if (atomic_read(&data->enable) != 0) { + atomic_set(&data->enable, 0); + cancel_delayed_work(&data->work); + + if (MPUACC_DEBUG) + pr_info("mpu_accel_disable : disabled\n"); + + if (mldl_cfg->accel_is_suspended == 1) { + if (MPUACC_DEBUG) + pr_info("mpu_accel_disable : turn off accel\n"); + + /*turn off accel */ + mpu_accel_activate_device(data, 0); + } + } + + return res; +} + +static ssize_t mpu_accel_delay_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct mpuaccel_data *data = input_get_drvdata(input_data); + + return sprintf(buf, "%d\n", atomic_read(&data->poll_delay)); +} + +static ssize_t mpu_accel_delay_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct mpuaccel_data *data = input_get_drvdata(input_data); + int value = simple_strtoul(buf, NULL, 10); + + atomic_set(&data->poll_delay, value); + return count; +} + +static ssize_t mpu_accel_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct input_dev *input_data = to_input_dev(dev); + struct mpuaccel_data *data = input_get_drvdata(input_data); + + return sprintf(buf, "%d\n", atomic_read(&data->enable)); +} + +static ssize_t +mpu_accel_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input_data = to_input_dev(dev); + struct mpuaccel_data *data = input_get_drvdata(input_data); + int value; + + value = simple_strtoul(buf, NULL, 10); + if (value != 0 && value != 1) + return count; + + mpu_accel_mutex_lock(data); + + if (value) + mpu_accel_enable(data); + else + mpu_accel_disable(data); + + mpu_accel_mutex_unlock(data); + + return count; +} + +int mpu_accel_is_active_device(void) +{ + int is_active = 0; + + if (pThisData != NULL) { + mpu_accel_mutex_lock(pThisData); + is_active = pThisData->device_is_on; + mpu_accel_mutex_unlock(pThisData); + } + + return is_active; +} + +#ifdef MPUACC_USES_CACHED_DATA +int mpu_accel_get_cached_data(unsigned char *cache) +{ + int res = ML_ERROR; + + if (pThisData != NULL) { + if (pThisData->device_is_on == 1) { + memcpy(cache, pThisData->cached_data, + sizeof(unsigned char) * 6); + pr_info("cached data:[%d][%d][%d][%d][%d][%d]\n", + cache[0], cache[1], + cache[2], cache[3], + cache[4], cache[5]); + res = ML_SUCCESS; + } + + } + + return res; +} +#endif /* MPUACC_USES_CACHED_DATA */ + +static DEVICE_ATTR(poll_delay, S_IRUGO | S_IWUSR | S_IWGRP, + mpu_accel_delay_show, mpu_accel_delay_store); +static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, + mpu_accel_enable_show, mpu_accel_enable_store); + +static struct attribute *mpuaccel_attributes[] = { + &dev_attr_poll_delay.attr, + &dev_attr_enable.attr, + NULL +}; + +static struct attribute_group mpuaccel_attribute_group = { + .attrs = mpuaccel_attributes +}; + +int mpu_accel_init(struct mldl_cfg *mldl_cfg, void *accel_handle) +{ + struct input_dev *input_data = NULL; + struct mpuaccel_data *data = NULL; + int res = 0; + + data = kzalloc(sizeof(struct mpuaccel_data), GFP_KERNEL); + if (data == NULL) { + res = -ENOMEM; + goto err; + } + + data->mldl_cfg = mldl_cfg; + data->accel_handle = accel_handle; + atomic_set(&data->enable, 0); + atomic_set(&data->poll_delay, 20); /* set 20ms to polling time */ + + mutex_init(&data->data_mutex); + + INIT_DELAYED_WORK(&data->work, mpu_accel_input_work_func); + + input_data = input_allocate_device(); + if (input_data == NULL) { + res = -ENOMEM; + pr_err( + "mpu_accel_probe: Failed to allocate input_data device\n"); + goto err; + } + + input_data->name = MPUACCEL_INPUT_NAME; + input_data->id.bustype = BUS_I2C; + + set_bit(EV_REL, input_data->evbit); + input_set_capability(input_data, EV_REL, REL_X); + input_set_capability(input_data, EV_REL, REL_Y); + input_set_capability(input_data, EV_REL, REL_Z); + + data->input_data = input_data; + + res = input_register_device(input_data); + if (res) { + pr_err( + "mpu_accel_init: Unable to register input_data device: %s\n", + input_data->name); + goto err; + } + + input_set_drvdata(input_data, data); + mldl_cfg->ext.mpuacc_data = (void *)data; + + pThisData = data; + + res = sysfs_create_group(&input_data->dev.kobj, + &mpuaccel_attribute_group); + if (res) { + pr_err( + "mpu_accel_init: sysfs_create_group failed[%s]\n", + input_data->name); + goto err; + } + + return res; + +err: + sysfs_remove_group(&input_data->dev.kobj, &mpuaccel_attribute_group); + input_free_device(input_data); + kfree(data); + return res; + +} + +int mpu_accel_exit(struct mldl_cfg *mldl_cfg) +{ + struct mpuaccel_data *data = NULL; + + if (mldl_cfg == NULL) + return ML_ERROR; + + data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data; + + if (data != NULL) { + sysfs_remove_group(&(data->input_data->dev.kobj), + &mpuaccel_attribute_group); + input_free_device(data->input_data); + + kfree(data); + data = NULL; + + mldl_cfg->ext.mpuacc_data = NULL; + } + + return ML_SUCCESS; +} + +int mpu_accel_suspend(struct mldl_cfg *mldl_cfg) +{ + int result = ML_SUCCESS; + int enable = 0; + struct mpuaccel_data *data = NULL; + + if (mldl_cfg == NULL) + return ML_ERROR; + + data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data; + + mpu_accel_mutex_lock(data); + enable = atomic_read(&data->enable); + + pr_info("%s: device_is_on = %d, enable = %d\n", + __func__, data->device_is_on, enable); + + if (data->device_is_on == 1 && enable == 0) { + pr_info("%s: mpu_accel_activate_device 0\n", __func__); + result = mpu_accel_activate_device(data, 0); + } + + mpu_accel_mutex_unlock(data); + + return result; +} + +int mpu_accel_resume(struct mldl_cfg *mldl_cfg) +{ + int result = ML_SUCCESS; + int enable = 0; + struct mpuaccel_data *data = NULL; + + if (mldl_cfg == NULL) + return ML_ERROR; + + data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data; + + mpu_accel_mutex_lock(data); + enable = atomic_read(&data->enable); + + pr_info("%s: device_is_on = %d, enable = %d\n", + __func__, data->device_is_on, enable); + + if (data->device_is_on == 0 && enable == 0) { + pr_info("%s: mpu_accel_activate_device 1\n", __func__); + result = mpu_accel_activate_device(data, 1); + } + + mpu_accel_mutex_unlock(data); + + return result; +} + +int mpu_accel_read(struct mldl_cfg *mldl_cfg, unsigned char *buffer) +{ + int result = ML_SUCCESS; + int enable = 0; + struct mpuaccel_data *data = NULL; + + if (mldl_cfg == NULL) + return ML_ERROR; + + data = (struct mpuaccel_data *)mldl_cfg->ext.mpuacc_data; + + mpu_accel_mutex_lock(data); + enable = atomic_read(&data->enable); +#ifdef MPUACC_USES_CACHED_DATA + if (enable == 1) + memcpy(buffer, data->cached_data, sizeof(unsigned char) * 6); + else +#endif /* MPUACC_USES_CACHED_DATA */ + result = mpu_accel_get_data_from_device(data, buffer); + mpu_accel_mutex_unlock(data); + + return result; +} diff --git a/drivers/misc/mpu3050/mpu-accel.h b/drivers/misc/mpu3050/mpu-accel.h new file mode 100755 index 00000000000..5dd57c92bd4 --- /dev/null +++ b/drivers/misc/mpu3050/mpu-accel.h @@ -0,0 +1,8 @@ +#undef MPUACC_USES_CACHED_DATA +#define MPUACC_USES_MOUNTING_MATRIX + +int mpu_accel_init(struct mldl_cfg *mldl_cfg, void *accel_handle); +int mpu_accel_exit(struct mldl_cfg *mldl_cfg); +int mpu_accel_suspend(struct mldl_cfg *mldl_cfg); +int mpu_accel_resume(struct mldl_cfg *mldl_cfg); +int mpu_accel_read(struct mldl_cfg *mldl_cfg, unsigned char *buffer); diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c new file mode 100755 index 00000000000..ef04ed7a96c --- /dev/null +++ b/drivers/misc/mpu3050/mpu-dev.c @@ -0,0 +1,2280 @@ +/* + mpu-dev.c - mpu3050 char device interface + + Copyright (C) 1995-97 Simon G. Vogl + Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl> + Copyright (C) 2003 Greg Kroah-Hartman <greg@kroah.com> + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. +*/ +/* Code inside mpudev_ioctl_rdrw is copied from i2c-dev.c + */ + +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/irq.h> +#include <linux/gpio.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/slab.h> +#include <linux/version.h> +#include <linux/pm.h> + +#ifdef CONFIG_HAS_EARLYSUSPEND +#include <linux/earlysuspend.h> +#endif + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> + +#include "mpuirq.h" +#include "slaveirq.h" +#include "mlsl.h" +#include "mlos.h" +#include "mpu-i2c.h" +#include "mldl_cfg.h" +#include "mpu-accel.h" + +#include "mpu.h" + +#define ACCEL_VENDOR_NAME "KIONIX" +#define ACCEL_CHIP_NAME "KXTF9" + +#define GYRO_VENDOR_NAME "INVENSENSE" +#define GYRO_CHIP_NAME "MPU-3050" + +#define MPU3050_EARLY_SUSPEND_IN_DRIVER 1 + +#define CALIBRATION_FILE_PATH "/efs/calibration_data" +#define CALIBRATION_DATA_AMOUNT 100 + +struct acc_data cal_data; + +/* Platform data for the MPU */ +struct mpu_private_data { + struct mldl_cfg mldl_cfg; + +#ifdef CONFIG_HAS_EARLYSUSPEND + struct early_suspend early_suspend; +#endif +}; + +static int is_lis3dh; + +#define IDEAL_X 0 +#define IDEAL_Y 0 +#define IDEAL_Z 1024 + +static int pid; + +static struct i2c_client *this_client; + +int read_accel_raw_xyz(struct acc_data *acc) +{ + unsigned char acc_data[6]; + s32 temp; + struct mldl_cfg *mldl_cfg; + + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + + if (!mpu) { + pr_info("%s : mpu data is NULL, mpu3050 Init error", __func__); + return 0; + } + + mldl_cfg = &mpu->mldl_cfg; + + if (mldl_cfg->accel_is_suspended == 1 || + (mldl_cfg->dmp_is_running == 0 + && mldl_cfg->accel_is_suspended == 0)) { + if (is_lis3dh) { + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x67); + MLOSSleep(1); + } + sensor_i2c_read(this_client->adapter, + 0x19, 0x28 | 0x80, 6, acc_data); + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x18); + MLOSSleep(1); + } + } else + sensor_i2c_read(this_client->adapter, + 0x0F, 0x06, 6, acc_data); + } else if (mldl_cfg->dmp_is_running && + mldl_cfg->accel_is_suspended == 0) { + if (sensor_i2c_read(this_client->adapter, + DEFAULT_MPU_SLAVEADDR, + 0x23, 6, acc_data) != 0) + return -1; + } else + return -1; + + if (is_lis3dh) { + acc->x = ((acc_data[0] << 8) | acc_data[1]); + acc->x = (acc->x >> 4); + acc->y = ((acc_data[2] << 8) | acc_data[3]); + acc->y = (acc->y >> 4); + acc->z = ((acc_data[4] << 8) | acc_data[5]); + acc->z = (acc->z >> 4); + } else { + temp = ((acc_data[1] << 4) | (acc_data[0] >> 4)); + if (temp < 2048) + acc->x = (s16) (-temp); + else + acc->x = (s16) (4096 - temp); + + temp = ((acc_data[3] << 4) | (acc_data[2] >> 4)); + if (temp < 2048) + acc->y = (s16) (-temp); + else + acc->y = (s16) (4096 - temp); + + temp = ((acc_data[5] << 4) | (acc_data[4] >> 4)); + if (temp < 2048) + acc->z = (s16) (1024 - temp); + else + acc->z = (s16) (3072 - temp); + } + return 0; +} + +static int accel_open_calibration(void) +{ + struct file *cal_filp = NULL; + int err = 0; + mm_segment_t old_fs; + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666); + if (IS_ERR(cal_filp)) { + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cal_filp); + + cal_data.x = 0; + cal_data.y = 0; + cal_data.z = 0; + + return err; + } + + err = cal_filp->f_op->read(cal_filp, + (char *)&cal_data, 3 * sizeof(s16), + &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't read the cal data from file\n", __func__); + err = -EIO; + } + + pr_info("%s : (%u,%u,%u)\n", __func__, + cal_data.x, cal_data.y, cal_data.z); + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int accel_do_calibrate(bool do_calib) +{ + struct acc_data data = { 0, }; + struct file *cal_filp = NULL; + int sum[3] = { 0, }; + int err = 0; + int i; + mm_segment_t old_fs; + + if (do_calib) { + for (i = 0; i < CALIBRATION_DATA_AMOUNT; i++) { + err = read_accel_raw_xyz(&data); + if (err < 0) { + pr_err("%s: accel_read_accel_raw_xyz() " + "failed in the %dth loop\n", + __func__, i); + return err; + } + + sum[0] += data.x; + sum[1] += data.y; + sum[2] += data.z; + } + + + if (is_lis3dh) { + cal_data.x = IDEAL_X - cal_data.x; + cal_data.y = IDEAL_Y - cal_data.y; + cal_data.z = IDEAL_Z - cal_data.z; + } else { + cal_data.x = sum[0] / CALIBRATION_DATA_AMOUNT; + cal_data.y = sum[1] / CALIBRATION_DATA_AMOUNT; + cal_data.z = sum[2] / CALIBRATION_DATA_AMOUNT; + } + } else { + cal_data.x = 0; + cal_data.y = 0; + cal_data.z = 0; + } + + pr_info("%s: cal data (%d,%d,%d)\n", __func__, + cal_data.x, cal_data.y, cal_data.z); + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, + O_CREAT | O_TRUNC | O_WRONLY, 0666); + if (IS_ERR(cal_filp)) { + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cal_filp); + return err; + } + + err = cal_filp->f_op->write(cal_filp, + (char *)&cal_data, 3 * sizeof(s16), + &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't write the cal data to file\n", __func__); + err = -EIO; + } + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int mpu_open(struct inode *inode, struct file *file) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + + accel_open_calibration(); + + pr_info("%s", __func__); + dev_dbg(&this_client->adapter->dev, "mpu_open\n"); + dev_dbg(&this_client->adapter->dev, "current->pid %d\n", current->pid); + pid = current->pid; + file->private_data = this_client; + + /* we could do some checking on the flags supplied by "open" */ + /* i.e. O_NONBLOCK */ + /* -> set some flag to disable interruptible_sleep_on in mpu_read */ + + /* Reset the sensors to the default */ + mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO; + if (mldl_cfg->accel && mldl_cfg->accel->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL; + + if (mldl_cfg->compass && mldl_cfg->compass->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS; + + if (mldl_cfg->pressure && mldl_cfg->pressure->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE; + + 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 i2c_client *client = (struct i2c_client *)file->private_data; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + int result = 0; + + pid = 0; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + result = mpu3050_suspend(mldl_cfg, client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); + pr_info("%s", __func__); + dev_dbg(&this_client->adapter->dev, "mpu_release\n"); + return result; +} + +static noinline int mpudev_ioctl_rdrw(struct i2c_client *client, + unsigned long arg) +{ + struct i2c_rdwr_ioctl_data rdwr_arg; + struct i2c_msg *rdwr_pa; + u8 __user **data_ptrs; + int i, res; + + if (copy_from_user(&rdwr_arg, + (struct i2c_rdwr_ioctl_data __user *)arg, + sizeof(rdwr_arg))) + return -EFAULT; + + /* Put an arbitrary limit on the number of messages that can + * be sent at once */ + if (rdwr_arg.nmsgs > I2C_RDRW_IOCTL_MAX_MSGS) + return -EINVAL; + + rdwr_pa = (struct i2c_msg *) + kmalloc(rdwr_arg.nmsgs * sizeof(struct i2c_msg), GFP_KERNEL); + if (!rdwr_pa) + return -ENOMEM; + + if (copy_from_user(rdwr_pa, rdwr_arg.msgs, + rdwr_arg.nmsgs * sizeof(struct i2c_msg))) { + kfree(rdwr_pa); + return -EFAULT; + } + + data_ptrs = kmalloc(rdwr_arg.nmsgs * sizeof(u8 __user *), GFP_KERNEL); + if (data_ptrs == NULL) { + kfree(rdwr_pa); + return -ENOMEM; + } + + res = 0; + for (i = 0; i < rdwr_arg.nmsgs; i++) { + /* Limit the size of the message to a sane amount; + * and don't let length change either. */ + if ((rdwr_pa[i].len > 8192) || + (rdwr_pa[i].flags & I2C_M_RECV_LEN)) { + res = -EINVAL; + break; + } + data_ptrs[i] = (u8 __user *) rdwr_pa[i].buf; + rdwr_pa[i].buf = kmalloc(rdwr_pa[i].len, GFP_KERNEL); + if (rdwr_pa[i].buf == NULL) { + res = -ENOMEM; + break; + } + if (copy_from_user(rdwr_pa[i].buf, data_ptrs[i], + rdwr_pa[i].len)) { + ++i; /* Needs to be kfreed too */ + res = -EFAULT; + break; + } + } + if (res < 0) { + int j; + for (j = 0; j < i; ++j) + kfree(rdwr_pa[j].buf); + kfree(data_ptrs); + kfree(rdwr_pa); + return res; + } + + res = i2c_transfer(client->adapter, rdwr_pa, rdwr_arg.nmsgs); + while (i-- > 0) { + if (res >= 0 && (rdwr_pa[i].flags & I2C_M_RD)) { + if (copy_to_user(data_ptrs[i], rdwr_pa[i].buf, + rdwr_pa[i].len)) + res = -EFAULT; + } + kfree(rdwr_pa[i].buf); + } + kfree(data_ptrs); + kfree(rdwr_pa); + return res; +} + +/* 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) +{ + char *tmp; + int ret; + + struct i2c_client *client = (struct i2c_client *)file->private_data; + + if (count > 8192) + count = 8192; + + tmp = kmalloc(count, GFP_KERNEL); + if (tmp == NULL) + return -ENOMEM; + + pr_info("%s: i2c-dev: i2c-%d reading %zu bytes.\n", __func__, + iminor(file->f_path.dentry->d_inode), count); + +/* @todo fix this to do a i2c trasnfer from the FIFO */ + ret = i2c_master_recv(client, tmp, count); + if (ret >= 0) { + ret = copy_to_user(buf, tmp, count) ? -EFAULT : ret; + if (ret) + ret = -EFAULT; + } + kfree(tmp); + return ret; +} + +static int mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg) +{ + int ii; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mpu3050_platform_data *pdata = mpu->mldl_cfg.pdata; + struct mpu3050_platform_data local_pdata; + + if (copy_from_user(&local_pdata, (unsigned char __user *)arg, + sizeof(local_pdata))) + return -EFAULT; + + pdata->int_config = local_pdata.int_config; + for (ii = 0; ii < DIM(pdata->orientation); ii++) + pdata->orientation[ii] = local_pdata.orientation[ii]; + pdata->level_shifter = local_pdata.level_shifter; + + pdata->accel.address = local_pdata.accel.address; + for (ii = 0; ii < DIM(pdata->accel.orientation); ii++) + pdata->accel.orientation[ii] = + local_pdata.accel.orientation[ii]; + + pdata->compass.address = local_pdata.compass.address; + for (ii = 0; ii < DIM(pdata->compass.orientation); ii++) + pdata->compass.orientation[ii] = + local_pdata.compass.orientation[ii]; + + pdata->pressure.address = local_pdata.pressure.address; + for (ii = 0; ii < DIM(pdata->pressure.orientation); ii++) + pdata->pressure.orientation[ii] = + local_pdata.pressure.orientation[ii]; + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + + return ML_SUCCESS; +} + +static int +mpu_ioctl_set_mpu_config(struct i2c_client *client, unsigned long arg) +{ + int ii; + int result = ML_SUCCESS; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mldl_cfg *temp_mldl_cfg; + + dev_dbg(&this_client->adapter->dev, "%s\n", __func__); + + temp_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL); + if (NULL == temp_mldl_cfg) + return -ENOMEM; + + /* + * User space is not allowed to modify accel compass pressure or + * pdata structs, as well as silicon_revision product_id or trim + */ + if (copy_from_user(temp_mldl_cfg, (struct mldl_cfg __user *)arg, + offsetof(struct mldl_cfg, silicon_revision))) { + result = -EFAULT; + goto out; + } + + if (mldl_cfg->gyro_is_suspended) { + if (mldl_cfg->addr != temp_mldl_cfg->addr) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->int_config != temp_mldl_cfg->int_config) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->ext_sync != temp_mldl_cfg->ext_sync) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->full_scale != temp_mldl_cfg->full_scale) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->lpf != temp_mldl_cfg->lpf) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->clk_src != temp_mldl_cfg->clk_src) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->divider != temp_mldl_cfg->divider) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_enable != temp_mldl_cfg->dmp_enable) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->fifo_enable != temp_mldl_cfg->fifo_enable) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_cfg1 != temp_mldl_cfg->dmp_cfg1) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_cfg2 != temp_mldl_cfg->dmp_cfg2) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->gyro_power != temp_mldl_cfg->gyro_power) + mldl_cfg->gyro_needs_reset = TRUE; + + for (ii = 0; ii < MPU_NUM_AXES; ii++) + if (mldl_cfg->offset_tc[ii] != + temp_mldl_cfg->offset_tc[ii]) + mldl_cfg->gyro_needs_reset = TRUE; + + for (ii = 0; ii < MPU_NUM_AXES; ii++) + if (mldl_cfg->offset[ii] != temp_mldl_cfg->offset[ii]) + mldl_cfg->gyro_needs_reset = TRUE; + + if (memcmp(mldl_cfg->ram, temp_mldl_cfg->ram, + MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE * + sizeof(unsigned char))) + mldl_cfg->gyro_needs_reset = TRUE; + } + + memcpy(mldl_cfg, temp_mldl_cfg, + offsetof(struct mldl_cfg, silicon_revision)); + +out: + kfree(temp_mldl_cfg); + return result; +} + +static int +mpu_ioctl_get_mpu_config(struct i2c_client *client, unsigned long arg) +{ + /* Have to be careful as there are 3 pointers in the mldl_cfg + * structure */ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mldl_cfg *local_mldl_cfg; + int retval = 0; + + local_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL); + if (NULL == local_mldl_cfg) + return -ENOMEM; + + retval = + copy_from_user(local_mldl_cfg, (struct mldl_cfg __user *)arg, + sizeof(struct mldl_cfg)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on arg\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + + /* Fill in the accel, compass, pressure and pdata pointers */ + if (mldl_cfg->accel) { + retval = copy_to_user((void __user *)local_mldl_cfg->accel, + mldl_cfg->accel, + sizeof(*mldl_cfg->accel)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on accel\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->compass) { + retval = copy_to_user((void __user *)local_mldl_cfg->compass, + mldl_cfg->compass, + sizeof(*mldl_cfg->compass)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on compass\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->pressure) { + retval = copy_to_user((void __user *)local_mldl_cfg->pressure, + mldl_cfg->pressure, + sizeof(*mldl_cfg->pressure)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on pressure\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->pdata) { + retval = copy_to_user((void __user *)local_mldl_cfg->pdata, + mldl_cfg->pdata, + sizeof(*mldl_cfg->pdata)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on pdata\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + /* Do not modify the accel, compass, pressure and pdata pointers */ + retval = copy_to_user((struct mldl_cfg __user *)arg, + mldl_cfg, offsetof(struct mldl_cfg, accel)); + + if (retval) + retval = -EFAULT; +out: + kfree(local_mldl_cfg); + return retval; +} + +/** + * Pass a requested slave configuration to the slave sensor + * + * @param adapter the adaptor to use to communicate with the slave + * @param mldl_cfg the mldl configuration structuer + * @param slave pointer to the slave descriptor + * @param usr_config The configuration to pass to the slave sensor + * + * @return 0 or non-zero error code + */ +static int slave_config(void *adapter, + struct mldl_cfg *mldl_cfg, + struct ext_slave_descr *slave, + struct ext_slave_config __user *usr_config) +{ + int retval = ML_SUCCESS; + if ((slave) && (slave->config)) { + struct ext_slave_config config; + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + if (config.len && config.data) { + int *data; + data = kzalloc(config.len, GFP_KERNEL); + if (!data) + return ML_ERROR_MEMORY_EXAUSTED; + + retval = copy_from_user(data, + (void __user *)config.data, + config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = slave->config(adapter, + slave, &mldl_cfg->pdata->accel, &config); + kfree(config.data); + } + return retval; +} + +/** + * Get a requested slave configuration from the slave sensor + * + * @param adapter the adaptor to use to communicate with the slave + * @param mldl_cfg the mldl configuration structuer + * @param slave pointer to the slave descriptor + * @param usr_config The configuration for the slave to fill out + * + * @return 0 or non-zero error code + */ +static int slave_get_config(void *adapter, + struct mldl_cfg *mldl_cfg, + struct ext_slave_descr *slave, + struct ext_slave_config __user *usr_config) +{ + int retval = ML_SUCCESS; + if ((slave) && (slave->get_config)) { + 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) { + int *data; + data = kzalloc(config.len, GFP_KERNEL); + if (!data) + return ML_ERROR_MEMORY_EXAUSTED; + + retval = copy_from_user(data, + (void __user *)config.data, + config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = slave->get_config(adapter, + slave, + &mldl_cfg->pdata->accel, &config); + 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; +} + +/* ioctl - I/O control */ +static long mpu_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + struct i2c_client *client = (struct i2c_client *)file->private_data; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + int retval = 0; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + switch (cmd) { + case I2C_RDWR: + mpudev_ioctl_rdrw(client, arg); + break; + case I2C_SLAVE: + if ((arg & 0x7E) != (client->addr & 0x7E)) { + dev_err(&this_client->adapter->dev, + "%s: Invalid I2C_SLAVE arg %lu\n", + __func__, arg); + } + break; + case MPU_SET_MPU_CONFIG: + retval = mpu_ioctl_set_mpu_config(client, arg); + break; + case MPU_SET_INT_CONFIG: + mldl_cfg->int_config = (unsigned char)arg; + break; + case MPU_SET_EXT_SYNC: + mldl_cfg->ext_sync = (enum mpu_ext_sync)arg; + break; + case MPU_SET_FULL_SCALE: + mldl_cfg->full_scale = (enum mpu_fullscale)arg; + break; + case MPU_SET_LPF: + mldl_cfg->lpf = (enum mpu_filter)arg; + break; + case MPU_SET_CLK_SRC: + mldl_cfg->clk_src = (enum mpu_clock_sel)arg; + break; + case MPU_SET_DIVIDER: + mldl_cfg->divider = (unsigned char)arg; + break; + case MPU_SET_LEVEL_SHIFTER: + mldl_cfg->pdata->level_shifter = (unsigned char)arg; + break; + case MPU_SET_DMP_ENABLE: + mldl_cfg->dmp_enable = (unsigned char)arg; + break; + case MPU_SET_FIFO_ENABLE: + mldl_cfg->fifo_enable = (unsigned char)arg; + break; + case MPU_SET_DMP_CFG1: + mldl_cfg->dmp_cfg1 = (unsigned char)arg; + break; + case MPU_SET_DMP_CFG2: + mldl_cfg->dmp_cfg2 = (unsigned char)arg; + break; + case MPU_SET_OFFSET_TC: + retval = copy_from_user(mldl_cfg->offset_tc, + (unsigned char __user *)arg, + sizeof(mldl_cfg->offset_tc)); + if (retval) + retval = -EFAULT; + break; + case MPU_SET_RAM: + retval = copy_from_user(mldl_cfg->ram, + (unsigned char __user *)arg, + sizeof(mldl_cfg->ram)); + if (retval) + retval = -EFAULT; + break; + case MPU_SET_PLATFORM_DATA: + retval = mpu_ioctl_set_mpu_pdata(client, arg); + break; + case MPU_GET_MPU_CONFIG: + retval = mpu_ioctl_get_mpu_config(client, arg); + break; + case MPU_GET_INT_CONFIG: + retval = put_user(mldl_cfg->int_config, + (unsigned char __user *)arg); + break; + case MPU_GET_EXT_SYNC: + retval = put_user(mldl_cfg->ext_sync, + (unsigned char __user *)arg); + break; + case MPU_GET_FULL_SCALE: + retval = put_user(mldl_cfg->full_scale, + (unsigned char __user *)arg); + break; + case MPU_GET_LPF: + retval = put_user(mldl_cfg->lpf, (unsigned char __user *)arg); + break; + case MPU_GET_CLK_SRC: + retval = put_user(mldl_cfg->clk_src, + (unsigned char __user *)arg); + break; + case MPU_GET_DIVIDER: + retval = put_user(mldl_cfg->divider, + (unsigned char __user *)arg); + break; + case MPU_GET_LEVEL_SHIFTER: + retval = put_user(mldl_cfg->pdata->level_shifter, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_ENABLE: + retval = put_user(mldl_cfg->dmp_enable, + (unsigned char __user *)arg); + break; + case MPU_GET_FIFO_ENABLE: + retval = put_user(mldl_cfg->fifo_enable, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_CFG1: + retval = put_user(mldl_cfg->dmp_cfg1, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_CFG2: + retval = put_user(mldl_cfg->dmp_cfg2, + (unsigned char __user *)arg); + break; + case MPU_GET_OFFSET_TC: + retval = copy_to_user((unsigned char __user *)arg, + mldl_cfg->offset_tc, + sizeof(mldl_cfg->offset_tc)); + if (retval) + retval = -EFAULT; + break; + case MPU_GET_RAM: + retval = copy_to_user((unsigned char __user *)arg, + mldl_cfg->ram, sizeof(mldl_cfg->ram)); + if (retval) + retval = -EFAULT; + break; + case MPU_CONFIG_ACCEL: + retval = slave_config(accel_adapter, mldl_cfg, + mldl_cfg->accel, + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_COMPASS: + retval = slave_config(compass_adapter, mldl_cfg, + mldl_cfg->compass, + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_PRESSURE: + retval = slave_config(pressure_adapter, mldl_cfg, + mldl_cfg->pressure, + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_ACCEL: + retval = slave_get_config(accel_adapter, mldl_cfg, + mldl_cfg->accel, + (struct ext_slave_config __user *) + arg); + break; + case MPU_GET_CONFIG_COMPASS: + retval = slave_get_config(compass_adapter, mldl_cfg, + mldl_cfg->compass, + (struct ext_slave_config __user *) + arg); + break; + case MPU_GET_CONFIG_PRESSURE: + retval = slave_get_config(pressure_adapter, mldl_cfg, + mldl_cfg->pressure, + (struct ext_slave_config __user *) + arg); + break; + case MPU_SUSPEND: + { + unsigned long sensors; + sensors = ~(mldl_cfg->requested_sensors); + retval = mpu3050_suspend(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + ((sensors & ML_THREE_AXIS_GYRO) + == ML_THREE_AXIS_GYRO), + ((sensors & + ML_THREE_AXIS_ACCEL) + == ML_THREE_AXIS_ACCEL), + ((sensors & + ML_THREE_AXIS_COMPASS) + == ML_THREE_AXIS_COMPASS), + ((sensors & + ML_THREE_AXIS_PRESSURE) + == ML_THREE_AXIS_PRESSURE)); + } + break; + case MPU_RESUME: + { + unsigned long sensors; + sensors = mldl_cfg->requested_sensors; + retval = mpu3050_resume(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & + ML_THREE_AXIS_PRESSURE); + } + break; + case MPU_READ_ACCEL: + { + unsigned char data[6]; + retval = mpu3050_read_accel(mldl_cfg, client->adapter, + data); + + if ((ML_SUCCESS == retval) && + (copy_to_user((unsigned char __user *)arg, + data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_COMPASS: + { + unsigned char data[6]; + struct i2c_adapter *compass_adapt = + i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + retval = mpu3050_read_compass(mldl_cfg, compass_adapt, + data); + if ((ML_SUCCESS == retval) && + (copy_to_user((unsigned char *)arg, + data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_PRESSURE: + { + unsigned char data[3]; + struct i2c_adapter *pressure_adapt = + i2c_get_adapter(mldl_cfg->pdata->pressure. + adapt_num); + retval = + mpu3050_read_pressure(mldl_cfg, pressure_adapt, + data); + if ((ML_SUCCESS == retval) + && + (copy_to_user + ((unsigned char __user *)arg, data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_MEMORY: + case MPU_WRITE_MEMORY: + default: + dev_err(&this_client->adapter->dev, + "%s: Unknown cmd %d, arg %lu\n", __func__, cmd, arg); + retval = -EINVAL; + } + + return retval; +} + +#ifdef CONFIG_HAS_EARLYSUSPEND +void mpu3050_early_suspend(struct early_suspend *h) +{ + struct mpu_private_data *mpu = container_of(h, + struct mpu_private_data, + early_suspend); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + dev_dbg(&this_client->adapter->dev, "%s: %d, %d\n", __func__, + h->level, mpu->mldl_cfg.gyro_is_suspended); + if (MPU3050_EARLY_SUSPEND_IN_DRIVER) + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); +} + +void mpu3050_early_resume(struct early_suspend *h) +{ + struct mpu_private_data *mpu = container_of(h, + struct mpu_private_data, + early_suspend); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + if (MPU3050_EARLY_SUSPEND_IN_DRIVER) { + if (pid) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, + this_client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + dev_dbg(&this_client->adapter->dev, + "%s for pid %d\n", __func__, pid); + } + } + dev_dbg(&this_client->adapter->dev, "%s: %d\n", __func__, h->level); + pr_info("%s: h->level = %d\n", __func__, h->level); +} +#endif + +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 *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, pressure_adapter, + TRUE, TRUE, TRUE, TRUE); + dev_dbg(&this_client->adapter->dev, "%s\n", __func__); +} + +int mpu_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 *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + pr_info("%s", __func__); + if (!mpu->mldl_cfg.gyro_is_suspended) { + dev_dbg(&this_client->adapter->dev, + "%s: suspending on event %d\n", __func__, mesg.event); + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); + } else { + dev_dbg(&this_client->adapter->dev, + "%s: Already suspended %d\n", __func__, mesg.event); + } + return 0; +} + +int mpu_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 *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + pr_info("%s: accel_adapter = %p\n", __func__, accel_adapter); + + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + if (pid) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, this_client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + dev_dbg(&this_client->adapter->dev, + "%s for pid %d\n", __func__, pid); + } + + pr_info("%s: pid = %d\n", __func__, pid); + return 0; +} + +/* define which file operations are supported */ +static const struct file_operations mpu_fops = { + .owner = THIS_MODULE, + .read = mpu_read, +#if HAVE_COMPAT_IOCTL + .compat_ioctl = mpu_ioctl, +#endif +#if HAVE_UNLOCKED_IOCTL + .unlocked_ioctl = mpu_ioctl, +#endif + .open = mpu_open, + .release = mpu_release, +}; + +static unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +static struct miscdevice i2c_mpu_device = { + .minor = MISC_DYNAMIC_MINOR, + .name = "mpu", /* Same for both 3050 and 6000 */ + .fops = &mpu_fops, +}; + +#define FACTORY_TEST +#ifdef FACTORY_TEST + +static ssize_t mpu3050_power_on(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int count = 0; + + pr_info("%s : this_client = %d\n", __func__, (int)this_client); + count = sprintf(buf, "%d\n", (this_client != NULL ? 1 : 0)); + + return count; +} + +static int mpu3050_factory_on(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + int prev_gyro_suspended; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + prev_gyro_suspended = mldl_cfg->gyro_is_suspended; + if (prev_gyro_suspended) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + } + + return prev_gyro_suspended; +} + +static void mpu3050_factory_off(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + (void)mpu3050_suspend(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); +} + +static ssize_t mpu3050_get_temp(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int count = 0; + short int temperature = 0; + unsigned char data[2]; + int prev_gyro_suspended; + pr_info("%s", __func__); + prev_gyro_suspended = mpu3050_factory_on(this_client); + + /*MPUREG_TEMP_OUT_H, 27 0x1b */ + /*MPUREG_TEMP_OUT_L, 28 0x1c */ + /* TEMP_OUT_H/L: 16-bit temperature data (2's complement data format) */ + sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR, + MPUREG_TEMP_OUT_H, 2, data); + temperature = (short)(((data[0]) << 8) | data[1]); + temperature = (((temperature + 13200) / 280) + 35); + pr_info("%s :read temperature = %d\n", __func__, temperature); + + count = sprintf(buf, "%d\n", temperature); + + if (prev_gyro_suspended) + mpu3050_factory_off(this_client); + + return count; +} + +/* + Defines +*/ + +#define DEBUG_OUT 1 + +#define DEF_GYRO_FULLSCALE (2000) /* gyro full scale dps */ +#define DEF_GYRO_SENS (32768.f/DEF_GYRO_FULLSCALE) + /* gyro sensitivity LSB/dps */ +#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ +#define DEF_TIMING_TOL (.05f) /* 5% */ +#define DEF_BIAS_THRESH (40*DEF_GYRO_SENS) + /* 40 dps in LSBs */ +#define DEF_RMS_THRESH_SQ (0.4f*0.4f*DEF_GYRO_SENS*DEF_GYRO_SENS) + /* (.2 dps in LSBs ) ^ 2 */ +#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting + data for each axis, + multiple of 600ms */ + +/* + Macros +*/ + +#define CHECK_TEST_ERROR(x) \ + if (x) { \ + pr_info("error %d @ %s|%d\n", x, __func__, __LINE__); \ + return -1; \ + } + +#define SHORT_TO_TEMP_C(shrt) (((shrt+13200)/280)+35) +#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) +#define fabs(x) (((x) < 0) ? -(x) : (x)) + +void mpu3050_usleep(unsigned long t) +{ + unsigned long start = MLOSGetTickCount(); + while (MLOSGetTickCount() - start < t / 1000) { + } +} + +#define X (0) +#define Y (1) +#define Z (2) + +static short mpu3050_selftest_gyro_avg[3]; +static int mpu3050_selftest_result; +static int mpu3050_selftest_bias[3]; +static int mpu3050_selftest_rms[3]; + +int mpu3050_test_gyro(struct i2c_client *client, short gyro_biases[3], + short *temp_avg) +{ + void *mlsl_handle = client->adapter; + int retVal = 0; + tMLError result; + + int total_count = 0; + int total_count_axis[3] = { 0, 0, 0 }; + int packet_count; + unsigned char regs[7]; + + char a_name[3][2] = { "X", "Y", "Z" }; + int temperature; + int Avg[3]; + int RMS[3]; + int i, j, tmp; + unsigned char dataout[20]; + + short *x, *y, *z; + + x = kzalloc(sizeof(*x) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + y = kzalloc(sizeof(*y) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + z = kzalloc(sizeof(*z) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + + temperature = 0; + + /* sample rate = 8ms */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_SMPLRT_DIV, 0x07); + + if (result) + goto out_i2c_faild; + + regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ + switch (DEF_GYRO_FULLSCALE) { + case 2000: + regs[0] |= 0x18; + break; + case 1000: + regs[0] |= 0x10; + break; + case 500: + regs[0] |= 0x08; + break; + case 250: + default: + regs[0] |= 0x00; + break; + } + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_DLPF_FS_SYNC, regs[0]); + if (result) + goto out_i2c_faild; + + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_INT_CFG, 0x00); + + /* 1st, timing test */ + for (j = 0; j < 3; j++) { + + pr_info("%s :Collecting gyro data from %s gyro PLL\n", + __func__, a_name[j]); + + /* turn on all gyros, use gyro X for clocking + Set to Y and Z for 2nd and 3rd iteration */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, j + 1); + if (result) + goto out_i2c_faild; + + /* wait for 2 ms after switching clock source */ + mpu3050_usleep(2000); + + /* we will enable XYZ gyro in FIFO and nothing else */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN2, 0x00); + if (result) + goto out_i2c_faild; + /* enable/reset FIFO */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_USER_CTRL, 0x42); + + tmp = (int)(DEF_TEST_TIME_PER_AXIS / 600); + + while (tmp-- > 0) { + /* enable XYZ gyro in FIFO and nothing else */ + result = + MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x70); + if (result) + goto out_i2c_faild; + + /* wait for 600 ms for data */ + mpu3050_usleep(600000); + + /* stop storing gyro in the FIFO */ + result = + MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x00); + if (result) + goto out_i2c_faild; + + /* Getting number of bytes in FIFO */ + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_FIFO_COUNTH, 2, dataout); + if (result) + goto out_i2c_faild; + /* number of 6 B packets in the FIFO */ + packet_count = CHARS_TO_SHORT(dataout) / 6; + pr_info("%s :Packet Count: %d - ", + __func__, packet_count); + + if (abs(packet_count - DEF_PACKET_THRESH) + <= /* Within +-5% range */ + (int)(DEF_TIMING_TOL * DEF_PACKET_THRESH + .5)) { + for (i = 0; i < packet_count; i++) { + /* getting FIFO data */ + result = + MLSLSerialReadFifo(mlsl_handle, + client->addr, 6, + dataout); + if (result) + goto out_i2c_faild; + + x[total_count + i] = + CHARS_TO_SHORT(&dataout[0]); + y[total_count + i] = + CHARS_TO_SHORT(&dataout[2]); + z[total_count + i] = + CHARS_TO_SHORT(&dataout[4]); + if (DEBUG_OUT && 0) { + pr_info("%s :Gyros %-4d " \ + ": %+13d %+13d %+13d\n", + __func__, total_count + i, + x[total_count + i], + y[total_count + i], + z[total_count + i]); + } + } + total_count += packet_count; + total_count_axis[j] += packet_count; + pr_info("%s :OK\n", __func__); + } else { + retVal |= 1 << j; + pr_info("%s :NOK - samples ignored\n", + __func__); + } + } + + /* remove gyros from FIFO */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x00); + if (result) + goto out_i2c_faild; + + /* Read Temperature */ + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_TEMP_OUT_H, 2, dataout); + temperature += (short)CHARS_TO_SHORT(dataout); + } + + pr_info("%s :\nTotal %d samples\n\n", __func__, total_count); + + /* 2nd, check bias from X and Y PLL clock source */ + tmp = total_count != 0 ? total_count : 1; + for (i = 0, Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; + i < total_count; i++) { + Avg[X] += x[i]; + Avg[Y] += y[i]; + Avg[Z] += z[i]; + } + + Avg[X] /= tmp; + Avg[Y] /= tmp; + Avg[Z] /= tmp; + + pr_info("%s :bias : %+13d %+13d %+13d (LSB)\n", + __func__, Avg[X], Avg[Y], Avg[Z]); + if (DEBUG_OUT) { + pr_info("%s : : %+13d %+13d %+13d (dps)\n", + __func__, Avg[X] / 131, Avg[Y] / 131, Avg[Z] / 131); + } + for (j = 0; j < 3; j++) { + if (abs(Avg[j]) > (int)DEF_BIAS_THRESH) { + pr_info("%s :%s-Gyro bias (%.0d) exceeded threshold " + "(threshold = %f)\n", __func__, + a_name[j], Avg[j], DEF_BIAS_THRESH); + retVal |= 1 << (3 + j); + } + } + + /* 3rd and finally, check RMS */ + for (i = 0, RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; + i < total_count; i++) { + RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); + RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); + RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); + } + + for (j = 0; j < 3; j++) { + if (RMS[j] > (int)DEF_RMS_THRESH_SQ * total_count) { + pr_info + ("%s :%s-Gyro RMS (%d) exceeded threshold (%.4f)\n", + __func__, a_name[j], RMS[j] / total_count, + DEF_RMS_THRESH_SQ); + retVal |= 1 << (6 + j); + } + } + + pr_info("%s :RMS^2 : %+13d %+13d %+13d (LSB-rms)\n", + __func__, + (RMS[X] / total_count), + (RMS[Y] / total_count), (RMS[Z] / total_count)); + if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { + /*If any of the RMS noise value returns zero, + then we might have dead gyro or FIFO/register failure, + or the part is sleeping */ + retVal |= 1 << 9; + } + + temperature /= 3; + if (DEBUG_OUT) + pr_info("%s :Temperature : %+13d %13s %13s (deg. C)\n", + __func__, SHORT_TO_TEMP_C(temperature), "", ""); + + /* load into final storage */ + *temp_avg = (short)temperature; + gyro_biases[X] = (short)Avg[X]; + gyro_biases[Y] = (short)Avg[Y]; + gyro_biases[Z] = (short)Avg[Z]; + + mpu3050_selftest_bias[X] = (int)Avg[X]; + mpu3050_selftest_bias[Y] = (int)Avg[Y]; + mpu3050_selftest_bias[Z] = (int)Avg[Z]; + + mpu3050_selftest_rms[X] = RMS[X] / total_count; + mpu3050_selftest_rms[Y] = RMS[Y] / total_count; + mpu3050_selftest_rms[Z] = RMS[Z] / total_count; + +out_i2c_faild: + if (result) + pr_info("%s : error %d", __func__, result); + + kfree(x); + kfree(y); + kfree(z); + + return retVal; +} + +int mpu3050_self_test_once(struct i2c_client *client) +{ + void *mlsl_handle = client->adapter; + int result = 0; + + short temp_avg; + + unsigned char regs[5]; + unsigned long testStart = MLOSGetTickCount(); + + pr_info("%s :Collecting %d groups of 600 ms samples for each axis\n\n", + __func__, DEF_TEST_TIME_PER_AXIS / 600); + + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_PWR_MGM, 1, regs); + CHECK_TEST_ERROR(result); + /* reset */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, regs[0] | 0x80); + CHECK_TEST_ERROR(result); + MLOSSleep(5); + /* wake up */ + if (regs[0] & 0x40) { + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, 0x00); + CHECK_TEST_ERROR(result); + } + MLOSSleep(60); + + /* collect gyro and temperature data */ + mpu3050_selftest_result = mpu3050_test_gyro(client, + mpu3050_selftest_gyro_avg, + &temp_avg); + + pr_info("%s :\nTest time : %ld ms\n", + __func__, MLOSGetTickCount() - testStart); + + return mpu3050_selftest_result; +} + +static ssize_t mpu3050_self_test(struct device *dev, + struct device_attribute *attr, char *buf) +{ + unsigned char gyro_data[6]; + short int raw[3]; + int count = 0; + int res = 0; + int prev_gyro_suspended; + +/*MPUREG_GYRO_XOUT_H, 29 0x1d */ +/*MPUREG_GYRO_XOUT_L, 30 0x1e */ +/*MPUREG_GYRO_YOUT_H, 31 0x1f */ +/*MPUREG_GYRO_YOUT_L, 32 0x20 */ +/*MPUREG_GYRO_ZOUT_H, 33 0x21 */ +/*MPUREG_GYRO_ZOUT_L, 34 0x22 */ + +/* GYRO_XOUT_H/L: 16-bit X gyro output data (2's complement data format) */ +/* GYRO_YOUT_H/L: 16-bit Y gyro output data (2's complement data format) */ +/* GYRO_ZOUT_H/L: 16-bit Z gyro output data (2's complement data format) */ + + prev_gyro_suspended = mpu3050_factory_on(this_client); + + mpu3050_self_test_once(this_client); + + res = sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR, + MPUREG_GYRO_XOUT_H, 6, gyro_data); + + if (res) + return 0; + + raw[0] = (short)(((gyro_data[0]) << 8) | gyro_data[1]); + raw[1] = (short)(((gyro_data[2]) << 8) | gyro_data[3]); + raw[2] = (short)(((gyro_data[4]) << 8) | gyro_data[5]); + + pr_info("%s: %s %s, %d, %d, %d, %d, %d, %d\n", __func__, buf, + (!mpu3050_selftest_result ? "OK" : "NG"), + raw[0], raw[1], raw[2], + mpu3050_selftest_gyro_avg[0], + mpu3050_selftest_gyro_avg[1], mpu3050_selftest_gyro_avg[2]); + + count = sprintf(buf, "%s, %d, %d, %d, %d, %d, %d\n", + (!mpu3050_selftest_result ? "OK" : "NG"), + mpu3050_selftest_bias[0], mpu3050_selftest_bias[1], + mpu3050_selftest_bias[2], + mpu3050_selftest_rms[0], + mpu3050_selftest_rms[1], mpu3050_selftest_rms[2]); + + if (prev_gyro_suspended) + mpu3050_factory_off(this_client); + + return count; +} + +static ssize_t mpu3050_acc_read(struct device *dev, + struct device_attribute *attr, char *buf) +{ + unsigned char acc_data[6]; + s16 x, y, z; + s32 temp; + + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + + if (mldl_cfg->accel_is_suspended == 1 || + (mldl_cfg->dmp_is_running == 0 + && mldl_cfg->accel_is_suspended == 0)) { + if (is_lis3dh) { + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x67); + MLOSSleep(1); + } + sensor_i2c_read(this_client->adapter, + 0x19, 0x28 | 0x80, 6, acc_data); + + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x18); + MLOSSleep(1); + } + } else + sensor_i2c_read(this_client->adapter, + 0x0F, 0x06, 6, acc_data); + } else if (mldl_cfg->dmp_is_running && + mldl_cfg->accel_is_suspended == 0) { + sensor_i2c_read(this_client->adapter, + DEFAULT_MPU_SLAVEADDR, 0x23, 6, acc_data); + } + + if (is_lis3dh) { + x = ((acc_data[0] << 8) | acc_data[1]); + x = (x >> 4) + cal_data.x; + y = ((acc_data[2] << 8) | acc_data[3]); + y = (y >> 4) + cal_data.y; + z = ((acc_data[4] << 8) | acc_data[5]); + z = (z >> 4) + cal_data.z; + } else { + temp = (s16) ((acc_data[1] << 4) | (acc_data[0] >> 4)) + + cal_data.x; + if (temp < 2048) + x = (s16) (temp); + else + x = (s16) ((4096 - temp)) * (-1); + + temp = (s16) ((acc_data[3] << 4) | (acc_data[2] >> 4)) + + cal_data.y; + if (temp < 2048) + y = (s16) (temp); + else + y = (s16) ((4096 - temp)) * (-1); + + temp = (s16) ((acc_data[5] << 4) | (acc_data[4] >> 4)) + + cal_data.z; + if (temp < 2048) + z = (s16) (temp); + else + z = (s16) ((4096 - temp)) * (-1); + } + +#if defined(CONFIG_MACH_P8) + /* x *= (-1); */ + /* y *= (-1); */ + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#elif defined(CONFIG_MACH_P8LTE) + x *= (-1); + /* y *= (-1); */ + /* z *= (-1); */ + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#elif defined(CONFIG_MACH_P2) + /* x *= (-1); */ + /* y *= (-1); */ + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#else + x *= (-1); + y *= (-1); + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#endif + +} + +static ssize_t accel_calibration_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int err; + + err = accel_open_calibration(); + if (err < 0) + pr_err("%s: accel_open_calibration() failed\n", __func__); + + pr_info("accel_calibration_show :%d %d %d\n", + cal_data.x, cal_data.y, cal_data.z); + + if (err < 0) + err = 0; + else + err = 1; + + if (cal_data.x == 0 && cal_data.y == 0 && cal_data.z == 0) + err = 0; + + return sprintf(buf, "%d %d %d %d\n", + err, cal_data.x, cal_data.y, cal_data.z); +} + +static ssize_t accel_calibration_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + bool do_calib; + int err; + int count = 0; + char str[11]; + + if (sysfs_streq(buf, "1")) + do_calib = true; + else if (sysfs_streq(buf, "0")) + do_calib = false; + else { + pr_debug("%s: invalid value %d\n", __func__, *buf); + return -EINVAL; + } + + err = accel_do_calibrate(do_calib); + if (err < 0) + pr_err("%s: accel_do_calibrate() failed\n", __func__); + + pr_info("accel_calibration_show :%d %d %d\n", + cal_data.x, cal_data.y, cal_data.z); + if (err > 0) + err = 0; + count = sprintf(str, "%d\n", err); + + strcpy(str, buf); + return count; +} + +static ssize_t get_accel_vendor_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", ACCEL_VENDOR_NAME); +} + +static ssize_t get_accel_chip_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", ACCEL_CHIP_NAME); +} + +static ssize_t get_gyro_vendor_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", GYRO_VENDOR_NAME); +} + +static ssize_t get_gyro_chip_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", GYRO_CHIP_NAME); +} + +static struct device_attribute dev_attr_accel_vendor = + __ATTR(vendor, S_IRUGO, get_accel_vendor_name, NULL); +static struct device_attribute dev_attr_accel_chip = + __ATTR(name, S_IRUGO, get_accel_chip_name, NULL); + +static struct device_attribute dev_attr_gyro_vendor = + __ATTR(vendor, S_IRUGO, get_gyro_vendor_name, NULL); +static struct device_attribute dev_attr_gyro_chip = + __ATTR(name, S_IRUGO, get_gyro_chip_name, NULL); + +static DEVICE_ATTR(calibration, 0664, + accel_calibration_show, accel_calibration_store); +static DEVICE_ATTR(raw_data, S_IRUGO, mpu3050_acc_read, NULL); + +static DEVICE_ATTR(power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL); +static DEVICE_ATTR(temperature, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL); +static DEVICE_ATTR(selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL); + +static DEVICE_ATTR(gyro_power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL); +static DEVICE_ATTR(gyro_get_temp, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL); +static DEVICE_ATTR(gyro_selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL); + + +static struct device_attribute *accel_sensor_attrs[] = { + &dev_attr_raw_data, + &dev_attr_calibration, + &dev_attr_accel_vendor, + &dev_attr_accel_chip, + NULL, +}; + +static struct device_attribute *gyro_sensor_attrs[] = { + &dev_attr_power_on, + &dev_attr_temperature, + &dev_attr_selftest, + &dev_attr_gyro_vendor, + &dev_attr_gyro_chip, + NULL, +}; + +extern struct class *sec_class; +extern struct class *sensors_class; + +static struct device *sec_mpu3050_dev; +static struct device *accel_sensor_device; +static struct device *gyro_sensor_device; + +extern int sensors_register(struct device *dev, void *drvdata, + struct device_attribute *attributes[], char *name); +#endif +#define FEATURE_MPU_AUTO_PROBING + +#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH) + +static int mpu350_auto_probe_accel(struct mpu3050_platform_data *pdata) +{ + int ret = ML_SUCCESS; + unsigned char reg = 0; + struct mpu_private_data *mpu; + struct mldl_cfg *mldl_cfg; + struct i2c_adapter *accel_adapter = NULL; + struct ext_slave_descr *slave_descr = NULL; + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + slave_descr = lis3dh_get_slave_descr(); + ret = MLSLSerialRead(accel_adapter, 0x19, 0x0, 1, ®); + if (ret == 0) { + pdata->accel.get_slave_descr = lis3dh_get_slave_descr; + pdata->accel.address = 0x19; + + mpu = (struct mpu_private_data *) + i2c_get_clientdata(this_client); + mldl_cfg = &mpu->mldl_cfg; + mldl_cfg->accel = slave_descr; +/* + printk("auto probe : found %s\n", + pdata->accel.get_slave_descr()->name); +*/ + is_lis3dh = 1; + } + return ret; +} + +#endif + +int mpu3050_probe(struct i2c_client *client, const struct i2c_device_id *devid) +{ + struct mpu3050_platform_data *pdata; + struct mpu_private_data *mpu; + struct mldl_cfg *mldl_cfg; + int res = 0; + int retry = 5; + + struct i2c_adapter *accel_adapter = NULL; + struct i2c_adapter *compass_adapter = NULL; + struct i2c_adapter *pressure_adapter = NULL; + + pr_info("================%s===============\n", __func__); + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + res = -ENODEV; + goto out_check_functionality_failed; + } +#ifdef FACTORY_TEST + res = sensors_register(accel_sensor_device, NULL, + accel_sensor_attrs, "accelerometer_sensor"); + if (res) + pr_err("%s: cound not register accelerometer "\ + "sensor device(%d).\n", __func__, res); + + res = sensors_register(gyro_sensor_device, NULL, + gyro_sensor_attrs, "gyro_sensor"); + if (res) + pr_err("%s: cound not register gyro "\ + "sensor device(%d).\n", __func__, res); + + sec_mpu3050_dev = device_create(sec_class, NULL, 0, NULL, + "sec_mpu3050"); + if (IS_ERR(sec_mpu3050_dev)) + pr_info("%s :Failed to create device!", __func__); + + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_power_on) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_power_on.attr.name); + return -1; + } + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_get_temp.attr.name); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on); + return -1; + } + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_selftest) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_selftest.attr.name); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp); + return -1; + } +#endif + mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL); + if (!mpu) { + res = -ENOMEM; + goto out_alloc_data_failed; + } + + i2c_set_clientdata(client, mpu); + this_client = client; + mldl_cfg = &mpu->mldl_cfg; + pdata = (struct mpu3050_platform_data *)client->dev.platform_data; + if (!pdata) { + dev_warn(&this_client->adapter->dev, + "Warning no platform data for mpu3050\n"); + } else { + mldl_cfg->pdata = pdata; + + pdata->accel.get_slave_descr = get_accel_slave_descr; + pdata->compass.get_slave_descr = get_compass_slave_descr; + pdata->pressure.get_slave_descr = get_pressure_slave_descr; + + is_lis3dh = 0; +#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH) + mpu350_auto_probe_accel(pdata); + if (pdata->accel.get_slave_descr && !is_lis3dh) { + mldl_cfg->accel = pdata->accel.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, mldl_cfg->accel->name); + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + if (!accel_adapter) { + pr_info("%s : accel_adapter i2c get fail", + __func__); + goto out_accel_failed; + } + + if (pdata->accel.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Accel irq using %d\n", + pdata->accel.irq); + res = slaveirq_init(accel_adapter, + &pdata->accel, "accelirq"); + if (res) + goto out_accelirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Accel irq not assigned\n"); + } + } else +#else + if (pdata->accel.get_slave_descr) { + mldl_cfg->accel = pdata->accel.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, mldl_cfg->accel->name); + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + if (!accel_adapter) { + pr_info("%s : accel_adapter i2c get fail", + __func__); + goto out_accel_failed; + } + + if (pdata->accel.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Accel irq using %d\n", + pdata->accel.irq); + res = slaveirq_init(accel_adapter, + &pdata->accel, "accelirq"); + if (res) + goto out_accelirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Accel irq not assigned\n"); + } + } else +#endif + { + dev_warn(&this_client->adapter->dev, + "%s: No Accel Present\n", MPU_NAME); + } + + if (pdata->compass.get_slave_descr) { + mldl_cfg->compass = pdata->compass.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, + mldl_cfg->compass->name); + compass_adapter = + i2c_get_adapter(pdata->compass.adapt_num); + + if (!compass_adapter) { + pr_info("%s : compass_adapter i2c get fail", + __func__); + goto out_compass_failed; + } + + if (pdata->compass.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Compass irq using %d\n", + pdata->compass.irq); + res = slaveirq_init(compass_adapter, + &pdata->compass, + "compassirq"); + if (res) + goto out_compassirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Compass irq not assigned\n"); + } + } else { + dev_warn(&this_client->adapter->dev, + "%s: No Compass Present\n", MPU_NAME); + } + + if (pdata->pressure.get_slave_descr) { + mldl_cfg->pressure = pdata->pressure.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, + mldl_cfg->pressure->name); + pressure_adapter = + i2c_get_adapter(pdata->pressure.adapt_num); + + if (!pressure_adapter) { + pr_info("%s : pressure_adapter i2c get fail", + __func__); + goto out_pressure_failed; + } + + if (pdata->pressure.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Pressure irq using %d\n", + pdata->pressure.irq); + res = slaveirq_init(pressure_adapter, + &pdata->pressure, + "pressureirq"); + if (res) + goto out_pressureirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Pressure irq not assigned\n"); + } + } else { + dev_warn(&this_client->adapter->dev, + "%s: No Pressure Present\n", MPU_NAME); + } + } + + mldl_cfg->addr = client->addr; + + do { + res = mpu3050_open(&mpu->mldl_cfg, client->adapter, + accel_adapter, compass_adapter, + pressure_adapter); + if (res) { + dev_err(&this_client->adapter->dev, + "%s i2c Init Error, ret = %d\n", MPU_NAME, res); + mpu3050_usleep(5000); + } + } while (retry-- && res); + + if (res) { + dev_err(&this_client->adapter->dev, + "Unable to open %s %d\n", MPU_NAME, res); + res = -ENODEV; + goto out_whoami_failed; + } + + res = misc_register(&i2c_mpu_device); + if (res < 0) { + dev_err(&this_client->adapter->dev, + "ERROR: misc_register returned %d\n", res); + goto out_misc_register_failed; + } + + if (this_client->irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing irq using %d\n", this_client->irq); + res = mpuirq_init(this_client); + if (res) + goto out_mpuirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: %s irq not assigned\n", MPU_NAME); + } + + mpu_accel_init(&mpu->mldl_cfg, client->adapter); + +#ifdef CONFIG_HAS_EARLYSUSPEND + mpu->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1; + mpu->early_suspend.suspend = mpu3050_early_suspend; + mpu->early_suspend.resume = mpu3050_early_resume; + register_early_suspend(&mpu->early_suspend); +#endif + return res; + +out_mpuirq_failed: + misc_deregister(&i2c_mpu_device); +out_misc_register_failed: + mpu3050_close(&mpu->mldl_cfg, client->adapter, + accel_adapter, compass_adapter, pressure_adapter); +out_whoami_failed: + if (pdata && pdata->pressure.get_slave_descr && pdata->pressure.irq) + slaveirq_exit(&pdata->pressure); +out_pressureirq_failed: +out_pressure_failed: + if (pdata && pdata->compass.get_slave_descr && pdata->compass.irq) + slaveirq_exit(&pdata->compass); +out_compassirq_failed: +out_compass_failed: + if (pdata && pdata->accel.get_slave_descr && pdata->accel.irq) + slaveirq_exit(&pdata->accel); +out_accelirq_failed: +out_accel_failed: + kfree(mpu); +out_alloc_data_failed: +out_check_functionality_failed: + dev_err(&this_client->adapter->dev, "%s failed %d\n", __func__, res); + return res; +} + +static int mpu3050_remove(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mpu3050_platform_data *pdata = mldl_cfg->pdata; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + +#ifdef CONFIG_HAS_EARLYSUSPEND + unregister_early_suspend(&mpu->early_suspend); +#endif + mpu3050_close(mldl_cfg, client->adapter, + accel_adapter, compass_adapter, pressure_adapter); + + if (client->irq) + mpuirq_exit(); + + if (pdata && pdata->pressure.get_slave_descr && pdata->pressure.irq) + slaveirq_exit(&pdata->pressure); + + if (pdata && pdata->compass.get_slave_descr && pdata->compass.irq) + slaveirq_exit(&pdata->compass); + + if (pdata && pdata->accel.get_slave_descr && pdata->accel.irq) + slaveirq_exit(&pdata->accel); + + misc_deregister(&i2c_mpu_device); + kfree(mpu); + + mpu_accel_exit(mldl_cfg); + + return 0; +} + +static const struct i2c_device_id mpu3050_id[] = { + {MPU_NAME, 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, mpu3050_id); + +static struct i2c_driver mpu3050_driver = { + .class = I2C_CLASS_HWMON, + .probe = mpu3050_probe, + .remove = mpu3050_remove, + .id_table = mpu3050_id, + .driver = { + .owner = THIS_MODULE, + .name = MPU_NAME, + }, + .address_list = normal_i2c, + .shutdown = mpu_shutdown, /* optional */ + .suspend = mpu_suspend, /* optional */ + .resume = mpu_resume, /* optional */ +}; + +static int __init mpu_init(void) +{ + int res = i2c_add_driver(&mpu3050_driver); + pid = 0; + + pr_info("%s res=%d\n", __func__, res); + if (res) + dev_err(&this_client->adapter->dev, "%s failed\n", __func__); + return res; +} + +static void __exit mpu_exit(void) +{ + pr_info(KERN_DEBUG "%s\n", __func__); + i2c_del_driver(&mpu3050_driver); +} + +module_init(mpu_init); +module_exit(mpu_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("User space character device interface for MPU3050"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS(MPU_NAME); diff --git a/drivers/misc/mpu3050/mpu-i2c.c b/drivers/misc/mpu3050/mpu-i2c.c new file mode 100755 index 00000000000..b1298d313ab --- /dev/null +++ b/drivers/misc/mpu3050/mpu-i2c.c @@ -0,0 +1,196 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +/** + * @defgroup + * @brief + * + * @{ + * @file mpu-i2c.c + * @brief + * + */ + +#include <linux/i2c.h> +#include "mpu.h" + +int sensor_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 (NULL == data || NULL == i2c_adap) + 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) + return res; + else + return 0; +} + +int sensor_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 sensor_i2c_write(i2c_adap, address, 2, data); +} + +int sensor_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 (NULL == data || NULL == i2c_adap) + 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) + return res; + else + return 0; +} + +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 ret; + + if (NULL == data || NULL == i2c_adap) + 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; + + ret = i2c_transfer(i2c_adap, msgs, 4); + if (ret != 4) + return ret; + else + return 0; +} + +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 ret; + + if (NULL == data || NULL == i2c_adap) + return -EINVAL; + if (len >= (sizeof(buf) - 1)) + 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; + + ret = i2c_transfer(i2c_adap, msgs, 3); + if (ret != 3) + return ret; + else + return 0; +} + +/** + * @} + */ diff --git a/drivers/misc/mpu3050/mpu-i2c.h b/drivers/misc/mpu3050/mpu-i2c.h new file mode 100755 index 00000000000..0bbc8c64594 --- /dev/null +++ b/drivers/misc/mpu3050/mpu-i2c.h @@ -0,0 +1,58 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +/** + * @defgroup + * @brief + * + * @{ + * @file mpu-i2c.c + * @brief + * + * + */ + +#ifndef __MPU_I2C_H__ +#define __MPU_I2C_H__ + +#include <linux/i2c.h> + +int sensor_i2c_write(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned int len, unsigned char const *data); + +int sensor_i2c_write_register(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned char reg, unsigned char value); + +int sensor_i2c_read(struct i2c_adapter *i2c_adap, + unsigned char address, + unsigned char reg, + unsigned int len, unsigned char *data); + +int mpu_memory_read(struct i2c_adapter *i2c_adap, + unsigned char mpu_addr, + unsigned short mem_addr, + unsigned int len, unsigned char *data); + +int mpu_memory_write(struct i2c_adapter *i2c_adap, + unsigned char mpu_addr, + unsigned short mem_addr, + unsigned int len, unsigned char const *data); + +#endif /* __MPU_I2C_H__ */ diff --git a/drivers/misc/mpu3050/mpuirq.c b/drivers/misc/mpu3050/mpuirq.c new file mode 100755 index 00000000000..8623855e8b5 --- /dev/null +++ b/drivers/misc/mpu3050/mpuirq.c @@ -0,0 +1,448 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/irq.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/workqueue.h> +#include <linux/poll.h> + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> + +#include "mpu.h" +#include "mpuirq.h" +#include "mldl_cfg.h" +#include "mpu-i2c.h" +#include "mpu-accel.h" + +#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT +#include <linux/init.h> +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/ioport.h> +#include <linux/delay.h> +#include <linux/serial_core.h> +#include <linux/io.h> +#include <linux/platform_device.h> + +#include <mach/regs-gpio.h> + +#include <mach/map.h> +#include <mach/regs-mem.h> +#include <mach/regs-clock.h> +#include <mach/media.h> +#include <mach/gpio.h> +#endif +#define MPUIRQ_NAME "mpuirq" + +/* function which gets accel data and sends it to MPU */ + +DECLARE_WAIT_QUEUE_HEAD(mpuirq_wait); + +struct mpuirq_dev_data { + struct work_struct work; + struct i2c_client *mpu_client; + struct miscdevice *dev; + int irq; + int pid; + int accel_divider; + int data_ready; + int timeout; +}; + +static struct mpuirq_dev_data mpuirq_dev_data; +static struct mpuirq_data mpuirq_data; +static char *interface = MPUIRQ_NAME; + +static void mpu_accel_data_work_fcn(struct work_struct *work); + +#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT +static irqreturn_t mpuirq_selftest_handler(int irq, void *dev_id); +unsigned long long selftest_get_times[10]; +#endif + +static int mpuirq_open(struct inode *inode, struct file *file); +static int mpuirq_release(struct inode *inode, struct file *file); +static ssize_t mpuirq_read(struct file *file, char *buf, size_t count, + loff_t *ppos); +unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll); +static long mpuirq_ioctl(struct file *file, unsigned int cmd, + unsigned long arg); +static irqreturn_t mpuirq_handler(int irq, void *dev_id); + +static int mpuirq_open(struct inode *inode, struct file *file) +{ + dev_dbg(mpuirq_dev_data.dev->this_device, + "%s current->pid %d\n", __func__, current->pid); + mpuirq_dev_data.pid = current->pid; + file->private_data = &mpuirq_dev_data; + /* we could do some checking on the flags supplied by "open" */ + /* i.e. O_NONBLOCK */ + /* -> set some flag to disable interruptible_sleep_on in mpuirq_read */ + return 0; +} + +/* close function - called when the "file" /dev/mpuirq is closed in userspace */ +static int mpuirq_release(struct inode *inode, struct file *file) +{ + dev_dbg(mpuirq_dev_data.dev->this_device, "mpuirq_release\n"); + return 0; +} + +/* read function called when from /dev/mpuirq is read */ +static ssize_t mpuirq_read(struct file *file, + char *buf, size_t count, loff_t * ppos) +{ + int len, err; + struct mpuirq_dev_data *p_mpuirq_dev_data = file->private_data; + + if (!mpuirq_dev_data.data_ready && mpuirq_dev_data.timeout > 0) { + wait_event_interruptible_timeout(mpuirq_wait, + mpuirq_dev_data.data_ready, + mpuirq_dev_data.timeout); + } + + if (mpuirq_dev_data.data_ready && NULL != buf + && count >= sizeof(mpuirq_data)) { + err = copy_to_user(buf, &mpuirq_data, sizeof(mpuirq_data)); + mpuirq_data.data_type = 0; + } else { + return 0; + } + if (err != 0) { + dev_err(p_mpuirq_dev_data->dev->this_device, + "Copy to user returned %d\n", err); + return -EFAULT; + } + mpuirq_dev_data.data_ready = 0; + len = sizeof(mpuirq_data); + return len; +} + +unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll) +{ + int mask = 0; + + poll_wait(file, &mpuirq_wait, poll); + if (mpuirq_dev_data.data_ready) + mask |= POLLIN | POLLRDNORM; + return mask; +} + +/* ioctl - I/O control */ +static long mpuirq_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + int retval = 0; + int data; + + switch (cmd) { + case MPUIRQ_SET_TIMEOUT: + mpuirq_dev_data.timeout = arg; + break; + + case MPUIRQ_GET_INTERRUPT_CNT: + data = mpuirq_data.interruptcount - 1; + if (mpuirq_data.interruptcount > 1) + mpuirq_data.interruptcount = 1; + + if (copy_to_user((int *)arg, &data, sizeof(int))) + return -EFAULT; + break; + case MPUIRQ_GET_IRQ_TIME: + if (copy_to_user((int *)arg, &mpuirq_data.irqtime, + sizeof(mpuirq_data.irqtime))) + return -EFAULT; + mpuirq_data.irqtime = 0; + break; + case MPUIRQ_SET_FREQUENCY_DIVIDER: + mpuirq_dev_data.accel_divider = arg; + break; + +#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT + case MPUIRQ_SET_SELFTEST_IRQ_HANDLER: + { + int res; + /* unregister mpuirq handler */ + if (mpuirq_dev_data.irq > 0) { + free_irq(mpuirq_dev_data.irq, + &mpuirq_dev_data.irq); + } + /* register selftest irq handler */ + if (mpuirq_dev_data.irq > 0) { + struct mldl_cfg *mldl_cfg = + (struct mldl_cfg *) + i2c_get_clientdata(mpuirq_dev_data. + mpu_client); + unsigned long flags; + + if (BIT_ACTL_LOW == + ((mldl_cfg->pdata->int_config) & BIT_ACTL)) + flags = IRQF_TRIGGER_FALLING; + else + flags = IRQF_TRIGGER_RISING; + + res = request_irq(mpuirq_dev_data.irq, + mpuirq_selftest_handler, flags, + interface, + &mpuirq_dev_data.irq); + + if (res) { + pr_info("MPUIRQ_SET_SELFTEST_IRQ_" \ + "HANDLER: cannot register IRQ %d\n", + mpuirq_dev_data.irq); + } + } + } + break; + case MPUIRQ_SET_MPUIRQ_HANDLER: + { + int res; + /* unregister selftest irq handler */ + if (mpuirq_dev_data.irq > 0) + free_irq(mpuirq_dev_data.irq, + &mpuirq_dev_data.irq); + + /* register mpuirq handler */ + if (mpuirq_dev_data.irq > 0) { + struct mldl_cfg *mldl_cfg = + (struct mldl_cfg *) + i2c_get_clientdata(mpuirq_dev_data. + mpu_client); + unsigned long flags; + + if (BIT_ACTL_LOW == + ((mldl_cfg->pdata->int_config) & BIT_ACTL)) + flags = IRQF_TRIGGER_FALLING; + else + flags = IRQF_TRIGGER_RISING; + + res = + request_irq(mpuirq_dev_data.irq, + mpuirq_handler, flags, + interface, + &mpuirq_dev_data.irq); + } + } + break; + case MPUIRQ_GET_MPUIRQ_JIFFIES: + { + + mpuirq_data.mpuirq_jiffies = 0; + + if (copy_to_user + ((void *)arg, selftest_get_times, + sizeof(selftest_get_times))) { + memset(selftest_get_times, 0, + sizeof(selftest_get_times)); + return -EFAULT; + } + + memset(selftest_get_times, 0, + sizeof(selftest_get_times)); + } + break; +#endif + default: + retval = -EINVAL; + } + return retval; +} + +static void mpu_accel_data_work_fcn(struct work_struct *work) +{ + struct mpuirq_dev_data *mpuirq_dev_data = + (struct mpuirq_dev_data *)work; + struct mldl_cfg *mldl_cfg = (struct mldl_cfg *) + i2c_get_clientdata(mpuirq_dev_data->mpu_client); + struct i2c_adapter *accel_adapter; + unsigned char wbuff[16]; + unsigned char rbuff[16]; + int ii; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + mldl_cfg->accel->read(accel_adapter, + mldl_cfg->accel, &mldl_cfg->pdata->accel, rbuff); + + /* @todo add other data formats here as well */ + if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) { + for (ii = 0; ii < 3; ii++) { + wbuff[2 * ii + 1] = rbuff[2 * ii + 1]; + wbuff[2 * ii + 2] = rbuff[2 * ii + 0]; + } + } else { + memcpy(wbuff + 1, rbuff, mldl_cfg->accel->len); + } + + wbuff[7] = 0; + wbuff[8] = 1; /*set semaphore */ + + mpu_memory_write(mpuirq_dev_data->mpu_client->adapter, + mldl_cfg->addr, 0x0108, 8, wbuff); +} + +#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT +static irqreturn_t mpuirq_selftest_handler(int irq, void *dev_id) +{ +/* static int mycount=0; */ + struct timeval irqtime; + unsigned long long temp_time; + + do_gettimeofday(&irqtime); + + temp_time = (((long long)irqtime.tv_sec) << 32); + temp_time += irqtime.tv_usec; + + if (mpuirq_data.mpuirq_jiffies < 10) + selftest_get_times[mpuirq_data.mpuirq_jiffies++] = temp_time; + + return IRQ_HANDLED; +} +#endif + +static irqreturn_t mpuirq_handler(int irq, void *dev_id) +{ + static int mycount; + struct timeval irqtime; + mycount++; + + mpuirq_data.interruptcount++; + + /* wake up (unblock) for reading data from userspace */ + /* and ignore first interrupt generated in module init */ + mpuirq_dev_data.data_ready = 1; + + do_gettimeofday(&irqtime); + mpuirq_data.irqtime = (((long long)irqtime.tv_sec) << 32); + mpuirq_data.irqtime += irqtime.tv_usec; + + if ((mpuirq_dev_data.accel_divider >= 0) && + (0 == (mycount % (mpuirq_dev_data.accel_divider + 1)))) { + schedule_work((struct work_struct *)(&mpuirq_dev_data)); + } + + wake_up_interruptible(&mpuirq_wait); + + return IRQ_HANDLED; + +} + +/* define which file operations are supported */ +const struct file_operations mpuirq_fops = { + .owner = THIS_MODULE, + .read = mpuirq_read, + .poll = mpuirq_poll, + +#if HAVE_COMPAT_IOCTL + .compat_ioctl = mpuirq_ioctl, +#endif +#if HAVE_UNLOCKED_IOCTL + .unlocked_ioctl = mpuirq_ioctl, +#endif + .open = mpuirq_open, + .release = mpuirq_release, +}; + +static struct miscdevice mpuirq_device = { + .minor = MISC_DYNAMIC_MINOR, + .name = MPUIRQ_NAME, + .fops = &mpuirq_fops, +}; + +int mpuirq_init(struct i2c_client *mpu_client) +{ + + int res; + struct mldl_cfg *mldl_cfg = + (struct mldl_cfg *)i2c_get_clientdata(mpu_client); + + /* work_struct initialization */ + INIT_WORK((struct work_struct *)&mpuirq_dev_data, + mpu_accel_data_work_fcn); + mpuirq_dev_data.mpu_client = mpu_client; + + dev_info(&mpu_client->adapter->dev, + "Module Param interface = %s\n", interface); + + mpuirq_dev_data.irq = mpu_client->irq; + mpuirq_dev_data.pid = 0; + mpuirq_dev_data.accel_divider = -1; + mpuirq_dev_data.data_ready = 0; + mpuirq_dev_data.timeout = 0; + mpuirq_dev_data.dev = &mpuirq_device; + + if (mpuirq_dev_data.irq) { + unsigned long flags; + if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL)) + flags = IRQF_TRIGGER_FALLING; + else + flags = IRQF_TRIGGER_RISING; + + res = + request_irq(mpuirq_dev_data.irq, mpuirq_handler, flags, + interface, &mpuirq_dev_data.irq); + if (res) { + dev_err(&mpu_client->adapter->dev, + "myirqtest: cannot register IRQ %d\n", + mpuirq_dev_data.irq); + } else { + res = misc_register(&mpuirq_device); + if (res < 0) { + dev_err(&mpu_client->adapter->dev, + "misc_register returned %d\n", res); + free_irq(mpuirq_dev_data.irq, + &mpuirq_dev_data.irq); + } + } + + } else { + res = 0; + } + + return res; +} + +void mpuirq_exit(void) +{ + /* Free the IRQ first before flushing the work */ + if (mpuirq_dev_data.irq > 0) + free_irq(mpuirq_dev_data.irq, &mpuirq_dev_data.irq); + + flush_scheduled_work(); + + dev_info(mpuirq_device.this_device, "Unregistering %s\n", MPUIRQ_NAME); + misc_deregister(&mpuirq_device); + + return; +} + +module_param(interface, charp, S_IRUGO | S_IWUSR); +MODULE_PARM_DESC(interface, "The Interface name"); diff --git a/drivers/misc/mpu3050/mpuirq.h b/drivers/misc/mpu3050/mpuirq.h new file mode 100755 index 00000000000..d6c1987ae16 --- /dev/null +++ b/drivers/misc/mpu3050/mpuirq.h @@ -0,0 +1,48 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#ifndef __MPUIRQ__ +#define __MPUIRQ__ + +#ifdef __KERNEL__ +#include <linux/i2c-dev.h> +#endif + +#define MPUIRQ_ENABLE_DEBUG (1) +#define MPUIRQ_GET_INTERRUPT_CNT (2) +#define MPUIRQ_GET_IRQ_TIME (3) +#define MPUIRQ_GET_LED_VALUE (4) +#define MPUIRQ_SET_TIMEOUT (5) +#define MPUIRQ_SET_ACCEL_INFO (6) +#define MPUIRQ_SET_FREQUENCY_DIVIDER (7) + +#ifdef FEATURE_GYRO_SELFTEST_INTERRUPT +#define MPUIRQ_GET_MPUIRQ_JIFFIES (8) +#define MPUIRQ_SET_SELFTEST_IRQ_HANDLER (9) +#define MPUIRQ_SET_MPUIRQ_HANDLER (10) +#endif + +#ifdef __KERNEL__ + +void mpuirq_exit(void); +int mpuirq_init(struct i2c_client *mpu_client); + +#endif + +#endif diff --git a/drivers/misc/mpu3050/sensors_core.c b/drivers/misc/mpu3050/sensors_core.c new file mode 100755 index 00000000000..b65263165b5 --- /dev/null +++ b/drivers/misc/mpu3050/sensors_core.c @@ -0,0 +1,100 @@ +/* + * Universal sensors core class + * + * Author : Ryunkyun Park <ryun.park@samsung.com> + */ + +#include <linux/module.h> +#include <linux/types.h> +#include <linux/init.h> +#include <linux/device.h> +#include <linux/fs.h> +#include <linux/err.h> +/* #include <linux/sensors_core.h> */ + +struct class *sensors_class; +static atomic_t sensor_count; +static DEFINE_MUTEX(sensors_mutex); + +/** + * Create sysfs interface + */ +static void set_sensor_attr(struct device *dev, + struct device_attribute *attributes[]) +{ + int i; + + for (i = 0; attributes[i] != NULL; i++) { + if ((device_create_file(dev, attributes[i])) < 0) { + pr_info("[SENSOR CORE] fail!!! device_create_file" \ + "( dev, attributes[%d] )\n", i); + } + } +} + +int sensors_register(struct device *dev, void *drvdata, + struct device_attribute *attributes[], char *name) +{ + int ret = 0; + + if (!sensors_class) { + sensors_class = class_create(THIS_MODULE, "sensors"); + if (IS_ERR(sensors_class)) + return PTR_ERR(sensors_class); + } + + mutex_lock(&sensors_mutex); + + dev = device_create(sensors_class, NULL, 0, drvdata, "%s", name); + + if (IS_ERR(dev)) { + ret = PTR_ERR(dev); + pr_err("[SENSORS CORE] device_create failed! [%d]\n", ret); + return ret; + } + + set_sensor_attr(dev, attributes); + + atomic_inc(&sensor_count); + + mutex_unlock(&sensors_mutex); + + return 0; +} + +void sensors_unregister(struct device *dev) +{ + /* TODO : Unregister device */ +} + +static int __init sensors_class_init(void) +{ + pr_info("[SENSORS CORE] sensors_class_init\n"); + sensors_class = class_create(THIS_MODULE, "sensors"); + + if (IS_ERR(sensors_class)) + return PTR_ERR(sensors_class); + + atomic_set(&sensor_count, 0); + sensors_class->dev_uevent = NULL; + + return 0; +} + +static void __exit sensors_class_exit(void) +{ + class_destroy(sensors_class); +} + +EXPORT_SYMBOL_GPL(sensors_register); +EXPORT_SYMBOL_GPL(sensors_unregister); + +/* exported for the APM Power driver, APM emulation */ +EXPORT_SYMBOL_GPL(sensors_class); + +subsys_initcall(sensors_class_init); +module_exit(sensors_class_exit); + +MODULE_DESCRIPTION("Universal sensors core class"); +MODULE_AUTHOR("Ryunkyun Park <ryun.park@samsung.com>"); +MODULE_LICENSE("GPL"); diff --git a/drivers/misc/mpu3050/slaveirq.c b/drivers/misc/mpu3050/slaveirq.c new file mode 100755 index 00000000000..2ee53851e12 --- /dev/null +++ b/drivers/misc/mpu3050/slaveirq.c @@ -0,0 +1,270 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/irq.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/poll.h> + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> +#include <linux/wait.h> +#include <linux/slab.h> + +#include "mpu.h" +#include "slaveirq.h" +#include "mldl_cfg.h" +#include "mpu-i2c.h" + +/* function which gets slave data and sends it to SLAVE */ + +struct slaveirq_dev_data { + struct miscdevice dev; + struct i2c_client *slave_client; + struct mpuirq_data data; + wait_queue_head_t slaveirq_wait; + int irq; + int pid; + int data_ready; + int timeout; +}; + +/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28 + * drivers: misc: pass miscdevice pointer via file private data + */ +static int slaveirq_open(struct inode *inode, struct file *file) +{ + /* Device node is availabe in the file->private_data, this is + * exactly what we want so we leave it there */ + struct slaveirq_dev_data *data = + container_of(file->private_data, struct slaveirq_dev_data, dev); + + dev_dbg(data->dev.this_device, + "%s current->pid %d\n", __func__, current->pid); + data->pid = current->pid; + return 0; +} + +static int slaveirq_release(struct inode *inode, struct file *file) +{ + struct slaveirq_dev_data *data = + container_of(file->private_data, struct slaveirq_dev_data, dev); + dev_dbg(data->dev.this_device, "slaveirq_release\n"); + return 0; +} + +/* read function called when from /dev/slaveirq is read */ +static ssize_t slaveirq_read(struct file *file, + char *buf, size_t count, loff_t *ppos) +{ + int len, err; + struct slaveirq_dev_data *data = + container_of(file->private_data, struct slaveirq_dev_data, dev); + + if (!data->data_ready) { + wait_event_interruptible_timeout(data->slaveirq_wait, + data->data_ready, + data->timeout); + } + + if (data->data_ready && NULL != buf + && count >= sizeof(data->data)) { + err = copy_to_user(buf, &data->data, sizeof(data->data)); + data->data.data_type = 0; + } else { + return 0; + } + if (err != 0) { + dev_err(data->dev.this_device, + "Copy to user returned %d\n", err); + return -EFAULT; + } + data->data_ready = 0; + len = sizeof(data->data); + return len; +} + +unsigned int slaveirq_poll(struct file *file, struct poll_table_struct *poll) +{ + int mask = 0; + struct slaveirq_dev_data *data = + container_of(file->private_data, struct slaveirq_dev_data, dev); + + poll_wait(file, &data->slaveirq_wait, poll); + if (data->data_ready) + mask |= POLLIN | POLLRDNORM; + return mask; +} + +/* ioctl - I/O control */ +static long slaveirq_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + int retval = 0; + int tmp; + struct slaveirq_dev_data *data = + container_of(file->private_data, struct slaveirq_dev_data, dev); + + switch (cmd) { + case SLAVEIRQ_SET_TIMEOUT: + data->timeout = arg; + break; + + case SLAVEIRQ_GET_INTERRUPT_CNT: + tmp = data->data.interruptcount - 1; + if (data->data.interruptcount > 1) + data->data.interruptcount = 1; + + if (copy_to_user((int *) arg, &tmp, sizeof(int))) + return -EFAULT; + break; + case SLAVEIRQ_GET_IRQ_TIME: + if (copy_to_user((int *) arg, &data->data.irqtime, + sizeof(data->data.irqtime))) + return -EFAULT; + data->data.irqtime = 0; + break; + default: + retval = -EINVAL; + } + return retval; +} + +static irqreturn_t slaveirq_handler(int irq, void *dev_id) +{ + struct slaveirq_dev_data *data = (struct slaveirq_dev_data *)dev_id; + static int mycount; + struct timeval irqtime; + mycount++; + + data->data.interruptcount++; + + /* wake up (unblock) for reading data from userspace */ + /* and ignore first interrupt generated in module init */ + data->data_ready = 1; + + do_gettimeofday(&irqtime); + data->data.irqtime = (((long long) irqtime.tv_sec) << 32); + data->data.irqtime += irqtime.tv_usec; + data->data.data_type |= 1; + + wake_up_interruptible(&data->slaveirq_wait); + + return IRQ_HANDLED; + +} + +/* define which file operations are supported */ +static const struct file_operations slaveirq_fops = { + .owner = THIS_MODULE, + .read = slaveirq_read, + .poll = slaveirq_poll, + +#if HAVE_COMPAT_IOCTL + .compat_ioctl = slaveirq_ioctl, +#endif +#if HAVE_UNLOCKED_IOCTL + .unlocked_ioctl = slaveirq_ioctl, +#endif + .open = slaveirq_open, + .release = slaveirq_release, +}; + +int slaveirq_init(struct i2c_adapter *slave_adapter, + struct ext_slave_platform_data *pdata, + char *name) +{ + + int res; + struct slaveirq_dev_data *data; + + if (!pdata->irq) + return -EINVAL; + + pdata->irq_data = kzalloc(sizeof(*data), + GFP_KERNEL); + data = (struct slaveirq_dev_data *) pdata->irq_data; + if (!data) + return -ENOMEM; + + data->dev.minor = MISC_DYNAMIC_MINOR; + data->dev.name = name; + data->dev.fops = &slaveirq_fops; + data->irq = pdata->irq; + data->pid = 0; + data->data_ready = 0; + data->timeout = 0; + + res = request_irq(data->irq, slaveirq_handler, IRQF_TRIGGER_RISING, + data->dev.name, data); + + if (res) { + dev_err(&slave_adapter->dev, + "myirqtest: cannot register IRQ %d\n", + data->irq); + goto out_request_irq; + } + + res = misc_register(&data->dev); + if (res < 0) { + dev_err(&slave_adapter->dev, + "misc_register returned %d\n", + res); + goto out_misc_register; + } + + init_waitqueue_head(&data->slaveirq_wait); + return res; + +out_misc_register: + free_irq(data->irq, data); +out_request_irq: + kfree(pdata->irq_data); + pdata->irq_data = NULL; + + return res; +} + +void slaveirq_exit(struct ext_slave_platform_data *pdata) +{ + struct slaveirq_dev_data *data = pdata->irq_data; + + if (!pdata->irq_data || data->irq <= 0) + return; + + dev_info(data->dev.this_device, "Unregistering %s\n", + data->dev.name); + + free_irq(data->irq, data); + misc_deregister(&data->dev); + kfree(pdata->irq_data); + pdata->irq_data = NULL; +} diff --git a/drivers/misc/mpu3050/slaveirq.h b/drivers/misc/mpu3050/slaveirq.h new file mode 100755 index 00000000000..ca9c7e496da --- /dev/null +++ b/drivers/misc/mpu3050/slaveirq.h @@ -0,0 +1,46 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#ifndef __SLAVEIRQ__ +#define __SLAVEIRQ__ + +#ifdef __KERNEL__ +#include <linux/i2c-dev.h> +#endif + +#include "mpu.h" +#include "mpuirq.h" + +#define SLAVEIRQ_ENABLE_DEBUG (1) +#define SLAVEIRQ_GET_INTERRUPT_CNT (2) +#define SLAVEIRQ_GET_IRQ_TIME (3) +#define SLAVEIRQ_GET_LED_VALUE (4) +#define SLAVEIRQ_SET_TIMEOUT (5) +#define SLAVEIRQ_SET_SLAVE_INFO (6) + +#ifdef __KERNEL__ + +void slaveirq_exit(struct ext_slave_platform_data *pdata); +int slaveirq_init(struct i2c_adapter *slave_adapter, + struct ext_slave_platform_data *pdata, + char *name); + +#endif + +#endif diff --git a/drivers/misc/mpu3050/timerirq.c b/drivers/misc/mpu3050/timerirq.c new file mode 100755 index 00000000000..53b42d29227 --- /dev/null +++ b/drivers/misc/mpu3050/timerirq.c @@ -0,0 +1,294 @@ +/* +$License: +Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. +$ +*/ +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/poll.h> + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> +#include <linux/timer.h> +#include <linux/slab.h> + +#include "mpu.h" +#include "mltypes.h" +#include "timerirq.h" + +/* function which gets timer data and sends it to TIMER */ +struct timerirq_data { + int pid; + int data_ready; + int run; + int timeout; + unsigned long period; + struct mpuirq_data data; + struct completion timer_done; + wait_queue_head_t timerirq_wait; + struct timer_list timer; + struct miscdevice *dev; +}; + +static struct miscdevice *timerirq_dev_data; + +static void timerirq_handler(unsigned long arg) +{ + struct timerirq_data *data = (struct timerirq_data *)arg; + struct timeval irqtime; + + /* dev_info(data->dev->this_device, + "%s, %ld\n", __func__, (unsigned long)data); */ + + data->data.interruptcount++; + + data->data_ready = 1; + + do_gettimeofday(&irqtime); + data->data.irqtime = (((long long) irqtime.tv_sec) << 32); + data->data.irqtime += irqtime.tv_usec; + data->data.data_type |= 1; + + wake_up_interruptible(&data->timerirq_wait); + + if (data->run) + mod_timer(&data->timer, + jiffies + msecs_to_jiffies(data->period)); + else + complete(&data->timer_done); +} + +static int start_timerirq(struct timerirq_data *data) +{ + dev_dbg(data->dev->this_device, + "%s current->pid %d\n", __func__, current->pid); + + /* Timer already running... success */ + if (data->run) + return 0; + + /* Don't allow a period of 0 since this would fire constantly */ + if (!data->period) + return -EINVAL; + + data->run = TRUE; + data->data_ready = FALSE; + + init_completion(&data->timer_done); + setup_timer(&data->timer, timerirq_handler, (unsigned long)data); + + return mod_timer(&data->timer, + jiffies + msecs_to_jiffies(data->period)); +} + +static int stop_timerirq(struct timerirq_data *data) +{ + dev_dbg(data->dev->this_device, + "%s current->pid %lx\n", __func__, (unsigned long)data); + + if (data->run) { + data->run = FALSE; + mod_timer(&data->timer, jiffies + 1); + wait_for_completion(&data->timer_done); + } + return 0; +} + +/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28 + * drivers: misc: pass miscdevice pointer via file private data + */ +static int timerirq_open(struct inode *inode, struct file *file) +{ + /* Device node is availabe in the file->private_data, this is + * exactly what we want so we leave it there */ + struct miscdevice *dev_data = file->private_data; + struct timerirq_data *data = kzalloc(sizeof(*data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + data->dev = dev_data; + file->private_data = data; + data->pid = current->pid; + init_waitqueue_head(&data->timerirq_wait); + + dev_dbg(data->dev->this_device, + "%s current->pid %d\n", __func__, current->pid); + return 0; +} + +static int timerirq_release(struct inode *inode, struct file *file) +{ + struct timerirq_data *data = file->private_data; + dev_dbg(data->dev->this_device, "timerirq_release\n"); + if (data->run) + stop_timerirq(data); + kfree(data); + return 0; +} + +/* read function called when from /dev/timerirq is read */ +static ssize_t timerirq_read(struct file *file, + char *buf, size_t count, loff_t *ppos) +{ + int len, err; + struct timerirq_data *data = file->private_data; + + if (!data->data_ready && + data->timeout) { + wait_event_interruptible_timeout(data->timerirq_wait, + data->data_ready, + data->timeout); + } + + if (data->data_ready && NULL != buf + && count >= sizeof(data->data)) { + err = copy_to_user(buf, &data->data, sizeof(data->data)); + data->data.data_type = 0; + } else { + return 0; + } + if (err != 0) { + dev_err(data->dev->this_device, + "Copy to user returned %d\n", err); + return -EFAULT; + } + data->data_ready = 0; + len = sizeof(data->data); + return len; +} + +static unsigned int timerirq_poll(struct file *file, + struct poll_table_struct *poll) +{ + int mask = 0; + struct timerirq_data *data = file->private_data; + + poll_wait(file, &data->timerirq_wait, poll); + if (data->data_ready) + mask |= POLLIN | POLLRDNORM; + return mask; +} + +/* ioctl - I/O control */ +static long timerirq_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + int retval = 0; + int tmp; + struct timerirq_data *data = file->private_data; + + dev_dbg(data->dev->this_device, + "%s current->pid %d, %d, %ld\n", + __func__, current->pid, cmd, arg); + + if (!data) + return -EFAULT; + + switch (cmd) { + case TIMERIRQ_SET_TIMEOUT: + data->timeout = arg; + break; + case TIMERIRQ_GET_INTERRUPT_CNT: + tmp = data->data.interruptcount - 1; + if (data->data.interruptcount > 1) + data->data.interruptcount = 1; + + if (copy_to_user((int *) arg, &tmp, sizeof(int))) + return -EFAULT; + break; + case TIMERIRQ_START: + data->period = arg; + retval = start_timerirq(data); + break; + case TIMERIRQ_STOP: + retval = stop_timerirq(data); + break; + default: + retval = -EINVAL; + } + return retval; +} + +/* define which file operations are supported */ +static const struct file_operations timerirq_fops = { + .owner = THIS_MODULE, + .read = timerirq_read, + .poll = timerirq_poll, + +#if HAVE_COMPAT_IOCTL + .compat_ioctl = timerirq_ioctl, +#endif +#if HAVE_UNLOCKED_IOCTL + .unlocked_ioctl = timerirq_ioctl, +#endif + .open = timerirq_open, + .release = timerirq_release, +}; + +static int __init timerirq_init(void) +{ + + int res; + static struct miscdevice *data; + + data = kzalloc(sizeof(*data), GFP_KERNEL); + if (!data) + return -ENOMEM; + timerirq_dev_data = data; + data->minor = MISC_DYNAMIC_MINOR; + data->name = "timerirq"; + data->fops = &timerirq_fops; + + res = misc_register(data); + if (res < 0) { + dev_err(data->this_device, + "misc_register returned %d\n", res); + kfree(data); + return res; + } + + return res; +} +module_init(timerirq_init); + +static void __exit timerirq_exit(void) +{ + struct miscdevice *data = timerirq_dev_data; + + dev_info(data->this_device, "Unregistering %s\n", + data->name); + + misc_deregister(data); + kfree(data); + + timerirq_dev_data = NULL; +} +module_exit(timerirq_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Timer IRQ device driver."); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("timerirq"); diff --git a/drivers/misc/mpu3050/timerirq.h b/drivers/misc/mpu3050/timerirq.h new file mode 100755 index 00000000000..a38b4907a4d --- /dev/null +++ b/drivers/misc/mpu3050/timerirq.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2010 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 <http://www.gnu.org/licenses/>. + $ + */ + +#ifndef __TIMERIRQ__ +#define __TIMERIRQ__ + +#define TIMERIRQ_SET_TIMEOUT (5) +#define TIMERIRQ_GET_INTERRUPT_CNT (7) +#define TIMERIRQ_START (8) +#define TIMERIRQ_STOP (9) + +#endif |