[PATCH v3 5/6] iio: imu: st_lsm6dsx: add motion report function and call from interrupt

[Sean Nyekjaer <sean@geanix.com>]

Report iio motion events to iio subsystem

Signed-off-by: Sean Nyekjaer <sean@geanix.com>
---
Changes since v1:
 * none

Changes since v2:
 * none

Should we include these new defines in device settings?

 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 53 ++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 513506caa460..2114c3c78888 100644
--- 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>
@@ -72,6 +73,12 @@
 #define ST_LSM6DSX_REG_PP_OD_ADDR		0x12
 #define ST_LSM6DSX_REG_PP_OD_MASK		BIT(4)
 
+#define ST_LSM6DSX_REG_WAKE_UP_SRC_ADDR		0x1B
+#define ST_LSM6DSX_REG_WAKE_UP_SRC_Z_WU_MASK	BIT(0)
+#define ST_LSM6DSX_REG_WAKE_UP_SRC_Y_WU_MASK	BIT(1)
+#define ST_LSM6DSX_REG_WAKE_UP_SRC_X_WU_MASK	BIT(2)
+#define ST_LSM6DSX_REG_WAKE_UP_SRC_WU_MASK	BIT(4)
+
 static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
 	ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
 	ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
@@ -1611,6 +1618,40 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
 	return iio_dev;
 }
 
+int 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 & ST_LSM6DSX_REG_WAKE_UP_SRC_Z_WU_MASK)
+		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 & ST_LSM6DSX_REG_WAKE_UP_SRC_Y_WU_MASK)
+		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 & ST_LSM6DSX_REG_WAKE_UP_SRC_X_WU_MASK)
+		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);
+
+	return 0;
+}
+
 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 {
 	return IRQ_WAKE_THREAD;
@@ -1620,7 +1661,19 @@ static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 {
 	struct st_lsm6dsx_hw *hw = private;
 	int count = 0;
+	int data, err;
+
+	if (hw->enable_event) {
+		err = regmap_read(hw->regmap,
+				  ST_LSM6DSX_REG_WAKE_UP_SRC_ADDR, &data);
+		if (err < 0)
+			goto try_fifo;
+
+		if (data & ST_LSM6DSX_REG_WAKE_UP_SRC_WU_MASK)
+			st_lsm6dsx_report_motion_event(hw, data);
+	}
 
+try_fifo:
 	if (hw->sip > 0) {
 		mutex_lock(&hw->fifo_lock);
 		count = st_lsm6dsx_read_fifo(hw);
-- 
2.23.0