Merge tag 'staging-4.21-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging

Pull staging/IIO driver updates from Greg KH:
 "Here is the big staging and iio driver pull request for 4.21-rc1.

  Lots and lots of tiny patches here, nothing major at all. Which is
  good, tiny cleanups is nice to see. No new huge driver removal or
  addition, this release cycle, although there are lots of good IIO
  driver changes, addtions, and movement from staging into the "real"
  part of the kernel, which is always great.

  Full details are in the shortlog, and all of these have been in
  linux-next for a while with no reported issues"

* tag 'staging-4.21-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging: (553 commits)
  staging: mt7621-mmc: Correct spelling mistakes in comments
  staging: wilc1000: fix missing read_write setting when reading data
  mt7621-mmc: char * array declaration might be better as static const
  mt7621-mmc: return statement in void function unnecessary
  mt7621-mmc: Alignment should match open parenthesis
  mt7621-mmc: Removed unnecessary blank lines
  mt7621-mmc: Fix some coding style issues
  staging: android: ashmem: doc: Fix spelling
  staging: rtl8188eu: cleanup brace coding style issues
  staging: rtl8188eu: add spaces around '&' in rtw_mlme_ext.c
  staging: rtl8188eu: change return type of is_basicrate() to bool
  staging: rtl8188eu: simplify null array initializations
  staging: rtl8188eu: change order of declarations to improve readability
  staging: rtl8188eu: make some arrays static in rtw_mlme_ext.c
  staging: rtl8188eu: constify some arrays
  staging: rtl8188eu: convert unsigned char arrays to u8
  staging: rtl8188eu: remove redundant declaration in rtw_mlme_ext.c
  staging: rtl8188eu: remove unused arrays WFD_OUI and WMM_INFO_OUI
  staging: rtl8188eu: remove unnecessary parentheses in rtw_mlme_ext.c
  staging: rtl8188eu: remove unnecessary comments in rtw_mlme_ext.c
  ...

9 files changed, 1276 insertions, 176 deletions

diff --git a/drivers/iio/imu/bmi160/bmi160.h b/drivers/iio/imu/bmi160/bmi160.h
index e7b11e74fd1d..2351049d930b 100644
--- a/drivers/iio/imu/bmi160/bmi160.h
+++ b/drivers/iio/imu/bmi160/bmi160.h
@@ -6,6 +6,5 @@ extern const struct regmap_config bmi160_regmap_config;
 
 int bmi160_core_probe(struct device *dev, struct regmap *regmap,
 		      const char *name, bool use_spi);
-void bmi160_core_remove(struct device *dev);
 
 #endif  /* BMI160_H_ */
diff --git a/drivers/iio/imu/bmi160/bmi160_core.c b/drivers/iio/imu/bmi160/bmi160_core.c
index c85659ca9507..b10330b0f93f 100644
--- a/drivers/iio/imu/bmi160/bmi160_core.c
+++ b/drivers/iio/imu/bmi160/bmi160_core.c
@@ -542,10 +542,12 @@ static int bmi160_chip_init(struct bmi160_data *data, bool use_spi)
 	return 0;
 }
 
