Skip to contentSkip to conversation

introduce i2c master controller support for LSM6DSM

PATCH Applied Lorenzo Bianconi 3 participants 15 messages Dec 14, 2019 – Dec 23, 2019

Subsystems (2)
lore
Share & tools
lore

Lorenzo Bianconi

author maintainer
#0 lore raw

cover introduce i2c master controller support for LSM6DSM

Enabled i2c master controller support for LSM6DSM devices. Enable ext_sensor0 for lsm6dsm (currently just one external sensor is supported). This series has been tested using LIS2MDL as slave device connected to the i2c controller of the LSM6DSM

Lorenzo Bianconi (6):
iio: imu: st_lsm6dsx: export max num of slave devices in
st_lsm6dsx_shub_settings
iio: imu: st_lsm6dsx: check if master_enable is located in primary
page
iio: imu: st_lsm6dsx: check if pull_up is located in primary page
iio: imu: st_lsm6dsx: check if shub_output reg is located in primary
page
iio: imu: st_lsm6dsx: rename st_lsm6dsx_shub_read_reg in
st_lsm6dsx_shub_read_output
iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       | 23 +++++-
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 27 +++++--
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 74 ++++++++++++++++--
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c  | 76 ++++++++++++-------
 4 files changed, 157 insertions(+), 43 deletions(-)
Signature · 1 line
2.21.0
author maintainer
#1 lore raw

PATCH 1/6 iio: imu: st_lsm6dsx: export max num of slave devices in st_lsm6dsx_shub_settings

Export max number of slave devices supported by the i2c master controller in st_lsm6dsx_shub_settings data structure. This is a preliminary patch to support i2c master controller on lsm6dsm sensors

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      | 2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 3 +--
 3 files changed, 5 insertions(+), 2 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +2 -0
index c605b153be41..fb76e0d6770d 100644
@@ -176,6 +176,7 @@ struct st_lsm6dsx_hw_ts_settings {
  * @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).
+ * @num_ext_dev: max number of slave devices.
  * @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.
@@ -187,6 +188,7 @@ struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
+	u8 num_ext_dev;
 	u8 shub_out;
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +2 -0
index a4ed72aef93e..bf995b58c333 100644
@@ -882,6 +882,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x14,
 				.mask = BIT(6),
 			},
+			.num_ext_dev = 3,
 			.shub_out = 0x02,
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
@@ -1256,6 +1257,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x14,
 				.mask = BIT(6),
 			},
+			.num_ext_dev = 3,
 			.shub_out = 0x02,
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +1 -2
index fa5d1001a46c..5684e663d0aa 100644
@@ -30,7 +30,6 @@
 
 #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)
@@ -770,7 +769,7 @@ int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
 		if (err < 0)
 			return err;
 
-		if (++num_ext_dev >= ST_LSM6DSX_MAX_SLV_NUM)
+		if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev)
 			break;
 		id++;
 	}
Signature · 1 line
2.21.0
maintainer Applied
#2 lore raw

Re: [PATCH 1/6] iio: imu: st_lsm6dsx: export max num of slave devices in st_lsm6dsx_shub_settings

Quoted 6 lines · On Sat, 14 Dec 2019 17:52:54 +0100 Lorenzo Bianconi <[email protected]> wrote:

Export max number of slave devices supported by the i2c master controller in st_lsm6dsx_shub_settings data structure. This is a preliminary patch to support i2c master controller on lsm6dsm sensors

Applied.

Thanks,

J

Quoted 67 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      | 2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 3 +--
 3 files changed, 5 insertions(+), 2 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +2 -0
index c605b153be41..fb76e0d6770d 100644
@@ -176,6 +176,7 @@ struct st_lsm6dsx_hw_ts_settings {
  * @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).
+ * @num_ext_dev: max number of slave devices.
  * @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.
@@ -187,6 +188,7 @@ struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
+	u8 num_ext_dev;
 	u8 shub_out;
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +2 -0
index a4ed72aef93e..bf995b58c333 100644
@@ -882,6 +882,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x14,
 				.mask = BIT(6),
 			},
+			.num_ext_dev = 3,
 			.shub_out = 0x02,
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
@@ -1256,6 +1257,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x14,
 				.mask = BIT(6),
 			},
+			.num_ext_dev = 3,
 			.shub_out = 0x02,
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +1 -2
index fa5d1001a46c..5684e663d0aa 100644
@@ -30,7 +30,6 @@
 
 #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)
@@ -770,7 +769,7 @@ int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
 		if (err < 0)
 			return err;
 
-		if (++num_ext_dev >= ST_LSM6DSX_MAX_SLV_NUM)
+		if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev)
 			break;
 		id++;
 	}
author maintainer
#3 lore raw

PATCH 2/6 iio: imu: st_lsm6dsx: check if master_enable is located in primary page

In reply to Lorenzo Bianconi

Check if the master enable register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  6 +++++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c |  2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 11 +++++++----
 3 files changed, 14 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +5 -1
