Rework fifo enable/disable in a separate function.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 2 +-
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 54 ++-------------
drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 66 ++++++++++++++-----
3 files changed, 55 insertions(+), 67 deletions(-)
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index a578789c9210..e328c98e362c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -435,7 +435,7 @@ enum inv_mpu6050_clock_sel_e {
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
-int inv_reset_fifo(struct iio_dev *indio_dev);
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable);
int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
unsigned int mask);
int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index d7397705974e..9511e4715e2c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -90,60 +90,14 @@ static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
return ts;
}
-int inv_reset_fifo(struct iio_dev *indio_dev)
+static int inv_reset_fifo(struct iio_dev *indio_dev)
{
int result;
- u8 d;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
- /* reset it timestamp validation */
- st->it_timestamp = 0;
-
- /* disable interrupt */
- result = regmap_write(st->map, st->reg->int_enable, 0);
- if (result) {
- dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
- result);
- return result;
- }
- /* disable the sensor output to FIFO */
- result = regmap_write(st->map, st->reg->fifo_en, 0);
- if (result)
- goto reset_fifo_fail;
- /* disable fifo reading */
- result = regmap_write(st->map, st->reg->user_ctrl,
- st->chip_config.user_ctrl);
- if (result)
- goto reset_fifo_fail;
-
- /* reset FIFO*/
- 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;
-
- /* enable interrupt */
- result = regmap_write(st->map, st->reg->int_enable,
- INV_MPU6050_BIT_DATA_RDY_EN);
- if (result)
- return result;
-
- /* 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 */
- d = 0;
- if (st->chip_config.gyro_fifo_enable)
- d |= INV_MPU6050_BITS_GYRO_OUT;
- if (st->chip_config.accl_fifo_enable)
- d |= INV_MPU6050_BIT_ACCEL_OUT;
- if (st->chip_config.temp_fifo_enable)
- d |= INV_MPU6050_BIT_TEMP_OUT;
- if (st->chip_config.magn_fifo_enable)
- d |= INV_MPU6050_BIT_SLAVE_0;
- result = regmap_write(st->map, st->reg->fifo_en, d);
+ /* disable fifo and reenable it */
+ inv_mpu6050_prepare_fifo(st, false);
+ result = inv_mpu6050_prepare_fifo(st, true);
if (result)
goto reset_fifo_fail;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index cfd7243159f6..f53f50d08b9e 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -100,6 +100,54 @@ static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
return skip_samples;
}
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
+{
+ uint8_t d;
+ int ret;
+
+ if (enable) {
+ st->it_timestamp = 0;
+ /* reset FIFO */
+ d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
+ ret = regmap_write(st->map, st->reg->user_ctrl, d);
+ if (ret)
+ return ret;
+ /* enable sensor output to FIFO */
+ d = 0;
+ if (st->chip_config.gyro_fifo_enable)
+ d |= INV_MPU6050_BITS_GYRO_OUT;
+ if (st->chip_config.accl_fifo_enable)
+ d |= INV_MPU6050_BIT_ACCEL_OUT;
+ if (st->chip_config.temp_fifo_enable)
+ d |= INV_MPU6050_BIT_TEMP_OUT;
+ if (st->chip_config.magn_fifo_enable)
+ d |= INV_MPU6050_BIT_SLAVE_0;
+ ret = regmap_write(st->map, st->reg->fifo_en, d);
+ if (ret)
+ return ret;
+ /* enable FIFO reading */
+ d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
+ ret = regmap_write(st->map, st->reg->user_ctrl, d);
+ if (ret)
+ return ret;
+ /* enable interrupt */
+ ret = regmap_write(st->map, st->reg->int_enable,
+ INV_MPU6050_BIT_DATA_RDY_EN);
+ } else {
+ ret = regmap_write(st->map, st->reg->int_enable, 0);
+ if (ret)
+ return ret;
+ ret = regmap_write(st->map, st->reg->fifo_en, 0);
+ if (ret)
+ return ret;
+ /* restore user_ctrl for disabling FIFO reading */
+ ret = regmap_write(st->map, st->reg->user_ctrl,
+ st->chip_config.user_ctrl);
+ }
+
+ return ret;
+}
+
/**
* inv_mpu6050_set_enable() - enable chip functions.
* @indio_dev: Device driver instance.
@@ -121,24 +169,13 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
if (result)
goto error_power_off;
st->skip_samples = inv_compute_skip_samples(st);
- result = inv_reset_fifo(indio_dev);
+ result = inv_mpu6050_prepare_fifo(st, true);
if (result)
goto error_sensors_off;
} else {
- result = regmap_write(st->map, st->reg->fifo_en, 0);
- if (result)
- goto error_fifo_off;
-
- result = regmap_write(st->map, st->reg->int_enable, 0);
- if (result)
- goto error_fifo_off;
-
- /* restore user_ctrl for disabling FIFO reading */
- result = regmap_write(st->map, st->reg->user_ctrl,
- st->chip_config.user_ctrl);
+ result = inv_mpu6050_prepare_fifo(st, false);
if (result)
goto error_sensors_off;
-
result = inv_mpu6050_switch_engine(st, false, scan);
if (result)
goto error_power_off;
@@ -150,9 +187,6 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
return 0;
-error_fifo_off:
- /* always restore user_ctrl to disable fifo properly */
- regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
error_sensors_off:
inv_mpu6050_switch_engine(st, false, scan);
error_power_off:
--
2.17.1
