Commit 4c1e0147 authored by Jean-Baptiste Maneyrol's avatar Jean-Baptiste Maneyrol Committed by Jonathan Cameron
Browse files

iio: imu: inv_mpu6050: factorize fifo enable/disable 



Rework fifo enable/disable in a separate function.

Signed-off-by: default avatarJean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent ffc9648a

drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h

+1 −1
Original line number Diff line number Diff line
@@ -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);

drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

+4 −50
Original line number Diff line number Diff line
@@ -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;

drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

+50 −16
Original line number Diff line number Diff line
@@ -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:

CRA Git | Maintained and supported by SUSTech CRA and CCSE