index fb76e0d6770d..0f8f3fbde67a 100644
@@ -184,7 +184,11 @@ struct st_lsm6dsx_hw_ts_settings {
  */
 struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg page_mux;
-	struct st_lsm6dsx_reg master_en;
+	struct {
+		bool sec_page;
+		u8 addr;
+		u8 mask;
+	} master_en;
 	struct st_lsm6dsx_reg pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +2 -0
index bf995b58c333..fe1aaaf68a9b 100644
@@ -867,6 +867,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.master_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(2),
 			},
@@ -1242,6 +1243,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.master_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(2),
 			},
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +7 -4
index 5684e663d0aa..767482e61a1e 100644
@@ -185,15 +185,18 @@ static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
 	mutex_lock(&hw->page_lock);
 
 	hub_settings = &hw->settings->shub_settings;
-	err = st_lsm6dsx_set_page(hw, true);
-	if (err < 0)
-		goto out;
+	if (hub_settings->master_en.sec_page) {
+		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);
+	if (hub_settings->master_en.sec_page)
+		st_lsm6dsx_set_page(hw, false);
 out:
 	mutex_unlock(&hw->page_lock);
 
Signature · 1 line
2.21.0
maintainer Applied
#4 lore raw

Re: [PATCH 2/6] iio: imu: st_lsm6dsx: check if master_enable is located in primary page

Quoted 5 lines · On Sat, 14 Dec 2019 17:52:55 +0100 Lorenzo Bianconi <[email protected]> wrote:

Check if the master enable register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

Applied.

Thanks,

Jonathan

Quoted 70 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  6 +++++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c |  2 ++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 11 +++++++----
 3 files changed, 14 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +5 -1
index fb76e0d6770d..0f8f3fbde67a 100644
@@ -184,7 +184,11 @@ struct st_lsm6dsx_hw_ts_settings {
  */
 struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg page_mux;
-	struct st_lsm6dsx_reg master_en;
+	struct {
+		bool sec_page;
+		u8 addr;
+		u8 mask;
+	} master_en;
 	struct st_lsm6dsx_reg pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +2 -0
index bf995b58c333..fe1aaaf68a9b 100644
@@ -867,6 +867,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.master_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(2),
 			},
@@ -1242,6 +1243,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.master_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(2),
 			},
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +7 -4
index 5684e663d0aa..767482e61a1e 100644
@@ -185,15 +185,18 @@ static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
 	mutex_lock(&hw->page_lock);
 
 	hub_settings = &hw->settings->shub_settings;
-	err = st_lsm6dsx_set_page(hw, true);
-	if (err < 0)
-		goto out;
+	if (hub_settings->master_en.sec_page) {
+		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);
+	if (hub_settings->master_en.sec_page)
+		st_lsm6dsx_set_page(hw, false);
 out:
 	mutex_unlock(&hw->page_lock);
 
author maintainer
#5 lore raw

PATCH 3/6 iio: imu: st_lsm6dsx: check if pull_up is located in primary page

In reply to Lorenzo Bianconi

Check if the pull up register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  6 +++++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 13 +++++++++----
 2 files changed, 14 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +5 -1
index 0f8f3fbde67a..b4bd02df2900 100644
@@ -189,7 +189,11 @@ struct st_lsm6dsx_shub_settings {
 		u8 addr;
 		u8 mask;
 	} master_en;
