On Wed, 19 Feb 2020 15:39:55 +0100
Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx> wrote:
> Rework fifo enable/disable in a separate function.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
Applied.
Thanks,
Jonathan
> ---
> 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:
