iio: imu: st_lsm6dsx: add hw FIFO support to i2c controller

Introduce hw FIFO support to lsm6dsx i2c controller.
st_lsm6dsx sensor-hub relies on SLV0 for slave configuration since SLV0
is the only channel that can be used to write into i2c slave devices.
SLV{1,2,3} channels are used to read external data and push them into
the hw FIFO

Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@redhat.com>
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 d20746eb3d2daa5d879dd893437e66faaccbd9bf..d1d8d07a07143d45e5c713206cd04008b23f4819 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -130,18 +130,22 @@ struct st_lsm6dsx_hw_ts_settings {
  * @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 {

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 4e7ff370cbe06667712a09c2bcd3c63b343d9e08..2c0d3763405ad69fdaf23ce14aca241d327d6b7a 100644 (file)
--- 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
@@ -460,6 +463,12 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
        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];
@@ -467,6 +476,24 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
        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;
        }
@@ -593,13 +620,21 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
                        goto out;
        }
 
-       err = st_lsm6dsx_sensor_set_enable(sensor, enable);
-       if (err < 0)
-               goto out;
+       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_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)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 149080acd8598ef7d3af43bf34522e2cbf9abe3e..12e29dda9b98f9c3e41ef8b068f0cedcc06c4911 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -337,9 +337,14 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .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),
                }
        },
 };

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
index 9c66e88a1c3a35c680bae4757749cb16cb086ee9..ee59b0cac84fd8d64305d534b71d1e09326e6dd7 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -148,6 +148,26 @@ out:
        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)
 {
@@ -238,6 +258,18 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 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++) {
@@ -319,11 +351,54 @@ st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
                                               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);