-	struct st_lsm6dsx_reg pullup_en;
+	struct {
+		bool sec_page;
+		u8 addr;
+		u8 mask;
+	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
 	u8 num_ext_dev;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +9 -4
index fe1aaaf68a9b..6adfc53ef8d0 100644
@@ -872,6 +872,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1248,6 +1249,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1863,16 +1865,19 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 	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;
+		if (hub_settings->pullup_en.sec_page) {
+			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 (hub_settings->pullup_en.sec_page)
+			st_lsm6dsx_set_page(hw, false);
 
 		if (err < 0)
 			return err;
Signature · 1 line
2.21.0
maintainer
#6 lore raw

Re: [PATCH 3/6] iio: imu: st_lsm6dsx: check if pull_up is located in primary page

Quoted 5 lines · On Sat, 14 Dec 2019 17:52:56 +0100 Lorenzo Bianconi <[email protected]> wrote:

Check if the pull up register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

There was a bit of fuzz on this one given Andy's series merged in the meantime. Looks like it went on cleanly but please check.

This has happened in this order because I got a bit swamped so it was easier to start at newest and go back in time (many older versions are already superceded). Sorry about that!

Thanks,

Jonathan

Quoted 66 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  6 +++++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 13 +++++++++----
 2 files changed, 14 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +5 -1
index 0f8f3fbde67a..b4bd02df2900 100644
@@ -189,7 +189,11 @@ struct st_lsm6dsx_shub_settings {
 		u8 addr;
 		u8 mask;
 	} master_en;
-	struct st_lsm6dsx_reg pullup_en;
+	struct {
+		bool sec_page;
+		u8 addr;
+		u8 mask;
+	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
 	u8 num_ext_dev;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +9 -4
index fe1aaaf68a9b..6adfc53ef8d0 100644
@@ -872,6 +872,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1248,6 +1249,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1863,16 +1865,19 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 	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;
+		if (hub_settings->pullup_en.sec_page) {
+			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 (hub_settings->pullup_en.sec_page)
+			st_lsm6dsx_set_page(hw, false);
 
 		if (err < 0)
 			return err;
maintainer Applied
#7 lore raw

Re: [PATCH 3/6] iio: imu: st_lsm6dsx: check if pull_up is located in primary page

In reply to Lorenzo Bianconi
Quoted 5 lines · On Sat, 14 Dec 2019 17:52:56 +0100 Lorenzo Bianconi <[email protected]> wrote:

Check if the pull up register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

Applied,

Thanks,

Jonathan

Quoted 66 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  6 +++++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 13 +++++++++----
 2 files changed, 14 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +5 -1
index 0f8f3fbde67a..b4bd02df2900 100644
@@ -189,7 +189,11 @@ struct st_lsm6dsx_shub_settings {
 		u8 addr;
 		u8 mask;
 	} master_en;
-	struct st_lsm6dsx_reg pullup_en;
+	struct {
+		bool sec_page;
+		u8 addr;
+		u8 mask;
+	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
 	u8 num_ext_dev;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +9 -4
index fe1aaaf68a9b..6adfc53ef8d0 100644
@@ -872,6 +872,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1248,6 +1249,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(2),
 			},
 			.pullup_en = {
+				.sec_page = true,
 				.addr = 0x14,
 				.mask = BIT(3),
 			},
@@ -1863,16 +1865,19 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 	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;
+		if (hub_settings->pullup_en.sec_page) {
+			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 (hub_settings->pullup_en.sec_page)
+			st_lsm6dsx_set_page(hw, false);
 
 		if (err < 0)
 			return err;
author maintainer
#8 lore raw

PATCH 4/6 iio: imu: st_lsm6dsx: check if shub_output reg is located in primary page

In reply to Lorenzo Bianconi

Check if the sensor hub output register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  5 +++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 10 +++++--
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 28 +++++++++++---------
 3 files changed, 28 insertions(+), 15 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +4 -1
index b4bd02df2900..c2d00c3dd044 100644
@@ -197,7 +197,10 @@ struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
 	u8 num_ext_dev;
-	u8 shub_out;
+	struct {
+		bool sec_page;
+		u8 addr;
+	} shub_out;
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
 	u8 batch_en;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +8 -2
index 6adfc53ef8d0..679831d84d78 100644
@@ -885,7 +885,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.num_ext_dev = 3,
-			.shub_out = 0x02,
+			.shub_out = {
+				.sec_page = true,
+				.addr = 0x02,
+			},
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
 			.batch_en = BIT(3),
@@ -1262,7 +1265,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.num_ext_dev = 3,
-			.shub_out = 0x02,
+			.shub_out = {
+				.sec_page = true,
+				.addr = 0x02,
+			},
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
 			.batch_en = BIT(3),
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +16 -12
index 767482e61a1e..ccc59682cb66 100644
@@ -105,20 +105,27 @@ static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
  *
  * Read st_lsm6dsx i2c controller register
  */
-static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
-				    u8 *data, int len)
+static int
+st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 *data,
+			 int len)
 {
+	const struct st_lsm6dsx_shub_settings *hub_settings;
 	int err;
 
 	mutex_lock(&hw->page_lock);
 
-	err = st_lsm6dsx_set_page(hw, true);
-	if (err < 0)
-		goto out;
+	hub_settings = &hw->settings->shub_settings;
+	if (hub_settings->shub_out.sec_page) {
+		err = st_lsm6dsx_set_page(hw, true);
+		if (err < 0)
+			goto out;
+	}
 
-	err = regmap_bulk_read(hw->regmap, addr, data, len);
+	err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr,
+			       data, len);
 
-	st_lsm6dsx_set_page(hw, false);
+	if (hub_settings->shub_out.sec_page)
+		st_lsm6dsx_set_page(hw, false);
 out:
 	mutex_unlock(&hw->page_lock);
 
@@ -236,8 +243,7 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_wait_complete(hw);
 
-	err = st_lsm6dsx_shub_read_reg(hw, hub_settings->shub_out, data,
-				       len & ST_LS6DSX_READ_OP_MASK);
+	err = st_lsm6dsx_shub_read_reg(hw, data, len & ST_LS6DSX_READ_OP_MASK);
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
@@ -719,9 +725,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 
 		st_lsm6dsx_shub_wait_complete(hw);
 
-		err = st_lsm6dsx_shub_read_reg(hw,
-					       hub_settings->shub_out,
-					       &data, sizeof(data));
+		err = st_lsm6dsx_shub_read_reg(hw, &data, sizeof(data));
 
 		st_lsm6dsx_shub_master_enable(sensor, false);
 
Signature · 1 line
2.21.0
maintainer Applied
#9 lore raw

Re: [PATCH 4/6] iio: imu: st_lsm6dsx: check if shub_output reg is located in primary page

Quoted 5 lines · On Sat, 14 Dec 2019 17:52:57 +0100 Lorenzo Bianconi <[email protected]> wrote:

Check if the sensor hub output register is located in the primary or in the secondary memory page. This is a preliminary patch to support i2c master controller on lsm6dsm devices

Applied.

Thanks,

Jonathan

Quoted 110 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  5 +++-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 10 +++++--
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 28 +++++++++++---------
 3 files changed, 28 insertions(+), 15 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +4 -1
index b4bd02df2900..c2d00c3dd044 100644
@@ -197,7 +197,10 @@ struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
 	u8 num_ext_dev;
-	u8 shub_out;
+	struct {
+		bool sec_page;
+		u8 addr;
+	} shub_out;
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
 	u8 batch_en;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +8 -2
index 6adfc53ef8d0..679831d84d78 100644
@@ -885,7 +885,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.num_ext_dev = 3,
-			.shub_out = 0x02,
+			.shub_out = {
+				.sec_page = true,
+				.addr = 0x02,
+			},
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
 			.batch_en = BIT(3),
@@ -1262,7 +1265,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = BIT(6),
 			},
 			.num_ext_dev = 3,
-			.shub_out = 0x02,
+			.shub_out = {
+				.sec_page = true,
+				.addr = 0x02,
+			},
 			.slv0_addr = 0x15,
 			.dw_slv0_addr = 0x21,
 			.batch_en = BIT(3),
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +16 -12
index 767482e61a1e..ccc59682cb66 100644
@@ -105,20 +105,27 @@ static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
  *
  * Read st_lsm6dsx i2c controller register
  */
-static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
-				    u8 *data, int len)
+static int
+st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 *data,
+			 int len)
 {
+	const struct st_lsm6dsx_shub_settings *hub_settings;
 	int err;
 
 	mutex_lock(&hw->page_lock);
 
-	err = st_lsm6dsx_set_page(hw, true);
-	if (err < 0)
-		goto out;
+	hub_settings = &hw->settings->shub_settings;
+	if (hub_settings->shub_out.sec_page) {
+		err = st_lsm6dsx_set_page(hw, true);
+		if (err < 0)
+			goto out;
+	}
 
-	err = regmap_bulk_read(hw->regmap, addr, data, len);
+	err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr,
+			       data, len);
 
