summaryrefslogtreecommitdiff
path: root/drivers
diff options
context:
space:
mode:
authorvsalve <vsalve@nvidia.com>2011-03-15 15:29:43 +0530
committerVarun Colbert <vcolbert@nvidia.com>2011-03-22 15:56:00 -0800
commit6c0a2b4e0a1439a15d44ff6e24c286277dc489f0 (patch)
tree21e2425d07a027c3f9b82bed9011c6c20a714573 /drivers
parentc04a044757f3a6959e7c4d5df30cb02fc8470a94 (diff)
misc: mpu3050: Invensense MPU code update v3.3.3
Update Invensense Motion Library (MPL) to production version v3.3.3. Previous version was engineering release and is not suitable for mass production. This version of the MPL most notably fixes problems with suspend/resume where touch screen would quit working during a suspend/resume cycle. Change-Id: Ia33433a677862b3d1e31d410aac7176d7a381a8f Reviewed-on: http://git-master/r/22987 Reviewed-by: Varun Colbert <vcolbert@nvidia.com> Tested-by: Varun Colbert <vcolbert@nvidia.com>
Diffstat (limited to 'drivers')
-rw-r--r--drivers/misc/Makefile2
-rw-r--r--drivers/misc/mpu3050/Kconfig122
-rw-r--r--drivers/misc/mpu3050/Makefile245
-rw-r--r--drivers/misc/mpu3050/accel/kxtf9.c588
-rw-r--r--drivers/misc/mpu3050/compass/ak8975.c95
-rw-r--r--drivers/misc/mpu3050/mldl_cfg.c1051
-rw-r--r--drivers/misc/mpu3050/mldl_cfg.h79
-rw-r--r--drivers/misc/mpu3050/mltypes.h22
-rw-r--r--drivers/misc/mpu3050/mpu-dev.c705
-rw-r--r--drivers/misc/mpu3050/mpuirq.c27
-rw-r--r--drivers/misc/mpu3050/slaveirq.c26
-rw-r--r--drivers/misc/mpu3050/timerirq.c295
-rw-r--r--drivers/misc/mpu3050/timerirq.h28
13 files changed, 2271 insertions, 1014 deletions
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 4329de81abf4..2ea924ceb9d3 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -42,7 +42,7 @@ obj-$(CONFIG_WL127X_RFKILL) += wl127x-rfkill.o
obj-$(CONFIG_APANIC) += apanic.o
obj-$(CONFIG_SENSORS_AK8975) += akm8975.o
obj-$(CONFIG_BCM4329_RFKILL) += bcm4329_rfkill.o
-obj-$(CONFIG_SENSORS_MPU3050) += mpu3050/
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050/
obj-$(CONFIG_SENSORS_NCT1008) += nct1008.o
obj-$(CONFIG_TEGRA_CRYPTO_DEV) += tegra-cryptodev.o
obj-$(CONFIG_MAX1749_VIBRATOR) += max1749.o
diff --git a/drivers/misc/mpu3050/Kconfig b/drivers/misc/mpu3050/Kconfig
index 57728e8c847a..de240fa0ad83 100644
--- a/drivers/misc/mpu3050/Kconfig
+++ b/drivers/misc/mpu3050/Kconfig
@@ -1,57 +1,65 @@
-
-menu "Motion Sensors Support"
-
-choice
- tristate "Motion Processing Unit"
- depends on I2C
- optional
-
-config SENSORS_MPU3050
- tristate "MPU3050"
- help
- If you say yes here you get support for the MPU3050 Gyroscope driver
- This driver can also be built as a module. If so, the module
- will be called mpu3050.
-
-config SENSORS_MPU6000
- tristate "MPU6000"
- help
- If you say yes here you get support for the MPU6000 Gyroscope driver
- This driver can also be built as a module. If so, the module
- will be called mpu6000.
-
-endchoice
-
-choice
- prompt "Accelerometer Type"
- depends on SENSORS_MPU3050
- optional
-
-config SENSORS_KXTF9_MPU
- bool "Kionix KXTF9"
- help
- This enables support for the Kionix KXFT9 accelerometer
-
-endchoice
-
-choice
- prompt "Compass Type"
- depends on SENSORS_MPU6000 || SENSORS_MPU3050
- optional
-
-config SENSORS_AK8975_MPU
- bool "AKM ak8975"
- help
- This enables support for the AKM ak8975 compass
-
-endchoice
-
-config SENSORS_MPU_DEBUG
- bool "MPU debug"
- depends on SENSORS_MPU3050 || SENSORS_MPU6000
- help
- If you say yes here you get extra debug messages from the MPU3050
- and other slave sensors.
-
-endmenu
-
+
+menu "Motion Sensors Support"
+
+choice
+ tristate "Motion Processing Unit"
+ depends on I2C
+ optional
+
+config MPU_SENSORS_MPU3050
+ tristate "MPU3050"
+ help
+ If you say yes here you get support for the MPU3050 Gyroscope driver
+ This driver can also be built as a module. If so, the module
+ will be called mpu3050.
+
+config MPU_SENSORS_MPU6000
+ tristate "MPU6000"
+ help
+ If you say yes here you get support for the MPU6000 Gyroscope driver
+ This driver can also be built as a module. If so, the module
+ will be called mpu6000.
+
+endchoice
+
+choice
+ prompt "Accelerometer Type"
+ depends on MPU_SENSORS_MPU3050
+ optional
+
+config MPU_SENSORS_KXTF9
+ bool "Kionix KXTF9"
+ help
+ This enables support for the Kionix KXFT9 accelerometer
+
+endchoice
+
+choice
+ prompt "Compass Type"
+ depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+ optional
+
+config MPU_SENSORS_AK8975
+ bool "AKM ak8975"
+ help
+ This enables support for the AKM ak8975 compass
+
+endchoice
+
+config MPU_SENSORS_TIMERIRQ
+ tristate "Timer IRQ"
+ help
+ If you say yes here you get access to the timerirq device handle which
+ can be used to select on. This can be used instead of IRQ's, sleeping,
+ or timer threads. Reading from this device returns the same type of
+ information as reading from the MPU and slave IRQ's.
+
+config MPU_SENSORS_DEBUG
+ bool "MPU debug"
+ depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 || MPU_SENSORS_TIMERIRQ
+ help
+ If you say yes here you get extra debug messages from the MPU3050
+ and other slave sensors.
+
+endmenu
+
diff --git a/drivers/misc/mpu3050/Makefile b/drivers/misc/mpu3050/Makefile
index c03e473e375b..c7e1b863e588 100644
--- a/drivers/misc/mpu3050/Makefile
+++ b/drivers/misc/mpu3050/Makefile
@@ -1,117 +1,128 @@
-
-# Kernel makefile for motions sensors
-#
-#
-
-# MPU
-obj-$(CONFIG_SENSORS_MPU3050) += mpu3050.o
-mpu3050-objs += mpuirq.o \
- slaveirq.o \
- mpu-dev.o \
- mpu-i2c.o \
- mlsl-kernel.o \
- mlos-kernel.o \
- $(MLLITE_DIR)mldl_cfg.o
-
-#
-# Accel options
-#
-ifdef CONFIG_SENSORS_ADXL346
-mpu3050-objs += $(MLLITE_DIR)accel/adxl346.o
-endif
-
-ifdef CONFIG_SENSORS_BMA150
-mpu3050-objs += $(MLLITE_DIR)accel/bma150.o
-endif
-
-ifdef CONFIG_SENSORS_BMA222
-mpu3050-objs += $(MLLITE_DIR)accel/bma222.o
-endif
-
-ifdef CONFIG_SENSORS_KXSD9
-mpu3050-objs += $(MLLITE_DIR)accel/kxsd9.o
-endif
-
-ifdef CONFIG_SENSORS_KXTF9_MPU
-mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
-endif
-
-ifdef CONFIG_SENSORS_LIS331DLH
-mpu3050-objs += $(MLLITE_DIR)accel/lis331.o
-endif
-
-ifdef CONFIG_SENSORS_LSM303DLHA
-mpu3050-objs += $(MLLITE_DIR)accel/lsm303a.o
-endif
-
-ifdef CONFIG_SENSORS_MMA8450
-mpu3050-objs += $(MLLITE_DIR)accel/mma8450.o
-endif
-
-ifdef CONFIG_SENSORS_MMA8451
-mpu3050-objs += $(MLLITE_DIR)accel/mma8451.o
-endif
-
-#
-# Compass options
-#
-ifdef CONFIG_SENSORS_AK8975_MPU
-mpu3050-objs += $(MLLITE_DIR)compass/ak8975.o
-endif
-
-ifdef CONFIG_SENSORS_AMI30X
-mpu3050-objs += $(MLLITE_DIR)compass/ami30x.o
-endif
-
-ifdef CONFIG_SENSORS_HMC5883
-mpu3050-objs += $(MLLITE_DIR)compass/hmc5883.o
-endif
-
-ifdef CONFIG_SENSORS_LSM303DLHM
-mpu3050-objs += $(MLLITE_DIR)compass/lsm303m.o
-endif
-
-ifdef CONFIG_SENSORS_MMC314X
-mpu3050-objs += $(MLLITE_DIR)compass/mmc314x.o
-endif
-
-ifdef CONFIG_SENSORS_YAS529
-mpu3050-objs += $(MLLITE_DIR)compass/yas529-kernel.o
-endif
-
-ifdef CONFIG_SENSORS_HSCDTD002B
-mpu3050-objs += $(MLLITE_DIR)compass/hscdtd002b.o
-endif
-
-#
-# Pressure options
-#
-ifdef CONFIG_SENSORS_BMA085
-mpu3050-objs += $(MLLITE_DIR)pressure/bma085.o
-endif
-
-EXTRA_CFLAGS += -I$(M)/$(MLLITE_DIR) \
- -I$(M)/../../include \
- -Idrivers/misc/mpu3050 \
- -Iinclude/linux
-
-ifdef CONFIG_SENSORS_MPU_DEBUG
-EXTRA_CFLAGS += -DDEBUG
-endif
-
-obj-$(CONFIG_SENSORS_MPU6000)= mpu6000.o
-mpu6000-objs += mpuirq.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_SENSORS_AK8975_MPU
-mpu6000-objs += $(MLLITE_DIR)compass/ak8975.o
-endif
-
-ifdef CONFIG_SENSORS_MPU6000
-EXTRA_CFLAGS += -DM_HW
-endif
+
+# 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 \
+ $(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_MPU_SENSORS_KXTF9
+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+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
+
+#
+# Compass options
+#
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu3050-objs += $(MLLITE_DIR)compass/ak8975.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_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
+
+EXTRA_CFLAGS += -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_AK8975
+mpu6000-objs += $(MLLITE_DIR)compass/ak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MPU6000
+EXTRA_CFLAGS += -DM_HW
+endif
+
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+
+ifdef CONFIG_MPU_SENSORS_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+
diff --git a/drivers/misc/mpu3050/accel/kxtf9.c b/drivers/misc/mpu3050/accel/kxtf9.c
index 11a9491dd7f1..e2490af1ca77 100644
--- a/drivers/misc/mpu3050/accel/kxtf9.c
+++ b/drivers/misc/mpu3050/accel/kxtf9.c
@@ -31,6 +31,9 @@
/* - Include Files. - */
/* ------------------ */
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
#ifdef __KERNEL__
#include <linux/module.h>
#endif
@@ -43,22 +46,322 @@
#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;
+};
+
/*****************************************
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, 0x1b, 0);
+ 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->suspend.reg_int_cfg1);
+ ERROR_CHECK(result);
+ /* WUF_THRESH: */
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+ KXTF9_WUF_THRESH,
+ private_data->suspend.reg_ths);
+ ERROR_CHECK(result);
+ /* DATA_CTRL_REG */
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+ KXTF9_DATA_CTRL_REG,
+ private_data->suspend.reg_odr);
+ ERROR_CHECK(result);
+ /* WUF_TIMER */
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+ KXTF9_WUF_TIMER, private_data->suspend.reg_dur);
ERROR_CHECK(result);
+
+ /* Normal operation */
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+ KXTF9_CTRL_REG1,
+ private_data->suspend.ctrl_reg1);
+ ERROR_CHECK(result);
+ result = MLSLSerialRead(mlsl_handle, pdata->address,
+ KXTF9_INT_REL, 1, &data);
+ ERROR_CHECK(result);
+
return result;
}
@@ -71,53 +374,245 @@ static int kxtf9_resume(void *mlsl_handle,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
- unsigned char reg;
+ unsigned char data;
+ struct kxtf9_private_data *private_data = pdata->private_data;
- /* RAM reset */
- result =
- MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1d, 0xcd);
- ERROR_CHECK(result);
- MLOSSleep(10);
/* Wake up */
- result =
- MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1b, 0x42);
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+ KXTF9_CTRL_REG1, 0x40);
ERROR_CHECK(result);
/* INT_CTRL_REG1: */
- result =
- MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1e, 0x14);
+ 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, 0x5a, 0x00);
+ 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, 0x21, 0x04);
+ 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, 0x29, 0x02);
- ERROR_CHECK(result);
-
- /* Full Scale */
- reg = 0xc2;
- reg &= ~ACCEL_KIONIX_CTRL_MASK;
- reg |= 0x00;
- 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,
+ KXTF9_WUF_TIMER, private_data->resume.reg_dur);
+ ERROR_CHECK(result);
/* Normal operation */
- result =
- MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1b, reg);
+ 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);
- MLOSSleep(50);
+
+ 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;
}
@@ -127,16 +622,29 @@ static int kxtf9_read(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
unsigned char *data)
{
- return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+ int result;
+ unsigned char reg;
+ result = MLSLSerialRead(mlsl_handle, pdata->address,
+ KXTF9_INT_SRC_REG2, 1, &reg);
+ ERROR_CHECK(result);
+
+ if (!(reg & 0x10))
+ return ML_ERROR_ACCEL_DATA_NOT_READY;
+
+ result = MLSLSerialRead(mlsl_handle, pdata->address,
+ slave->reg, slave->len, data);
+ ERROR_CHECK(result);
+ return result;
}
static struct ext_slave_descr kxtf9_descr = {
- /*.init = */ NULL,
- /*.exit = */ NULL,
+ /*.init = */ kxtf9_init,
+ /*.exit = */ kxtf9_exit,
/*.suspend = */ kxtf9_suspend,
/*.resume = */ kxtf9_resume,
/*.read = */ kxtf9_read,
- /*.config = */ NULL,
+ /*.config = */ kxtf9_config,
+ /*.get_config = */ kxtf9_get_config,
/*.name = */ "kxtf9",
/*.type = */ EXT_SLAVE_TYPE_ACCELEROMETER,
/*.id = */ ACCEL_ID_KXTF9,
diff --git a/drivers/misc/mpu3050/compass/ak8975.c b/drivers/misc/mpu3050/compass/ak8975.c
index 18606e240f1c..991de77dbd01 100644
--- a/drivers/misc/mpu3050/compass/ak8975.c
+++ b/drivers/misc/mpu3050/compass/ak8975.c
@@ -25,12 +25,14 @@
* @{
* @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
@@ -84,44 +86,78 @@ int ak8975_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata, unsigned char *data)
{
- unsigned char stat;
- unsigned char stat2;
+ unsigned char regs[8];
+ unsigned char *stat = &regs[0];
+ unsigned char *stat2 = &regs[7];
int result = ML_SUCCESS;
+ int status = ML_SUCCESS;
result =
- MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1, 1,
- &stat);
+ MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1,
+ 8, regs);
ERROR_CHECK(result);
- if (stat & 0x01) {
- result =
- MLSLSerialRead(mlsl_handle, pdata->address,
- AK8975_REG_HXL, 6,
- (unsigned char *) data);
- ERROR_CHECK(result);
- result =
- MLSLSerialRead(mlsl_handle, pdata->address,
- AK8975_REG_ST2, 1, &stat2);
- ERROR_CHECK(result);
- if (stat2 & 0x04) /* data error */
- return ML_ERROR_COMPASS_DATA_NOT_READY;
- if (stat2 & 0x08)
- return ML_ERROR_COMPASS_DATA_OVERFLOW;
+ /*
+ * ST : data ready -
+ * Measurement has been completed and data is ready to be read.
+ */
+ if (*stat & 0x01) {
+ memcpy(data, &regs[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 ML_SUCCESS;
- } else if (stat & 0x02) {
- result =
- MLSLSerialRead(mlsl_handle, pdata->address,
- AK8975_REG_ST2, 1, &stat2);
- ERROR_CHECK(result);
- return ML_ERROR_COMPASS_DATA_OVERFLOW;
- } else {
- return ML_ERROR_COMPASS_DATA_NOT_READY;
}
+
+ return status;
}
struct ext_slave_descr ak8975_descr = {
@@ -131,6 +167,7 @@ struct ext_slave_descr ak8975_descr = {
/*.resume = */ ak8975_resume,
/*.read = */ ak8975_read,
/*.config = */ NULL,
+ /*.get_config = */ NULL,
/*.name = */ "ak8975",
/*.type = */ EXT_SLAVE_TYPE_COMPASS,
/*.id = */ COMPASS_ID_AKM,
@@ -148,4 +185,4 @@ EXPORT_SYMBOL(ak8975_get_slave_descr);
/**
* @}
-**/
+ */
diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c
index 668a4cf03ccc..66f447e7ba85 100644
--- a/drivers/misc/mpu3050/mldl_cfg.c
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -57,99 +57,85 @@
#define STANDBY 1
#endif
-/* --------------------- */
-/* - Prototypes. - */
-/* --------------------- */
-#ifdef M_HW
-static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
- void *mlsl_handle,
- unsigned char reset,
- unsigned char powerselection);
-static tMLError MLDLStandByGyros(struct mldl_cfg *pdata,
- void *mlsl_handle,
- unsigned char disable_gx,
- unsigned char disable_gy,
- unsigned char disable_gz);
-#else
-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);
-#endif
+/*---------------------*/
+/*- Prototypes. -*/
+/*---------------------*/
-/* ---------------------- */
-/* - Static Functions. - */
-/* ---------------------- */
+/*----------------------*/
+/*- Static Functions. -*/
+/*----------------------*/
-/**
- * @internal
- * @brief MLDLCfgDMP configures the Digital Motion Processor internal to
- * the MPU. The DMP can be enabled or disabled and the start address
- * can be set.
- *
- * @param enableRun Enables the DMP processing if set to TRUE.
- * @param enableFIFO Enables DMP output to the FIFO if set to TRUE.
- *
- * @return Zero if the command is successful, an error code otherwise.
- */
-static int MLDLCtrlDmp(struct mldl_cfg *pdata, void *mlsl_handle,
- bool enableRun, bool enableFIFO)
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
{
- unsigned char b;
+ unsigned char userCtrlReg;
+ int result;
- MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_USER_CTRL, 1, &b);
- if (enableRun)
- b |= BIT_DMP_EN;
- else
- b &= ~BIT_DMP_EN;
+ if (!mldl_cfg->dmp_is_running)
+ return ML_SUCCESS;
- if (enableFIFO)
- b |= BIT_FIFO_EN;
+ 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;
- b |= BIT_DMP_RST;
+ result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+ MPUREG_USER_CTRL, userCtrlReg);
+ ERROR_CHECK(result);
+ mldl_cfg->dmp_is_running = 0;
- MLSLSerialWriteSingle(mlsl_handle, pdata->addr, MPUREG_USER_CTRL,
- b);
+ return result;
- return ML_SUCCESS;
}
-
/**
* @brief Starts the DMP running
*
* @return ML_SUCCESS or non-zero error code
*/
-static int MLDLDmpStart(struct mldl_cfg *pdata, void *mlsl_handle)
+static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
{
- unsigned char fifoBuf[2];
- unsigned char tries = 0;
unsigned char userCtrlReg;
int result;
- unsigned short len = !0;
+
+ 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);
- while (len != 0 && tries < 6) {
- result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
- MPUREG_USER_CTRL,
- ((userCtrlReg & (~BIT_FIFO_EN))
- | BIT_FIFO_RST));
- MLSLSerialRead(mlsl_handle, pdata->addr,
- MPUREG_FIFO_COUNTH, 2, fifoBuf);
- len = (((unsigned short) fifoBuf[0] << 8)
- | (unsigned short) fifoBuf[1]);
- tries++;
- }
+ 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;
- MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
- MPUREG_USER_CTRL, userCtrlReg);
+ userCtrlReg |= BIT_DMP_RST;
- return MLDLCtrlDmp(pdata, mlsl_handle,
- pdata->dmp_enable, pdata->fifo_enable);
+ result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+ MPUREG_USER_CTRL, userCtrlReg);
+ ERROR_CHECK(result);
+ pdata->dmp_is_running = pdata->dmp_enable;
+
+ return result;
}
/**
@@ -166,22 +152,15 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
unsigned char b;
int result;
-#ifdef ML_USE_DMP_SIM
- /* done this way so that pc demo */
- /* w/arm board works with universal api */
- if (!MLGetGyroPresent())
+ if ((mldl_cfg->gyro_is_bypassed && enable) ||
+ (!mldl_cfg->gyro_is_bypassed && !enable))
return ML_SUCCESS;
-#endif
/*---- get current 'USER_CTRL' into b ----*/
result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL, 1, &b);
ERROR_CHECK(result);
- /* No change */
- if ((b & BIT_AUX_IF_EN) != (enable * BIT_AUX_IF_EN))
- return ML_SUCCESS;
-
b &= ~BIT_AUX_IF_EN;
if (!enable) {
@@ -215,7 +194,7 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
* 3) wait for up to one MPU cycle then restore the slave
* address
*/
- MLOSSleep(5);
+ MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_AUX_SLV_ADDR,
mldl_cfg->pdata->
@@ -238,6 +217,7 @@ static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
ERROR_CHECK(result);
MLOSSleep(2);
}
+ mldl_cfg->gyro_is_bypassed = enable;
return result;
}
@@ -254,11 +234,14 @@ struct tsProdRevMap {
#define OLDEST_PROD_REV_SUPPORTED 1
static struct tsProdRevMap prodRevsMap[] = {
{0, 0},
- {MPU_SILICON_REV_A1, 131}, /* 1 A1 */
- {MPU_SILICON_REV_A1, 131}, /* 2 A1 */
- {MPU_SILICON_REV_A1, 131}, /* 3 A1 */
- {MPU_SILICON_REV_A1, 131}, /* 4 A1 */
- {MPU_SILICON_REV_A1, 131}, /* 5 A1 */
+ {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 */
@@ -411,11 +394,11 @@ static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata,
/**
* @internal
* @param reset 1 to reset hardware
-**/
-static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
- void *mlsl_handle,
- unsigned char reset,
- unsigned char powerselection)
+ */
+static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata,
+ void *mlsl_handle,
+ unsigned char reset,
+ unsigned char powerselection)
{
unsigned char b;
tMLError result;
@@ -434,7 +417,7 @@ static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
/* 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 | 0x80);
+ MPUREG_PWR_MGM, b | BIT_H_RESET);
#define M_HW_RESET_ERRATTA
#ifndef M_HW_RESET_ERRATTA
ERROR_CHECK(result);
@@ -446,9 +429,9 @@ static tMLError MLDLPowerMgmtMantis(struct mldl_cfg *pdata,
b |= (powerselection << 4);
if (b & BITS_PWRSEL)
- pdata->is_suspended = FALSE;
+ pdata->gyro_is_suspended = FALSE;
else
- pdata->is_suspended = TRUE;
+ pdata->gyro_is_suspended = TRUE;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_PWR_MGM, b);
@@ -570,29 +553,34 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
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) {
- result =
- MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
- MPUREG_PWR_MGM, b | BIT_H_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);
}
- /* 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->is_suspended = TRUE;
+ pdata->gyro_is_suspended = TRUE;
else
- pdata->is_suspended = FALSE;
+ pdata->gyro_is_suspended = FALSE;
- if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG))
- == (((sleep != 0) * BIT_SLEEP) |
+ /* 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))) {
@@ -608,7 +596,7 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
*/
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)) {
+ && ((!sleep) && disable_gx && disable_gy && disable_gz)) {
result = MLDLPowerMgmtMPU(pdata, mlsl_handle, 0, 1, 0, 0, 0);
if (result)
return result;
@@ -616,7 +604,6 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
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);
@@ -626,26 +613,25 @@ static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_PWR_MGM, b);
ERROR_CHECK(result);
- pdata->is_suspended = TRUE;
+ pdata->gyro_is_suspended = TRUE;
} else {
b &= ~BIT_SLEEP;
-
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_PWR_MGM, b);
ERROR_CHECK(result);
- pdata->is_suspended = FALSE;
+ 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
- ---*/
+ /*---
+ 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);
+ MPUREG_PWR_MGM, b);
ERROR_CHECK(result);
}
if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
@@ -701,6 +687,8 @@ void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
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",
@@ -839,6 +827,275 @@ void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
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,
+ &reg);
+ 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, &reg);
+ 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, &reg);
+ 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, &reg);
+ 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, &reg);
+ 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
@@ -877,6 +1134,13 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
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;
@@ -891,20 +1155,21 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
* read the product_id, sillicon rev and whoami
*/
#ifdef M_HW
- result =
- MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, RESET, WAKE_UP);
+ result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+ RESET, WAKE_UP);
#else
- result =
- MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
+ 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,
@@ -922,7 +1187,8 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
/* Configure the MPU */
#ifdef M_HW
- result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, SLEEP);
+ result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+ FALSE, SLEEP);
#else
result =
MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
@@ -931,26 +1197,25 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
if (mldl_cfg->accel && mldl_cfg->accel->init) {
result = mldl_cfg->accel->init(accel_handle,
- mldl_cfg->accel,
- &mldl_cfg->pdata->accel);
+ 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);
+ 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);
+ mldl_cfg->pressure,
+ &mldl_cfg->pdata->pressure);
if (ML_SUCCESS != result) {
MPL_LOGE("mldl_cfg->pressure->init returned %d\n",
result);
@@ -958,6 +1223,16 @@ int mpu3050_open(struct mldl_cfg *mldl_cfg,
}
}
+ 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:
@@ -1028,7 +1303,10 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
/**
* @brief resume the MPU3050 device and all the other sensor
* devices from their low power state.
- * @param mlsl_handle
+ *
+ * @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
@@ -1045,6 +1323,10 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
* 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
@@ -1060,318 +1342,131 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
* @return ML_SUCCESS or a non-zero error code.
*/
int mpu3050_resume(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
+ void *gyro_handle,
void *accel_handle,
void *compass_handle,
void *pressure_handle,
- bool resume_accel, bool resume_compass, bool resume_pressure)
+ bool resume_gyro,
+ bool resume_accel,
+ bool resume_compass,
+ bool resume_pressure)
{
- int result;
- int ii;
- int jj;
- unsigned char reg;
- unsigned char regs[7];
-
+ int result = ML_SUCCESS;
-#ifdef CONFIG_SENSORS_MPU_DEBUG
+#ifdef CONFIG_MPU_SENSORS_DEBUG
mpu_print_cfg(mldl_cfg);
#endif
- /* Wake up the part */
-#ifdef M_HW
- result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, WAKE_UP);
-#else
- result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 1, 0,
- mldl_cfg->gyro_power & BIT_STBY_XG,
- mldl_cfg->gyro_power & BIT_STBY_YG,
- mldl_cfg->gyro_power & BIT_STBY_ZG);
-#endif
- ERROR_CHECK(result);
-
- /* Configure the MPU */
-#ifdef M_HW
- result = MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 1);
- ERROR_CHECK(result);
- result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, RESET, SLEEP);
- ERROR_CHECK(result);
- result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, WAKE_UP);
- ERROR_CHECK(result);
- /* setting int_config with the propert flag BIT_BYPASS_EN should be
- done by the setup functions */
- result = MLSLSerialWriteSingle(mlsl_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(mlsl_handle, mldl_cfg->addr,
- MPUREG_INT_ENABLE,
- (mldl_cfg->int_config & 0x0f));
- ERROR_CHECK(result);
-#else
- result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
- MPUREG_INT_CFG,
- (mldl_cfg->
- int_config | mldl_cfg->pdata->
- int_config));
- ERROR_CHECK(result);
-#endif
-
- result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
- MPUREG_PWR_MGM, 1, &reg);
- ERROR_CHECK(result);
- reg &= ~BITS_CLKSEL;
- result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
- MPUREG_PWR_MGM,
- mldl_cfg->clk_src | reg);
- ERROR_CHECK(result);
- result = MLSLSerialWriteSingle(mlsl_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(mlsl_handle, mldl_cfg->addr,
- MPUREG_GYRO_CONFIG, reg);
- reg = DLPF_FS_SYNC_VALUE(0, 0, mldl_cfg->lpf);
- result = MLSLSerialWriteSingle(mlsl_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(mlsl_handle, mldl_cfg->addr,
- MPUREG_DLPF_FS_SYNC, reg);
-#endif
- ERROR_CHECK(result);
- result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
- MPUREG_DMP_CFG_1,
- mldl_cfg->dmp_cfg1);
- ERROR_CHECK(result);
- result = MLSLSerialWriteSingle(mlsl_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];
+ 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;
- result = MLSLSerialWriteMem(mlsl_handle, mldl_cfg->addr,
- ((ii << 8) | 0x00),
- MPU_MEM_BANK_SIZE,
- mldl_cfg->ram[ii]);
- ERROR_CHECK(result);
- result = MLSLSerialReadMem(mlsl_handle, mldl_cfg->addr,
- ((ii << 8) | 0x00),
- MPU_MEM_BANK_SIZE, read);
+ if (resume_gyro && mldl_cfg->gyro_is_suspended) {
+ result = gyro_resume(mldl_cfg, gyro_handle);
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(mlsl_handle, mldl_cfg->addr,
- MPUREG_XG_OFFS_TC,
- mldl_cfg->offset_tc[0]);
- ERROR_CHECK(result);
- result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
- MPUREG_YG_OFFS_TC,
- mldl_cfg->offset_tc[1]);
- ERROR_CHECK(result);
- result = MLSLSerialWriteSingle(mlsl_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(mlsl_handle, mldl_cfg->addr, 7, regs);
- ERROR_CHECK(result);
-
- /* Configure slaves */
- result = MLDLSetLevelShifterBit(mldl_cfg, mlsl_handle,
- mldl_cfg->pdata->level_shifter);
- ERROR_CHECK(result);
-
- if (resume_accel) {
- if ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume))
- return ML_ERROR_INVALID_PARAMETER;
+ 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);
+ }
result = mldl_cfg->accel->resume(accel_handle,
mldl_cfg->accel,
&mldl_cfg->pdata->accel);
ERROR_CHECK(result);
- if (EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
- /* Address */
- result =
- MLSLSerialWriteSingle(accel_handle,
- mldl_cfg->addr,
- MPUREG_AUX_SLV_ADDR,
- mldl_cfg->pdata->
- accel.address);
- ERROR_CHECK(result);
- /* Register */
- result =
- MLSLSerialRead(accel_handle, mldl_cfg->addr,
- MPUREG_ACCEL_BURST_ADDR, 1,
- &reg);
- ERROR_CHECK(result);
- reg = ((reg & 0x80) | mldl_cfg->accel->reg);
- /* Set VDDIO bit for ST accel */
- if ((ACCEL_ID_LIS331 == mldl_cfg->accel->id)
- || (ACCEL_ID_LSM303 == mldl_cfg->accel->id)) {
- reg |= 0x80;
- }
- result =
- MLSLSerialWriteSingle(accel_handle,
- mldl_cfg->addr,
- MPUREG_ACCEL_BURST_ADDR,
- reg);
- ERROR_CHECK(result);
- /* Length */
- result =
- MLSLSerialRead(accel_handle, mldl_cfg->addr,
- MPUREG_USER_CTRL, 1, &reg);
- ERROR_CHECK(result);
- reg = (reg & ~BIT_AUX_RD_LENG);
- result =
- MLSLSerialWriteSingle(accel_handle,
- mldl_cfg->addr,
- MPUREG_USER_CTRL, reg);
- ERROR_CHECK(result);
- result =
- MLDLSetI2CBypass(mldl_cfg, accel_handle, 0);
- ERROR_CHECK(result);
- }
+ mldl_cfg->accel_is_suspended = FALSE;
}
- if (resume_compass) {
- if ((mldl_cfg->compass) && (mldl_cfg->compass->resume)) {
- result = mldl_cfg->compass->resume(compass_handle,
- mldl_cfg->compass,
- &mldl_cfg->pdata->
- compass);
+ 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);
- if (EXT_SLAVE_BUS_SECONDARY ==
- mldl_cfg->pdata->compass.bus) {
- /* Address */
- result =
- MLSLSerialWriteSingle(mlsl_handle,
- mldl_cfg->addr,
- MPUREG_AUX_SLV_ADDR,
- mldl_cfg->pdata->
- compass.address);
- ERROR_CHECK(result);
- /* Register */
- result =
- MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
- MPUREG_ACCEL_BURST_ADDR, 1,
- &reg);
- ERROR_CHECK(result);
- reg = ((reg & 0x80) | mldl_cfg->compass->reg);
- result =
- MLSLSerialWriteSingle(mlsl_handle,
- mldl_cfg->addr,
- MPUREG_ACCEL_BURST_ADDR,
- reg);
- ERROR_CHECK(result);
+ mldl_cfg->compass_is_suspended = FALSE;
+ }
-#ifdef M_HW
- /* Length, byte swapping, grouping & enable */
- if (mldl_cfg->compass->len > BITS_SLV_LENG) {
- MPL_LOGW("Limiting slave burst read length to "
- "the allowed maximum (15B, req. %d)\n",
- mldl_cfg->compass->len);
- mldl_cfg->compass->len = BITS_SLV_LENG;
- }
- reg = mldl_cfg->compass->len;
- if (mldl_cfg->compass->endian ==
- EXT_SLAVE_LITTLE_ENDIAN)
- reg |= BIT_SLV_BYTE_SW;
- reg |= BIT_SLV_GRP;
- reg |= BIT_SLV_ENABLE;
+ 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);
+ }
- result =
- MLSLSerialWriteSingle(mlsl_handle,
- mldl_cfg->addr,
- MPUREG_I2C_SLV0_CTRL,
- reg);
-#else
- /* Length */
- result =
- MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
- MPUREG_USER_CTRL, 1, &reg);
- ERROR_CHECK(result);
- reg = (reg & ~BIT_AUX_RD_LENG);
- result =
- MLSLSerialWriteSingle(mlsl_handle,
- mldl_cfg->addr,
- MPUREG_USER_CTRL, reg);
- ERROR_CHECK(result);
-#endif
- result =
- MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 0);
+ 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 (resume_pressure) {
- if ((mldl_cfg->pressure) && (mldl_cfg->pressure->resume)) {
- result = mldl_cfg->pressure->resume(pressure_handle,
- mldl_cfg->pressure,
- &mldl_cfg->pdata->
- pressure);
- ERROR_CHECK(result);
- }
+ 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 */
- result = MLDLDmpStart(mldl_cfg, mlsl_handle);
- ERROR_CHECK(result);
+ 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 mlsl_handle
+ * @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
+ * 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 mlsl_handle if
+ * 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 mlsl_handle if
+ * onto a separate bus. Can match gyro_handle if
* the pressure sensor device operates on the same
* primary bus of MPU.
* @param accel
@@ -1388,44 +1483,77 @@ int mpu3050_resume(struct mldl_cfg *mldl_cfg,
* mode of some sort).
* @return ML_SUCCESS or a non-zero error code.
*/
-int mpu3050_suspend(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+ void *gyro_handle,
void *accel_handle,
void *compass_handle,
void *pressure_handle,
- bool accel, bool compass, bool pressure)
+ bool suspend_gyro,
+ bool suspend_accel,
+ bool suspend_compass,
+ bool suspend_pressure)
{
- int result;
- /* This puts the bus into bypass mode */
+ int result = ML_SUCCESS;
+ if (suspend_gyro && !mldl_cfg->gyro_is_suspended) {
#ifdef M_HW
- result = MLDLSetI2CBypass(mldl_cfg, mlsl_handle, 1);
- ERROR_CHECK(result);
- result = MLDLPowerMgmtMantis(mldl_cfg, mlsl_handle, 0, SLEEP);
+ 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, mlsl_handle, 0, SLEEP, 0, 0, 0);
+ result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle,
+ 0, SLEEP, 0, 0, 0);
#endif
- if (ML_SUCCESS == result &&
- accel && mldl_cfg->accel && mldl_cfg->accel->suspend) {
+ 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);
+ }
result = mldl_cfg->accel->suspend(accel_handle,
mldl_cfg->accel,
&mldl_cfg->pdata->accel);
+ ERROR_CHECK(result);
+ mldl_cfg->accel_is_suspended = TRUE;
}
- if (ML_SUCCESS == result && compass &&
+ 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 (ML_SUCCESS == result && pressure &&
+ 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;
}
@@ -1434,18 +1562,26 @@ int mpu3050_suspend(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
/**
* @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 *mlsl_handle, unsigned char *data)
+ void *accel_handle, unsigned char *data)
{
if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read)
- return mldl_cfg->accel->read(mlsl_handle,
- mldl_cfg->accel,
- &mldl_cfg->pdata->accel,
- data);
+ 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;
}
@@ -1453,18 +1589,26 @@ int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
/**
* @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 *mlsl_handle, unsigned char *data)
+ void *compass_handle, unsigned char *data)
{
if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read)
- return mldl_cfg->compass->read(mlsl_handle,
- mldl_cfg->compass,
- &mldl_cfg->pdata->compass,
- data);
+ 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;
}
@@ -1472,18 +1616,27 @@ int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
/**
* @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 *mlsl_handle, unsigned char *data)
+ void *pressure_handle, unsigned char *data)
{
if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read)
- return mldl_cfg->pressure->read(mlsl_handle,
- mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure,
- data);
+ 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;
}
@@ -1494,9 +1647,9 @@ int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
{
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);
+ mldl_cfg->accel,
+ &mldl_cfg->pdata->accel,
+ data);
else
return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
@@ -1508,9 +1661,9 @@ int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
{
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);
+ mldl_cfg->compass,
+ &mldl_cfg->pdata->compass,
+ data);
else
return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
@@ -1522,6 +1675,48 @@ int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
{
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);
diff --git a/drivers/misc/mpu3050/mldl_cfg.h b/drivers/misc/mpu3050/mldl_cfg.h
index d714a0ee3ed8..ee3a1e3dd05f 100644
--- a/drivers/misc/mpu3050/mldl_cfg.h
+++ b/drivers/misc/mpu3050/mldl_cfg.h
@@ -39,12 +39,52 @@
/* - Defines. - */
/* --------------------- */
-#define SAMPLING_PERIOD(mldl_cfg) \
- (((((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7)) \
+ /*************************************************************************/
+ /* 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))
+ / ((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. - */
/* --------------------- */
@@ -52,6 +92,7 @@
/* 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;
@@ -74,13 +115,20 @@ struct mldl_cfg {
unsigned short trim;
/* Driver/Kernel related state information */
- int is_suspended;
+ 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;
};
@@ -96,20 +144,23 @@ int mpu3050_close(struct mldl_cfg *mldl_cfg,
void *compass_handle,
void *pressure_handle);
int mpu3050_resume(struct mldl_cfg *mldl_cfg,
- void *mlsl_handle,
+ 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 *mlsl_handle,
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+ void *gyro_handle,
void *accel_handle,
void *compass_handle,
void *pressure_handle,
- bool resume_accel,
- bool resume_compass,
- bool resume_pressure);
+ 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);
@@ -129,6 +180,16 @@ 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/mltypes.h b/drivers/misc/mpu3050/mltypes.h
index 447e208ee0a5..5c1b684e5b50 100644
--- a/drivers/misc/mpu3050/mltypes.h
+++ b/drivers/misc/mpu3050/mltypes.h
@@ -16,11 +16,6 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
$
*/
-/******************************************************************************
- *
- * $Id: mltypes.h 4436 2011-01-13 05:07:53Z mcaramello $
- *
- *****************************************************************************/
/**
* @defgroup MLERROR
@@ -68,6 +63,7 @@
* - (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
@@ -93,11 +89,12 @@
---------------------------*/
/**
- * @struct tMLError The MPL Error Code return type.
+ * @struct tMLError mltypes.h "mltypes"
+ * @brief The MPL Error Code return type.
*
- * @code
+ * @code
* typedef unsigned char tMLError;
- * @endcode
+ * @endcode
*/
typedef unsigned char tMLError;
@@ -204,6 +201,7 @@ typedef int_fast8_t bool;
#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)
@@ -211,9 +209,15 @@ typedef int_fast8_t bool;
#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)
+#define EXPORT_SYMBOL(x)
#endif
/*---------------------------
diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c
index 6cde9eeccced..115639f84704 100644
--- a/drivers/misc/mpu3050/mpu-dev.c
+++ b/drivers/misc/mpu3050/mpu-dev.c
@@ -73,6 +73,10 @@ static struct i2c_client *this_client;
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;
+
dev_dbg(&this_client->adapter->dev, "mpu_open\n");
dev_dbg(&this_client->adapter->dev, "current->pid %d\n",
current->pid);
@@ -81,6 +85,18 @@ static int mpu_open(struct inode *inode, struct file *file)
/* 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;
}
@@ -92,26 +108,20 @@ static int mpu_release(struct inode *inode, struct file *file)
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;
- if (!mldl_cfg->is_suspended) {
- 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);
- result =
- mpu3050_suspend(mldl_cfg, client->adapter,
- accel_adapter, compass_adapter,
- pressure_adapter,
- TRUE, TRUE, TRUE);
- }
+ 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);
dev_dbg(&this_client->adapter->dev, "mpu_release\n");
return result;
@@ -220,8 +230,11 @@ static ssize_t mpu_read(struct file *file,
/* @todo fix this to do a i2c trasnfer from the FIFO */
ret = i2c_master_recv(client, tmp, count);
- if (ret >= 0)
+ if (ret >= 0) {
ret = copy_to_user(buf, tmp, count) ? -EFAULT : ret;
+ if (ret)
+ ret = -EFAULT;
+ }
kfree(tmp);
return ret;
}
@@ -230,15 +243,14 @@ static int
mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg)
{
int ii;
- int result;
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;
- result = copy_from_user(&local_pdata,
- (unsigned char *) arg,
- sizeof(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++)
@@ -262,27 +274,93 @@ mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg)
dev_dbg(&client->adapter->dev, "%s\n", __func__);
- return result;
+ 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 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(mldl_cfg, (struct mldl_cfg *) arg,
- offsetof(struct mldl_cfg, silicon_revision)))
- return -EFAULT;
+ if (copy_from_user(temp_mldl_cfg, (struct mldl_cfg __user *) arg,
+ offsetof(struct mldl_cfg, silicon_revision))) {
+ result = -EFAULT;
+ goto out;
+ }
- return 0;
+ 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
@@ -301,53 +379,189 @@ mpu_ioctl_get_mpu_config(struct i2c_client *client, unsigned long arg)
return -ENOMEM;
retval =
- copy_from_user(local_mldl_cfg, (void *) arg,
+ copy_from_user(local_mldl_cfg, (struct mldl_cfg __user *) arg,
sizeof(struct mldl_cfg));
- if (retval)
+ 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(local_mldl_cfg->accel,
+ retval = copy_to_user((void __user *)local_mldl_cfg->accel,
mldl_cfg->accel,
sizeof(*mldl_cfg->accel));
- if (retval)
+ 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(local_mldl_cfg->compass,
+ retval = copy_to_user((void __user *)local_mldl_cfg->compass,
mldl_cfg->compass,
sizeof(*mldl_cfg->compass));
- if (retval)
+ 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(local_mldl_cfg->pressure,
+ retval = copy_to_user((void __user *)local_mldl_cfg->pressure,
mldl_cfg->pressure,
sizeof(*mldl_cfg->pressure));
- if (retval)
+ 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(local_mldl_cfg->pdata,
+ retval = copy_to_user((void __user *)local_mldl_cfg->pdata,
mldl_cfg->pdata,
sizeof(*mldl_cfg->pdata));
- if (retval)
+ 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 *) arg,
+ 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)
@@ -417,13 +631,18 @@ static long mpu_ioctl(struct file *file,
break;
case MPU_SET_OFFSET_TC:
retval = copy_from_user(mldl_cfg->offset_tc,
- (unsigned char *) arg,
+ (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 *) arg,
+ (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);
@@ -432,282 +651,169 @@ static long mpu_ioctl(struct file *file,
retval = mpu_ioctl_get_mpu_config(client, arg);
break;
case MPU_GET_INT_CONFIG:
- mldl_cfg->int_config = (unsigned char) arg;
+ retval = put_user(mldl_cfg->int_config,
+ (unsigned char __user *) arg);
break;
case MPU_GET_EXT_SYNC:
- mldl_cfg->ext_sync = (enum mpu_ext_sync) arg;
+ retval = put_user(mldl_cfg->ext_sync,
+ (unsigned char __user *) arg);
break;
case MPU_GET_FULL_SCALE:
- mldl_cfg->full_scale = (enum mpu_fullscale) arg;
+ retval = put_user(mldl_cfg->full_scale,
+ (unsigned char __user *) arg);
break;
case MPU_GET_LPF:
- mldl_cfg->lpf = (enum mpu_filter) arg;
+ retval = put_user(mldl_cfg->lpf,
+ (unsigned char __user *) arg);
break;
case MPU_GET_CLK_SRC:
- mldl_cfg->clk_src = (enum mpu_clock_sel) arg;
+ retval = put_user(mldl_cfg->clk_src,
+ (unsigned char __user *) arg);
break;
case MPU_GET_DIVIDER:
- mldl_cfg->divider = (unsigned char) arg;
+ retval = put_user(mldl_cfg->divider,
+ (unsigned char __user *) arg);
break;
case MPU_GET_LEVEL_SHIFTER:
- mldl_cfg->pdata->level_shifter = (unsigned char) arg;
+ retval = put_user(mldl_cfg->pdata->level_shifter,
+ (unsigned char __user *) arg);
break;
case MPU_GET_DMP_ENABLE:
- mldl_cfg->dmp_enable = (unsigned char) arg;
+ retval = put_user(mldl_cfg->dmp_enable,
+ (unsigned char __user *) arg);
break;
case MPU_GET_FIFO_ENABLE:
- mldl_cfg->fifo_enable = (unsigned char) arg;
+ retval = put_user(mldl_cfg->fifo_enable,
+ (unsigned char __user *) arg);
break;
case MPU_GET_DMP_CFG1:
- mldl_cfg->dmp_cfg1 = (unsigned char) arg;
+ retval = put_user(mldl_cfg->dmp_cfg1,
+ (unsigned char __user *) arg);
break;
case MPU_GET_DMP_CFG2:
- mldl_cfg->dmp_cfg2 = (unsigned char) arg;
+ retval = put_user(mldl_cfg->dmp_cfg2,
+ (unsigned char __user *) arg);
break;
case MPU_GET_OFFSET_TC:
- retval = copy_to_user((unsigned char *) arg,
+ 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 *) arg,
+ retval = copy_to_user((unsigned char __user *) arg,
mldl_cfg->ram,
sizeof(mldl_cfg->ram));
+ if (retval)
+ retval = -EFAULT;
break;
case MPU_CONFIG_ACCEL:
- {
- if ((mldl_cfg->accel) && (mldl_cfg->accel->config)) {
- struct ext_slave_config config;
- retval = copy_from_user(
- &config,
- (struct ext_slave_config *)arg,
- sizeof(config));
- if (retval)
- break;
-
- if (config.len && config.data) {
- int *data;
- data = kzalloc(config.len, GFP_KERNEL);
- if (!data) {
- retval = ML_ERROR_MEMORY_EXAUSTED;
- break;
- }
- retval = copy_from_user(data,
- (void *)config.data,
- config.len);
- if (retval) {
- kfree(data);
- break;
- }
- config.data = data;
- }
- retval = mldl_cfg->accel->config(
- accel_adapter,
+ retval = slave_config(accel_adapter, mldl_cfg,
mldl_cfg->accel,
- &mldl_cfg->pdata->accel,
- &config);
- kfree(config.data);
- }
+ (struct ext_slave_config __user *) arg);
break;
- }
case MPU_CONFIG_COMPASS:
- {
- if ((mldl_cfg->compass) && (mldl_cfg->compass->config)) {
- struct ext_slave_config config;
- retval = copy_from_user(
- &config,
- (struct ext_slave_config *)arg,
- sizeof(config));
- if (retval)
- break;
-
- if (config.len && config.data) {
- int *data;
- data = kzalloc(config.len, GFP_KERNEL);
- if (!data) {
- retval = ML_ERROR_MEMORY_EXAUSTED;
- break;
- }
- retval = copy_from_user(data,
- (void *)config.data,
- config.len);
- if (retval) {
- kfree(data);
- break;
- }
- config.data = data;
- }
- retval = mldl_cfg->compass->config(
- compass_adapter,
+ retval = slave_config(compass_adapter, mldl_cfg,
mldl_cfg->compass,
- &mldl_cfg->pdata->compass,
- &config);
- kfree(config.data);
- }
+ (struct ext_slave_config __user *) arg);
break;
- }
case MPU_CONFIG_PRESSURE:
- {
- if ((mldl_cfg->pressure) && (mldl_cfg->pressure->config)) {
- struct ext_slave_config config;
- retval = copy_from_user(
- &config,
- (struct ext_slave_config *)arg,
- sizeof(config));
- if (retval)
- break;
-
- if (config.len && config.data) {
- int *data;
- data = kzalloc(config.len, GFP_KERNEL);
- if (!data) {
- retval = ML_ERROR_MEMORY_EXAUSTED;
- break;
- }
- retval = copy_from_user(data,
- (void *)config.data,
- config.len);
- if (retval) {
- kfree(data);
- break;
- }
- config.data = data;
- }
- retval = mldl_cfg->pressure->config(
- pressure_adapter,
+ retval = slave_config(pressure_adapter, mldl_cfg,
mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure,
- &config);
- kfree(config.data);
- }
+ (struct ext_slave_config __user *) arg);
break;
- }
- case MPU_READ_MEMORY:
- case MPU_WRITE_MEMORY:
- case MPU_SUSPEND:
- {
- struct mpu_suspend_resume suspend;
- retval =
- copy_from_user(&suspend,
- (struct mpu_suspend_resume *)
- arg, sizeof(suspend));
- if (retval)
- break;
- if (suspend.gyro) {
- retval =
- mpu3050_suspend(mldl_cfg,
- client->adapter,
- accel_adapter,
- compass_adapter,
- pressure_adapter,
- suspend.accel,
- suspend.compass,
- suspend.pressure);
- } else {
- /* Cannot suspend the pressure compass or
- * accel while the MPU is running */
- retval = ML_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- }
+ 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_RESUME:
- {
- struct mpu_suspend_resume resume;
- retval =
- copy_from_user(&resume,
- (struct mpu_suspend_resume *)
- arg, sizeof(resume));
- if (retval)
- break;
- if (resume.gyro) {
- retval =
- mpu3050_resume(mldl_cfg,
- client->adapter,
- accel_adapter,
- compass_adapter,
- pressure_adapter,
- resume.accel,
- resume.compass,
- resume.pressure);
- } else if (mldl_cfg->is_suspended) {
- if (resume.accel) {
- retval =
- mldl_cfg->
- accel->resume(accel_adapter,
- mldl_cfg->accel,
- &mldl_cfg->
- pdata->accel);
- if (retval)
- break;
- }
-
- if (resume.compass) {
- retval =
- mldl_cfg->
- compass->resume
- (compass_adapter,
- mldl_cfg->compass,
- &mldl_cfg->pdata->compass);
- if (retval)
- break;
- }
-
- if (resume.pressure)
- retval =
- mldl_cfg->
- pressure->resume
- (pressure_adapter,
- mldl_cfg->pressure,
- &mldl_cfg->pdata->pressure);
- } else {
- /* Cannot resume the pressure compass or
- * accel while the MPU is running */
- retval = ML_ERROR_FEATURE_NOT_IMPLEMENTED;
- }
- }
+ 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_READ_ACCEL:
- {
- unsigned char data[6];
- retval =
- mpu3050_read_accel(mldl_cfg, client->adapter,
- data);
- if (ML_SUCCESS == retval)
- retval =
- copy_to_user((unsigned char *) arg,
- data, sizeof(data));
- }
+ 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,
+ {
+ 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)
- retval =
- copy_to_user((unsigned char *) arg,
- data, sizeof(data));
- }
- break;
+ 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)
- retval =
- copy_to_user((unsigned char *) arg,
- data, sizeof(data));
- }
- break;
+ {
+ 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,
@@ -737,11 +843,11 @@ void mpu3050_early_suspend(struct early_suspend *h)
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.is_suspended);
+ 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);
+ accel_adapter, compass_adapter,
+ pressure_adapter, TRUE, TRUE, TRUE, TRUE);
}
void mpu3050_early_resume(struct early_suspend *h)
@@ -763,12 +869,16 @@ void mpu3050_early_resume(struct early_suspend *h)
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,
- TRUE, TRUE, TRUE);
+ 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);
}
@@ -794,7 +904,7 @@ void mpu_shutdown(struct i2c_client *client)
(void) mpu3050_suspend(mldl_cfg, this_client->adapter,
accel_adapter, compass_adapter, pressure_adapter,
- TRUE, TRUE, TRUE);
+ TRUE, TRUE, TRUE, TRUE);
dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
}
@@ -813,14 +923,14 @@ int mpu_suspend(struct i2c_client *client, pm_message_t mesg)
pressure_adapter =
i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
- if (!mpu->mldl_cfg.is_suspended) {
+ 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, TRUE, TRUE, TRUE);
} else {
dev_dbg(&this_client->adapter->dev,
"%s: Already suspended %d\n", __func__,
@@ -845,11 +955,15 @@ int mpu_resume(struct i2c_client *client)
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,
- TRUE, TRUE, TRUE);
+ 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);
}
@@ -888,6 +1002,7 @@ int mpu3050_probe(struct i2c_client *client,
{
struct mpu3050_platform_data *pdata;
struct mpu_private_data *mpu;
+ struct mldl_cfg *mldl_cfg;
int res = 0;
struct i2c_adapter *accel_adapter = NULL;
struct i2c_adapter *compass_adapter = NULL;
@@ -908,27 +1023,27 @@ int mpu3050_probe(struct i2c_client *client,
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 {
- mpu->mldl_cfg.pdata = pdata;
+ mldl_cfg->pdata = pdata;
-#if defined(CONFIG_SENSORS_MPU3050_MODULE) || \
- defined(CONFIG_SENSORS_MPU6000_MODULE)
+#if defined(CONFIG_MPU_SENSORS_MPU3050_MODULE) || \
+ defined(CONFIG_MPU_SENSORS_MPU6000_MODULE)
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;
#endif
if (pdata->accel.get_slave_descr) {
- mpu->mldl_cfg.accel =
+ mldl_cfg->accel =
pdata->accel.get_slave_descr();
dev_info(&this_client->adapter->dev,
"%s: +%s\n", MPU_NAME,
- mpu->mldl_cfg.accel->name);
+ mldl_cfg->accel->name);
accel_adapter =
i2c_get_adapter(pdata->accel.adapt_num);
if (pdata->accel.irq > 0) {
@@ -950,11 +1065,11 @@ int mpu3050_probe(struct i2c_client *client,
}
if (pdata->compass.get_slave_descr) {
- mpu->mldl_cfg.compass =
+ mldl_cfg->compass =
pdata->compass.get_slave_descr();
dev_info(&this_client->adapter->dev,
"%s: +%s\n", MPU_NAME,
- mpu->mldl_cfg.compass->name);
+ mldl_cfg->compass->name);
compass_adapter =
i2c_get_adapter(pdata->compass.adapt_num);
if (pdata->compass.irq > 0) {
@@ -976,11 +1091,11 @@ int mpu3050_probe(struct i2c_client *client,
}
if (pdata->pressure.get_slave_descr) {
- mpu->mldl_cfg.pressure =
+ mldl_cfg->pressure =
pdata->pressure.get_slave_descr();
dev_info(&this_client->adapter->dev,
"%s: +%s\n", MPU_NAME,
- mpu->mldl_cfg.pressure->name);
+ mldl_cfg->pressure->name);
pressure_adapter =
i2c_get_adapter(pdata->pressure.adapt_num);
@@ -1003,7 +1118,7 @@ int mpu3050_probe(struct i2c_client *client,
}
}
- mpu->mldl_cfg.addr = client->addr;
+ mldl_cfg->addr = client->addr;
res = mpu3050_open(&mpu->mldl_cfg, client->adapter,
accel_adapter, compass_adapter, pressure_adapter);
diff --git a/drivers/misc/mpu3050/mpuirq.c b/drivers/misc/mpu3050/mpuirq.c
index b6f21856cf32..691e5bc5b052 100644
--- a/drivers/misc/mpu3050/mpuirq.c
+++ b/drivers/misc/mpu3050/mpuirq.c
@@ -61,7 +61,7 @@ struct mpuirq_dev_data {
};
static struct mpuirq_dev_data mpuirq_dev_data;
-static struct irq_data mpuirq_data;
+static struct mpuirq_data mpuirq_data;
static char *interface = MPUIRQ_NAME;
static void mpu_accel_data_work_fcn(struct work_struct *work);
@@ -206,23 +206,20 @@ static irqreturn_t mpuirq_handler(int irq, void *dev_id)
/* wake up (unblock) for reading data from userspace */
/* and ignore first interrupt generated in module init */
- if (mpuirq_data.interruptcount > 1) {
- 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));
- }
+ mpuirq_dev_data.data_ready = 1;
+
+ do_gettimeofday(&irqtime);
+ mpuirq_data.irqtime = (((long long) irqtime.tv_sec) << 32);
+ mpuirq_data.irqtime += irqtime.tv_usec;
- wake_up_interruptible(&mpuirq_wait);
+ 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;
}
diff --git a/drivers/misc/mpu3050/slaveirq.c b/drivers/misc/mpu3050/slaveirq.c
index 3791868505ee..2ee53851e125 100644
--- a/drivers/misc/mpu3050/slaveirq.c
+++ b/drivers/misc/mpu3050/slaveirq.c
@@ -34,22 +34,22 @@
#include <linux/mm.h>
#include <linux/sched.h>
#include <linux/wait.h>
-#include <asm/uaccess.h>
-#include <asm/io.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"
-#include <linux/wait.h>
-#include <linux/slab.h>
/* function which gets slave data and sends it to SLAVE */
struct slaveirq_dev_data {
struct miscdevice dev;
struct i2c_client *slave_client;
- struct irq_data data;
+ struct mpuirq_data data;
wait_queue_head_t slaveirq_wait;
int irq;
int pid;
@@ -83,7 +83,7 @@ static int slaveirq_release(struct inode *inode, struct file *file)
/* 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)
+ char *buf, size_t count, loff_t *ppos)
{
int len, err;
struct slaveirq_dev_data *data =
@@ -169,16 +169,14 @@ static irqreturn_t slaveirq_handler(int irq, void *dev_id)
/* wake up (unblock) for reading data from userspace */
/* and ignore first interrupt generated in module init */
- if (data->data.interruptcount > 1) {
- data->data_ready = 1;
+ 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;
+ 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);
- }
+ wake_up_interruptible(&data->slaveirq_wait);
return IRQ_HANDLED;
diff --git a/drivers/misc/mpu3050/timerirq.c b/drivers/misc/mpu3050/timerirq.c
new file mode 100644
index 000000000000..94dd095306a5
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.c
@@ -0,0 +1,295 @@
+/*
+ $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) {
+ 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;
+}
+
+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);
+ 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 100644
index 000000000000..a38b4907a4d4
--- /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
generated by cgit v1.2.3 (git 2.0.4) at 2024-12-28 22:31:50 +0000