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@xxxxxxxxxx>
---
Changes since v4:
* Updated bitmask as pr Jonathans comments
Changes since v5:
* None
Changes since v6:
* None
Changes since v7:
* None
Changes since v8:
* None
Changes since v9:
* Merged with "iio: imu: st_lsm6dsx: filter motion events in driver"
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 5 +
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 97 +++++++++++++++++++-
2 files changed, 98 insertions(+), 4 deletions(-)
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 6b0ba48394eb..fd02d0e184f3 100644
--- 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 6b03c50f4732..f4f71cf4187a 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>
@@ -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;
}
+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);
--
2.23.0