-	st_lsm6dsx_set_page(hw, false);
+	if (hub_settings->shub_out.sec_page)
+		st_lsm6dsx_set_page(hw, false);
 out:
 	mutex_unlock(&hw->page_lock);
 
@@ -236,8 +243,7 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_wait_complete(hw);
 
-	err = st_lsm6dsx_shub_read_reg(hw, hub_settings->shub_out, data,
-				       len & ST_LS6DSX_READ_OP_MASK);
+	err = st_lsm6dsx_shub_read_reg(hw, data, len & ST_LS6DSX_READ_OP_MASK);
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
@@ -719,9 +725,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 
 		st_lsm6dsx_shub_wait_complete(hw);
 
-		err = st_lsm6dsx_shub_read_reg(hw,
-					       hub_settings->shub_out,
-					       &data, sizeof(data));
+		err = st_lsm6dsx_shub_read_reg(hw, &data, sizeof(data));
 
 		st_lsm6dsx_shub_master_enable(sensor, false);
 
author maintainer
#10 lore raw

PATCH 5/6 iio: imu: st_lsm6dsx: rename st_lsm6dsx_shub_read_reg in st_lsm6dsx_shub_read_output

In reply to Lorenzo Bianconi

Rename st_lsm6dsx_shub_read_reg routine in st_lsm6dsx_shub_read_output since it is used to read from sensorhub channel0 output register

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +6 -5
index ccc59682cb66..dc739dfb36f2 100644
@@ -101,13 +101,13 @@ static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
 }
 
 /**
- * st_lsm6dsx_shub_read_reg - read i2c controller register
+ * st_lsm6dsx_shub_read_output - read i2c controller register
  *
  * Read st_lsm6dsx i2c controller register
  */
 static int
-st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 *data,
-			 int len)
+st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data,
+			    int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
 	int err;
@@ -243,7 +243,8 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_wait_complete(hw);
 
-	err = st_lsm6dsx_shub_read_reg(hw, data, len & ST_LS6DSX_READ_OP_MASK);
+	err = st_lsm6dsx_shub_read_output(hw, data,
+					  len & ST_LS6DSX_READ_OP_MASK);
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
@@ -725,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 
 		st_lsm6dsx_shub_wait_complete(hw);
 
