Merge branch 'master' into fixes

We have a fix to apply on top of commit 96d4f267e40f ("Remove 'type'
argument from access_ok() function"), so merge master to get it.

5 files changed, 1262 insertions, 135 deletions

diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile
index 35919febea2a..e5f733ce6e11 100644
--- a/drivers/iio/imu/st_lsm6dsx/Makefile
+++ b/drivers/iio/imu/st_lsm6dsx/Makefile
@@ -1,4 +1,5 @@
-st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o
+st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o \
+		st_lsm6dsx_shub.o
 
 obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o
 obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index ef73519a0fb6..d1d8d07a0714 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -43,6 +43,24 @@ enum st_lsm6dsx_hw_id {
 					 * ST_LSM6DSX_TAGGED_SAMPLE_SIZE)
 #define ST_LSM6DSX_SHIFT_VAL(val, mask)	(((val) << __ffs(mask)) & (mask))
 
+#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx)		\
+{									\
+	.type = chan_type,						\
+	.address = addr,						\
+	.modified = 1,							\
+	.channel2 = mod,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
+			      BIT(IIO_CHAN_INFO_SCALE),			\
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+	.scan_index = scan_idx,						\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = 16,						\
+		.storagebits = 16,					\
+		.endianness = IIO_LE,					\
+	},								\
+}
+
 struct st_lsm6dsx_reg {
 	u8 addr;
 	u8 mask;
@@ -50,6 +68,28 @@ struct st_lsm6dsx_reg {
 
 struct st_lsm6dsx_hw;
 
+struct st_lsm6dsx_odr {
+	u16 hz;
+	u8 val;
+};
+
+#define ST_LSM6DSX_ODR_LIST_SIZE	6
+struct st_lsm6dsx_odr_table_entry {
+	struct st_lsm6dsx_reg reg;
+	struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
+};
+
+struct st_lsm6dsx_fs {
+	u32 gain;
+	u8 val;
+};
+
+#define ST_LSM6DSX_FS_LIST_SIZE		4
+struct st_lsm6dsx_fs_table_entry {
+	struct st_lsm6dsx_reg reg;
+	struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
+};
+
 /**
  * struct st_lsm6dsx_fifo_ops - ST IMU FIFO settings
  * @read_fifo: Read FIFO callback.
@@ -85,6 +125,70 @@ struct st_lsm6dsx_hw_ts_settings {
 };
 
 /**
+ * struct st_lsm6dsx_shub_settings - ST IMU hw i2c controller settings
+ * @page_mux: register page mux info (addr + mask).
+ * @master_en: master config register info (addr + mask).
+ * @pullup_en: i2c controller pull-up register info (addr + mask).
+ * @aux_sens: aux sensor register info (addr + mask).
+ * @wr_once: write_once register info (addr + mask).
+ * @shub_out: sensor hub first output register info.
+ * @slv0_addr: slave0 address in secondary page.
+ * @dw_slv0_addr: slave0 write register address in secondary page.
+ * @batch_en: Enable/disable FIFO batching.
+ */
+struct st_lsm6dsx_shub_settings {
+	struct st_lsm6dsx_reg page_mux;
+	struct st_lsm6dsx_reg master_en;
+	struct st_lsm6dsx_reg pullup_en;
+	struct st_lsm6dsx_reg aux_sens;
+	struct st_lsm6dsx_reg wr_once;
+	u8 shub_out;
+	u8 slv0_addr;
+	u8 dw_slv0_addr;
+	u8 batch_en;
+};
+
+enum st_lsm6dsx_ext_sensor_id {
+	ST_LSM6DSX_ID_MAGN,
+};
+
+/**
+ * struct st_lsm6dsx_ext_dev_settings - i2c controller slave settings
+ * @i2c_addr: I2c slave address list.
+ * @wai: Wai address info.
+ * @id: external sensor id.
+ * @odr: Output data rate of the sensor [Hz].
+ * @gain: Configured sensor sensitivity.
+ * @temp_comp: Temperature compensation register info (addr + mask).
+ * @pwr_table: Power on register info (addr + mask).
+ * @off_canc: Offset cancellation register info (addr + mask).
+ * @bdu: Block data update register info (addr + mask).
+ * @out: Output register info.
+ */
+struct st_lsm6dsx_ext_dev_settings {
+	u8 i2c_addr[2];
+	struct {
+		u8 addr;
+		u8 val;
+	} wai;
+	enum st_lsm6dsx_ext_sensor_id id;
+	struct st_lsm6dsx_odr_table_entry odr_table;
+	struct st_lsm6dsx_fs_table_entry fs_table;
+	struct st_lsm6dsx_reg temp_comp;
+	struct {
+		struct st_lsm6dsx_reg reg;
+		u8 off_val;
+		u8 on_val;
+	} pwr_table;
+	struct st_lsm6dsx_reg off_canc;
+	struct st_lsm6dsx_reg bdu;
+	struct {
+		u8 addr;
+		u8 len;
+	} out;
+};
+
+/**
  * struct st_lsm6dsx_settings - ST IMU sensor settings
  * @wai: Sensor WhoAmI default value.
  * @max_fifo_size: Sensor max fifo length in FIFO words.
@@ -93,6 +197,7 @@ struct st_lsm6dsx_hw_ts_settings {
  * @batch: List of FIFO batching register info (addr + mask).
  * @fifo_ops: Sensor hw FIFO parameters.
  * @ts_settings: Hw timer related settings.
+ * @shub_settings: i2c controller related settings.
  */
 struct st_lsm6dsx_settings {
 	u8 wai;
@@ -102,11 +207,15 @@ struct st_lsm6dsx_settings {
 	struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
 	struct st_lsm6dsx_fifo_ops fifo_ops;
 	struct st_lsm6dsx_hw_ts_settings ts_settings;
+	struct st_lsm6dsx_shub_settings shub_settings;
 };
 
 enum st_lsm6dsx_sensor_id {
-	ST_LSM6DSX_ID_ACC,
 	ST_LSM6DSX_ID_GYRO,
+	ST_LSM6DSX_ID_ACC,
+	ST_LSM6DSX_ID_EXT0,
+	ST_LSM6DSX_ID_EXT1,
+	ST_LSM6DSX_ID_EXT2,
 	ST_LSM6DSX_ID_MAX,
 };
 
@@ -126,6 +235,7 @@ enum st_lsm6dsx_fifo_mode {
  * @sip: Number of samples in a given pattern.
  * @decimator: FIFO decimation factor.
  * @ts_ref: Sensor timestamp reference for hw one.
+ * @ext_info: Sensor settings if it is connected to i2c controller
  */
 struct st_lsm6dsx_sensor {
 	char name[32];
@@ -139,6 +249,11 @@ struct st_lsm6dsx_sensor {
 	u8 sip;
 	u8 decimator;
 	s64 ts_ref;
+
+	struct {
+		const struct st_lsm6dsx_ext_dev_settings *settings;
+		u8 addr;
+	} ext_info;
 };
 
 /**
@@ -148,6 +263,7 @@ struct st_lsm6dsx_sensor {
  * @irq: Device interrupt line (I2C or SPI).
  * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
  * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
+ * @page_lock: Mutex to prevent concurrent memory page configuration.
  * @fifo_mode: FIFO operating mode supported by the device.
  * @enable_mask: Enabled sensor bitmask.
  * @ts_sip: Total number of timestamp samples in a given pattern.
@@ -163,6 +279,7 @@ struct st_lsm6dsx_hw {
 
 	struct mutex fifo_lock;
 	struct mutex conf_lock;
+	struct mutex page_lock;
 
 	enum st_lsm6dsx_fifo_mode fifo_mode;
 	u8 enable_mask;
@@ -176,13 +293,15 @@ struct st_lsm6dsx_hw {
 	const struct st_lsm6dsx_settings *settings;
 };
 
+static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
 extern const struct dev_pm_ops st_lsm6dsx_pm_ops;
 
 int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		     struct regmap *regmap);
-int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
-int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor,
+				 bool enable);
 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw);
+int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val);
 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor,
 				u16 watermark);
 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw);
@@ -191,5 +310,47 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw);
 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw);
 int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val);
+int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name);
+int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable);
+int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable);
+
+static inline int
+st_lsm6dsx_update_bits_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
+			      unsigned int mask, unsigned int val)
+{
+	int err;
+
+	mutex_lock(&hw->page_lock);
+	err = regmap_update_bits(hw->regmap, addr, mask, val);
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+static inline int
+st_lsm6dsx_read_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
+		       void *val, unsigned int len)
+{
+	int err;
+
+	mutex_lock(&hw->page_lock);
+	err = regmap_bulk_read(hw->regmap, addr, val, len);
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+static inline int
+st_lsm6dsx_write_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
+			unsigned int val)
+{
+	int err;
+
+	mutex_lock(&hw->page_lock);
+	err = regmap_write(hw->regmap, addr, val);
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
 
 #endif /* ST_LSM6DSX_H */
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index b5263fc522ca..2c0d3763405a 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -68,6 +68,9 @@ enum st_lsm6dsx_fifo_tag {
 	ST_LSM6DSX_GYRO_TAG = 0x01,
 	ST_LSM6DSX_ACC_TAG = 0x02,
 	ST_LSM6DSX_TS_TAG = 0x04,
+	ST_LSM6DSX_EXT0_TAG = 0x0f,
+	ST_LSM6DSX_EXT1_TAG = 0x10,
+	ST_LSM6DSX_EXT2_TAG = 0x11,
 };
 
 static const
@@ -102,6 +105,9 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
 
 	*max_odr = 0, *min_odr = ~0;
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		sensor = iio_priv(hw->iio_devs[i]);
 
 		if (!(hw->enable_mask & BIT(sensor->id)))
@@ -125,6 +131,9 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
 		const struct st_lsm6dsx_reg *dec_reg;
 
+		if (!hw->iio_devs[i])
+			continue;
+
 		sensor = iio_priv(hw->iio_devs[i]);
 		/* update fifo decimators and sample in pattern */
 		if (hw->enable_mask & BIT(sensor->id)) {
@@ -142,8 +151,9 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 		if (dec_reg->addr) {
 			int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
 
-			err = regmap_update_bits(hw->regmap, dec_reg->addr,
-						 dec_reg->mask, val);
+			err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
+							    dec_reg->mask,
+							    val);
 			if (err < 0)
 				return err;
 		}
@@ -162,8 +172,8 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 		int val, ts_dec = !!hw->ts_sip;
 
 		val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
-		err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,
-					 ts_dec_reg->mask, val);
+		err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
+						    ts_dec_reg->mask, val);
 	}
 	return err;
 }
@@ -171,12 +181,12 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
 int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
 			     enum st_lsm6dsx_fifo_mode fifo_mode)
 {
+	unsigned int data;
 	int err;
 
-	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
-				 ST_LSM6DSX_FIFO_MODE_MASK,
-				 FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
-					    fifo_mode));
+	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
+	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+					    ST_LSM6DSX_FIFO_MODE_MASK, data);
 	if (err < 0)
 		return err;
 
