iio: imu: st_lsm6dsx: add motion report function and call from interrupt

Report iio motion events to iio subsystem and filter motion events.
Wakeup will still be on all channels as it's not possible to do the filtering
in hw.

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 6b0ba48394ebaa75e510c496dbb283de63cff74b..fd02d0e184f382f0c5f2000b3cebb479840129da 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -186,6 +186,11 @@ struct st_lsm6dsx_shub_settings {
 struct st_lsm6dsx_event_settings {
        struct st_lsm6dsx_reg enable_reg;
        struct st_lsm6dsx_reg wakeup_reg;
+       u8 wakeup_src_reg;
+       u8 wakeup_src_status_mask;
+       u8 wakeup_src_z_mask;
+       u8 wakeup_src_y_mask;
+       u8 wakeup_src_x_mask;
 };
 
 enum st_lsm6dsx_ext_sensor_id {

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 26acc852982da620ca1f06d0d0a1131001b6f14e..8a813ddba19c3832685862d0888f6112ef3d7a26 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -48,6 +48,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/delay.h>
+#include <linux/iio/events.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/interrupt.h>
@@ -287,6 +288,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .addr = 0x5B,
                                .mask = GENMASK(5, 0),
                        },
+                       .wakeup_src_reg = 0x1b,
+                       .wakeup_src_status_mask = BIT(3),
+                       .wakeup_src_z_mask = BIT(0),
+                       .wakeup_src_y_mask = BIT(1),
+                       .wakeup_src_x_mask = BIT(2),
                },
        },
        {
@@ -412,6 +418,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .addr = 0x5B,
                                .mask = GENMASK(5, 0),
                        },
+                       .wakeup_src_reg = 0x1b,
+                       .wakeup_src_status_mask = BIT(3),
+                       .wakeup_src_z_mask = BIT(0),
+                       .wakeup_src_y_mask = BIT(1),
+                       .wakeup_src_x_mask = BIT(2),
                },
        },
        {
@@ -550,6 +561,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .addr = 0x5B,
                                .mask = GENMASK(5, 0),
                        },
+                       .wakeup_src_reg = 0x1b,
+                       .wakeup_src_status_mask = BIT(3),
+                       .wakeup_src_z_mask = BIT(0),
+                       .wakeup_src_y_mask = BIT(1),
+                       .wakeup_src_x_mask = BIT(2),
                },
        },
        {
@@ -816,6 +832,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .addr = 0x5B,
                                .mask = GENMASK(5, 0),
                        },
+                       .wakeup_src_reg = 0x1b,
+                       .wakeup_src_status_mask = BIT(3),
+                       .wakeup_src_z_mask = BIT(0),
+                       .wakeup_src_y_mask = BIT(1),
+                       .wakeup_src_x_mask = BIT(2),
                },
        },
        {
@@ -970,6 +991,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
                                .addr = 0x5B,
                                .mask = GENMASK(5, 0),
                        },
+                       .wakeup_src_reg = 0x1b,
+                       .wakeup_src_status_mask = BIT(3),
+                       .wakeup_src_z_mask = BIT(0),
+                       .wakeup_src_y_mask = BIT(1),
+                       .wakeup_src_x_mask = BIT(2),
                }
        },
 };
@@ -1334,7 +1360,7 @@ static int st_lsm6dsx_read_event_config(struct iio_dev *iio_dev,
        if (type != IIO_EV_TYPE_THRESH)
                return -EINVAL;
 
-       return hw->enable_event;
+       return !!(hw->enable_event & BIT(chan->channel2));
 }
 
 static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
@@ -1345,13 +1371,28 @@ static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
 {
        struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
        struct st_lsm6dsx_hw *hw = sensor->hw;
+       u8 enable_event;
        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)
+       if (state) {
+               enable_event = hw->enable_event | BIT(chan->channel2);
+
+               /* do not enable events if they are already enabled */
+               if (hw->enable_event)
+                       goto out;
+       } else {
+               enable_event = hw->enable_event & ~BIT(chan->channel2);
+
+               /* only turn off sensor if no events is enabled */
+               if (enable_event)
+                       goto out;
+       }
+
+       /* stop here if no changes have been made */
+       if (hw->enable_event == enable_event)
                return 0;
 
        err = st_lsm6dsx_event_setup(hw, state);
@@ -1362,7 +1403,8 @@ static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
        if (err < 0)
                return err;
 
-       hw->enable_event = state;
+out:
+       hw->enable_event = enable_event;
 
        return 0;
 }
@@ -1715,10 +1757,57 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
        return iio_dev;
 }
 
+static void st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw, int data)
+{
+       s64 timestamp = iio_get_time_ns(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+
+       if ((data & hw->settings->event_settings.wakeup_src_z_mask) &&
+           (hw->enable_event & BIT(IIO_MOD_Z)))
+               iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
+                              IIO_MOD_EVENT_CODE(IIO_ACCEL,
+                                                 0,
+                                                 IIO_MOD_Z,
+                                                 IIO_EV_TYPE_THRESH,
+                                                 IIO_EV_DIR_EITHER),
+                                                 timestamp);
+
+       if ((data & hw->settings->event_settings.wakeup_src_y_mask) &&
+           (hw->enable_event & BIT(IIO_MOD_Y)))
+               iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
+                              IIO_MOD_EVENT_CODE(IIO_ACCEL,
+                                                 0,
+                                                 IIO_MOD_Y,
+                                                 IIO_EV_TYPE_THRESH,
+                                                 IIO_EV_DIR_EITHER),
+                                                 timestamp);
+
+       if ((data & hw->settings->event_settings.wakeup_src_x_mask) &&
+           (hw->enable_event & BIT(IIO_MOD_X)))
+               iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
+                              IIO_MOD_EVENT_CODE(IIO_ACCEL,
+                                                 0,
+                                                 IIO_MOD_X,
+                                                 IIO_EV_TYPE_THRESH,
+                                                 IIO_EV_DIR_EITHER),
+                                                 timestamp);
+}
+
 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 {
        struct st_lsm6dsx_hw *hw = private;
        int count;
+       int data, err;
+
+       if (hw->enable_event) {
+               err = regmap_read(hw->regmap,
+                                 hw->settings->event_settings.wakeup_src_reg,
+                                 &data);
+               if (err < 0)
+                       return IRQ_NONE;
+
+               if (data & hw->settings->event_settings.wakeup_src_status_mask)
+                       st_lsm6dsx_report_motion_event(hw, data);
+       }
 
        mutex_lock(&hw->fifo_lock);
        count = hw->settings->fifo_ops.read_fifo(hw);