-		err = st_lsm6dsx_shub_read_reg(hw, &data, sizeof(data));
+		err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data));
 
 		st_lsm6dsx_shub_master_enable(sensor, false);
 
Signature · 1 line
2.21.0
maintainer Applied
#11 lore raw

Re: [PATCH 5/6] iio: imu: st_lsm6dsx: rename st_lsm6dsx_shub_read_reg in st_lsm6dsx_shub_read_output

Quoted 4 lines · On Sat, 14 Dec 2019 17:52:58 +0100 Lorenzo Bianconi <[email protected]> wrote:

Rename st_lsm6dsx_shub_read_reg routine in st_lsm6dsx_shub_read_output since it is used to read from sensorhub channel0 output register

Applied.

Thanks,

Jonathan

Quoted 44 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +6 -5
index ccc59682cb66..dc739dfb36f2 100644
@@ -101,13 +101,13 @@ static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
 }
 
 /**
- * st_lsm6dsx_shub_read_reg - read i2c controller register
+ * st_lsm6dsx_shub_read_output - read i2c controller register
  *
  * Read st_lsm6dsx i2c controller register
  */
 static int
-st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 *data,
-			 int len)
+st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data,
+			    int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
 	int err;
@@ -243,7 +243,8 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_wait_complete(hw);
 
-	err = st_lsm6dsx_shub_read_reg(hw, data, len & ST_LS6DSX_READ_OP_MASK);
+	err = st_lsm6dsx_shub_read_output(hw, data,
+					  len & ST_LS6DSX_READ_OP_MASK);
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
@@ -725,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 
 		st_lsm6dsx_shub_wait_complete(hw);
 
-		err = st_lsm6dsx_shub_read_reg(hw, &data, sizeof(data));
+		err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data));
 
 		st_lsm6dsx_shub_master_enable(sensor, false);
 
author maintainer
#12 lore raw

PATCH 6/6 iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

In reply to Lorenzo Bianconi

Enabled i2c master controller support for LSM6DSM sensor. Enable ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as slave device connected to the i2c controller of the LSM6DSM

---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 ++
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 27 ++++++++---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 47 +++++++++++++++++++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c  | 31 ++++++++----
 4 files changed, 94 insertions(+), 15 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +4 -0
index c2d00c3dd044..1c528b7aefdf 100644
@@ -176,11 +176,13 @@ struct st_lsm6dsx_hw_ts_settings {
  * @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).
+ * @emb_func:  embedded function register info (addr + mask).
  * @num_ext_dev: max number of slave devices.
  * @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.
+ * @pause: controller pause value.
  */
 struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg page_mux;
@@ -196,6 +198,7 @@ struct st_lsm6dsx_shub_settings {
 	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
+	struct st_lsm6dsx_reg emb_func;
 	u8 num_ext_dev;
 	struct {
 		bool sec_page;
@@ -204,6 +207,7 @@ struct st_lsm6dsx_shub_settings {
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
 	u8 batch_en;
+	u8 pause;
 };
 
 struct st_lsm6dsx_event_settings {
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +20 -7
index d416990ae309..cc2b19b60d61 100644
@@ -332,12 +332,13 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
  */
 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 {
+	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
+	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
 	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
-	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
-	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
+	u8 ext_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	bool reset_ts = false;
 	__le16 fifo_status;
 	s64 ts = 0;
@@ -360,6 +361,8 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 
 	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
+	if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
+		ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
 
 	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 		err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
@@ -387,12 +390,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 		 * following pattern is repeated every 9 samples:
 		 *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
 		 */
+		ext_sip = ext_sensor ? ext_sensor->sip : 0;
 		gyro_sip = gyro_sensor->sip;
 		acc_sip = acc_sensor->sip;
 		ts_sip = hw->ts_sip;
 		offset = 0;
 
-		while (acc_sip > 0 || gyro_sip > 0) {
+		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
 			if (gyro_sip > 0) {
 				memcpy(gyro_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
@@ -403,6 +407,11 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
+			if (ext_sip > 0) {
+				memcpy(ext_buff, &hw->buff[offset],
+				       ST_LSM6DSX_SAMPLE_SIZE);
+				offset += ST_LSM6DSX_SAMPLE_SIZE;
+			}
 
 			if (ts_sip-- > 0) {
 				u8 data[ST_LSM6DSX_SAMPLE_SIZE];
@@ -436,6 +445,10 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					acc_buff, acc_sensor->ts_ref + ts);
+			if (ext_sip-- > 0)
+				iio_push_to_buffers_with_timestamp(
+					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
+					ext_buff, ext_sensor->ts_ref + ts);
 		}
 	}
 
@@ -628,12 +641,12 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 		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)
 		goto out;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +47 -0

65 lines not on this page · raw message

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +23 -8
index dc739dfb36f2..eea555617d4a 100644
@@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		     u8 *data, int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
+	u8 config[3], slv_addr, slv_config = 0;
 	struct st_lsm6dsx_hw *hw = sensor->hw;
-	u8 config[3], slv_addr;
+	const struct st_lsm6dsx_reg *aux_sens;
 	int err;
 
 	hub_settings = &hw->settings->shub_settings;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	aux_sens = &hw->settings->shub_settings.aux_sens;
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	config[0] = (sensor->ext_info.addr << 1) | 1;
 	config[1] = addr;
-	config[2] = len & ST_LS6DSX_READ_OP_MASK;
+	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
 
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
@@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					 sizeof(config));
 }
@@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		st_lsm6dsx_shub_master_enable(sensor, false);
 	}
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
 }
 
