iio: imu: inv_mpu6050: fix user_ctrl register overwritten

When in spi mode, we are setting i2c disable bit in user_ctrl
register. But the register is overwritten after when turning fifo
on. So save user_ctrl init value and always use it when updating
the register.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 7358935fb391338268ef2af3f76c00d96e7f351f..50c33e2319482feefac0648e09f7a32a02faf452 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -88,6 +88,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = {
        .gyro_fifo_enable = false,
        .accl_fifo_enable = false,
        .accl_fs = INV_MPU6050_FS_02G,
+       .user_ctrl = 0,
 };
 
 /* Indexed by enum inv_devices */
@@ -972,15 +973,15 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
        if (result)
                return result;
 
-       if (inv_mpu_bus_setup)
-               inv_mpu_bus_setup(indio_dev);
-
        result = inv_mpu6050_init_config(indio_dev);
        if (result) {
                dev_err(dev, "Could not initialize device.\n");
                return result;
        }
 
+       if (inv_mpu_bus_setup)
+               inv_mpu_bus_setup(indio_dev);
+
        dev_set_drvdata(dev, indio_dev);
        indio_dev->dev.parent = dev;
        /* name will be NULL when enumerated via ACPI */

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index dfb9e4e8c40a3f93047ce91ed209f36ce1f92696..c54da777945daa8745c79307d0905982b3641067 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -97,6 +97,7 @@ struct inv_mpu6050_chip_config {
        unsigned int accl_fifo_enable:1;
        unsigned int gyro_fifo_enable:1;
        u16 fifo_rate;
+       u8 user_ctrl;
 };
 
 /**

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 1b5735474728e3e84e6e9284df8b1e45b736998b..e7b23e327fe07363528ded9de9ad623678000da0 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -51,13 +51,14 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
        if (result)
                goto reset_fifo_fail;
        /* disable fifo reading */
-       result = regmap_write(st->map, st->reg->user_ctrl, 0);
+       result = regmap_write(st->map, st->reg->user_ctrl,
+                             st->chip_config.user_ctrl);
        if (result)
                goto reset_fifo_fail;
 
        /* reset FIFO*/
-       result = regmap_write(st->map, st->reg->user_ctrl,
-                             INV_MPU6050_BIT_FIFO_RST);
+       d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
+       result = regmap_write(st->map, st->reg->user_ctrl, d);
        if (result)
                goto reset_fifo_fail;
 
@@ -72,9 +73,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
                if (result)
                        return result;
        }
-       /* enable FIFO reading and I2C master interface*/
-       result = regmap_write(st->map, st->reg->user_ctrl,
-                             INV_MPU6050_BIT_FIFO_EN);
+       /* enable FIFO reading */
+       d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
+       result = regmap_write(st->map, st->reg->user_ctrl, d);
        if (result)
                goto reset_fifo_fail;
        /* enable sensor output to FIFO */

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
index fe0bf5ac4245a490ce9edddd16b77f16222e16c9..227f50afff22f9c5329aeca129364e8770088dfb 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
@@ -31,8 +31,9 @@ static int inv_mpu_i2c_disable(struct iio_dev *indio_dev)
        if (ret)
                return ret;
 
-       ret = regmap_write(st->map, INV_MPU6050_REG_USER_CTRL,
-                          INV_MPU6050_BIT_I2C_IF_DIS);
+       st->chip_config.user_ctrl |= INV_MPU6050_BIT_I2C_IF_DIS;
+       ret = regmap_write(st->map, st->reg->user_ctrl,
+                          st->chip_config.user_ctrl);
        if (ret) {
                inv_mpu6050_set_power_itg(st, false);
                return ret;

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index f4b1c7135bfdceea68a2cb5a2fd55ecd8cdccccb..6c3e1652a687be0be073fb8addcde9af99339221 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -76,7 +76,8 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
                if (result)
                        goto error_accl_off;
 
-               result = regmap_write(st->map, st->reg->user_ctrl, 0);
+               result = regmap_write(st->map, st->reg->user_ctrl,
+                                     st->chip_config.user_ctrl);
                if (result)
                        goto error_accl_off;