mirrors/linux-stable
1
0
Fork 0
mirror of https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git synced 2024-09-04 18:09:57 +00:00
Code Issues Releases Wiki Activity

iio: imu: inv_mpu6050: factorize fifo enable/disable

Browse source 
Rework fifo enable/disable in a separate function.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Jean-Baptiste Maneyrol 2020-02-19 15:39:55 +01:00 committed by Jonathan Cameron
parent ffc9648a52
commit 4c1e014701
3 changed files with 55 additions and 67 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
View file

@ -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);

54
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
View file

@ -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;

66
drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
View file

@ -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: