Report iio motion events to iio subsystem
Signed-off-by: Sean Nyekjaer <sean@xxxxxxxxxx>
---
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
