[PATCH v2 6/7] iio: imu: fxos8700: fix ODR register readback and initialization

[carlos.song@xxxxxxx]

From: Carlos Song <carlos.song@xxxxxxx>

Use the hexadecimal number to read and write the incorrect bits of
the ODR register. It fails to set MAX ODR mode in chip initialization
and it will return an incorrect ODR mode to userspace.

Use of regmap_write() instead of regmap_update_bits() to update bits
is good for readability. FIELD_GET()/FIELD_PREP() can help clear register
bit definition and avoid separately to define an offset. Unified use of
regmap_write(), FIELD_GET()/FIELD_PREP() to read and write correct ODR
register bits.

Fixes: 84e5ddd5c46e ("iio: imu: Add support for the FXOS8700 IMU")
Signed-off-by: Carlos Song <carlos.song@xxxxxxx>
---
Changes for V2:
- Modify FXOS8700_CTRL_ODR_MSK MASK instead of hexadecimal
- Use of regmap_write() instead of regmap_update_bits()
- Use FIELD_GET()/FIELD_PREP() to avoid to separately define an offset
- Rework commit log

diff --git a/drivers/iio/imu/fxos8700_core.c b/drivers/iio/imu/fxos8700_core.c
index 773f62203bf0..b4baef82f6d5 100644
--- a/drivers/iio/imu/fxos8700_core.c
+++ b/drivers/iio/imu/fxos8700_core.c
@@ -10,6 +10,7 @@
 #include <linux/regmap.h>
 #include <linux/acpi.h>
 #include <linux/bitops.h>
+#include <linux/bitfield.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -144,9 +145,9 @@
 #define FXOS8700_NVM_DATA_BNK0      0xa7
 
 /* Bit definitions for FXOS8700_CTRL_REG1 */
-#define FXOS8700_CTRL_ODR_MSK       0x38
 #define FXOS8700_CTRL_ODR_MAX       0x00
 #define FXOS8700_CTRL_ODR_MIN       GENMASK(4, 3)
+#define FXOS8700_CTRL_ODR_MSK       GENMASK(5, 3)
 
 /* Bit definitions for FXOS8700_M_CTRL_REG1 */
 #define FXOS8700_HMS_MASK           GENMASK(1, 0)
@@ -481,6 +482,7 @@ static int fxos8700_set_odr(struct fxos8700_data *data, enum fxos8700_sensor t,
 			    int odr, int uodr)
 {
 	int i, ret, val;
+	int odr_mode;
 	bool active_mode;
 	static const int odr_num = ARRAY_SIZE(fxos8700_odr);
 
@@ -508,10 +510,10 @@ static int fxos8700_set_odr(struct fxos8700_data *data, enum fxos8700_sensor t,
 	if (i >= odr_num)
 		return -EINVAL;
 
-	return regmap_update_bits(data->regmap,
-				  FXOS8700_CTRL_REG1,
-				  FXOS8700_CTRL_ODR_MSK + FXOS8700_ACTIVE,
-				  fxos8700_odr[i].bits << 3 | active_mode);
+	odr_mode &= ~FXOS8700_CTRL_ODR_MSK;
+	odr_mode |= FIELD_PREP(FXOS8700_CTRL_ODR_MSK, fxos8700_odr[i].bits);
+	return regmap_write(data->regmap, FXOS8700_CTRL_REG1,
+			   odr_mode | active_mode);
 }
 
 static int fxos8700_get_odr(struct fxos8700_data *data, enum fxos8700_sensor t,
@@ -524,7 +526,7 @@ static int fxos8700_get_odr(struct fxos8700_data *data, enum fxos8700_sensor t,
 	if (ret)
 		return ret;
 
-	val &= FXOS8700_CTRL_ODR_MSK;
+	val = FIELD_GET(FXOS8700_CTRL_ODR_MSK, val);
 
 	for (i = 0; i < odr_num; i++)
 		if (val == fxos8700_odr[i].bits)
@@ -612,6 +614,7 @@ static const struct iio_info fxos8700_info = {
 static int fxos8700_chip_init(struct fxos8700_data *data, bool use_spi)
 {
 	int ret;
+	int odr_mode;
 	unsigned int val;
 	struct device *dev = regmap_get_device(data->regmap);
 
@@ -664,8 +667,10 @@ static int fxos8700_chip_init(struct fxos8700_data *data, bool use_spi)
 		return ret;
 
 	/* Max ODR (800Hz individual or 400Hz hybrid), active mode */
+	odr_mode &= ~FXOS8700_CTRL_ODR_MSK;
+	odr_mode |= FIELD_PREP(FXOS8700_CTRL_ODR_MSK, FXOS8700_CTRL_ODR_MAX);
 	return regmap_write(data->regmap, FXOS8700_CTRL_REG1,
-			   FXOS8700_CTRL_ODR_MAX | FXOS8700_ACTIVE);
+			   odr_mode | FXOS8700_ACTIVE);
 }
 
 static void fxos8700_chip_uninit(void *data)
-- 
2.34.1