Input: mpu3050 - configure the sampling method

This will improve the output of the sensor.

Signed-off-by: Heikki Krogerus <heikki.krogerus@linux.intel.com>
Signed-off-by: Kirill A. Shutemov <kirill.shutemov@linux.intel.com>
Signed-off-by: Alan Cox <alan@linux.intel.com>
Signed-off-by: Dmitry Torokhov <dtor@mail.ru>

1 files changed, 103 insertions, 5 deletions

diff --git a/drivers/input/misc/mpu3050.c b/drivers/input/misc/mpu3050.c
index ebec0baa1e36..208d1a1cc7f3 100644
--- a/drivers/input/misc/mpu3050.c
+++ b/drivers/input/misc/mpu3050.c
@@ -41,18 +41,67 @@
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
 
-#define MPU3050_CHIP_ID_REG     0x00
 #define MPU3050_CHIP_ID         0x69
-#define MPU3050_XOUT_H          0x1D
-#define MPU3050_PWR_MGM         0x3E
-#define MPU3050_PWR_MGM_POS     6
-#define MPU3050_PWR_MGM_MASK    0x40
 
 #define MPU3050_AUTO_DELAY      1000
 
 #define MPU3050_MIN_VALUE       -32768
 #define MPU3050_MAX_VALUE       32767
 
+#define MPU3050_DEFAULT_POLL_INTERVAL   200
+#define MPU3050_DEFAULT_FS_RANGE        3
+
+/* Register map */
+#define MPU3050_CHIP_ID_REG     0x00
+#define MPU3050_SMPLRT_DIV      0x15
+#define MPU3050_DLPF_FS_SYNC    0x16
+#define MPU3050_INT_CFG         0x17
+#define MPU3050_XOUT_H          0x1D
+#define MPU3050_PWR_MGM         0x3E
+#define MPU3050_PWR_MGM_POS     6
+
+/* Register bits */
+
+/* DLPF_FS_SYNC */
+#define MPU3050_EXT_SYNC_NONE           0x00
+#define MPU3050_EXT_SYNC_TEMP           0x20
+#define MPU3050_EXT_SYNC_GYROX          0x40
+#define MPU3050_EXT_SYNC_GYROY          0x60
+#define MPU3050_EXT_SYNC_GYROZ          0x80
+#define MPU3050_EXT_SYNC_ACCELX 0xA0
+#define MPU3050_EXT_SYNC_ACCELY 0xC0
+#define MPU3050_EXT_SYNC_ACCELZ 0xE0
+#define MPU3050_EXT_SYNC_MASK           0xE0
+#define MPU3050_FS_250DPS               0x00
+#define MPU3050_FS_500DPS               0x08
+#define MPU3050_FS_1000DPS              0x10
+#define MPU3050_FS_2000DPS              0x18
+#define MPU3050_FS_MASK         0x18
+#define MPU3050_DLPF_CFG_256HZ_NOLPF2   0x00
+#define MPU3050_DLPF_CFG_188HZ          0x01
+#define MPU3050_DLPF_CFG_98HZ           0x02
+#define MPU3050_DLPF_CFG_42HZ           0x03
+#define MPU3050_DLPF_CFG_20HZ           0x04
+#define MPU3050_DLPF_CFG_10HZ           0x05
+#define MPU3050_DLPF_CFG_5HZ            0x06
+#define MPU3050_DLPF_CFG_2100HZ_NOLPF   0x07
+#define MPU3050_DLPF_CFG_MASK           0x07
+/* INT_CFG */
+#define MPU3050_RAW_RDY_EN              0x01
+#define MPU3050_MPU_RDY_EN              0x02
+#define MPU3050_LATCH_INT_EN            0x04
+/* PWR_MGM */
+#define MPU3050_PWR_MGM_PLL_X           0x01
+#define MPU3050_PWR_MGM_PLL_Y           0x02
+#define MPU3050_PWR_MGM_PLL_Z           0x03
+#define MPU3050_PWR_MGM_CLKSEL          0x07
+#define MPU3050_PWR_MGM_STBY_ZG 0x08
+#define MPU3050_PWR_MGM_STBY_YG 0x10
+#define MPU3050_PWR_MGM_STBY_XG 0x20
+#define MPU3050_PWR_MGM_SLEEP           0x40
+#define MPU3050_PWR_MGM_RESET           0x80
+#define MPU3050_PWR_MGM_MASK            0x40
+
 struct axis_data {
         s16 x;
         s16 y;
@@ -203,6 +252,51 @@ static irqreturn_t mpu3050_interrupt_thread(int irq, void *data)
 }
 
 /**
+ *      mpu3050_hw_init -       initialize hardware
+ *      @sensor: the sensor
+ *
+ *      Called during device probe; configures the sampling method.
+ */
+static int __devinit mpu3050_hw_init(struct mpu3050_sensor *sensor)
+{
+        struct i2c_client *client = sensor->client;
+        int ret;
+        u8 reg;
+
+        /* Reset */
+        ret = i2c_smbus_write_byte_data(client, MPU3050_PWR_MGM,
+                                        MPU3050_PWR_MGM_RESET);
+        if (ret < 0)
+                return ret;
+
+        ret = i2c_smbus_read_byte_data(client, MPU3050_PWR_MGM);
+        if (ret < 0)
+                return ret;
+
+        ret &= ~MPU3050_PWR_MGM_CLKSEL;
+        ret |= MPU3050_PWR_MGM_PLL_Z;
+        ret = i2c_smbus_write_byte_data(client, MPU3050_PWR_MGM, ret);
+        if (ret < 0)
+                return ret;
+
+        /* Output frequency divider. The poll interval */
+        ret = i2c_smbus_write_byte_data(client, MPU3050_SMPLRT_DIV,
+                                        MPU3050_DEFAULT_POLL_INTERVAL - 1);
+        if (ret < 0)
+                return ret;
+
+        /* Set low pass filter and full scale */
+        reg = MPU3050_DEFAULT_FS_RANGE;
+        reg |= MPU3050_DLPF_CFG_42HZ << 3;
+        reg |= MPU3050_EXT_SYNC_NONE << 5;
+        ret = i2c_smbus_write_byte_data(client, MPU3050_DLPF_FS_SYNC, reg);
+        if (ret < 0)
+                return ret;
+
+        return 0;
+}
+
+/**
  *      mpu3050_probe   -       device detection callback
  *      @client: i2c client of found device
  *      @id: id match information
@@ -267,6 +361,10 @@ static int __devinit mpu3050_probe(struct i2c_client *client,
 
         pm_runtime_set_active(&client->dev);
 
+        error = mpu3050_hw_init(sensor);
+        if (error)
+                goto err_pm_set_suspended;
+
         error = request_threaded_irq(client->irq,
                                      NULL, mpu3050_interrupt_thread,
                                      IRQF_TRIGGER_RISING,