@@ -697,14 +705,19 @@ 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;
+	u8 config[3], data, slv_addr, slv_config = 0;
+	const struct st_lsm6dsx_reg *aux_sens;
 	struct st_lsm6dsx_sensor *sensor;
-	u8 config[3], data, slv_addr;
 	bool found = false;
 	int i, err;
 
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	hub_settings = &hw->settings->shub_settings;
+	aux_sens = &hw->settings->shub_settings.aux_sens;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
-	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
 		if (!settings->i2c_addr[i])
@@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 		/* read wai slave register */
 		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
 		config[1] = settings->wai.addr;
-		config[2] = 0x1;
+		config[2] = 0x1 | slv_config;
 
 		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 						sizeof(config));
@@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 	}
 
 	/* reset SLV0 channel */
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
 	if (err < 0)
Signature · 1 line
2.21.0
maintainer
#13 lore raw

Re: [PATCH 6/6] iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

Quoted 5 lines · On Sat, 14 Dec 2019 17:52:59 +0100 Lorenzo Bianconi <[email protected]> wrote:

Enabled i2c master controller support for LSM6DSM sensor. Enable ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as slave device connected to the i2c controller of the LSM6DSM

One question inline. However I'm happy enough to take it with that outstanding on basis I am not going to push out as non rebasing for a while yet.

Applied to the togreg branch of iio.git and pushed out as testing for the autobuilders to play with it.

Thanks,

Jonathan

Quoted 109 lines
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h       |  4 ++
 .../iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c    | 27 ++++++++---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c  | 47 +++++++++++++++++++
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c  | 31 ++++++++----
 4 files changed, 94 insertions(+), 15 deletions(-)
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +4 -0
index c2d00c3dd044..1c528b7aefdf 100644
@@ -176,11 +176,13 @@ struct st_lsm6dsx_hw_ts_settings {
  * @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).
+ * @emb_func:  embedded function register info (addr + mask).
  * @num_ext_dev: max number of slave devices.
  * @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.
+ * @pause: controller pause value.
  */
 struct st_lsm6dsx_shub_settings {
 	struct st_lsm6dsx_reg page_mux;
@@ -196,6 +198,7 @@ struct st_lsm6dsx_shub_settings {
 	} pullup_en;
 	struct st_lsm6dsx_reg aux_sens;
 	struct st_lsm6dsx_reg wr_once;
+	struct st_lsm6dsx_reg emb_func;
 	u8 num_ext_dev;
 	struct {
 		bool sec_page;
@@ -204,6 +207,7 @@ struct st_lsm6dsx_shub_settings {
 	u8 slv0_addr;
 	u8 dw_slv0_addr;
 	u8 batch_en;
+	u8 pause;
 };
 
 struct st_lsm6dsx_event_settings {
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +16 -3
index d416990ae309..cc2b19b60d61 100644
@@ -332,12 +332,13 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
  */
 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 {
+	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
+	int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
 	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
 	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
-	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
-	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
 	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
+	u8 ext_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
 	bool reset_ts = false;
 	__le16 fifo_status;
 	s64 ts = 0;
@@ -360,6 +361,8 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 
 	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
+	if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
+		ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
 
 	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
 		err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
@@ -387,12 +390,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 		 * following pattern is repeated every 9 samples:
 		 *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
 		 */
+		ext_sip = ext_sensor ? ext_sensor->sip : 0;
 		gyro_sip = gyro_sensor->sip;
 		acc_sip = acc_sensor->sip;
 		ts_sip = hw->ts_sip;
 		offset = 0;
 
-		while (acc_sip > 0 || gyro_sip > 0) {
+		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
 			if (gyro_sip > 0) {
 				memcpy(gyro_buff, &hw->buff[offset],
 				       ST_LSM6DSX_SAMPLE_SIZE);
@@ -403,6 +407,11 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				       ST_LSM6DSX_SAMPLE_SIZE);
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
+			if (ext_sip > 0) {
+				memcpy(ext_buff, &hw->buff[offset],
+				       ST_LSM6DSX_SAMPLE_SIZE);
+				offset += ST_LSM6DSX_SAMPLE_SIZE;
+			}
 
 			if (ts_sip-- > 0) {
 				u8 data[ST_LSM6DSX_SAMPLE_SIZE];
@@ -436,6 +445,10 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					acc_buff, acc_sensor->ts_ref + ts);
+			if (ext_sip-- > 0)
+				iio_push_to_buffers_with_timestamp(
+					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
+					ext_buff, ext_sensor->ts_ref + ts);
 		}
 	}
 

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) · line 645

Quoted 9 lines
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +0 -4
@@ -628,12 +641,12 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 		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;
 	}
 

Why this change? I'm not sure what the original logic was so hard to follow ;)

Quoted 170 lines
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +4 -0
+	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
+	if (err < 0)
+		goto out;
+
 	err = st_lsm6dsx_update_decimators(hw);
 	if (err < 0)
 		goto out;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +47 -0
index 679831d84d78..1f00b0d74b25 100644
@@ -655,6 +655,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x08,
 				.mask = GENMASK(5, 3),
 			},
