iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm

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

Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 001179e..a763ff4 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -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 {

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index d416990..cc2b19b 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -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;

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 49ad70c..0c64e35 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -656,6 +656,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,
@@ -688,6 +692,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,
@@ -1902,6 +1939,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;

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
index dc739df..eea5556 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -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)