mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-09-04 18:09:57 +00:00
iio: imu: st_lsm6dsx: add support to LSM6DSO
Add support to STM LSM6DSO 6-axis (acc + gyro) Mems sensor https://www.st.com/resource/en/datasheet/lsm6dso.pdf Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@redhat.com> Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
parent
5b3c87fd92
commit
801a6e0af0
6 changed files with 205 additions and 8 deletions
|
@ -9,7 +9,7 @@ config IIO_ST_LSM6DSX
|
|||
help
|
||||
Say yes here to build support for STMicroelectronics LSM6DSx imu
|
||||
sensor. Supported devices: lsm6ds3, lsm6ds3h, lsm6dsl, lsm6dsm,
|
||||
ism330dlc
|
||||
ism330dlc, lsm6dso
|
||||
|
||||
To compile this driver as a module, choose M here: the module
|
||||
will be called st_lsm6dsx.
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
#define ST_LSM6DSL_DEV_NAME "lsm6dsl"
|
||||
#define ST_LSM6DSM_DEV_NAME "lsm6dsm"
|
||||
#define ST_ISM330DLC_DEV_NAME "ism330dlc"
|
||||
#define ST_LSM6DSO_DEV_NAME "lsm6dso"
|
||||
|
||||
enum st_lsm6dsx_hw_id {
|
||||
ST_LSM6DS3_ID,
|
||||
|
@ -26,14 +27,20 @@ enum st_lsm6dsx_hw_id {
|
|||
ST_LSM6DSL_ID,
|
||||
ST_LSM6DSM_ID,
|
||||
ST_ISM330DLC_ID,
|
||||
ST_LSM6DSO_ID,
|
||||
ST_LSM6DSX_MAX_ID,
|
||||
};
|
||||
|
||||
#define ST_LSM6DSX_BUFF_SIZE 400
|
||||
#define ST_LSM6DSX_BUFF_SIZE 512
|
||||
#define ST_LSM6DSX_CHAN_SIZE 2
|
||||
#define ST_LSM6DSX_SAMPLE_SIZE 6
|
||||
#define ST_LSM6DSX_TAG_SIZE 1
|
||||
#define ST_LSM6DSX_TAGGED_SAMPLE_SIZE (ST_LSM6DSX_SAMPLE_SIZE + \
|
||||
ST_LSM6DSX_TAG_SIZE)
|
||||
#define ST_LSM6DSX_MAX_WORD_LEN ((32 / ST_LSM6DSX_SAMPLE_SIZE) * \
|
||||
ST_LSM6DSX_SAMPLE_SIZE)
|
||||
#define ST_LSM6DSX_MAX_TAGGED_WORD_LEN ((32 / ST_LSM6DSX_TAGGED_SAMPLE_SIZE) \
|
||||
* ST_LSM6DSX_TAGGED_SAMPLE_SIZE)
|
||||
#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask))
|
||||
|
||||
struct st_lsm6dsx_reg {
|
||||
|
@ -83,6 +90,7 @@ struct st_lsm6dsx_hw_ts_settings {
|
|||
* @max_fifo_size: Sensor max fifo length in FIFO words.
|
||||
* @id: List of hw id supported by the driver configuration.
|
||||
* @decimator: List of decimator register info (addr + mask).
|
||||
* @batch: List of FIFO batching register info (addr + mask).
|
||||
* @fifo_ops: Sensor hw FIFO parameters.
|
||||
* @ts_settings: Hw timer related settings.
|
||||
*/
|
||||
|
@ -91,6 +99,7 @@ struct st_lsm6dsx_settings {
|
|||
u16 max_fifo_size;
|
||||
enum st_lsm6dsx_hw_id id[ST_LSM6DSX_MAX_ID];
|
||||
struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID];
|
||||
struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
|
||||
struct st_lsm6dsx_fifo_ops fifo_ops;
|
||||
struct st_lsm6dsx_hw_ts_settings ts_settings;
|
||||
};
|
||||
|
@ -180,6 +189,7 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw);
|
|||
int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
|
||||
enum st_lsm6dsx_fifo_mode fifo_mode);
|
||||
int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw);
|
||||
int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw);
|
||||
int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val);
|
||||
|
||||
#endif /* ST_LSM6DSX_H */
|
||||
|
|
|
@ -12,6 +12,11 @@
|
|||
* buffer contains the data of all the enabled FIFO data sets
|
||||
* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
|
||||
* value of the decimation factor and ODR set for each FIFO data set.
|
||||
*
|
||||
* LSM6DSO: The FIFO buffer can be configured to store data from gyroscope and
|
||||
* accelerometer. Each sample is queued with a tag (1B) indicating data source
|
||||
* (gyroscope, accelerometer, hw timer).
|
||||
*
|
||||
* FIFO supported modes:
|
||||
* - BYPASS: FIFO disabled
|
||||
* - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
|
||||
|
@ -46,6 +51,7 @@
|
|||
#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
|
||||
#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
|
||||
#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
|
||||
#define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78
|
||||
#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
|
||||
|
||||
#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
|
||||
|
@ -58,6 +64,12 @@ struct st_lsm6dsx_decimator_entry {
|
|||
u8 val;
|
||||
};
|
||||
|
||||
enum st_lsm6dsx_fifo_tag {
|
||||
ST_LSM6DSX_GYRO_TAG = 0x01,
|
||||
ST_LSM6DSX_ACC_TAG = 0x02,
|
||||
ST_LSM6DSX_TS_TAG = 0x04,
|
||||
};
|
||||
|
||||
static const
|
||||
struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
|
||||
{ 0, 0x0 },
|
||||
|
@ -177,12 +189,34 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
|
|||
bool enable)
|
||||
{
|
||||
struct st_lsm6dsx_hw *hw = sensor->hw;
|
||||
const struct st_lsm6dsx_reg *batch_reg;
|
||||
u8 data;
|
||||
|
||||
data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
|
||||
return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
|
||||
ST_LSM6DSX_FIFO_ODR_MASK,
|
||||
FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
|
||||
batch_reg = &hw->settings->batch[sensor->id];
|
||||
if (batch_reg->addr) {
|
||||
int val;
|
||||
|
||||
if (enable) {
|
||||
int err;
|
||||
|
||||
err = st_lsm6dsx_check_odr(sensor, sensor->odr,
|
||||
&data);
|
||||
if (err < 0)
|
||||
return err;
|
||||
} else {
|
||||
data = 0;
|
||||
}
|
||||
val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
|
||||
return regmap_update_bits(hw->regmap, batch_reg->addr,
|
||||
batch_reg->mask, val);
|
||||
} else {
|
||||
data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
|
||||
return regmap_update_bits(hw->regmap,
|
||||
ST_LSM6DSX_REG_FIFO_MODE_ADDR,
|
||||
ST_LSM6DSX_FIFO_ODR_MASK,
|
||||
FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
|
||||
data));
|
||||
}
|
||||
}
|
||||
|
||||
int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
|
||||
|
@ -251,8 +285,8 @@ static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
|
|||
}
|
||||
|
||||
/*
|
||||
* Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
|
||||
* a kmalloc for each bus access
|
||||
* Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
|
||||
* in order to avoid a kmalloc for each bus access
|
||||
*/
|
||||
static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
|
||||
u8 *data, unsigned int data_len,
|
||||
|
@ -403,6 +437,104 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
|
|||
return read_len;
|
||||
}
|
||||
|
||||
/**
|
||||
* st_lsm6dsx_read_tagged_fifo() - LSM6DSO read FIFO routine
|
||||
* @hw: Pointer to instance of struct st_lsm6dsx_hw.
|
||||
*
|
||||
* Read samples from the hw FIFO and push them to IIO buffers.
|
||||
*
|
||||
* Return: Number of bytes read from the FIFO
|
||||
*/
|
||||
int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
|
||||
{
|
||||
u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
|
||||
u16 fifo_len, fifo_diff_mask;
|
||||
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
|
||||
u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE], tag;
|
||||
bool reset_ts = false;
|
||||
int i, err, read_len;
|
||||
__le16 fifo_status;
|
||||
s64 ts = 0;
|
||||
|
||||
err = regmap_bulk_read(hw->regmap,
|
||||
hw->settings->fifo_ops.fifo_diff.addr,
|
||||
&fifo_status, sizeof(fifo_status));
|
||||
if (err < 0) {
|
||||
dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
|
||||
err);
|
||||
return err;
|
||||
}
|
||||
|
||||
fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
|
||||
fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
|
||||
ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
|
||||
if (!fifo_len)
|
||||
return 0;
|
||||
|
||||
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
|
||||
gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
|
||||
|
||||
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
|
||||
err = st_lsm6dsx_read_block(hw,
|
||||
ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
|
||||
hw->buff, pattern_len,
|
||||
ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
|
||||
if (err < 0) {
|
||||
dev_err(hw->dev,
|
||||
"failed to read pattern from fifo (err=%d)\n",
|
||||
err);
|
||||
return err;
|
||||
}
|
||||
|
||||
for (i = 0; i < pattern_len;
|
||||
i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
|
||||
memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
|
||||
ST_LSM6DSX_SAMPLE_SIZE);
|
||||
|
||||
tag = hw->buff[i] >> 3;
|
||||
switch (tag) {
|
||||
case ST_LSM6DSX_TS_TAG:
|
||||
/*
|
||||
* hw timestamp is 4B long and it is stored
|
||||
* in FIFO according to this schema:
|
||||
* B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
|
||||
* B3 = ts[31:24]
|
||||
*/
|
||||
ts = le32_to_cpu(*((__le32 *)iio_buff));
|
||||
/*
|
||||
* check if hw timestamp engine is going to
|
||||
* reset (the sensor generates an interrupt
|
||||
* to signal the hw timestamp will reset in
|
||||
* 1.638s)
|
||||
*/
|
||||
if (!reset_ts && ts >= 0xffff0000)
|
||||
reset_ts = true;
|
||||
ts *= ST_LSM6DSX_TS_SENSITIVITY;
|
||||
break;
|
||||
case ST_LSM6DSX_GYRO_TAG:
|
||||
iio_push_to_buffers_with_timestamp(
|
||||
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
|
||||
iio_buff, gyro_sensor->ts_ref + ts);
|
||||
break;
|
||||
case ST_LSM6DSX_ACC_TAG:
|
||||
iio_push_to_buffers_with_timestamp(
|
||||
hw->iio_devs[ST_LSM6DSX_ID_ACC],
|
||||
iio_buff, acc_sensor->ts_ref + ts);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (unlikely(reset_ts)) {
|
||||
err = st_lsm6dsx_reset_hw_ts(hw);
|
||||
if (err < 0)
|
||||
return err;
|
||||
}
|
||||
return read_len;
|
||||
}
|
||||
|
||||
int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
|
||||
{
|
||||
int err;
|
||||
|
|
|
@ -23,6 +23,12 @@
|
|||
* - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
|
||||
* - FIFO size: 4KB
|
||||
*
|
||||
* - LSM6DSO
|
||||
* - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
|
||||
* - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
|
||||
* - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
|
||||
* - FIFO size: 3KB
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*
|
||||
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
|
||||
|
@ -297,6 +303,45 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
|
|||
},
|
||||
},
|
||||
},
|
||||
{
|
||||
.wai = 0x6c,
|
||||
.max_fifo_size = 512,
|
||||
.id = {
|
||||
[0] = ST_LSM6DSO_ID,
|
||||
},
|
||||
.batch = {
|
||||
[ST_LSM6DSX_ID_ACC] = {
|
||||
.addr = 0x09,
|
||||
.mask = GENMASK(3, 0),
|
||||
},
|
||||
[ST_LSM6DSX_ID_GYRO] = {
|
||||
.addr = 0x09,
|
||||
.mask = GENMASK(7, 4),
|
||||
},
|
||||
},
|
||||
.fifo_ops = {
|
||||
.read_fifo = st_lsm6dsx_read_tagged_fifo,
|
||||
.fifo_th = {
|
||||
.addr = 0x07,
|
||||
.mask = GENMASK(8, 0),
|
||||
},
|
||||
.fifo_diff = {
|
||||
.addr = 0x3a,
|
||||
.mask = GENMASK(8, 0),
|
||||
},
|
||||
.th_wl = 1,
|
||||
},
|
||||
.ts_settings = {
|
||||
.timer_en = {
|
||||
.addr = 0x19,
|
||||
.mask = BIT(5),
|
||||
},
|
||||
.decimator = {
|
||||
.addr = 0x0a,
|
||||
.mask = GENMASK(7, 6),
|
||||
},
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
|
||||
|
|
|
@ -61,6 +61,10 @@ static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
|
|||
.compatible = "st,ism330dlc",
|
||||
.data = (void *)ST_ISM330DLC_ID,
|
||||
},
|
||||
{
|
||||
.compatible = "st,lsm6dso",
|
||||
.data = (void *)ST_LSM6DSO_ID,
|
||||
},
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match);
|
||||
|
@ -71,6 +75,7 @@ static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = {
|
|||
{ ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID },
|
||||
{ ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
|
||||
{ ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID },
|
||||
{ ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID },
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table);
|
||||
|
|
|
@ -61,6 +61,10 @@ static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
|
|||
.compatible = "st,ism330dlc",
|
||||
.data = (void *)ST_ISM330DLC_ID,
|
||||
},
|
||||
{
|
||||
.compatible = "st,lsm6dso",
|
||||
.data = (void *)ST_LSM6DSO_ID,
|
||||
},
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match);
|
||||
|
@ -71,6 +75,7 @@ static const struct spi_device_id st_lsm6dsx_spi_id_table[] = {
|
|||
{ ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID },
|
||||
{ ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
|
||||
{ ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID },
|
||||
{ ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID },
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table);
|
||||
|
|
Loading…
Reference in a new issue