diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h index 784434f7098f..b5e8fc85e346 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -463,7 +463,7 @@ struct st_lsm6dsx_hw { u8 sip; u8 irq_routing; - u8 enable_event; + u8 enable_event[ST_LSM6DSX_EVENT_MAX]; 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 129f974a038c..721abaf5315d 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -1756,11 +1756,16 @@ static int st_lsm6dsx_check_events(struct st_lsm6dsx_sensor *sensor) { struct st_lsm6dsx_hw *hw = sensor->hw; + int event; if (sensor->id != ST_LSM6DSX_ID_ACC) return 0; - return hw->enable_event; + for (event = 0; event < ST_LSM6DSX_EVENT_MAX; event++) { + if (hw->enable_event[event]) + return true; + } + return false; } int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor, @@ -1873,9 +1878,10 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, return err; } -static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, bool state) +static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_event_id event, int axis, + bool state) { - const struct st_lsm6dsx_reg *reg; const struct st_lsm6dsx_event_src *src; unsigned int data; int err; @@ -1883,22 +1889,24 @@ static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, bool state) if (!hw->irq_routing) return -ENOTSUPP; - reg = &hw->settings->event_settings.enable_reg; - if (reg->addr) { - data = ST_LSM6DSX_SHIFT_VAL(state, reg->mask); - err = st_lsm6dsx_update_bits_locked(hw, reg->addr, - reg->mask, data); - if (err < 0) - return err; - } - - /* Enable wakeup interrupt */ - src = &hw->settings->event_settings.sources[ST_LSM6DSX_EVENT_WAKEUP]; + /* Enable/disable event interrupt */ + src = &hw->settings->event_settings.sources[event]; data = ST_LSM6DSX_SHIFT_VAL(state, src->enable_mask); return st_lsm6dsx_update_bits_locked(hw, hw->irq_routing, src->enable_mask, data); } +static enum st_lsm6dsx_event_id +st_lsm6dsx_get_event_id(enum iio_event_type type) +{ + switch (type) { + case IIO_EV_TYPE_THRESH: + return ST_LSM6DSX_EVENT_WAKEUP; + default: + return ST_LSM6DSX_EVENT_MAX; + } +} + static int st_lsm6dsx_read_event(struct iio_dev *iio_dev, const struct iio_chan_spec *chan, enum iio_event_type type, @@ -1906,16 +1914,17 @@ static int st_lsm6dsx_read_event(struct iio_dev *iio_dev, enum iio_event_info info, int *val, int *val2) { + enum st_lsm6dsx_event_id event = st_lsm6dsx_get_event_id(type); struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsx_hw *hw = sensor->hw; const struct st_lsm6dsx_reg *reg; u8 data; int err; - if (type != IIO_EV_TYPE_THRESH) + if (event == ST_LSM6DSX_EVENT_MAX) return -EINVAL; - reg = &hw->settings->event_settings.sources[ST_LSM6DSX_EVENT_WAKEUP].value; + reg = &hw->settings->event_settings.sources[event].value; err = st_lsm6dsx_read_locked(hw, reg->addr, &data, sizeof(data)); if (err < 0) return err; @@ -1934,19 +1943,20 @@ st_lsm6dsx_write_event(struct iio_dev *iio_dev, enum iio_event_info info, int val, int val2) { + enum st_lsm6dsx_event_id event = st_lsm6dsx_get_event_id(type); struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsx_hw *hw = sensor->hw; const struct st_lsm6dsx_reg *reg; unsigned int data; int err; - if (type != IIO_EV_TYPE_THRESH) + if (event == ST_LSM6DSX_EVENT_MAX) return -EINVAL; if (val < 0 || val > 31) return -EINVAL; - reg = &hw->settings->event_settings.sources[ST_LSM6DSX_EVENT_WAKEUP].value; + reg = &hw->settings->event_settings.sources[event].value; data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask); err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data); @@ -1962,13 +1972,56 @@ st_lsm6dsx_read_event_config(struct iio_dev *iio_dev, enum iio_event_type type, enum iio_event_direction dir) { + enum st_lsm6dsx_event_id event = st_lsm6dsx_get_event_id(type); struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsx_hw *hw = sensor->hw; - if (type != IIO_EV_TYPE_THRESH) + if (event == ST_LSM6DSX_EVENT_MAX) return -EINVAL; - return !!(hw->enable_event & BIT(chan->channel2)); + return !!(hw->enable_event[event] & BIT(chan->channel2)); +} + +/** + * st_lsm6dsx_check_other_events - Check for enabled sensor events. + * @hw: Sensor hardware instance. + * @curr: Current event type. + * + * Return: whether any events other than @curr are enabled. + */ +static bool st_lsm6dsx_check_other_events(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_event_id curr) +{ + enum st_lsm6dsx_event_id other; + + for (other = 0; other < ST_LSM6DSX_EVENT_MAX; other++) { + if (other != curr && hw->enable_event[other]) + return true; + } + + return false; +} + +static int st_lsm6dsx_events_enable(struct st_lsm6dsx_sensor *sensor, + bool state) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + + reg = &hw->settings->event_settings.enable_reg; + if (reg->addr) { + int err; + + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(state, reg->mask)); + if (err) + return err; + } + + if (state || !(hw->fifo_mask & BIT(sensor->id))) + return __st_lsm6dsx_sensor_set_enable(sensor, state); + + return 0; } static int @@ -1977,22 +2030,23 @@ st_lsm6dsx_write_event_config(struct iio_dev *iio_dev, enum iio_event_type type, enum iio_event_direction dir, bool state) { + enum st_lsm6dsx_event_id event = st_lsm6dsx_get_event_id(type); struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsx_hw *hw = sensor->hw; u8 enable_event; int err; - if (type != IIO_EV_TYPE_THRESH) + if (event == ST_LSM6DSX_EVENT_MAX) return -EINVAL; if (state) { - enable_event = hw->enable_event | BIT(chan->channel2); + enable_event = hw->enable_event[event] | BIT(chan->channel2); /* do not enable events if they are already enabled */ - if (hw->enable_event) + if (hw->enable_event[event]) goto out; } else { - enable_event = hw->enable_event & ~BIT(chan->channel2); + enable_event = hw->enable_event[event] & ~BIT(chan->channel2); /* only turn off sensor if no events is enabled */ if (enable_event) @@ -2000,22 +2054,24 @@ st_lsm6dsx_write_event_config(struct iio_dev *iio_dev, } /* stop here if no changes have been made */ - if (hw->enable_event == enable_event) + if (hw->enable_event[event] == enable_event) return 0; - err = st_lsm6dsx_event_setup(hw, state); + err = st_lsm6dsx_event_setup(hw, event, chan->channel2, state); if (err < 0) return err; mutex_lock(&hw->conf_lock); - if (enable_event || !(hw->fifo_mask & BIT(sensor->id))) - err = __st_lsm6dsx_sensor_set_enable(sensor, state); + if (enable_event) + err = st_lsm6dsx_events_enable(sensor, true); + else if (!st_lsm6dsx_check_other_events(hw, event)) + err = st_lsm6dsx_events_enable(sensor, false); mutex_unlock(&hw->conf_lock); if (err < 0) return err; out: - hw->enable_event = enable_event; + hw->enable_event[event] = enable_event; return 0; } @@ -2419,18 +2475,19 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, } static bool -st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw) +st_lsm6dsx_report_events(struct st_lsm6dsx_hw *hw, enum st_lsm6dsx_event_id id, + enum iio_event_type type, enum iio_event_direction dir) { const struct st_lsm6dsx_event_settings *event_settings; const struct st_lsm6dsx_event_src *src; int err, data; s64 timestamp; - if (!hw->enable_event) + if (!hw->enable_event[id]) return false; event_settings = &hw->settings->event_settings; - src = &event_settings->sources[ST_LSM6DSX_EVENT_WAKEUP]; + src = &event_settings->sources[id]; err = st_lsm6dsx_read_locked(hw, src->status.addr, &data, sizeof(data)); if (err < 0) @@ -2438,38 +2495,49 @@ st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw) timestamp = iio_get_time_ns(hw->iio_devs[ST_LSM6DSX_ID_ACC]); if ((data & src->status_z_mask) && - (hw->enable_event & BIT(IIO_MOD_Z))) + (hw->enable_event[id] & 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), + type, + dir), timestamp); if ((data & src->status_y_mask) && - (hw->enable_event & BIT(IIO_MOD_Y))) + (hw->enable_event[id] & 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), + type, + dir), timestamp); if ((data & src->status_x_mask) && - (hw->enable_event & BIT(IIO_MOD_X))) + (hw->enable_event[id] & 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), + type, + dir), timestamp); return data & src->status.mask; } +static bool st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw) +{ + bool events_found; + + events_found = st_lsm6dsx_report_events(hw, ST_LSM6DSX_EVENT_WAKEUP, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER); + + return events_found; +} + static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) { struct st_lsm6dsx_hw *hw = private;