-static void bmi160_chip_uninit(struct bmi160_data *data)
+static void bmi160_chip_uninit(void *data)
 {
-	bmi160_set_mode(data, BMI160_GYRO, false);
-	bmi160_set_mode(data, BMI160_ACCEL, false);
+	struct bmi160_data *bmi_data = data;
+
+	bmi160_set_mode(bmi_data, BMI160_GYRO, false);
+	bmi160_set_mode(bmi_data, BMI160_ACCEL, false);
 }
 
 int bmi160_core_probe(struct device *dev, struct regmap *regmap,
@@ -567,6 +569,10 @@ int bmi160_core_probe(struct device *dev, struct regmap *regmap,
 	if (ret < 0)
 		return ret;
 
+	ret = devm_add_action_or_reset(dev, bmi160_chip_uninit, data);
+	if (ret < 0)
+		return ret;
+
 	if (!name && ACPI_HANDLE(dev))
 		name = bmi160_match_acpi_device(dev);
 
@@ -577,35 +583,19 @@ int bmi160_core_probe(struct device *dev, struct regmap *regmap,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &bmi160_info;
 
-	ret = iio_triggered_buffer_setup(indio_dev, NULL,
-					 bmi160_trigger_handler, NULL);
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+					      bmi160_trigger_handler, NULL);
 	if (ret < 0)
-		goto uninit;
+		return ret;
 
-	ret = iio_device_register(indio_dev);
+	ret = devm_iio_device_register(dev, indio_dev);
 	if (ret < 0)
-		goto buffer_cleanup;
+		return ret;
 
 	return 0;
-buffer_cleanup:
-	iio_triggered_buffer_cleanup(indio_dev);
-uninit:
-	bmi160_chip_uninit(data);
-	return ret;
 }
 EXPORT_SYMBOL_GPL(bmi160_core_probe);
 
-void bmi160_core_remove(struct device *dev)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct bmi160_data *data = iio_priv(indio_dev);
-
-	iio_device_unregister(indio_dev);
-	iio_triggered_buffer_cleanup(indio_dev);
-	bmi160_chip_uninit(data);
-}
-EXPORT_SYMBOL_GPL(bmi160_core_remove);
-
 MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com");
 MODULE_DESCRIPTION("Bosch BMI160 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/bmi160/bmi160_i2c.c b/drivers/iio/imu/bmi160/bmi160_i2c.c
index 155a31f72445..5b1f7e6af651 100644
--- a/drivers/iio/imu/bmi160/bmi160_i2c.c
+++ b/drivers/iio/imu/bmi160/bmi160_i2c.c
@@ -38,13 +38,6 @@ static int bmi160_i2c_probe(struct i2c_client *client,
 	return bmi160_core_probe(&client->dev, regmap, name, false);
 }
 
-static int bmi160_i2c_remove(struct i2c_client *client)
-{
-	bmi160_core_remove(&client->dev);
-
-	return 0;
-}
-
 static const struct i2c_device_id bmi160_i2c_id[] = {
 	{"bmi160", 0},
 	{}
@@ -72,7 +65,6 @@ static struct i2c_driver bmi160_i2c_driver = {
 		.of_match_table		= of_match_ptr(bmi160_of_match),
 	},
 	.probe		= bmi160_i2c_probe,
-	.remove		= bmi160_i2c_remove,
 	.id_table	= bmi160_i2c_id,
 };
 module_i2c_driver(bmi160_i2c_driver);
diff --git a/drivers/iio/imu/bmi160/bmi160_spi.c b/drivers/iio/imu/bmi160/bmi160_spi.c
index d34dfdfd1a7d..e521ad14eeac 100644
--- a/drivers/iio/imu/bmi160/bmi160_spi.c
+++ b/drivers/iio/imu/bmi160/bmi160_spi.c
@@ -29,13 +29,6 @@ static int bmi160_spi_probe(struct spi_device *spi)
 	return bmi160_core_probe(&spi->dev, regmap, id->name, true);
 }
 
-static int bmi160_spi_remove(struct spi_device *spi)
-{
-	bmi160_core_remove(&spi->dev);
-
-	return 0;
-}
-
 static const struct spi_device_id bmi160_spi_id[] = {
 	{"bmi160", 0},
 	{}
@@ -58,7 +51,6 @@ MODULE_DEVICE_TABLE(of, bmi160_of_match);
 
 static struct spi_driver bmi160_spi_driver = {
 	.probe		= bmi160_spi_probe,
-	.remove		= bmi160_spi_remove,
 	.id_table	= bmi160_spi_id,
 	.driver = {
 		.acpi_match_table	= ACPI_PTR(bmi160_acpi_match),
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;
+}