iio: imu: st_lsm6dsx: add st_lsm6dsx_push_tagged_data routine

Introduce st_lsm6dsx_push_tagged_data routine to push samples
to iio buffers. st_lsm6dsx_push_tagged_data will be reused adding
hw fifo support to st_lsm6dsx i2c embedded controller in order to
improve code readability

Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@redhat.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 6a37976..4e7ff37 100644 (file)
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -453,6 +453,31 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
        return read_len;
 }
 
+static int
+st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
+                           u8 *data, s64 ts)
+{
+       struct st_lsm6dsx_sensor *sensor;
+       struct iio_dev *iio_dev;
+
+       switch (tag) {
+       case ST_LSM6DSX_GYRO_TAG:
+               iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
+               break;
+       case ST_LSM6DSX_ACC_TAG:
+               iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
+               break;
+       default:
+               return -EINVAL;
+       }
+
+       sensor = iio_priv(iio_dev);
+       iio_push_to_buffers_with_timestamp(iio_dev, data,
+                                          ts + sensor->ts_ref);
+
+       return 0;
+}
+
 /**
  * st_lsm6dsx_read_tagged_fifo() - LSM6DSO read FIFO routine
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
@@ -508,8 +533,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
                               ST_LSM6DSX_SAMPLE_SIZE);
 
                        tag = hw->buff[i] >> 3;
-                       switch (tag) {
-                       case ST_LSM6DSX_TS_TAG:
+                       if (tag == ST_LSM6DSX_TS_TAG) {
                                /*
                                 * hw timestamp is 4B long and it is stored
                                 * in FIFO according to this schema:
@@ -526,19 +550,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
                                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;
+                       } else {
+                               st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
+                                                           ts);
                        }
                }
        }