[PATCH] iio: imu: st_lsm6dsx: deselect shub page before reading whoami

[Andreas Kempe]

As part of driver initialisation, e.g. st_lsm6dsx_init_shub() selects
the shub register page using st_lsm6dsx_set_page(). Selecting the shub
register page shadows the regular register space so whoami, among other
registers, is no longer accessible.

In applications where the IMU is permanently powered separately from the
processor, there is a window where a reset of the CPU leaves the IMU in
the shub register page. Once this occurs, any subsequent probe attempt
fails because of the register shadowing.

Using the ism330dlc, the error typically looks like

	st_lsm6dsx_i2c 3-006a: unsupported whoami [10]

with the unknown whoami read from a reserved register in the shub page.

The reset register is also shadowed by the page select, preventing a
simple reset from recovering the chip.

Add a readout of the shub register page selection and deselect the page
if needed before reading whoami. This allows the driver to recover and
probe correctly.

Signed-off-by: Andreas Kempe <andreas.kempe@xxxxxxxx>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 42 +++++++++++++++++++-
 1 file changed, 41 insertions(+), 1 deletion(-)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 630e2cae6f19..6fef99f2e9f1 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -1692,10 +1692,27 @@ int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable)
 	return err;
 }
 
+static int st_lsm6dsx_get_page(struct st_lsm6dsx_hw *hw, bool *enable)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	unsigned int data;
+	int err;
+
+	hub_settings = &hw->settings->shub_settings;
+	err = regmap_read(hw->regmap, hub_settings->page_mux.addr, &data);
+	if (err < 0)
+		return err;
+
+	*enable = data & hub_settings->page_mux.mask;
+
+	return 0;
+}
+
 static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id,
 				   const char **name)
 {
 	int err, i, j, data;
+	bool enable;
 
 	for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
 		for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) {
@@ -1712,6 +1729,30 @@ static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id,
 		return -ENODEV;
 	}
 
+	hw->settings = &st_lsm6dsx_sensor_settings[i];
+
+	if (hw->settings->shub_settings.page_mux.addr) {
+		/*
+		 * whoami is not available in the shub register page.
+		 * Deselect the shub page if needed so whoami can be
+		 * correctly read.
+		 */
+		err = st_lsm6dsx_get_page(hw, &enable);
+		if (err < 0) {
+			dev_err(hw->dev, "failed to get shub page\n");
+			return err;
+		}
+
+		if (enable) {
+			dev_warn(hw->dev, "shub page selected; clearing it\n");
+			err = st_lsm6dsx_set_page(hw, false);
+			if (err < 0) {
+				dev_err(hw->dev, "failed to clear shub page\n");
+				return err;
+			}
+		}
+	}
+
 	err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data);
 	if (err < 0) {
 		dev_err(hw->dev, "failed to read whoami register\n");
@@ -1724,7 +1765,6 @@ static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id,
 	}
 
 	*name = st_lsm6dsx_sensor_settings[i].id[j].name;
-	hw->settings = &st_lsm6dsx_sensor_settings[i];
 
 	return 0;
 }
-- 
2.53.0