aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c')
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c166
1 files changed, 150 insertions, 16 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 631360b14ca7..b5263fc522ca 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -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)
@@ -250,21 +284,21 @@ 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 *data,
- unsigned int data_len)
+static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
+ u8 *data, unsigned int data_len,
+ unsigned int max_word_len)
{
unsigned int word_len, read_len = 0;
int err;
while (read_len < data_len) {
word_len = min_t(unsigned int, data_len - read_len,
- ST_LSM6DSX_MAX_WORD_LEN);
- err = regmap_bulk_read(hw->regmap,
- ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
- data + read_len, word_len);
+ max_word_len);
+ err = regmap_bulk_read(hw->regmap, addr, data + read_len,
+ word_len);
if (err < 0)
return err;
read_len += word_len;
@@ -282,7 +316,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 *data,
*
* Return: Number of bytes read from the FIFO
*/
-static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
+int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
@@ -314,7 +348,9 @@ static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
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, hw->buff, 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",
@@ -400,13 +436,111 @@ static 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;
mutex_lock(&hw->fifo_lock);
- st_lsm6dsx_read_fifo(hw);
+ hw->settings->fifo_ops.read_fifo(hw);
err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
mutex_unlock(&hw->fifo_lock);
@@ -478,7 +612,7 @@ static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
int count;
mutex_lock(&hw->fifo_lock);
- count = st_lsm6dsx_read_fifo(hw);
+ count = hw->settings->fifo_ops.read_fifo(hw);
mutex_unlock(&hw->fifo_lock);
return !count ? IRQ_NONE : IRQ_HANDLED;
Copyright © 1996 – 2023 Jason A. Donenfeld. All Rights Reverse Engineered.