iio: imu: st_lsm6dsx: add event configurability on a per axis basis

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>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index b5e8fc85e346ed936e9cc3ca84cea85d61e6e3b6..64c03a00456f2cfababbd9317b2fe9fdac6ce88f 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -268,7 +268,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 721abaf5315d86edcbe438038afba1b64bffb7e7..7f9f6ba52d48d98985f02df88455f46451ddec31 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -1885,12 +1885,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);
@@ -1907,6 +1945,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,
@@ -1924,7 +1995,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;
@@ -1956,7 +2030,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);
@@ -2039,20 +2116,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;
@@ -2070,7 +2138,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;