@@ -207,15 +217,15 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
 			data = 0;
 		}
 		val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
-		return regmap_update_bits(hw->regmap, batch_reg->addr,
-					  batch_reg->mask, val);
+		return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
+						     batch_reg->mask, val);
 	} else {
 		data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
-		return regmap_update_bits(hw->regmap,
-					  ST_LSM6DSX_REG_FIFO_MODE_ADDR,
-					  ST_LSM6DSX_FIFO_ODR_MASK,
-					  FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
-						     data));
+		return st_lsm6dsx_update_bits_locked(hw,
+					ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+					ST_LSM6DSX_FIFO_ODR_MASK,
+					FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
+						   data));
 	}
 }
 
@@ -231,6 +241,9 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 		return 0;
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		cur_sensor = iio_priv(hw->iio_devs[i]);
 
 		if (!(hw->enable_mask & BIT(cur_sensor->id)))
@@ -246,19 +259,23 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
 	fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
 	fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
 
+	mutex_lock(&hw->page_lock);
 	err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
 			  &data);
 	if (err < 0)
-		return err;
+		goto out;
 
 	fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
 	fifo_watermark = ((data << 8) & ~fifo_th_mask) |
 			 (fifo_watermark & fifo_th_mask);
 
 	wdata = cpu_to_le16(fifo_watermark);
