iio: imu: st_lsm6dsx: enable drdy-mask if available

Enable drdy mask if available in order to mark invalid samples during
sensor bootstrap phase

Tested-by: Mario Tesi <mario.tesi@st.com>
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 858b4e9..b766c22 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -245,6 +245,7 @@ struct st_lsm6dsx_ext_dev_settings {
  * @id: List of hw id/device name supported by the driver configuration.
  * @channels: IIO channels supported by the device.
  * @irq_config: interrupts related registers.
+ * @drdy_mask: register info for data-ready mask (addr + mask).
  * @odr_table: Hw sensors odr table (Hz + val).
  * @fs_table: Hw sensors gain table (gain + val).
  * @decimator: List of decimator register info (addr + mask).
@@ -277,6 +278,7 @@ struct st_lsm6dsx_settings {
                struct st_lsm6dsx_reg hla;
                struct st_lsm6dsx_reg od;
        } irq_config;
+       struct st_lsm6dsx_reg drdy_mask;
        struct st_lsm6dsx_odr_table_entry odr_table[2];
        struct st_lsm6dsx_fs_table_entry fs_table[2];
        struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID];

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index cabd4bf..1f68734 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -450,13 +450,19 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
        return read_len;
 }
 
+#define ST_LSM6DSX_INVALID_SAMPLE      0x7ffd
 static int
 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
                            u8 *data, s64 ts)
 {
+       s16 val = le16_to_cpu(*(__le16 *)data);
        struct st_lsm6dsx_sensor *sensor;
        struct iio_dev *iio_dev;
 
+       /* invalid sample during bootstrap phase */
+       if (val >= ST_LSM6DSX_INVALID_SAMPLE)
+               return -EINVAL;
+
        /*
         * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
         * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 8f70a64..4f2a787 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -723,6 +723,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels),
                        },
                },
+               .drdy_mask = {
+                       .addr = 0x13,
+                       .mask = BIT(3),
+               },
                .odr_table = {
                        [ST_LSM6DSX_ID_ACC] = {
                                .reg = {
@@ -913,6 +917,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels),
                        },
                },
+               .drdy_mask = {
+                       .addr = 0x13,
+                       .mask = BIT(3),
+               },
                .odr_table = {
                        [ST_LSM6DSX_ID_ACC] = {
                                .reg = {
@@ -1080,6 +1088,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels),
                        },
                },
+               .drdy_mask = {
+                       .addr = 0x13,
+                       .mask = BIT(3),
+               },
                .odr_table = {
                        [ST_LSM6DSX_ID_ACC] = {
                                .reg = {
@@ -1943,6 +1955,15 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
                }
        }
 
+       /* enable drdy-mas if available */
+       if (hw->settings->drdy_mask.addr) {
+               reg = &hw->settings->drdy_mask;
+               err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
+                                        ST_LSM6DSX_SHIFT_VAL(1, reg->mask));
+               if (err < 0)
+                       return err;
+       }
+
        err = st_lsm6dsx_init_shub(hw);
        if (err < 0)
                return err;