Skip to content

Commit

Permalink
iio: imu: st_lsm6dsx: add motion events
Browse files Browse the repository at this point in the history 
Add event channels that controls the creation of motion events.
Tested on ISM330DLC

Signed-off-by: Sean Nyekjaer <[email protected]>
Reviewed-by: Lorenzo Bianconi <[email protected]>
Signed-off-by: Jonathan Cameron <[email protected]>
  • Loading branch information
@sknsean @jic23
sknsean authored and jic23 committed Oct 5, 2019
1 parent 6ee6a36 commit b5969ab
Show file tree
Hide file tree
Showing 2 changed files with 228 additions and 4 deletions.
41 changes: 41 additions & 0 deletions drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#define ST_LSM6DSX_H

#include <linux/device.h>
#include <linux/iio/iio.h>

#define ST_LSM6DS3_DEV_NAME "lsm6ds3"
#define ST_LSM6DS3H_DEV_NAME "lsm6ds3h"
Expand Down Expand Up @@ -54,6 +55,26 @@ enum st_lsm6dsx_hw_id {
* ST_LSM6DSX_TAGGED_SAMPLE_SIZE)
#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask))

#define ST_LSM6DSX_CHANNEL_ACC(chan_type, addr, mod, scan_idx) \
{ \
.type = chan_type, \
.address = addr, \
.modified = 1, \
.channel2 = mod, \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
.scan_index = scan_idx, \
.scan_type = { \
.sign = 's', \
.realbits = 16, \
.storagebits = 16, \
.endianness = IIO_LE, \
}, \
.event_spec = &st_lsm6dsx_event, \
.num_event_specs = 1, \
}

#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
{ \
.type = chan_type, \
Expand Down Expand Up @@ -162,6 +183,11 @@ struct st_lsm6dsx_shub_settings {
u8 batch_en;
};

struct st_lsm6dsx_event_settings {
struct st_lsm6dsx_reg enable_reg;
struct st_lsm6dsx_reg wakeup_reg;
};

enum st_lsm6dsx_ext_sensor_id {
ST_LSM6DSX_ID_MAGN,
};
Expand Down Expand Up @@ -225,6 +251,9 @@ struct st_lsm6dsx_settings {
u8 wai;
u8 int1_addr;
u8 int2_addr;
u8 int1_func_addr;
u8 int2_func_addr;
u8 int_func_mask;
u8 reset_addr;
u16 max_fifo_size;
struct {
Expand All @@ -244,6 +273,7 @@ struct st_lsm6dsx_settings {
struct st_lsm6dsx_fifo_ops fifo_ops;
struct st_lsm6dsx_hw_ts_settings ts_settings;
struct st_lsm6dsx_shub_settings shub_settings;
struct st_lsm6dsx_event_settings event_settings;
};

enum st_lsm6dsx_sensor_id {
Expand Down Expand Up @@ -324,13 +354,24 @@ struct st_lsm6dsx_hw {
u8 ts_sip;
u8 sip;

u8 event_threshold;
u8 enable_event;
struct st_lsm6dsx_reg irq_routing;

u8 *buff;

struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];

const struct st_lsm6dsx_settings *settings;
};

static const struct iio_event_spec st_lsm6dsx_event = {
.type = IIO_EV_TYPE_THRESH,
.dir = IIO_EV_DIR_EITHER,
.mask_separate = BIT(IIO_EV_INFO_VALUE) |
BIT(IIO_EV_INFO_ENABLE)
};

static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
extern const struct dev_pm_ops st_lsm6dsx_pm_ops;

Expand Down
191 changes: 187 additions & 4 deletions drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,9 +73,9 @@
#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)

static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
ST_LSM6DSX_CHANNEL(IIO_ACCEL, 0x2c, IIO_MOD_Z, 2),
ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0),
ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2c, IIO_MOD_Z, 2),
IIO_CHAN_SOFT_TIMESTAMP(3),
};

