[PATCH v3 6/8] iio: imu: inv_icm42607: Implement MREGx register access

[Kanak Shilledar <kanak.shilledar@axis.com>]

The device supports indirect register access to different banks. A
specific routine needs to be followed when accessing the registers in
another bank as documented in the datasheet (section 13). This is
required for accessing registers configured via the user and
implementing buffer support.

Datasheet: https://www.invensense.tdk.com/en-us/products/3-axis/icm-42370-p
Datasheet: https://www.lcsc.com/product-detail/C5129967.html
Assisted-by: LLM
Signed-off-by: Kanak Shilledar <kanak.shilledar@axis.com>
---
LLM was used to improve the mreg_check function to include the case
where the sensor is in a POWER OFF or LOW POWER mode to utilize the RC
oscillator for initiating the bank access.
---
 drivers/iio/imu/inv_icm42607/inv_icm42607.h      |  29 ++++++
 drivers/iio/imu/inv_icm42607/inv_icm42607_core.c | 108 ++++++++++++++++++++++-
 2 files changed, 135 insertions(+), 2 deletions(-)

diff --git a/drivers/iio/imu/inv_icm42607/inv_icm42607.h b/drivers/iio/imu/inv_icm42607/inv_icm42607.h
index a183a8566617..a522850e0268 100644
--- a/drivers/iio/imu/inv_icm42607/inv_icm42607.h
+++ b/drivers/iio/imu/inv_icm42607/inv_icm42607.h
@@ -111,6 +111,13 @@ enum inv_icm42607_temp_filter_bw {
 	/* value 7 also corresponds to 4Hz */
 };
 
