[PATCH v9 6/6] iio: imu: st_lsm6dsx: filter motion events in driver

[Sean Nyekjaer <sean@xxxxxxxxxx>]

Do not report non enabled 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>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      |  2 +-
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 38 +++++++++++++++-----
 2 files changed, 31 insertions(+), 9 deletions(-)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 0a782af9445b..fd02d0e184f3 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -360,7 +360,7 @@ struct st_lsm6dsx_hw {
 	u8 sip;
 
 	u8 event_threshold;
-	bool enable_event;
+	u8 enable_event;
 	struct st_lsm6dsx_reg irq_routing;
 
 	u8 *buff;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 6b88d93dca2a..92fee4555dd5 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -1202,7 +1202,7 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
 	if (err < 0)
 		return err;
 
-	if (!hw->enable_event)
+	if (0 == hw->enable_event)
 		st_lsm6dsx_sensor_set_enable(sensor, false);
 
 	*val = (s16)le16_to_cpu(data);
@@ -1360,7 +1360,10 @@ static int st_lsm6dsx_read_event_config(struct iio_dev *iio_dev,
 	if (type != IIO_EV_TYPE_THRESH)
 		return -EINVAL;
 
-	return hw->enable_event;
+	if (hw->enable_event & BIT(chan->channel2))
+		return 1;
+
+	return 0;
 }
 
 static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
@@ -1371,13 +1374,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);
@@ -1388,7 +1406,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;
 }
@@ -1745,7 +1764,8 @@ 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)
+	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,
@@ -1754,7 +1774,8 @@ void st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw, int data)
 						  IIO_EV_DIR_EITHER),
 						  timestamp);
 
-	if (data & hw->settings->event_settings.wakeup_src_y_mask)
+	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,
@@ -1763,7 +1784,8 @@ void st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw, int data)
 						  IIO_EV_DIR_EITHER),
 						  timestamp);
 
-	if (data & hw->settings->event_settings.wakeup_src_x_mask)
+	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,
-- 
2.23.0