iio: imu: inv_mpu6050: add accel lpf setting for chip >= MPU6500

Starting from MPU6500, accelerometer dlpf is set in a separate
register named ACCEL_CONFIG_2.
Add this new register in the map and set it for the corresponding
chips.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Cc: stable@vger.kernel.org
Signed-off-by: Jonathan Cameron <jic23@kernel.org>

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 96dabbd2f004b3d91c1cde6cca2f6dfdaf091d58..88a7c5d4e4d2850ae9755b5fb79ff595855cac7c 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -41,6 +41,7 @@ static const int accel_scale[] = {598, 1196, 2392, 4785};
 static const struct inv_mpu6050_reg_map reg_set_6500 = {
        .sample_rate_div        = INV_MPU6050_REG_SAMPLE_RATE_DIV,
        .lpf                    = INV_MPU6050_REG_CONFIG,
+       .accel_lpf              = INV_MPU6500_REG_ACCEL_CONFIG_2,
        .user_ctrl              = INV_MPU6050_REG_USER_CTRL,
        .fifo_en                = INV_MPU6050_REG_FIFO_EN,
        .gyro_config            = INV_MPU6050_REG_GYRO_CONFIG,
@@ -210,6 +211,37 @@ int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
 }
 EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg);
 
+/**
+ *  inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
+ *
+ *  MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
+ *  MPU6500 and above have a dedicated register for accelerometer
+ */
+static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
+                                   enum inv_mpu6050_filter_e val)
+{
+       int result;
+
+       result = regmap_write(st->map, st->reg->lpf, val);
+       if (result)
+               return result;
+
+       switch (st->chip_type) {
+       case INV_MPU6050:
+       case INV_MPU6000:
+       case INV_MPU9150:
+               /* old chips, nothing to do */
+               result = 0;
+               break;
+       default:
+               /* set accel lpf */
+               result = regmap_write(st->map, st->reg->accel_lpf, val);
+               break;
+       }
+
+       return result;
+}
+
 /**
  *  inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
  *
@@ -233,8 +265,7 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
        if (result)
                return result;
 
-       d = INV_MPU6050_FILTER_20HZ;
-       result = regmap_write(st->map, st->reg->lpf, d);
+       result = inv_mpu6050_set_lpf_regs(st, INV_MPU6050_FILTER_20HZ);
        if (result)
                return result;
 
@@ -537,6 +568,8 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
  *                  would be alising. This function basically search for the
  *                  correct low pass parameters based on the fifo rate, e.g,
  *                  sampling frequency.
+ *
+ *  lpf is set automatically when setting sampling rate to avoid any aliases.
  */
 static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
 {
@@ -552,7 +585,7 @@ static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
        while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
                i++;
        data = d[i];
-       result = regmap_write(st->map, st->reg->lpf, data);
+       result = inv_mpu6050_set_lpf_regs(st, data);
        if (result)
                return result;
        st->chip_config.lpf = data;

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index ef13de7a2c20fd6ef3e450f8d3094483af71b9a9..953a0c09d5685a18d447e4ad07f3f334b32cd8cf 100644 (file)
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -28,6 +28,7 @@
  *  struct inv_mpu6050_reg_map - Notable registers.
  *  @sample_rate_div:  Divider applied to gyro output rate.
  *  @lpf:              Configures internal low pass filter.
+ *  @accel_lpf:                Configures accelerometer low pass filter.
  *  @user_ctrl:                Enables/resets the FIFO.
  *  @fifo_en:          Determines which data will appear in FIFO.
  *  @gyro_config:      gyro config register.
@@ -47,6 +48,7 @@
 struct inv_mpu6050_reg_map {
        u8 sample_rate_div;
        u8 lpf;
+       u8 accel_lpf;
        u8 user_ctrl;
        u8 fifo_en;
        u8 gyro_config;
@@ -188,6 +190,7 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_FIFO_THRESHOLD           500
 
 /* mpu6500 registers */
+#define INV_MPU6500_REG_ACCEL_CONFIG_2      0x1D
 #define INV_MPU6500_REG_ACCEL_OFFSET        0x77
 
 /* delay time in milliseconds */