+			[ST_LSM6DSX_ID_EXT0] = {
+				.addr = 0x09,
+				.mask = GENMASK(2, 0),
+			},
 		},
 		.fifo_ops = {
 			.update_fifo = st_lsm6dsx_update_fifo,
@@ -687,6 +691,39 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = GENMASK(5, 3),
 			},
 		},
+		.shub_settings = {
+			.page_mux = {
+				.addr = 0x01,
+				.mask = BIT(7),
+			},
+			.master_en = {
+				.addr = 0x1a,
+				.mask = BIT(0),
+			},
+			.pullup_en = {
+				.addr = 0x1a,
+				.mask = BIT(3),
+			},
+			.aux_sens = {
+				.addr = 0x04,
+				.mask = GENMASK(5, 4),
+			},
+			.wr_once = {
+				.addr = 0x07,
+				.mask = BIT(5),
+			},
+			.emb_func = {
+				.addr = 0x19,
+				.mask = BIT(2),
+			},
+			.num_ext_dev = 1,
+			.shub_out = {
+				.addr = 0x2e,
+			},
+			.slv0_addr = 0x02,
+			.dw_slv0_addr = 0x0e,
+			.pause = 0x7,
+		},
 		.event_settings = {
 			.enable_reg = {
 				.addr = 0x58,
@@ -1901,6 +1938,16 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 					 hub_settings->aux_sens.mask, data);
 
 		st_lsm6dsx_set_page(hw, false);
+
+		if (err < 0)
+			return err;
+	}
+
+	if (hub_settings->emb_func.addr) {
+		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask);
+		err = regmap_update_bits(hw->regmap,
+					 hub_settings->emb_func.addr,
+					 hub_settings->emb_func.mask, data);
 	}
 
 	return err;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +23 -8
index dc739dfb36f2..eea555617d4a 100644
@@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		     u8 *data, int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
+	u8 config[3], slv_addr, slv_config = 0;
 	struct st_lsm6dsx_hw *hw = sensor->hw;
-	u8 config[3], slv_addr;
+	const struct st_lsm6dsx_reg *aux_sens;
 	int err;
 
 	hub_settings = &hw->settings->shub_settings;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	aux_sens = &hw->settings->shub_settings.aux_sens;
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	config[0] = (sensor->ext_info.addr << 1) | 1;
 	config[1] = addr;
-	config[2] = len & ST_LS6DSX_READ_OP_MASK;
+	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
 
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
@@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					 sizeof(config));
 }
@@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		st_lsm6dsx_shub_master_enable(sensor, false);
 	}
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
 }
 
@@ -697,14 +705,19 @@ 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;
+	u8 config[3], data, slv_addr, slv_config = 0;
+	const struct st_lsm6dsx_reg *aux_sens;
 	struct st_lsm6dsx_sensor *sensor;
-	u8 config[3], data, slv_addr;
 	bool found = false;
 	int i, err;
 
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	hub_settings = &hw->settings->shub_settings;
+	aux_sens = &hw->settings->shub_settings.aux_sens;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
-	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
 		if (!settings->i2c_addr[i])
@@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 		/* read wai slave register */
 		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
 		config[1] = settings->wai.addr;
-		config[2] = 0x1;
+		config[2] = 0x1 | slv_config;
 
 		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 						sizeof(config));
@@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 	}
 
 	/* reset SLV0 channel */
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
 	if (err < 0)
#14 lore raw

Re: [PATCH 6/6] iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

Quoted 20 lines
Quoted 5 lines · On Sat, 14 Dec 2019 17:52:59 +0100 Lorenzo Bianconi <[email protected]> wrote:

Enabled i2c master controller support for LSM6DSM sensor. Enable ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as slave device connected to the i2c controller of the LSM6DSM

One question inline. However I'm happy enough to take it with that outstanding on basis I am not going to push out as non rebasing for a while yet.

Applied to the togreg branch of iio.git and pushed out as testing for the autobuilders to play with it.

Thanks,

Jonathan

Quoted 1 line

---

[...]

Quoted 10 lines
Quoted 10 lines
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +4 -0
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					acc_buff, acc_sensor->ts_ref + ts);
+			if (ext_sip-- > 0)
+				iio_push_to_buffers_with_timestamp(
+					hw->iio_devs[ST_LSM6DSX_ID_EXT0],
+					ext_buff, ext_sensor->ts_ref + ts);
 		}
 	}
