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

iio: imu: inv_mpu6050: add fifo support for magnetometer data 



Put read magnetometer data by mpu inside the fifo.

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

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

+1 −0
Original line number Diff line number Diff line
@@ -104,6 +104,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = { 
	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),

	.gyro_fifo_enable = false,

	.accl_fifo_enable = false,

	.magn_fifo_enable = false,

	.accl_fs = INV_MPU6050_FS_02G,

	.user_ctrl = 0,

};

drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h

+2 −0
Original line number Diff line number Diff line
@@ -86,6 +86,7 @@ enum inv_devices { 
 *  @accl_fs:		accel full scale range.

 *  @accl_fifo_enable:	enable accel data output

 *  @gyro_fifo_enable:	enable gyro data output

 *  @magn_fifo_enable:	enable magn data output

 *  @divider:		chip sample rate divider (sample rate divider - 1)

 */

struct inv_mpu6050_chip_config {
@@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config { 
	unsigned int accl_fs:2;

	unsigned int accl_fifo_enable:1;

	unsigned int gyro_fifo_enable:1;

	unsigned int magn_fifo_enable:1;

	u8 divider;

	u8 user_ctrl;

};

drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

+9 −2
Original line number Diff line number Diff line
@@ -124,7 +124,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev) 


	/* enable interrupt */

	if (st->chip_config.accl_fifo_enable ||

	    st->chip_config.gyro_fifo_enable) {

	    st->chip_config.gyro_fifo_enable ||

	    st->chip_config.magn_fifo_enable) {

		result = regmap_write(st->map, st->reg->int_enable,

				      INV_MPU6050_BIT_DATA_RDY_EN);

		if (result)
@@ -141,6 +142,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev) 
		d |= INV_MPU6050_BITS_GYRO_OUT;

	if (st->chip_config.accl_fifo_enable)

		d |= INV_MPU6050_BIT_ACCEL_OUT;

	if (st->chip_config.magn_fifo_enable)

		d |= INV_MPU6050_BIT_SLAVE_0;

	result = regmap_write(st->map, st->reg->fifo_en, d);

	if (result)

		goto reset_fifo_fail;
@@ -190,7 +193,8 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 
	}



	if (!(st->chip_config.accl_fifo_enable |

		st->chip_config.gyro_fifo_enable))

		st->chip_config.gyro_fifo_enable |

		st->chip_config.magn_fifo_enable))

		goto end_session;

	bytes_per_datum = 0;

	if (st->chip_config.accl_fifo_enable)
@@ -202,6 +206,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 
	if (st->chip_type == INV_ICM20602)

		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;



	if (st->chip_config.magn_fifo_enable)

		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;



	/*

	 * read fifo_count register to know how many bytes are inside the FIFO

	 * right now

drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

+76 −10
Original line number Diff line number Diff line
@@ -5,7 +5,7 @@ 


#include "inv_mpu_iio.h"



static void inv_scan_query(struct iio_dev *indio_dev)

static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)

{

	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
@@ -26,6 +26,60 @@ static void inv_scan_query(struct iio_dev *indio_dev) 
			 indio_dev->active_scan_mask);

}



static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);



	inv_scan_query_mpu6050(indio_dev);



	/* no magnetometer if i2c auxiliary bus is used */

	if (st->magn_disabled)

		return;



	st->chip_config.magn_fifo_enable =

		test_bit(INV_MPU9X50_SCAN_MAGN_X,

			 indio_dev->active_scan_mask) ||

		test_bit(INV_MPU9X50_SCAN_MAGN_Y,

			 indio_dev->active_scan_mask) ||

		test_bit(INV_MPU9X50_SCAN_MAGN_Z,

			 indio_dev->active_scan_mask);

}



static void inv_scan_query(struct iio_dev *indio_dev)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);



	switch (st->chip_type) {

	case INV_MPU9250:

	case INV_MPU9255:

		return inv_scan_query_mpu9x50(indio_dev);

	default:

		return inv_scan_query_mpu6050(indio_dev);

	}

}



static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)

{

	unsigned int gyro_skip = 0;

	unsigned int magn_skip = 0;

	unsigned int skip_samples;



	/* gyro first sample is out of specs, skip it */

	if (st->chip_config.gyro_fifo_enable)

		gyro_skip = 1;



	/* mag first sample is always not ready, skip it */

	if (st->chip_config.magn_fifo_enable)

		magn_skip = 1;



	/* compute first samples to skip */

	skip_samples = gyro_skip;

	if (magn_skip > skip_samples)

		skip_samples = magn_skip;



	return skip_samples;

}



/**

 *  inv_mpu6050_set_enable() - enable chip functions.

 *  @indio_dev:	Device driver instance.
@@ -34,6 +88,7 @@ static void inv_scan_query(struct iio_dev *indio_dev) 
static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);

	uint8_t d;

	int result;



	if (enable) {
@@ -41,14 +96,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 
		if (result)

			return result;

		inv_scan_query(indio_dev);

		st->skip_samples = 0;

		if (st->chip_config.gyro_fifo_enable) {

			result = inv_mpu6050_switch_engine(st, true,

					INV_MPU6050_BIT_PWR_GYRO_STBY);

			if (result)

				goto error_power_off;

			/* gyro first sample is out of specs, skip it */

			st->skip_samples = 1;

		}

		if (st->chip_config.accl_fifo_enable) {

			result = inv_mpu6050_switch_engine(st, true,
@@ -56,22 +108,32 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 
			if (result)

				goto error_gyro_off;

		}

		result = inv_reset_fifo(indio_dev);

		if (st->chip_config.magn_fifo_enable) {

			d = st->chip_config.user_ctrl |

					INV_MPU6050_BIT_I2C_MST_EN;

			result = regmap_write(st->map, st->reg->user_ctrl, d);

			if (result)

				goto error_accl_off;

			st->chip_config.user_ctrl = d;

		}

		st->skip_samples = inv_compute_skip_samples(st);

		result = inv_reset_fifo(indio_dev);

		if (result)

			goto error_magn_off;

	} else {

		result = regmap_write(st->map, st->reg->fifo_en, 0);

		if (result)

			goto error_accl_off;

			goto error_magn_off;



		result = regmap_write(st->map, st->reg->int_enable, 0);

		if (result)

			goto error_accl_off;

			goto error_magn_off;



		result = regmap_write(st->map, st->reg->user_ctrl,

				      st->chip_config.user_ctrl);

		d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;

		result = regmap_write(st->map, st->reg->user_ctrl, d);

		if (result)

			goto error_accl_off;

			goto error_magn_off;

		st->chip_config.user_ctrl = d;



		result = inv_mpu6050_switch_engine(st, false,

					INV_MPU6050_BIT_PWR_ACCL_STBY);
@@ -90,6 +152,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 


	return 0;



error_magn_off:

	/* always restore user_ctrl to disable fifo properly */

	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;

	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);

error_accl_off:

	if (st->chip_config.accl_fifo_enable)

		inv_mpu6050_switch_engine(st, false,

CRA Git | Maintained and supported by SUSTech CRA and CCSE