drivers/sensor: lsm6dso: Fix shub init and configuration failures

There is a flaw with I2C communication to peripherals behind the shub
that causes sporadic failures.  Especially calls to configure a device
after the lsm6dso initialization is finished, e.g. to set the ODR, can
fail to work correctly.

Access to shub peripheral registers is done by putting the parameters of
the operation into SLV0 and then waiting for the lsm6dso to perform the
xfer on the shub I2C bus.  The lsm6dso does this in sync with the
accelerometer update rate.  Once the shub is enabled, it peforms the
xfer repeatedly as the accelermeter is sampled.

The wait has a problem:  It might detect that a previous shub xfer has
finished, which was done before SLV0 was programmed with new parameters.

The shub status register is read-to-clear.  This isn't in the data sheet
or app note, but it is.  By reading the status before enabling the
sensor and after programming SLV0, we can be sure when it becomes set it
has finished the current operation and not a previous one.

Also set the write-once flag before shub init.  This causes the shub to
only perform I2C writes once instead of continuously.  This was set at
the end of init, so any writes done during it would repeat until the
shub was disabled.

Put a timeout in the code that polls for the sensor hub op complete.  It
could possibly poll forever.  More importantly, if there is no device
connected to the sensor hub, the lsm6dso does not timeout on the
operation for ~13 seconds.  Since the shub init does a probe for devices
on startup, this will happen if shub support is enabled but a lsm6dso
has no sensor hub devices.  There could be multiple devices, some with
additional sensors and some without.  Initialization of the devices
without additional sensors takes tens of seconds without this timeout
being added.

Add a 300 µs wait after disabling the sensor hub.  This is necessary
according to the ST app note AN5192 §7.2.1.

Read the shub status from the main bank register instead of the shub
bank register.  This avoids an extra bank switch before and after each
status poll.  Actually two bank switches on each side, since the lsm6dso
driver switched banks and then the ST HAL function to get the status
register switches again.

The wait for the shub I2C transaction to finish is not needed when the
shub is enabled at the end of init.  We aren't starting a new I2C write
or reading the result of a read.

Signed-off-by: Trent Piepho <trent.piepho@igorinstitute.com>

drivers/sensor/lsm6dso/lsm6dso_shub.c

@@ -54,6 +54,13 @@ static int lsm6dso_shub_read_slave_reg(const struct device *dev,
 				       uint8_t *value, uint16_t len);
 static void lsm6dso_shub_enable(const struct device *dev, uint8_t enable);
 
+
+/* ST HAL skips this register, only supports it via the slower lsm6dso_sh_status_get() */
+static int32_t lsm6dso_sh_status_mainpage_get(stmdev_ctx_t *ctx, lsm6dso_status_master_t *val)
+{
+	return lsm6dso_read_reg(ctx, LSM6DSO_STATUS_MASTER_MAINPAGE, (uint8_t *)val, 1);
+}
+
 /*
  * LIS2MDL magn device specific part
  */
@@ -415,14 +422,21 @@ static struct lsm6dso_shub_slist {
 #endif /* CONFIG_LSM6DSO_EXT_LPS22HH */
 };
 
-static inline void lsm6dso_shub_wait_completed(stmdev_ctx_t *ctx)
+static int lsm6dso_shub_wait_completed(stmdev_ctx_t *ctx)
 {
 	lsm6dso_status_master_t status;
+	int tries = 200; /* Should be max ~160 ms, from 2 cycles at slowest ODR 12.5 Hz */
 
 	do {
+		if (!--tries) {
+			LOG_DBG("shub: Timeout waiting for operation to complete");
+			return -ETIMEDOUT;
+		}
 		k_msleep(1);
-		lsm6dso_sh_status_get(ctx, &status);
+		lsm6dso_sh_status_mainpage_get(ctx, &status);
 	} while (status.sens_hub_endop == 0);
+
+	return 1;
 }
 
 static inline void lsm6dso_shub_embedded_en(stmdev_ctx_t *ctx, bool on)
@@ -486,7 +500,12 @@ static void lsm6dso_shub_enable(const struct device *dev, uint8_t enable)
 		}
 	}
 
-	lsm6dso_shub_embedded_en(ctx, true);
+	if (enable) {
+		lsm6dso_status_master_t status;
+
+		/* Clear any pending status flags */
+		lsm6dso_sh_status_mainpage_get(ctx, &status);
+	}
 
 	if (lsm6dso_sh_master_set(ctx, enable) < 0) {
 		LOG_DBG("shub: failed to set master on");
@@ -494,7 +513,10 @@ static void lsm6dso_shub_enable(const struct device *dev, uint8_t enable)
 		return;
 	}
 
-	lsm6dso_shub_embedded_en(ctx, false);
+	if (!enable) {
+		/* Necessary per AN5192 §7.2.1 */
+		k_busy_wait(300);
+	}
 }
 
 /* must be called with master on */
@@ -537,7 +559,7 @@ static int lsm6dso_shub_read_slave_reg(const struct device *dev,
 		return -EIO;
 	}
 
-	/* turn SH on */
+	/* turn SH on, wait for shub i2c read to finish */
 	lsm6dso_shub_enable(dev, 1);
 	lsm6dso_shub_wait_completed(ctx);
 
@@ -590,7 +612,7 @@ static int lsm6dso_shub_write_slave_reg(const struct device *dev,
 			return -EIO;
 		}
 
-		/* turn SH on */
+		/* turn SH on, wait for shub i2c write to finish */
 		lsm6dso_shub_enable(dev, 1);
 		lsm6dso_shub_wait_completed(ctx);
 
@@ -658,17 +680,9 @@ static int lsm6dso_shub_set_data_channel(const struct device *dev)
 		return -EIO;
 	}
 
-	lsm6dso_write_once_t wo = LSM6DSO_ONLY_FIRST_CYCLE;
 
-	if (lsm6dso_sh_write_mode_set(ctx, wo) < 0) {
-		LOG_DBG("shub: error setting write once");
-		return -EIO;
-	}
-
-
-	/* turn SH on */
+	/* turn SH on, no need to wait for 1st shub i2c read, if any, to complete */
 	lsm6dso_shub_enable(dev, 1);
-	lsm6dso_shub_wait_completed(ctx);
 
 	return 0;
 }
@@ -750,11 +764,20 @@ int lsm6dso_shub_config(const struct device *dev, enum sensor_channel chan,
 int lsm6dso_shub_init(const struct device *dev)
 {
 	struct lsm6dso_data *data = dev->data;
+	const struct lsm6dso_config *cfg = dev->config;
+	stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
 	uint8_t i, n = 0, regn;
 	uint8_t chip_id;
 	struct lsm6dso_shub_slist *sp;
 
 	LOG_INF("shub: start sensorhub for %s", dev->name);
+
+	/* This must be set or lsm6dso_shub_write_slave_reg() will repeatedly write the same reg */
+	if (lsm6dso_sh_write_mode_set(ctx, LSM6DSO_ONLY_FIRST_CYCLE) < 0) {
+		LOG_DBG("shub: error setting write once");
+		return -EIO;
+	}
+
 	for (n = 0; n < ARRAY_SIZE(lsm6dso_shub_slist); n++) {
 		if (data->num_ext_dev >= LSM6DSO_SHUB_MAX_NUM_SLVS) {
 			break;