In order to be able to configure event detection on a per axis
basis (for either setting an event threshold/sensitivity value, or
enabling/disabling event detection), add new axis-specific fields
to struct st_lsm6dsx_event_src, and modify the logic that handles
event configuration to properly handle axis-specific settings when
supported by a given event source.
A future commit will add actual event sources with per-axis
configurability.
Signed-off-by: Francesco Lavra <flavra@baylibre.com>
---
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 7 ++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 95 +++++++++++++++++---
2 files changed, 88 insertions(+), 14 deletions(-)
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 2aae56b7db0b..515aadbee3a4 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -250,7 +250,14 @@ enum st_lsm6dsx_event_id {
struct st_lsm6dsx_event_src {
struct st_lsm6dsx_reg value;
+ struct st_lsm6dsx_reg x_value;
+ struct st_lsm6dsx_reg y_value;
+ struct st_lsm6dsx_reg z_value;
u8 enable_mask;
+ u8 enable_axis_reg;
+ u8 enable_x_mask;
+ u8 enable_y_mask;
+ u8 enable_z_mask;
struct st_lsm6dsx_reg status;
u8 status_x_mask;
u8 status_y_mask;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 6dc6cda54d05..50c00dd38c63 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -1888,12 +1888,50 @@ static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw,
const struct st_lsm6dsx_event_src *src;
unsigned int data;
int err;
+ u8 old_enable, new_enable;
if (!hw->irq_routing)
return -ENOTSUPP;
/* Enable/disable event interrupt */
src = &hw->settings->event_settings.sources[event];
+ if (src->enable_axis_reg) {
+ u8 enable_mask;
+
+ switch (axis) {
+ case IIO_MOD_X:
+ enable_mask = src->enable_x_mask;
+ break;
+ case IIO_MOD_Y:
+ enable_mask = src->enable_y_mask;
+ break;
+ case IIO_MOD_Z:
+ enable_mask = src->enable_z_mask;
+ break;
+ default:
+ enable_mask = 0;
+ }
+ if (enable_mask) {
+ data = ST_LSM6DSX_SHIFT_VAL(state, enable_mask);
+ err = st_lsm6dsx_update_bits_locked(hw,
+ src->enable_axis_reg,
+ enable_mask, data);
+ if (err < 0)
+ return err;
+ }
+ }
+
+ /*
+ * If the set of axes for which the event source is enabled does not
+ * change from empty to non-empty or vice versa, there is nothing else
+ * to do.
+ */
+ old_enable = hw->enable_event[event];
+ new_enable = state ? (old_enable | BIT(axis)) :
+ (old_enable & ~BIT(axis));
+ if (!!old_enable == !!new_enable)
+ return 0;
+
data = ST_LSM6DSX_SHIFT_VAL(state, src->enable_mask);
return st_lsm6dsx_update_bits_locked(hw, hw->irq_routing,
src->enable_mask, data);
@@ -1910,6 +1948,39 @@ st_lsm6dsx_get_event_id(enum iio_event_type type)
}
}
+static const struct st_lsm6dsx_reg *
+st_lsm6dsx_get_event_reg(struct st_lsm6dsx_hw *hw,
+ enum st_lsm6dsx_event_id event,
+ const struct iio_chan_spec *chan)
+{
+ const struct st_lsm6dsx_event_src *src;
+ const struct st_lsm6dsx_reg *reg;
+
+ src = &hw->settings->event_settings.sources[event];
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = &src->x_value;
+ break;
+ case IIO_MOD_Y:
+ reg = &src->y_value;
+ break;
+ case IIO_MOD_Z:
+ reg = &src->z_value;
+ break;
+ default:
+ return NULL;
+ }
+ if (reg->addr)
+ return reg;
+
+ /*
+ * The sensor does not support configuring this event source on a per
+ * axis basis: return the register to configure the event source for all
+ * axes.
+ */
+ return &src->value;
+}
+
static int st_lsm6dsx_read_event(struct iio_dev *iio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
@@ -1927,7 +1998,10 @@ static int st_lsm6dsx_read_event(struct iio_dev *iio_dev,
if (event == ST_LSM6DSX_EVENT_MAX)
return -EINVAL;
- reg = &hw->settings->event_settings.sources[event].value;
+ reg = st_lsm6dsx_get_event_reg(hw, event, chan);
+ if (!reg)
+ return -EINVAL;
+
err = st_lsm6dsx_read_locked(hw, reg->addr, &data, sizeof(data));
if (err < 0)
return err;
@@ -1959,7 +2033,10 @@ st_lsm6dsx_write_event(struct iio_dev *iio_dev,
if (val < 0 || val > 31)
return -EINVAL;
- reg = &hw->settings->event_settings.sources[event].value;
+ reg = st_lsm6dsx_get_event_reg(hw, event, chan);
+ if (!reg)
+ return -EINVAL;
+
data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
err = st_lsm6dsx_update_bits_locked(hw, reg->addr,
reg->mask, data);
@@ -2042,20 +2119,11 @@ st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
if (event == ST_LSM6DSX_EVENT_MAX)
return -EINVAL;
- if (state) {
+ if (state)
enable_event = hw->enable_event[event] | BIT(chan->channel2);
-
- /* do not enable events if they are already enabled */
- if (hw->enable_event[event])
- goto out;
- } else {
+ else
enable_event = hw->enable_event[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[event] == enable_event)
return 0;
@@ -2073,7 +2141,6 @@ st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
if (err < 0)
return err;
-out:
hw->enable_event[event] = enable_event;
return 0;
--
2.39.5