mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-09-04 18:09:57 +00:00
iio: imu: inv_mpu6050: factorize fifo enable/disable
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:
parent
ffc9648a52
commit
4c1e014701
3 changed files with 55 additions and 67 deletions
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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:
|
||||
|
|
Loading…
Reference in a new issue