Expand Down Expand Up @@ -168,6 +168,9 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.wai = 0x69,
.int1_addr = 0x0d,
.int2_addr = 0x0e,
.int1_func_addr = 0x5e,
.int2_func_addr = 0x5f,
.int_func_mask = BIT(5),
.reset_addr = 0x12,
.max_fifo_size = 1365,
.id = {
Expand Down Expand Up @@ -279,11 +282,20 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.mask = GENMASK(5, 3),
},
},
.event_settings = {
.wakeup_reg = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
},
},
{
.wai = 0x69,
.int1_addr = 0x0d,
.int2_addr = 0x0e,
.int1_func_addr = 0x5e,
.int2_func_addr = 0x5f,
.int_func_mask = BIT(5),
.reset_addr = 0x12,
.max_fifo_size = 682,
.id = {
Expand Down Expand Up @@ -395,11 +407,20 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.mask = GENMASK(5, 3),
},
},
.event_settings = {
.wakeup_reg = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
},
},
{
.wai = 0x6a,
.int1_addr = 0x0d,
.int2_addr = 0x0e,
.int1_func_addr = 0x5e,
.int2_func_addr = 0x5f,
.int_func_mask = BIT(5),
.reset_addr = 0x12,
.max_fifo_size = 682,
.id = {
Expand Down Expand Up @@ -520,6 +541,16 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.mask = GENMASK(5, 3),
},
},
.event_settings = {
.enable_reg = {
.addr = 0x58,
.mask = BIT(7),
},
.wakeup_reg = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
},
},
{
.wai = 0x6c,
Expand Down Expand Up @@ -666,6 +697,9 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.wai = 0x6b,
.int1_addr = 0x0d,
.int2_addr = 0x0e,
.int1_func_addr = 0x5e,
.int2_func_addr = 0x5f,
.int_func_mask = BIT(5),
.reset_addr = 0x12,
.max_fifo_size = 512,
.id = {
Expand Down Expand Up @@ -773,11 +807,24 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.mask = GENMASK(7, 6),
},
},
.event_settings = {
.enable_reg = {
.addr = 0x58,
.mask = BIT(7),
},
.wakeup_reg = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
},
},
{
.wai = 0x6b,
.int1_addr = 0x0d,
.int2_addr = 0x0e,
.int1_func_addr = 0x5e,
.int2_func_addr = 0x5f,
.int_func_mask = BIT(5),
.reset_addr = 0x12,
.max_fifo_size = 512,
.id = {
Expand Down Expand Up @@ -913,6 +960,16 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.slv0_addr = 0x15,
.dw_slv0_addr = 0x21,
.batch_en = BIT(3),
},
.event_settings = {
.enable_reg = {
.addr = 0x58,
.mask = BIT(7),
},
.wakeup_reg = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
}
},
};
Expand Down Expand Up @@ -1119,7 +1176,8 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
if (err < 0)
return err;

st_lsm6dsx_sensor_set_enable(sensor, false);
if (!hw->enable_event)
st_lsm6dsx_sensor_set_enable(sensor, false);

*val = (s16)le16_to_cpu(data);

Expand Down Expand Up @@ -1192,6 +1250,123 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
return err;
}

static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, int state)
{
int err;
u8 enable = 0;

if (!hw->settings->int1_func_addr)
return -ENOTSUPP;

enable = state ? hw->settings->event_settings.enable_reg.mask : 0;

err = regmap_update_bits(hw->regmap,
hw->settings->event_settings.enable_reg.addr,
hw->settings->event_settings.enable_reg.mask,
enable);
if (err < 0)
return err;

enable = state ? hw->irq_routing.mask : 0;

/* Enable wakeup interrupt */
return regmap_update_bits(hw->regmap, hw->irq_routing.addr,
hw->irq_routing.mask,
enable);
}

static int st_lsm6dsx_read_event(struct iio_dev *iio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
enum iio_event_info info,
int *val, int *val2)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;

if (type != IIO_EV_TYPE_THRESH)
return -EINVAL;

*val2 = 0;
*val = hw->event_threshold;

return IIO_VAL_INT;
}

static int st_lsm6dsx_write_event(struct iio_dev *iio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
enum iio_event_info info,
int val, int val2)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
int err;

if (type != IIO_EV_TYPE_THRESH)
return -EINVAL;

if (val < 0 || val > 31)
return -EINVAL;

err = regmap_update_bits(hw->regmap,
hw->settings->event_settings.wakeup_reg.addr,
hw->settings->event_settings.wakeup_reg.mask,
val);
if (err)
return -EINVAL;

hw->event_threshold = val;

return 0;
}

static int st_lsm6dsx_read_event_config(struct iio_dev *iio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;

if (type != IIO_EV_TYPE_THRESH)
return -EINVAL;

return hw->enable_event;
}

static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
int state)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
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)
return 0;

err = st_lsm6dsx_event_setup(hw, state);
if (err < 0)
return err;

err = st_lsm6dsx_sensor_set_enable(sensor, state);
if (err < 0)
return err;

hw->enable_event = state;

return 0;
}

int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
Expand Down Expand Up @@ -1276,6 +1451,10 @@ static const struct iio_info st_lsm6dsx_acc_info = {
.attrs = &st_lsm6dsx_acc_attribute_group,
.read_raw = st_lsm6dsx_read_raw,
.write_raw = st_lsm6dsx_write_raw,
.read_event_value = st_lsm6dsx_read_event,
.write_event_value = st_lsm6dsx_write_event,
.read_event_config = st_lsm6dsx_read_event_config,
.write_event_config = st_lsm6dsx_write_event_config,
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};

Expand Down Expand Up @@ -1321,9 +1500,13 @@ static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
switch (drdy_pin) {
case 1:
*drdy_reg = hw->settings->int1_addr;
hw->irq_routing.addr = hw->settings->int1_func_addr;
hw->irq_routing.mask = hw->settings->int_func_mask;
break;
case 2:
*drdy_reg = hw->settings->int2_addr;
hw->irq_routing.addr = hw->settings->int2_func_addr;
hw->irq_routing.mask = hw->settings->int_func_mask;
break;
default:
dev_err(hw->dev, "unsupported data ready pin\n");
Expand Down

0 comments on commit b5969ab

Please sign in to comment.