-	return regmap_bulk_write(hw->regmap,
-				 hw->settings->fifo_ops.fifo_th.addr,
-				 &wdata, sizeof(wdata));
+	err = regmap_bulk_write(hw->regmap,
+				hw->settings->fifo_ops.fifo_th.addr,
+				&wdata, sizeof(wdata));
+out:
+	mutex_unlock(&hw->page_lock);
+	return err;
 }
 
 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
@@ -267,12 +284,15 @@ static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
 	int i, err;
 
 	/* reset hw ts counter */
-	err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,
-			   ST_LSM6DSX_TS_RESET_VAL);
+	err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
+				      ST_LSM6DSX_TS_RESET_VAL);
 	if (err < 0)
 		return err;
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		sensor = iio_priv(hw->iio_devs[i]);
 		/*
 		 * store enable buffer timestamp as reference for
@@ -297,8 +317,8 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
 	while (read_len < data_len) {
 		word_len = min_t(unsigned int, data_len - read_len,
 				 max_word_len);
-		err = regmap_bulk_read(hw->regmap, addr, data + read_len,
-				       word_len);
+		err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
+					     word_len);
 		if (err < 0)
 			return err;
 		read_len += word_len;
@@ -328,9 +348,9 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 	__le16 fifo_status;
 	s64 ts = 0;
 
-	err = regmap_bulk_read(hw->regmap,
-			       hw->settings->fifo_ops.fifo_diff.addr,
-			       &fifo_status, sizeof(fifo_status));
+	err = st_lsm6dsx_read_locked(hw,
+				     hw->settings->fifo_ops.fifo_diff.addr,
+				     &fifo_status, sizeof(fifo_status));
 	if (err < 0) {
 		dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 			err);
@@ -436,6 +456,55 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 	return read_len;
 }
 
+static int
+st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
+			    u8 *data, s64 ts)
+{
+	struct st_lsm6dsx_sensor *sensor;
+	struct iio_dev *iio_dev;
+
+	/*
+	 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
+	 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
+	 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
+	 * channel
+	 */
+	switch (tag) {
+	case ST_LSM6DSX_GYRO_TAG:
+		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
+		break;
+	case ST_LSM6DSX_ACC_TAG:
+		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
+		break;
+	case ST_LSM6DSX_EXT0_TAG:
+		if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
+			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
+		else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
+			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
+		else
+			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
+		break;
+	case ST_LSM6DSX_EXT1_TAG:
+		if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
+		    (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
+			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
+		else
+			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
+		break;
+	case ST_LSM6DSX_EXT2_TAG:
+		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sensor = iio_priv(iio_dev);
+	iio_push_to_buffers_with_timestamp(iio_dev, data,
+					   ts + sensor->ts_ref);
+
+	return 0;
+}
+
 /**
  * st_lsm6dsx_read_tagged_fifo() - LSM6DSO read FIFO routine
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
@@ -455,9 +524,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
 	__le16 fifo_status;
 	s64 ts = 0;
 
-	err = regmap_bulk_read(hw->regmap,
-			       hw->settings->fifo_ops.fifo_diff.addr,
-			       &fifo_status, sizeof(fifo_status));
+	err = st_lsm6dsx_read_locked(hw,
+				     hw->settings->fifo_ops.fifo_diff.addr,
+				     &fifo_status, sizeof(fifo_status));
 	if (err < 0) {
 		dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
 			err);
@@ -491,8 +560,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
 			       ST_LSM6DSX_SAMPLE_SIZE);
 
 			tag = hw->buff[i] >> 3;
-			switch (tag) {
-			case ST_LSM6DSX_TS_TAG:
+			if (tag == ST_LSM6DSX_TS_TAG) {
 				/*
 				 * hw timestamp is 4B long and it is stored
 				 * in FIFO according to this schema:
@@ -509,19 +577,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
 				if (!reset_ts && ts >= 0xffff0000)
 					reset_ts = true;
 				ts *= ST_LSM6DSX_TS_SENSITIVITY;
-				break;
-			case ST_LSM6DSX_GYRO_TAG:
-				iio_push_to_buffers_with_timestamp(
-					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
-					iio_buff, gyro_sensor->ts_ref + ts);
-				break;
-			case ST_LSM6DSX_ACC_TAG:
-				iio_push_to_buffers_with_timestamp(
-					hw->iio_devs[ST_LSM6DSX_ID_ACC],
-					iio_buff, acc_sensor->ts_ref + ts);
-				break;
-			default:
-				break;
+			} else {
+				st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
+							    ts);
 			}
 		}
 	}
@@ -562,19 +620,21 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 			goto out;
 	}
 
-	if (enable) {
-		err = st_lsm6dsx_sensor_enable(sensor);
+	if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
+	    sensor->id == ST_LSM6DSX_ID_EXT1 ||
+	    sensor->id == ST_LSM6DSX_ID_EXT2) {
+		err = st_lsm6dsx_shub_set_enable(sensor, enable);
 		if (err < 0)
 			goto out;
 	} else {
-		err = st_lsm6dsx_sensor_disable(sensor);
+		err = st_lsm6dsx_sensor_set_enable(sensor, enable);
 		if (err < 0)
 			goto out;
-	}
 
-	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
-	if (err < 0)
-		goto out;
+		err = st_lsm6dsx_set_fifo_odr(sensor, enable);
+		if (err < 0)
+			goto out;
+	}
 
 	err = st_lsm6dsx_update_decimators(hw);
 	if (err < 0)
@@ -690,6 +750,9 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
 	}
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		buffer = devm_iio_kfifo_allocate(hw->dev);
 		if (!buffer)
 			return -ENOMEM;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 2ad3c610e4b6..12e29dda9b98 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -56,6 +56,7 @@
 #define ST_LSM6DSX_REG_WHOAMI_ADDR		0x0f
 #define ST_LSM6DSX_REG_RESET_ADDR		0x12
 #define ST_LSM6DSX_REG_RESET_MASK		BIT(0)
+#define ST_LSM6DSX_REG_BOOT_MASK		BIT(7)
 #define ST_LSM6DSX_REG_BDU_ADDR			0x12
 #define ST_LSM6DSX_REG_BDU_MASK			BIT(6)
 #define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR	0x13
@@ -87,17 +88,6 @@
 #define ST_LSM6DSX_GYRO_FS_1000_GAIN		IIO_DEGREE_TO_RAD(35000)
 #define ST_LSM6DSX_GYRO_FS_2000_GAIN		IIO_DEGREE_TO_RAD(70000)
 
-struct st_lsm6dsx_odr {
-	u16 hz;
-	u8 val;
-};
-
-#define ST_LSM6DSX_ODR_LIST_SIZE	6
-struct st_lsm6dsx_odr_table_entry {
-	struct st_lsm6dsx_reg reg;
-	struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
-};
-
 static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
 	[ST_LSM6DSX_ID_ACC] = {
 		.reg = {
@@ -125,17 +115,6 @@ static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
 	}
 };
 
-struct st_lsm6dsx_fs {
-	u32 gain;
-	u8 val;
-};
-
-#define ST_LSM6DSX_FS_LIST_SIZE		4
-struct st_lsm6dsx_fs_table_entry {
-	struct st_lsm6dsx_reg reg;
-	struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
-};
-
 static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
 	[ST_LSM6DSX_ID_ACC] = {
 		.reg = {
@@ -341,27 +320,35 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = GENMASK(7, 6),
 			},
 		},
+		.shub_settings = {
+			.page_mux = {
+				.addr = 0x01,
+				.mask = BIT(6),
+			},
+			.master_en = {
+				.addr = 0x14,
+				.mask = BIT(2),
+			},
+			.pullup_en = {
+				.addr = 0x14,
+				.mask = BIT(3),
+			},
+			.aux_sens = {
+				.addr = 0x14,
+				.mask = GENMASK(1, 0),
+			},
+			.wr_once = {
+				.addr = 0x14,
+				.mask = BIT(6),
+			},
+			.shub_out = 0x02,
+			.slv0_addr = 0x15,
+			.dw_slv0_addr = 0x21,
+			.batch_en = BIT(3),
+		}
 	},
 };
 
-#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx)		\
-{									\
-	.type = chan_type,						\
-	.address = addr,						\
-	.modified = 1,							\
-	.channel2 = mod,						\
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
-			      BIT(IIO_CHAN_INFO_SCALE),			\
-	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
-	.scan_index = scan_idx,						\
-	.scan_type = {							\
-		.sign = 's',						\
-		.realbits = 16,						\
-		.storagebits = 16,					\
-		.endianness = IIO_LE,					\
-	},								\
-}
-
 static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
 	ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
 			   IIO_MOD_X, 0),
@@ -382,6 +369,21 @@ static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
+int st_lsm6dsx_set_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;
+	data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->page_mux.mask);
+	err = regmap_update_bits(hw->regmap, hub_settings->page_mux.addr,
+				 hub_settings->page_mux.mask, data);
+	usleep_range(100, 150);
+
+	return err;
+}
+
 static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
 {
 	int err, i, j, data;
@@ -421,6 +423,7 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
 {
 	struct st_lsm6dsx_hw *hw = sensor->hw;
 	const struct st_lsm6dsx_reg *reg;
+	unsigned int data;
 	int i, err;
 	u8 val;
 
@@ -433,8 +436,8 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
 
 	val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;
 	reg = &st_lsm6dsx_fs_table[sensor->id].reg;
-	err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
-				 ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
+	data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
+	err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
 	if (err < 0)
 		return err;
 
@@ -448,7 +451,11 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val)
 	int i;
 
 	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
-		if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr)
+		/*
+		 * ext devices can run at different odr respect to
+		 * accel sensor
+		 */
+		if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz >= odr)
 			break;
 
 	if (i == ST_LSM6DSX_ODR_LIST_SIZE)
