On Mon, 16 Sep 2019 09:42:09 +0000 Jean-Baptiste Maneyrol <JManeyrol@xxxxxxxxxxxxxx> wrote: > Put read magnetometer data by mpu inside the fifo. > > Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx> Applied to the togreg branch of iio.git and pushed out as testing for the autobuilders to poke at it. Thanks, Jonathan > --- > drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 1 + > drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 2 + > drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 11 ++- > drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 86 ++++++++++++++++--- > 4 files changed, 88 insertions(+), 12 deletions(-) > > diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c > index f1c65e0dd1a5..354030e9bed5 100644 > --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c > +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c > @@ -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, > }; > diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h > index 953f85618199..52fcf45050a5 100644 > --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h > +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h > @@ -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; > }; > diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c > index 5f9a5de0bab4..bbf68b474556 100644 > --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c > +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c > @@ -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 > diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c > index dd55e70b6f77..d7d951927a44 100644 > --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c > +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c > @@ -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; > } > + 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_accl_off; > + 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,