+enum inv_icm42607_mregs {
+	INV_ICM42607_MREG1,
+	INV_ICM42607_MREG2 = 0x28,
+	INV_ICM42607_MREG3 = 0x50,
+	INV_ICM42370_NB
+};
+
 /* Signed so that negative values can signify an invalid condition. */
 struct inv_icm42607_sensor_conf {
 	int mode;
@@ -170,6 +177,7 @@ struct inv_icm42607_sensor_state {
 
 /* Register Map for User Bank 0 */
 #define INV_ICM42607_REG_MCLK_RDY			0x00
+#define INV_ICM42607_MCLK_RDY_BIT			BIT(3)
 
 #define INV_ICM42607_REG_DEVICE_CONFIG			0x01
 #define INV_ICM42607_DEVICE_CONFIG_SPI_AP_4WIRE		BIT(2)
@@ -370,6 +378,24 @@ struct inv_icm42607_sensor_state {
 #define INV_ICM42607_WHOAMI				0x67
 #define INV_ICM42370P_WHOAMI				0x0D
 
+#define INV_ICM42607_REG_BLK_SEL_W			0x79
+#define INV_ICM42607_REG_MADDR_W			0x7A
+#define INV_ICM42607_REG_M_W				0x7B
+#define INV_ICM42607_REG_BLK_SEL_R			0x7C
+#define INV_ICM42607_REG_MADDR_R			0x7D
+#define INV_ICM42607_REG_M_R				0x7E
+
+/* User Bank MREG 1 registers */
+#define INV_ICM42607_REG_OFFSET_USER0			0x4E
+#define INV_ICM42607_REG_OFFSET_USER1			0x4F
+#define INV_ICM42607_REG_OFFSET_USER2			0x50
+#define INV_ICM42607_REG_OFFSET_USER3			0x51
+#define INV_ICM42607_REG_OFFSET_USER4			0x52
+#define INV_ICM42607_REG_OFFSET_USER5			0x53
+#define INV_ICM42607_REG_OFFSET_USER6			0x54
+#define INV_ICM42607_REG_OFFSET_USER7			0x55
+#define INV_ICM42607_REG_OFFSET_USER8			0x56
+
 /*
  * Timings as listed in section 3 of datasheet, all values listed in datasheet
  * in ms except temp startup time... setting all values in us and using
@@ -400,6 +426,9 @@ const struct iio_mount_matrix *
 inv_icm42607_get_mount_matrix(struct iio_dev *indio_dev,
 			      const struct iio_chan_spec *chan);
 
+int inv_icm42607_mreg_read(struct inv_icm42607_state *st, u8 bank, u8 addr, u8 *val);
+int inv_icm42607_mreg_write(struct inv_icm42607_state *st, u8 bank, u8 addr, u8 val);
+
 int inv_icm42607_get_pwr_mgmt0(struct inv_icm42607_state *st,
 			       enum inv_icm42607_sensor_mode *gyro,
 			       enum inv_icm42607_sensor_mode *accel);
diff --git a/drivers/iio/imu/inv_icm42607/inv_icm42607_core.c b/drivers/iio/imu/inv_icm42607/inv_icm42607_core.c
index e77d72e0f7bc..6e514b8682a0 100644
--- a/drivers/iio/imu/inv_icm42607/inv_icm42607_core.c
+++ b/drivers/iio/imu/inv_icm42607/inv_icm42607_core.c
@@ -31,6 +31,7 @@ static bool inv_icm42607_is_readable_reg(struct device *dev, unsigned int reg)
 	case INV_ICM42607_REG_APEX_DATA4 ... INV_ICM42607_REG_INTF_CONFIG1:
 	case INV_ICM42607_REG_INT_STATUS_DRDY ... INV_ICM42607_REG_FIFO_DATA:
 	case INV_ICM42607_REG_WHOAMI:
+	case INV_ICM42607_REG_BLK_SEL_W ... INV_ICM42607_REG_M_R:
 		return true;
 	}
 
@@ -43,6 +44,7 @@ static bool inv_icm42607_is_writeable_reg(struct device *dev, unsigned int reg)
 	case INV_ICM42607_REG_DEVICE_CONFIG ... INV_ICM42607_REG_INT_CONFIG:
 	case INV_ICM42607_REG_PWR_MGMT0 ... INV_ICM42607_REG_INT_SOURCE4:
 	case INV_ICM42607_REG_INTF_CONFIG0 ... INV_ICM42607_REG_INTF_CONFIG1:
+	case INV_ICM42607_REG_BLK_SEL_W ... INV_ICM42607_REG_M_R:
 		return true;
 	}
 
@@ -59,6 +61,7 @@ static bool inv_icm42607_is_volatile_reg(struct device *dev, unsigned int reg)
 	case INV_ICM42607_REG_FIFO_LOST_PKT0 ... INV_ICM42607_REG_APEX_DATA3:
 	case INV_ICM42607_REG_INT_STATUS_DRDY:
 	case INV_ICM42607_REG_INT_STATUS ... INV_ICM42607_REG_FIFO_DATA:
+	case INV_ICM42607_REG_BLK_SEL_W ... INV_ICM42607_REG_M_R:
 		return true;
 	}
 
@@ -71,7 +74,7 @@ const struct regmap_config inv_icm42607_regmap_config = {
 	.writeable_reg = inv_icm42607_is_writeable_reg,
 	.readable_reg = inv_icm42607_is_readable_reg,
 	.volatile_reg = inv_icm42607_is_volatile_reg,
-	.max_register = INV_ICM42607_REG_WHOAMI,
+	.max_register = INV_ICM42607_REG_M_R,
 	.cache_type = REGCACHE_MAPLE,
 };
 EXPORT_SYMBOL_NS_GPL(inv_icm42607_regmap_config, "IIO_ICM42607");
@@ -96,7 +99,7 @@ static const struct inv_icm42607_conf inv_icm42607_default_conf = {
 static const struct inv_icm42607_conf inv_icm42370_default_conf = {
 	.gyro = { },
 	.accel = {
-		.mode = INV_ICM42607_SENSOR_MODE_OFF,
+		.mode = INV_ICM42607_SENSOR_MODE_LOW_POWER,
 		.fs = INV_ICM42607_ACCEL_FS_4G,
 		.odr = INV_ICM42607_ODR_100HZ,
 		.filter = INV_ICM42607_FILTER_BW_25HZ,
@@ -352,6 +355,107 @@ int inv_icm42607_set_sensor_conf(struct inv_icm42607_state *st,
 	}
 }
 
+static int inv_icm42607_mreg_check(struct inv_icm42607_state *st)
+{
+	struct regmap *map = st->map;
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(map, INV_ICM42607_REG_MCLK_RDY, &val);
+	if (ret)
+		return ret;
+
+	if (val & INV_ICM42607_MCLK_RDY_BIT)
+		return 0;
+
+	/*
+	 * Clock isn't running: we're either in Sleep mode or Accel LP
+	 * mode with WUOSC. Force the RC oscillator on via IDLE, then
+	 * wait for MCLK_RDY.
+	 */
+	ret = regmap_set_bits(map, INV_ICM42607_REG_PWR_MGMT0,
+			       INV_ICM42607_PWR_MGMT0_IDLE);
+	if (ret)
+		return ret;
+
+	/*
+	 * After setting the IDLE bit to 1 in PWR_MGMT0 register, wait for anywhere between
+	 * 10us to 200us which are the ACCEL_STARTUP time and accelerometer transition time
+	 * from OFF respectively.
+	 */
+	return regmap_read_poll_timeout(map, INV_ICM42607_REG_MCLK_RDY, val,
+			val & INV_ICM42607_MCLK_RDY_BIT, 10, 200);
+}
+
+int inv_icm42607_mreg_write(struct inv_icm42607_state *st, const u8 bank,
+			    const u8 addr, const u8 val)
+{
+	int ret;
+
+	ret = inv_icm42607_mreg_check(st);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_BLK_SEL_W, bank);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_MADDR_W, addr);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_M_W, val);
+	if (ret)
+		return ret;
+
+	/*
+	 * As per the Datasheet Section 13 Accessing MREGx Registers,
+	 * there should be no read/writes to the device for 10us
+	 * after performing bank access.
+	 */
+	fsleep(10);
+	return regmap_write(st->map, INV_ICM42607_REG_BLK_SEL_W, 0x00);
+}
+
+int inv_icm42607_mreg_read(struct inv_icm42607_state *st, const u8 bank, const u8 addr, u8 *val)
+{
+	unsigned int read_val;
+	int ret;
+
+	ret = inv_icm42607_mreg_check(st);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_BLK_SEL_R, bank);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_ICM42607_REG_MADDR_R, addr);
+	if (ret)
+		return ret;
+
+	/*
+	 * As per the Datasheet Section 13 Accessing MREGx Registers,
+	 * there should be no read/writes to the device for 10us
+	 * after performing bank access.
+	 */
+	fsleep(10);
+	ret = regmap_read(st->map, INV_ICM42607_REG_M_R, &read_val);
+	if (ret)
+		return ret;
+
+	*val = (u8)read_val;
+
+	/*
+	 * As per the Datasheet Section 13 Accessing MREGx Registers,
+	 * there should be no read/writes to the device for 10us
+	 * after performing bank access.
+	 */
+	fsleep(10);
+
+	return regmap_write(st->map, INV_ICM42607_REG_BLK_SEL_R, 0x00);
+}
+
 int inv_icm42607_read_sensor(struct iio_dev *indio_dev,
 			     struct iio_chan_spec const *chan,
 			     s16 *val)

-- 
2.43.0