@@ -459,48 +466,86 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val)
 	return 0;
 }
 
-static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
+static u16 st_lsm6dsx_check_odr_dependency(struct st_lsm6dsx_hw *hw, u16 odr,
+					   enum st_lsm6dsx_sensor_id id)
 {
-	struct st_lsm6dsx_hw *hw = sensor->hw;
-	const struct st_lsm6dsx_reg *reg;
-	int err;
-	u8 val;
-
-	err = st_lsm6dsx_check_odr(sensor, odr, &val);
-	if (err < 0)
-		return err;
-
-	reg = &st_lsm6dsx_odr_table[sensor->id].reg;
-	return regmap_update_bits(hw->regmap, reg->addr, reg->mask,
-				  ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
+	struct st_lsm6dsx_sensor *ref = iio_priv(hw->iio_devs[id]);
+
+	if (odr > 0) {
+		if (hw->enable_mask & BIT(id))
+			return max_t(u16, ref->odr, odr);
+		else
+			return odr;
+	} else {
+		return (hw->enable_mask & BIT(id)) ? ref->odr : 0;
+	}
 }
 
-int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
+static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 req_odr)
 {
+	struct st_lsm6dsx_sensor *ref_sensor = sensor;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	const struct st_lsm6dsx_reg *reg;
+	unsigned int data;
+	u8 val = 0;
 	int err;
 
-	err = st_lsm6dsx_set_odr(sensor, sensor->odr);
-	if (err < 0)
-		return err;
+	switch (sensor->id) {
+	case ST_LSM6DSX_ID_EXT0:
+	case ST_LSM6DSX_ID_EXT1:
+	case ST_LSM6DSX_ID_EXT2:
+	case ST_LSM6DSX_ID_ACC: {
+		u16 odr;
+		int i;
+
+		/*
+		 * i2c embedded controller relies on the accelerometer sensor as
+		 * bus read/write trigger so we need to enable accel device
+		 * at odr = max(accel_odr, ext_odr) in order to properly
+		 * communicate with i2c slave devices
+		 */
+		ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+		for (i = ST_LSM6DSX_ID_ACC; i < ST_LSM6DSX_ID_MAX; i++) {
+			if (!hw->iio_devs[i] || i == sensor->id)
+				continue;
+
+			odr = st_lsm6dsx_check_odr_dependency(hw, req_odr, i);
+			if (odr != req_odr)
+				/* device already configured */
+				return 0;
+		}
+		break;
+	}
+	default:
+		break;
+	}
 
-	sensor->hw->enable_mask |= BIT(sensor->id);
+	if (req_odr > 0) {
+		err = st_lsm6dsx_check_odr(ref_sensor, req_odr, &val);
+		if (err < 0)
+			return err;
+	}
 
-	return 0;
+	reg = &st_lsm6dsx_odr_table[ref_sensor->id].reg;
+	data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
+	return st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
 }
 
