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

Put read magnetometer data by mpu inside the fifo.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index f1c65e0dd1a58681b351ca17460e4cdca9b3d029..354030e9bed5686849e77a584bc846f6ce8a3ca6 100644 (file)
--- 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 953f85618199c387cc255dd727b7fadedbaaebdb..52fcf45050a537d629c73e0e0f8a27c694b4dfe6 100644 (file)
--- 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 5f9a5de0bab44abe645598fd7bf6beba6a6a98e4..bbf68b474556be2f22a1d80ad1e1ec958e45cdbf 100644 (file)
--- 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 dd55e70b6f777209a96ed70a5d5bbf6a7251b8dc..d7d951927a442dfec1e3b312868246665188959e 100644 (file)
--- 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,