iio: imu: fxos8700: fix ACCEL measurement range selection

commit 9d61c1820598a5ea474576ed55318a6dadee37ed upstream.

When device is in active mode, it fails to set an ACCEL full-scale
range(2g/4g/8g) in FXOS8700_XYZ_DATA_CFG. This is not align with the
datasheet, but it is a fxos8700 chip behavior.

Keep the device in standby mode before setting ACCEL full-scale range
into FXOS8700_XYZ_DATA_CFG in chip initialization phase and setting
scale phase.

Fixes: 2c7e7594a2fd ("iio: imu: Add support for the FXOS8700 IMU")
Signed-off-by: Carlos Song <carlos.song@nxp.com>
Link: https://lore.kernel.org/r/20221208071911.2405922-6-carlos.song@nxp.com
Cc: <Stable@vger.kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/iio/imu/fxos8700_core.c b/drivers/iio/imu/fxos8700_core.c
index 423cfe526f2a13aecb32461d6da68c37eb9b8b45..d19f47227b5f6d734bf7ba8cef12630b5676dac3 100644 (file)
--- a/drivers/iio/imu/fxos8700_core.c
+++ b/drivers/iio/imu/fxos8700_core.c
@@ -345,7 +345,8 @@ static int fxos8700_set_active_mode(struct fxos8700_data *data,
 static int fxos8700_set_scale(struct fxos8700_data *data,
                              enum fxos8700_sensor t, int uscale)
 {
-       int i;
+       int i, ret, val;
+       bool active_mode;
        static const int scale_num = ARRAY_SIZE(fxos8700_accel_scale);
        struct device *dev = regmap_get_device(data->regmap);
 
@@ -354,6 +355,25 @@ static int fxos8700_set_scale(struct fxos8700_data *data,
                return -EINVAL;
        }
 
+       /*
+        * When device is in active mode, it failed to set an ACCEL
+        * full-scale range(2g/4g/8g) in FXOS8700_XYZ_DATA_CFG.
+        * This is not align with the datasheet, but it is a fxos8700
+        * chip behavier. Set the device in standby mode before setting
+        * an ACCEL full-scale range.
+        */
+       ret = regmap_read(data->regmap, FXOS8700_CTRL_REG1, &val);
+       if (ret)
+               return ret;
+
+       active_mode = val & FXOS8700_ACTIVE;
+       if (active_mode) {
+               ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1,
+                                  val & ~FXOS8700_ACTIVE);
+               if (ret)
+                       return ret;
+       }
+
        for (i = 0; i < scale_num; i++)
                if (fxos8700_accel_scale[i].uscale == uscale)
                        break;
@@ -361,8 +381,12 @@ static int fxos8700_set_scale(struct fxos8700_data *data,
        if (i == scale_num)
                return -EINVAL;
 
-       return regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG,
+       ret = regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG,
                            fxos8700_accel_scale[i].bits);
+       if (ret)
+               return ret;
+       return regmap_write(data->regmap, FXOS8700_CTRL_REG1,
+                                 active_mode);
 }
 
 static int fxos8700_get_scale(struct fxos8700_data *data,
@@ -592,14 +616,17 @@ static int fxos8700_chip_init(struct fxos8700_data *data, bool use_spi)
        if (ret)
                return ret;
 
-       /* Max ODR (800Hz individual or 400Hz hybrid), active mode */
-       ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1,
-                          FXOS8700_CTRL_ODR_MAX | FXOS8700_ACTIVE);
+       /*
+        * Set max full-scale range (+/-8G) for ACCEL sensor in chip
+        * initialization then activate the device.
+        */
+       ret = regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG, MODE_8G);
        if (ret)
                return ret;
 
-       /* Set for max full-scale range (+/-8G) */
-       return regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG, MODE_8G);
+       /* Max ODR (800Hz individual or 400Hz hybrid), active mode */
+       return regmap_write(data->regmap, FXOS8700_CTRL_REG1,
+                          FXOS8700_CTRL_ODR_MAX | FXOS8700_ACTIVE);
 }
 
 static void fxos8700_chip_uninit(void *data)