iio: imu: st_lsm6dsx: add motion events

Add event channels that controls the creation of motion events.
Tested on ISM330DLC

Signed-off-by: Sean Nyekjaer <sean@geanix.com>
Reviewed-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 21d1407..6b0ba48 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -12,6 +12,7 @@
 #define ST_LSM6DSX_H
 
 #include <linux/device.h>
+#include <linux/iio/iio.h>
 
 #define ST_LSM6DS3_DEV_NAME    "lsm6ds3"
 #define ST_LSM6DS3H_DEV_NAME   "lsm6ds3h"
@@ -54,6 +55,26 @@ 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_ACC(chan_type, addr, mod, scan_idx)         \
+{                                                                      \
+       .type = chan_type,                                              \
+       .address = addr,                                                \
+       .modified = 1,                                                  \
+       .channel2 = mod,                                                \
+       .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),                   \
+       .info_mask_shared_by_type = 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,                                   \
+       },                                                              \
+       .event_spec = &st_lsm6dsx_event,                                \
+       .num_event_specs = 1,                                           \
+}
+
 #define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx)             \
 {                                                                      \
        .type = chan_type,                                              \
@@ -162,6 +183,11 @@ struct st_lsm6dsx_shub_settings {
        u8 batch_en;
 };
 
+struct st_lsm6dsx_event_settings {
+       struct st_lsm6dsx_reg enable_reg;
+       struct st_lsm6dsx_reg wakeup_reg;
+};
+
 enum st_lsm6dsx_ext_sensor_id {
        ST_LSM6DSX_ID_MAGN,
 };
@@ -225,6 +251,9 @@ struct st_lsm6dsx_settings {
        u8 wai;
        u8 int1_addr;
        u8 int2_addr;
+       u8 int1_func_addr;
+       u8 int2_func_addr;
+       u8 int_func_mask;
        u8 reset_addr;
        u16 max_fifo_size;
        struct {
@@ -244,6 +273,7 @@ struct st_lsm6dsx_settings {
        struct st_lsm6dsx_fifo_ops fifo_ops;
        struct st_lsm6dsx_hw_ts_settings ts_settings;
        struct st_lsm6dsx_shub_settings shub_settings;
+       struct st_lsm6dsx_event_settings event_settings;
 };
 
 enum st_lsm6dsx_sensor_id {
@@ -324,6 +354,10 @@ struct st_lsm6dsx_hw {
        u8 ts_sip;
        u8 sip;
 
+       u8 event_threshold;
+       u8 enable_event;
+       struct st_lsm6dsx_reg irq_routing;
+
        u8 *buff;
 
        struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];
@@ -331,6 +365,13 @@ struct st_lsm6dsx_hw {
        const struct st_lsm6dsx_settings *settings;
 };
 
+static const struct iio_event_spec st_lsm6dsx_event = {
+       .type = IIO_EV_TYPE_THRESH,
+       .dir = IIO_EV_DIR_EITHER,
+       .mask_separate = BIT(IIO_EV_INFO_VALUE) |
+                        BIT(IIO_EV_INFO_ENABLE)
+};
+
 static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
 extern const struct dev_pm_ops st_lsm6dsx_pm_ops;
 

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index ef83820..806935c 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -73,9 +73,9 @@
 #define ST_LSM6DSX_REG_PP_OD_MASK              BIT(4)
 
 static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
-       ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
-       ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
-       ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x2c, IIO_MOD_Z, 2),
+       ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
+       ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
+       ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2c, IIO_MOD_Z, 2),
        IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
@@ -168,6 +168,9 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                .wai = 0x69,
                .int1_addr = 0x0d,
                .int2_addr = 0x0e,
+               .int1_func_addr = 0x5e,
+               .int2_func_addr = 0x5f,
+               .int_func_mask = BIT(5),
                .reset_addr = 0x12,
                .max_fifo_size = 1365,
                .id = {
@@ -279,11 +282,20 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(5, 3),
                        },
                },
+               .event_settings = {
+                       .wakeup_reg = {
+                               .addr = 0x5B,
+                               .mask = GENMASK(5, 0),
+                       },
+               },
        },
        {
                .wai = 0x69,
                .int1_addr = 0x0d,
                .int2_addr = 0x0e,
+               .int1_func_addr = 0x5e,
+               .int2_func_addr = 0x5f,
+               .int_func_mask = BIT(5),
                .reset_addr = 0x12,
                .max_fifo_size = 682,
                .id = {
@@ -395,11 +407,20 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(5, 3),
                        },
                },
