in imu/st_lsm6dsx/st_lsm6dsx_buffer.c [350:492]
int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
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;
bool reset_ts = false;
__le16 fifo_status;
s64 ts = 0;
err = st_lsm6dsx_read_locked(hw,
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;
}
if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
return 0;
fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
ST_LSM6DSX_CHAN_SIZE;
fifo_len = (fifo_len / pattern_len) * pattern_len;
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
hw->buff, pattern_len,
ST_LSM6DSX_MAX_WORD_LEN);
if (err < 0) {
dev_err(hw->dev,
"failed to read pattern from fifo (err=%d)\n",
err);
return err;
}
/*
* Data are written to the FIFO with a specific pattern
* depending on the configured ODRs. The first sequence of data
* stored in FIFO contains the data of all enabled sensors
* (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
* depending on the value of the decimation factor set for each
* sensor.
*
* Supposing the FIFO is storing data from gyroscope and
* accelerometer at different ODRs:
* - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
* Since the gyroscope ODR is twice the accelerometer one, the
* following pattern is repeated every 9 samples:
* - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
*/
ext_sip = ext_sensor ? ext_sensor->sip : 0;
gyro_sip = gyro_sensor->sip;
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 && !(sip % gyro_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
}
if (ts_sip-- > 0) {
u8 data[ST_LSM6DSX_SAMPLE_SIZE];
memcpy(data, &hw->buff[offset], sizeof(data));
/*
* hw timestamp is 3B long and it is stored
* in FIFO using 6B as 4th FIFO data set
* according to this schema:
* B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
*/
ts = data[1] << 16 | data[0] << 8 | data[3];
/*
* 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 >= 0xff0000)
reset_ts = true;
ts *= hw->ts_gain;
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
&hw->scan[ST_LSM6DSX_ID_GYRO],
gyro_sensor->ts_ref + ts);
gyro_sip--;
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_ACC],
&hw->scan[ST_LSM6DSX_ID_ACC],
acc_sensor->ts_ref + ts);
acc_sip--;
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_EXT0],
&hw->scan[ST_LSM6DSX_ID_EXT0],
ext_sensor->ts_ref + ts);
ext_sip--;
}
sip++;
}
}
if (unlikely(reset_ts)) {
err = st_lsm6dsx_reset_hw_ts(hw);
if (err < 0) {
dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
err);
return err;
}
}
return read_len;
}