Quoted 12 lines

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) · line 645

Quoted 10 lines
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +0 -4
@@ -628,12 +641,12 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 		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;
 	}
 

Why this change? I'm not sure what the original logic was so hard to follow ;)

Hi Jonathan,

we need to set FIFO odr to 1.66KHz for LSM6DSM even if we enable just the sensor hub (and not other sensors).

Regards,
Lorenzo

Quoted 172 lines
Quoted 171 lines
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +4 -0
+	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
+	if (err < 0)
+		goto out;
+
 	err = st_lsm6dsx_update_decimators(hw);
 	if (err < 0)
 		goto out;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +47 -0
index 679831d84d78..1f00b0d74b25 100644
@@ -655,6 +655,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.addr = 0x08,
 				.mask = GENMASK(5, 3),
 			},
+			[ST_LSM6DSX_ID_EXT0] = {
+				.addr = 0x09,
+				.mask = GENMASK(2, 0),
+			},
 		},
 		.fifo_ops = {
 			.update_fifo = st_lsm6dsx_update_fifo,
@@ -687,6 +691,39 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.mask = GENMASK(5, 3),
 			},
 		},
+		.shub_settings = {
+			.page_mux = {
+				.addr = 0x01,
+				.mask = BIT(7),
+			},
+			.master_en = {
+				.addr = 0x1a,
+				.mask = BIT(0),
+			},
+			.pullup_en = {
+				.addr = 0x1a,
+				.mask = BIT(3),
+			},
+			.aux_sens = {
+				.addr = 0x04,
+				.mask = GENMASK(5, 4),
+			},
+			.wr_once = {
+				.addr = 0x07,
+				.mask = BIT(5),
+			},
+			.emb_func = {
+				.addr = 0x19,
+				.mask = BIT(2),
+			},
+			.num_ext_dev = 1,
+			.shub_out = {
+				.addr = 0x2e,
+			},
+			.slv0_addr = 0x02,
+			.dw_slv0_addr = 0x0e,
+			.pause = 0x7,
+		},
 		.event_settings = {
 			.enable_reg = {
 				.addr = 0x58,
@@ -1901,6 +1938,16 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
 					 hub_settings->aux_sens.mask, data);
 
 		st_lsm6dsx_set_page(hw, false);
+
+		if (err < 0)
+			return err;
+	}
+
+	if (hub_settings->emb_func.addr) {
+		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask);
+		err = regmap_update_bits(hw->regmap,
+					 hub_settings->emb_func.addr,
+					 hub_settings->emb_func.mask, data);
 	}
 
 	return err;
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +23 -8
index dc739dfb36f2..eea555617d4a 100644
@@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		     u8 *data, int len)
 {
 	const struct st_lsm6dsx_shub_settings *hub_settings;
+	u8 config[3], slv_addr, slv_config = 0;
 	struct st_lsm6dsx_hw *hw = sensor->hw;
-	u8 config[3], slv_addr;
+	const struct st_lsm6dsx_reg *aux_sens;
 	int err;
 
 	hub_settings = &hw->settings->shub_settings;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+	aux_sens = &hw->settings->shub_settings.aux_sens;
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	config[0] = (sensor->ext_info.addr << 1) | 1;
 	config[1] = addr;
-	config[2] = len & ST_LS6DSX_READ_OP_MASK;
+	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
 
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
@@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
 
 	st_lsm6dsx_shub_master_enable(sensor, false);
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					 sizeof(config));
 }
@@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
 		st_lsm6dsx_shub_master_enable(sensor, false);
 	}
 
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
 	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
 }
 
@@ -697,14 +705,19 @@ 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;
+	u8 config[3], data, slv_addr, slv_config = 0;
+	const struct st_lsm6dsx_reg *aux_sens;
 	struct st_lsm6dsx_sensor *sensor;
-	u8 config[3], data, slv_addr;
 	bool found = false;
 	int i, err;
 
+	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
 	hub_settings = &hw->settings->shub_settings;
+	aux_sens = &hw->settings->shub_settings.aux_sens;
 	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
-	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+	/* do not overwrite aux_sens */
+	if (slv_addr + 2 == aux_sens->addr)
+		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
 
 	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
 		if (!settings->i2c_addr[i])
@@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 		/* read wai slave register */
 		config[0] = (settings->i2c_addr[i] << 1) | 0x1;
 		config[1] = settings->wai.addr;
-		config[2] = 0x1;
+		config[2] = 0x1 | slv_config;
 
 		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 						sizeof(config));
@@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
 	}
 
 	/* reset SLV0 channel */
-	memset(config, 0, sizeof(config));
+	config[0] = hub_settings->pause;
+	config[1] = 0;
+	config[2] = slv_config;
 	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
 					sizeof(config));
 	if (err < 0)