-int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
+int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor,
+				 bool enable)
 {
 	struct st_lsm6dsx_hw *hw = sensor->hw;
-	const struct st_lsm6dsx_reg *reg;
+	u16 odr = enable ? sensor->odr : 0;
 	int err;
 
-	reg = &st_lsm6dsx_odr_table[sensor->id].reg;
-	err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
-				 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
+	err = st_lsm6dsx_set_odr(sensor, odr);
 	if (err < 0)
 		return err;
 
-	sensor->hw->enable_mask &= ~BIT(sensor->id);
+	if (enable)
+		hw->enable_mask |= BIT(sensor->id);
+	else
+		hw->enable_mask &= ~BIT(sensor->id);
 
 	return 0;
 }
@@ -512,18 +557,18 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
 	int err, delay;
 	__le16 data;
 
-	err = st_lsm6dsx_sensor_enable(sensor);
+	err = st_lsm6dsx_sensor_set_enable(sensor, true);
 	if (err < 0)
 		return err;
 
 	delay = 1000000 / sensor->odr;
 	usleep_range(delay, 2 * delay);
 
-	err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data));
+	err = st_lsm6dsx_read_locked(hw, addr, &data, sizeof(data));
 	if (err < 0)
 		return err;
 
-	st_lsm6dsx_sensor_disable(sensor);
+	st_lsm6dsx_sensor_set_enable(sensor, false);
 
 	*val = (s16)le16_to_cpu(data);
 
@@ -596,7 +641,7 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
 	return err;
 }
 
-static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
+int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
 {
 	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
 	struct st_lsm6dsx_hw *hw = sensor->hw;
@@ -692,8 +737,6 @@ static const struct iio_info st_lsm6dsx_gyro_info = {
 	.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
 };
 
-static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
-
 static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
 {
 	struct device_node *np = hw->dev->of_node;
@@ -732,6 +775,51 @@ static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
 	return err;
 }
 
+static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	struct device_node *np = hw->dev->of_node;
+	struct st_sensors_platform_data *pdata;
+	unsigned int data;
+	int err = 0;
+
+	hub_settings = &hw->settings->shub_settings;
+
+	pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
+	if ((np && of_property_read_bool(np, "st,pullups")) ||
+	    (pdata && pdata->pullups)) {
+		err = st_lsm6dsx_set_page(hw, true);
+		if (err < 0)
+			return err;
+
+		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->pullup_en.mask);
+		err = regmap_update_bits(hw->regmap,
+					 hub_settings->pullup_en.addr,
+					 hub_settings->pullup_en.mask, data);
+
+		st_lsm6dsx_set_page(hw, false);
+
+		if (err < 0)
+			return err;
+	}
+
+	if (hub_settings->aux_sens.addr) {
+		/* configure aux sensors */
+		err = st_lsm6dsx_set_page(hw, true);
+		if (err < 0)
+			return err;
+
+		data = ST_LSM6DSX_SHIFT_VAL(3, hub_settings->aux_sens.mask);
+		err = regmap_update_bits(hw->regmap,
+					 hub_settings->aux_sens.addr,
+					 hub_settings->aux_sens.mask, data);
+
+		st_lsm6dsx_set_page(hw, false);
+	}
+
+	return err;
+}
+
 static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
 {
 	const struct st_lsm6dsx_hw_ts_settings *ts_settings;
@@ -775,12 +863,23 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
 	u8 drdy_int_reg;
 	int err;
 
-	err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
-			   ST_LSM6DSX_REG_RESET_MASK);
+	/* device sw reset */
+	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
+				 ST_LSM6DSX_REG_RESET_MASK,
+				 FIELD_PREP(ST_LSM6DSX_REG_RESET_MASK, 1));
+	if (err < 0)
+		return err;
+
+	msleep(50);
+
+	/* reload trimming parameter */
+	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
+				 ST_LSM6DSX_REG_BOOT_MASK,
+				 FIELD_PREP(ST_LSM6DSX_REG_BOOT_MASK, 1));
 	if (err < 0)
 		return err;
 
-	msleep(200);
+	msleep(50);
 
 	/* enable Block Data Update */
 	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR,
@@ -801,6 +900,10 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
 	if (err < 0)
 		return err;
 
+	err = st_lsm6dsx_init_shub(hw);
+	if (err < 0)
+		return err;
+
 	return st_lsm6dsx_init_hw_timer(hw);
 }
 
@@ -854,6 +957,7 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
 int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		     struct regmap *regmap)
 {
+	const struct st_lsm6dsx_shub_settings *hub_settings;
 	struct st_lsm6dsx_hw *hw;
 	int i, err;
 
@@ -865,6 +969,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 
 	mutex_init(&hw->fifo_lock);
 	mutex_init(&hw->conf_lock);
+	mutex_init(&hw->page_lock);
 
 	hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL);
 	if (!hw->buff)
@@ -878,7 +983,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 	if (err < 0)
 		return err;
 
-	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+	for (i = 0; i < ST_LSM6DSX_ID_EXT0; i++) {
 		hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
 		if (!hw->iio_devs[i])
 			return -ENOMEM;
@@ -888,6 +993,13 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 	if (err < 0)
 		return err;
 
+	hub_settings = &hw->settings->shub_settings;
+	if (hub_settings->master_en.addr) {
+		err = st_lsm6dsx_shub_probe(hw, name);
+		if (err < 0)
+			return err;
+	}
+
 	if (hw->irq > 0) {
 		err = st_lsm6dsx_fifo_setup(hw);
 		if (err < 0)
@@ -895,6 +1007,9 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 	}
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
 		if (err)
 			return err;
@@ -909,16 +1024,21 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
 	struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
 	struct st_lsm6dsx_sensor *sensor;
 	const struct st_lsm6dsx_reg *reg;
+	unsigned int data;
 	int i, err = 0;
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		sensor = iio_priv(hw->iio_devs[i]);
 		if (!(hw->enable_mask & BIT(sensor->id)))
 			continue;
 
 		reg = &st_lsm6dsx_odr_table[sensor->id].reg;
-		err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
-					 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
+		data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);
+		err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask,
+						    data);
 		if (err < 0)
 			return err;
 	}
