Message ID | 20190916094128.30122-8-jmaneyrol@invensense.com (mailing list archive) |
---|---|
State | New, archived |
Headers | show |
Series | add magnetometer support for MPU925x | expand |
On Mon, 16 Sep 2019 09:42:09 +0000 Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote: > Put read magnetometer data by mpu inside the fifo. > > Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> 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,
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,
Put read magnetometer data by mpu inside the fifo. Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> --- 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(-)