UbiquitousOS
/
OpenCloudOS-Kernel
mirror of https://gitee.com/OpenCloudOS/OpenCloudOS-Kernel.git
11
0
Fork 0
Code Issues Projects Releases Wiki Activity

iio: imu: st_lsm6dsx: fix read misalignment on untagged FIFO 

st_lsm6dsx suffers of a read misalignment on untagged FIFO when
all 3 supported sensors (accel, gyro and ext device) are running
at different ODRs (the use-case is reported in the LSM6DSM Application
Note at pag 100).
Fix the issue taking into account decimation factor reading the FIFO
pattern.

Fixes: e485e2a2cf ("iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm")
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Lorenzo Bianconi 2020-03-13 18:54:41 +01:00 committed by Jonathan Cameron
parent e2042d2936
commit 7762902c89
2 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
View File

@ -337,6 +337,7 @@ enum st_lsm6dsx_fifo_mode {
* @gain: Configured sensor sensitivity.
* @odr: Output data rate of the sensor [Hz].
* @watermark: Sensor watermark level.
* @decimator: Sensor decimation factor.
* @sip: Number of samples in a given pattern.
* @ts_ref: Sensor timestamp reference for hw one.
* @ext_info: Sensor settings if it is connected to i2c controller
@ -350,6 +351,7 @@ struct st_lsm6dsx_sensor {
u32 odr;
u16 watermark;
u8 decimator;
u8 sip;
s64 ts_ref;

23
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
View File

@ -93,6 +93,7 @@ st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
break;
}
sensor->decimator = decimator;
return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
}
@ -337,7 +338,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
int err, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
@ -399,19 +400,20 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
acc_sip = acc_sensor->sip;
ts_sip = hw->ts_sip;
offset = 0;
sip = 0;
while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
if (gyro_sip > 0) {
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
memcpy(gyro_buff, &hw->buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (acc_sip > 0) {
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
memcpy(acc_buff, &hw->buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (ext_sip > 0) {
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
memcpy(ext_buff, &hw->buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
offset += ST_LSM6DSX_SAMPLE_SIZE;
@ -441,18 +443,25 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (gyro_sip-- > 0)
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
gyro_buff, gyro_sensor->ts_ref + ts);
if (acc_sip-- > 0)
gyro_sip--;
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_ACC],
acc_buff, acc_sensor->ts_ref + ts);
if (ext_sip-- > 0)
acc_sip--;
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_EXT0],
ext_buff, ext_sensor->ts_ref + ts);
ext_sip--;
}
sip++;
}
}