@@ -936,6 +1056,9 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
 	int i, err = 0;
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
 		sensor = iio_priv(hw->iio_devs[i]);
 		if (!(hw->enable_mask & BIT(sensor->id)))
 			continue;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
new file mode 100644
index 000000000000..8e47dccdd40f
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -0,0 +1,779 @@
+/*
+ * STMicroelectronics st_lsm6dsx i2c controller driver
+ *
+ * i2c controller embedded in lsm6dx series can connect up to four
+ * slave devices using accelerometer sensor as trigger for i2c
+ * read/write operations. Current implementation relies on SLV0 channel
+ * for slave configuration and SLV{1,2,3} to read data and push them into
+ * the hw FIFO
+ *
+ * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/bitfield.h>
+
+#include "st_lsm6dsx.h"
+
+#define ST_LSM6DSX_MAX_SLV_NUM			3
+#define ST_LSM6DSX_SLV_ADDR(n, base)		((base) + (n) * 3)
+#define ST_LSM6DSX_SLV_SUB_ADDR(n, base)	((base) + 1 + (n) * 3)
+#define ST_LSM6DSX_SLV_CONFIG(n, base)		((base) + 2 + (n) * 3)
+
+#define ST_LS6DSX_READ_OP_MASK			GENMASK(2, 0)
+
+static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = {
+	/* LIS2MDL */
+	{
+		.i2c_addr = { 0x1e },
+		.wai = {
+			.addr = 0x4f,
+			.val = 0x40,
+		},
+		.id = ST_LSM6DSX_ID_MAGN,
+		.odr_table = {
+			.reg = {
+				.addr = 0x60,
+				.mask = GENMASK(3, 2),
+			},
+			.odr_avl[0] = {  10, 0x0 },
+			.odr_avl[1] = {  20, 0x1 },
+			.odr_avl[2] = {  50, 0x2 },
+			.odr_avl[3] = { 100, 0x3 },
+		},
+		.fs_table = {
+			.fs_avl[0] = {
+				.gain = 1500,
+				.val = 0x0,
+			}, /* 1500 uG/LSB */
+		},
+		.temp_comp = {
+			.addr = 0x60,
+			.mask = BIT(7),
+		},
+		.pwr_table = {
+			.reg = {
+				.addr = 0x60,
+				.mask = GENMASK(1, 0),
+			},
+			.off_val = 0x2,
+			.on_val = 0x0,
+		},
+		.off_canc = {
+			.addr = 0x61,
+			.mask = BIT(1),
+		},
+		.bdu = {
+			.addr = 0x62,
+			.mask = BIT(4),
+		},
+		.out = {
+			.addr = 0x68,
+			.len = 6,
+		},
+	},
+};
+
+static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
+{
+	struct st_lsm6dsx_sensor *sensor;
+
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+	msleep((2000U / sensor->odr) + 1);
+}
+
+/**
+ * st_lsm6dsx_shub_read_reg - read i2c controller register
+ *
+ * Read st_lsm6dsx i2c controller register
+ */
+static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
+				    u8 *data, int len)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	int err;
+
+	mutex_lock(&hw->page_lock);
+
+	hub_settings = &hw->settings->shub_settings;
+	err = st_lsm6dsx_set_page(hw, true);
+	if (err < 0)
+		goto out;
+
+	err = regmap_bulk_read(hw->regmap, addr, data, len);
+
+	st_lsm6dsx_set_page(hw, false);
+out:
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+/**
+ * st_lsm6dsx_shub_write_reg - write i2c controller register
+ *
+ * Write st_lsm6dsx i2c controller register
+ */
+static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr,
+				     u8 *data, int len)
+{
+	int err;
+
+	mutex_lock(&hw->page_lock);
+	err = st_lsm6dsx_set_page(hw, true);
+	if (err < 0)
+		goto out;
+
+	err = regmap_bulk_write(hw->regmap, addr, data, len);
+
+	st_lsm6dsx_set_page(hw, false);
+out:
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+static int
+st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,
+				    u8 mask, u8 val)
+{
+	int err;
+
+	mutex_lock(&hw->page_lock);
+	err = st_lsm6dsx_set_page(hw, true);
+	if (err < 0)
+		goto out;
+
+	err = regmap_update_bits(hw->regmap, addr, mask, val);
+
+	st_lsm6dsx_set_page(hw, false);
+out:
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
+					 bool enable)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	unsigned int data;
+	int err;
+
+	/* enable acc sensor as trigger */
+	err = st_lsm6dsx_sensor_set_enable(sensor, enable);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&hw->page_lock);
+
+	hub_settings = &hw->settings->shub_settings;
+	err = st_lsm6dsx_set_page(hw, true);
+	if (err < 0)
+		goto out;
+
+	data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);
+	err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,
+				 hub_settings->master_en.mask, data);
+
+	st_lsm6dsx_set_page(hw, false);
+out:
+	mutex_unlock(&hw->page_lock);
+
+	return err;
+}
+
+/**
+ * st_lsm6dsx_shub_read - read data from slave device register
+ *
+ * Read data from slave device register. SLV0 is used for
+ * one-shot read operation
+ */
+static int
+st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
+		     u8 *data, int len)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	u8 config[3], slv_addr;
+	int err;
+
+	hub_settings = &hw->settings->shub_settings;
+	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+
+	config[0] = (sensor->ext_info.addr << 1) | 1;
+	config[1] = addr;
+	config[2] = len & ST_LS6DSX_READ_OP_MASK;
+
+	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+					sizeof(config));
+	if (err < 0)
+		return err;
+
+	err = st_lsm6dsx_shub_master_enable(sensor, true);
+	if (err < 0)
+		return err;
+
+	st_lsm6dsx_shub_wait_complete(hw);
+
+	err = st_lsm6dsx_shub_read_reg(hw, hub_settings->shub_out, data,
+				       len & ST_LS6DSX_READ_OP_MASK);
+
+	st_lsm6dsx_shub_master_enable(sensor, false);
+
+	memset(config, 0, sizeof(config));
+	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+					 sizeof(config));
+}
+
+/**
+ * st_lsm6dsx_shub_write - write data to slave device register
+ *
+ * Write data from slave device register. SLV0 is used for
+ * one-shot write operation
+ */
+static int
+st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
+		      u8 *data, int len)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	u8 config[2], slv_addr;
+	int err, i;
+
+	hub_settings = &hw->settings->shub_settings;
+	if (hub_settings->wr_once.addr) {
+		unsigned int data;
+
+		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);
+		err = st_lsm6dsx_shub_write_reg_with_mask(hw,
+			hub_settings->wr_once.addr,
+			hub_settings->wr_once.mask,
+			data);
+		if (err < 0)
+			return err;
+	}
+
+	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	config[0] = sensor->ext_info.addr << 1;
+	for (i = 0 ; i < len; i++) {
+		config[1] = addr + i;
+
+		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+						sizeof(config));
+		if (err < 0)
+			return err;
+
+		err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
+						&data[i], 1);
+		if (err < 0)
+			return err;
+
+		err = st_lsm6dsx_shub_master_enable(sensor, true);
+		if (err < 0)
+			return err;
+
+		st_lsm6dsx_shub_wait_complete(hw);
+
+		st_lsm6dsx_shub_master_enable(sensor, false);
+	}
+
+	memset(config, 0, sizeof(config));
+	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
+}
+
+static int
+st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor,
+				u8 addr, u8 mask, u8 val)
+{
+	int err;
+	u8 data;
+
+	err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data));
+	if (err < 0)
+		return err;
+
+	data = ((data & ~mask) | (val << __ffs(mask) & mask));
+
+	return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data));
+}
+
+static int
+st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor,
+			    u16 odr, u16 *val)
+{
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	int i;
+
+	settings = sensor->ext_info.settings;
+	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
+		if (settings->odr_table.odr_avl[i].hz == odr)
+			break;
+
+	if (i == ST_LSM6DSX_ODR_LIST_SIZE)
+		return -EINVAL;
+
+	*val = settings->odr_table.odr_avl[i].val;
+	return 0;
+}
+
+static int
+st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
+{
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	u16 val;
+	int err;
+
+	err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val);
+	if (err < 0)
+		return err;
+
+	settings = sensor->ext_info.settings;
+	return st_lsm6dsx_shub_write_with_mask(sensor,
+					       settings->odr_table.reg.addr,
+					       settings->odr_table.reg.mask,
+					       val);
+}
+
+/* use SLV{1,2,3} for FIFO read operations */
+static int
+st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,
+				bool enable)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	u8 config[9] = {}, enable_mask, slv_addr;
+	struct st_lsm6dsx_hw *hw = sensor->hw;
+	struct st_lsm6dsx_sensor *cur_sensor;
+	int i, j = 0;
+
+	hub_settings = &hw->settings->shub_settings;
+	if (enable)
+		enable_mask = hw->enable_mask | BIT(sensor->id);
+	else
+		enable_mask = hw->enable_mask & ~BIT(sensor->id);
+
+	for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {
+		if (!hw->iio_devs[i])
+			continue;
+
+		cur_sensor = iio_priv(hw->iio_devs[i]);
+		if (!(enable_mask & BIT(cur_sensor->id)))
+			continue;
+
+		settings = cur_sensor->ext_info.settings;
+		config[j] = (sensor->ext_info.addr << 1) | 1;
+		config[j + 1] = settings->out.addr;
+		config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |
+				hub_settings->batch_en;
+		j += 3;
+	}
+
+	slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);
+	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+					 sizeof(config));
+}
+
+int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
+{
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	int err;
+
+	err = st_lsm6dsx_shub_config_channels(sensor, enable);
+	if (err < 0)
+		return err;
+
+	settings = sensor->ext_info.settings;
+	if (enable) {
+		err = st_lsm6dsx_shub_set_odr(sensor, sensor->odr);
+		if (err < 0)
+			return err;
+	} else {
+		err = st_lsm6dsx_shub_write_with_mask(sensor,
+					settings->odr_table.reg.addr,
+					settings->odr_table.reg.mask, 0);
+		if (err < 0)
+			return err;
+	}
+
+	if (settings->pwr_table.reg.addr) {
+		u8 val;
+
+		val = enable ? settings->pwr_table.on_val
+			     : settings->pwr_table.off_val;
+		err = st_lsm6dsx_shub_write_with_mask(sensor,
+					settings->pwr_table.reg.addr,
+					settings->pwr_table.reg.mask, val);
+		if (err < 0)
+			return err;
+	}
+
+	return st_lsm6dsx_shub_master_enable(sensor, enable);
+}
+
+static int
+st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor,
+			     struct iio_chan_spec const *ch,
+			     int *val)
+{
+	int err, delay, len;
+	u8 data[4];
+
+	err = st_lsm6dsx_shub_set_enable(sensor, true);
+	if (err < 0)
+		return err;
+
+	delay = 1000000 / sensor->odr;
+	usleep_range(delay, 2 * delay);
+
+	len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);
+	err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);
+
+	st_lsm6dsx_shub_set_enable(sensor, false);
+
+	if (err < 0)
+		return err;
+
+	switch (len) {
+	case 2:
+		*val = (s16)le16_to_cpu(*((__le16 *)data));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int
+st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,
+			 struct iio_chan_spec const *ch,
+			 int *val, int *val2, long mask)
+{
+	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(iio_dev);
+		if (ret)
+			break;
+
+		ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);
+		iio_device_release_direct_mode(iio_dev);
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = sensor->odr;
+		ret = IIO_VAL_INT;
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = sensor->gain;
+		ret = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
+			  struct iio_chan_spec const *chan,
+			  int val, int val2, long mask)
+{
+	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+	int err;
+
+	err = iio_device_claim_direct_mode(iio_dev);
+	if (err)
+		return err;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ: {
+		u16 data;
+
+		err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);
+		if (!err)
+			sensor->odr = val;
+		break;
+	}
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	iio_device_release_direct_mode(iio_dev);
+
+	return err;
+}
+
+static ssize_t
+st_lsm6dsx_shub_sampling_freq_avail(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	int i, len = 0;
+
+	settings = sensor->ext_info.settings;
+	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
+		u16 val = settings->odr_table.odr_avl[i].hz;
+
+		if (val > 0)
+			len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
+					 val);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	int i, len = 0;
+
+	settings = sensor->ext_info.settings;
+	for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) {
+		u16 val = settings->fs_table.fs_avl[i].gain;
+
+		if (val > 0)
+			len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
+					 val);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail);
+static IIO_DEVICE_ATTR(in_scale_available, 0444,
+		       st_lsm6dsx_shub_scale_avail, NULL, 0);
+static struct attribute *st_lsm6dsx_ext_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_in_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6dsx_ext_attribute_group = {
+	.attrs = st_lsm6dsx_ext_attributes,
+};
+
+static const struct iio_info st_lsm6dsx_ext_info = {
+	.attrs = &st_lsm6dsx_ext_attribute_group,
+	.read_raw = st_lsm6dsx_shub_read_raw,
+	.write_raw = st_lsm6dsx_shub_write_raw,
+	.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
+};
+
+static struct iio_dev *
+st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw,
+			     enum st_lsm6dsx_sensor_id id,
+			     const struct st_lsm6dsx_ext_dev_settings *info,
+			     u8 i2c_addr, const char *name)
+{
+	struct iio_chan_spec *ext_channels;
+	struct st_lsm6dsx_sensor *sensor;
+	struct iio_dev *iio_dev;
+
+	iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
+	if (!iio_dev)
+		return NULL;
+
+	iio_dev->modes = INDIO_DIRECT_MODE;
+	iio_dev->dev.parent = hw->dev;
+	iio_dev->info = &st_lsm6dsx_ext_info;
+
+	sensor = iio_priv(iio_dev);
+	sensor->id = id;
+	sensor->hw = hw;
+	sensor->odr = info->odr_table.odr_avl[0].hz;
+	sensor->gain = info->fs_table.fs_avl[0].gain;
+	sensor->ext_info.settings = info;
+	sensor->ext_info.addr = i2c_addr;
+	sensor->watermark = 1;
+
+	switch (info->id) {
+	case ST_LSM6DSX_ID_MAGN: {
+		const struct iio_chan_spec magn_channels[] = {
+			ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr,
+					   IIO_MOD_X, 0),
+			ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2,
+					   IIO_MOD_Y, 1),
+			ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4,
+					   IIO_MOD_Z, 2),
+			IIO_CHAN_SOFT_TIMESTAMP(3),
+		};
+
+		ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels),
+					    GFP_KERNEL);
+		if (!ext_channels)
+			return NULL;
+
+		memcpy(ext_channels, magn_channels, sizeof(magn_channels));
+		iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
+		iio_dev->channels = ext_channels;
+		iio_dev->num_channels = ARRAY_SIZE(magn_channels);
+
+		scnprintf(sensor->name, sizeof(sensor->name), "%s_magn",
+			  name);
+		break;
+	}
+	default:
+		return NULL;
+	}
+	iio_dev->name = sensor->name;
+
+	return iio_dev;
+}
+
+static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor)
+{
+	const struct st_lsm6dsx_ext_dev_settings *settings;
+	int err;
+
+	settings = sensor->ext_info.settings;
+	if (settings->bdu.addr) {
+		err = st_lsm6dsx_shub_write_with_mask(sensor,
+						      settings->bdu.addr,
+						      settings->bdu.mask, 1);
+		if (err < 0)
+			return err;
+	}
+
+	if (settings->temp_comp.addr) {
+		err = st_lsm6dsx_shub_write_with_mask(sensor,
+					settings->temp_comp.addr,
+					settings->temp_comp.mask, 1);
+		if (err < 0)
+			return err;
+	}
+
+	if (settings->off_canc.addr) {
+		err = st_lsm6dsx_shub_write_with_mask(sensor,
+					settings->off_canc.addr,
+					settings->off_canc.mask, 1);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+static int
+st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
+			  const struct st_lsm6dsx_ext_dev_settings *settings)
+{
+	const struct st_lsm6dsx_shub_settings *hub_settings;
+	struct st_lsm6dsx_sensor *sensor;
+	u8 config[3], data, slv_addr;
+	bool found = false;
+	int i, err;
+
+	hub_settings = &hw->settings->shub_settings;
+	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+
+	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
+		if (!settings->i2c_addr[i])
+			continue;
+
+		/* read wai slave register */
+		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
+		config[1] = settings->wai.addr;
+		config[2] = 0x1;
+
+		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+						sizeof(config));
+		if (err < 0)
+			return err;
+
+		err = st_lsm6dsx_shub_master_enable(sensor, true);
+		if (err < 0)
+			return err;
+
+		st_lsm6dsx_shub_wait_complete(hw);
+
+		err = st_lsm6dsx_shub_read_reg(hw,
+					       hub_settings->shub_out,
+					       &data, sizeof(data));
+
+		st_lsm6dsx_shub_master_enable(sensor, false);
+
+		if (err < 0)
+			return err;
+
+		if (data != settings->wai.val)
+			continue;
+
+		*i2c_addr = settings->i2c_addr[i];
+		found = true;
+		break;
+	}
+
+	/* reset SLV0 channel */
+	memset(config, 0, sizeof(config));
+	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
+					sizeof(config));
+	if (err < 0)
+		return err;
+
+	return found ? 0 : -ENODEV;
+}
+
+int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
+{
+	enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0;
+	struct st_lsm6dsx_sensor *sensor;
+	int err, i, num_ext_dev = 0;
+	u8 i2c_addr = 0;
+
+	for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) {
+		err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr,
+					&st_lsm6dsx_ext_dev_table[i]);
+		if (err == -ENODEV)
+			continue;
+		else if (err < 0)
+			return err;
+
+		hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id,
+						&st_lsm6dsx_ext_dev_table[i],
+						i2c_addr, name);
+		if (!hw->iio_devs[id])
+			return -ENOMEM;
+
+		sensor = iio_priv(hw->iio_devs[id]);
+		err = st_lsm6dsx_shub_init_device(sensor);
+		if (err < 0)
+			return err;
+
+		if (++num_ext_dev >= ST_LSM6DSX_MAX_SLV_NUM)
+			break;
+		id++;
+	}
+
+	return 0;
+}