Commit 14c7c6e1 authored by Lorenzo Bianconi's avatar Lorenzo Bianconi Committed by Jonathan Cameron
Browse files

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: default avatarLorenzo Bianconi <lorenzo.bianconi@redhat.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent c91c1c84

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+29 −15
Original line number Diff line number Diff line
@@ -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);

			}

		}

	}

CRA Git | Maintained and supported by SUSTech CRA and CCSE