+               .event_settings = {
+                       .wakeup_reg = {
+                               .addr = 0x5B,
+                               .mask = GENMASK(5, 0),
+                       },
+               },
        },
        {
                .wai = 0x6a,
                .int1_addr = 0x0d,
                .int2_addr = 0x0e,
+               .int1_func_addr = 0x5e,
+               .int2_func_addr = 0x5f,
+               .int_func_mask = BIT(5),
                .reset_addr = 0x12,
                .max_fifo_size = 682,
                .id = {
@@ -520,6 +541,16 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(5, 3),
                        },
                },
+               .event_settings = {
+                       .enable_reg = {
+                               .addr = 0x58,
+                               .mask = BIT(7),
+                       },
+                       .wakeup_reg = {
+                               .addr = 0x5B,
+                               .mask = GENMASK(5, 0),
+                       },
+               },
        },
        {
                .wai = 0x6c,
@@ -666,6 +697,9 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                .wai = 0x6b,
                .int1_addr = 0x0d,
                .int2_addr = 0x0e,
+               .int1_func_addr = 0x5e,
+               .int2_func_addr = 0x5f,
+               .int_func_mask = BIT(5),
                .reset_addr = 0x12,
                .max_fifo_size = 512,
                .id = {
@@ -773,11 +807,24 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .mask = GENMASK(7, 6),
                        },
                },
+               .event_settings = {
+                       .enable_reg = {
+                               .addr = 0x58,
+                               .mask = BIT(7),
+                       },
+                       .wakeup_reg = {
+                               .addr = 0x5B,
+                               .mask = GENMASK(5, 0),
+                       },
+               },
        },
        {
                .wai = 0x6b,
                .int1_addr = 0x0d,
                .int2_addr = 0x0e,
+               .int1_func_addr = 0x5e,
+               .int2_func_addr = 0x5f,
+               .int_func_mask = BIT(5),
                .reset_addr = 0x12,
                .max_fifo_size = 512,
                .id = {
@@ -913,6 +960,16 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                        .slv0_addr = 0x15,
                        .dw_slv0_addr = 0x21,
                        .batch_en = BIT(3),
+               },
+               .event_settings = {
+                       .enable_reg = {
+                               .addr = 0x58,
+                               .mask = BIT(7),
+                       },
+                       .wakeup_reg = {
+                               .addr = 0x5B,
+                               .mask = GENMASK(5, 0),
+                       },
                }
        },
 };
@@ -1119,7 +1176,8 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
        if (err < 0)
                return err;
 
-       st_lsm6dsx_sensor_set_enable(sensor, false);
+       if (!hw->enable_event)
+               st_lsm6dsx_sensor_set_enable(sensor, false);
 
        *val = (s16)le16_to_cpu(data);
 
@@ -1192,6 +1250,123 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
        return err;
 }
 
+static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, int state)
+{
+       int err;
+       u8 enable = 0;
+
+       if (!hw->settings->int1_func_addr)
+               return -ENOTSUPP;
+
+       enable = state ? hw->settings->event_settings.enable_reg.mask : 0;
+
+       err = regmap_update_bits(hw->regmap,
+                                hw->settings->event_settings.enable_reg.addr,
+                                hw->settings->event_settings.enable_reg.mask,
+                                enable);
+       if (err < 0)
+               return err;
+
+       enable = state ? hw->irq_routing.mask : 0;
+
+       /* Enable wakeup interrupt */
+       return regmap_update_bits(hw->regmap, hw->irq_routing.addr,
+                                 hw->irq_routing.mask,
+                                 enable);
+}
+
+static int st_lsm6dsx_read_event(struct iio_dev *iio_dev,
+                                  const struct iio_chan_spec *chan,
+                                  enum iio_event_type type,
+                                  enum iio_event_direction dir,
+                                  enum iio_event_info info,
+                                  int *val, int *val2)
+{
+       struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+       struct st_lsm6dsx_hw *hw = sensor->hw;
+
+       if (type != IIO_EV_TYPE_THRESH)
+               return -EINVAL;
+
+       *val2 = 0;
+       *val = hw->event_threshold;
+
+       return IIO_VAL_INT;
+}
+
+static int st_lsm6dsx_write_event(struct iio_dev *iio_dev,
+                                   const struct iio_chan_spec *chan,
+                                   enum iio_event_type type,
+                                   enum iio_event_direction dir,
+                                   enum iio_event_info info,
+                                   int val, int val2)
+{
+       struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+       struct st_lsm6dsx_hw *hw = sensor->hw;
+       int err;
+
+       if (type != IIO_EV_TYPE_THRESH)
+               return -EINVAL;
+
+       if (val < 0 || val > 31)
+               return -EINVAL;
+
+       err = regmap_update_bits(hw->regmap,
+                                hw->settings->event_settings.wakeup_reg.addr,
+                                hw->settings->event_settings.wakeup_reg.mask,
+                                val);
+       if (err)
+               return -EINVAL;
+
+       hw->event_threshold = val;
+
+       return 0;
+}
+
+static int st_lsm6dsx_read_event_config(struct iio_dev *iio_dev,
+                                         const struct iio_chan_spec *chan,
+                                         enum iio_event_type type,
+                                         enum iio_event_direction dir)
+{
+       struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+       struct st_lsm6dsx_hw *hw = sensor->hw;
+
+       if (type != IIO_EV_TYPE_THRESH)
+               return -EINVAL;
+
+       return hw->enable_event;
+}
+
+static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
+                                          const struct iio_chan_spec *chan,
+                                          enum iio_event_type type,
+                                          enum iio_event_direction dir,
+                                          int state)
+{
+       struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
+       struct st_lsm6dsx_hw *hw = sensor->hw;
+       int err = 0;
+
+       if (type != IIO_EV_TYPE_THRESH)
+               return -EINVAL;
+
+       /* do not enable events if they are already enabled */
+       if (state && hw->enable_event)
+               return 0;
+
+       err = st_lsm6dsx_event_setup(hw, state);
+       if (err < 0)
+               return err;
+
+       err = st_lsm6dsx_sensor_set_enable(sensor, state);
+       if (err < 0)
+               return err;
+
+       hw->enable_event = state;
+
+       return 0;
+}
+
 int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
 {
        struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
@@ -1276,6 +1451,10 @@ static const struct iio_info st_lsm6dsx_acc_info = {
        .attrs = &st_lsm6dsx_acc_attribute_group,
        .read_raw = st_lsm6dsx_read_raw,
        .write_raw = st_lsm6dsx_write_raw,
+       .read_event_value = st_lsm6dsx_read_event,
+       .write_event_value = st_lsm6dsx_write_event,
+       .read_event_config = st_lsm6dsx_read_event_config,
+       .write_event_config = st_lsm6dsx_write_event_config,
        .hwfifo_set_watermark = st_lsm6dsx_set_watermark,
 };
 
@@ -1321,9 +1500,13 @@ static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
        switch (drdy_pin) {
        case 1:
                *drdy_reg = hw->settings->int1_addr;
+               hw->irq_routing.addr = hw->settings->int1_func_addr;
+               hw->irq_routing.mask = hw->settings->int_func_mask;
                break;
        case 2:
                *drdy_reg = hw->settings->int2_addr;
+               hw->irq_routing.addr = hw->settings->int2_func_addr;
+               hw->irq_routing.mask = hw->settings->int_func_mask;
                break;
        default:
                dev_err(hw->dev, "unsupported data ready pin\n");