Commit 6d0205fd authored by Lorenzo Bianconi's avatar Lorenzo Bianconi Committed by Jonathan Cameron
Browse files

iio: imu: st_lsm6dsx: add hw FIFO support to i2c controller 



Introduce hw FIFO support to lsm6dsx i2c controller.
st_lsm6dsx sensor-hub relies on SLV0 for slave configuration since SLV0
is the only channel that can be used to write into i2c slave devices.
SLV{1,2,3} channels are used to read external data and push them into
the hw FIFO

Signed-off-by: default avatarLorenzo Bianconi <lorenzo.bianconi@redhat.com>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 14c7c6e1

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h

+4 −0
Original line number Diff line number Diff line
@@ -130,18 +130,22 @@ struct st_lsm6dsx_hw_ts_settings { 
 * @master_en: master config register info (addr + mask).

 * @pullup_en: i2c controller pull-up register info (addr + mask).

 * @aux_sens: aux sensor register info (addr + mask).

 * @wr_once: write_once register info (addr + mask).

 * @shub_out: sensor hub first output register info.

 * @slv0_addr: slave0 address in secondary page.

 * @dw_slv0_addr: slave0 write register address in secondary page.

 * @batch_en: Enable/disable FIFO batching.

 */

struct st_lsm6dsx_shub_settings {

	struct st_lsm6dsx_reg page_mux;

	struct st_lsm6dsx_reg master_en;

	struct st_lsm6dsx_reg pullup_en;

	struct st_lsm6dsx_reg aux_sens;

	struct st_lsm6dsx_reg wr_once;

	u8 shub_out;

	u8 slv0_addr;

	u8 dw_slv0_addr;

	u8 batch_en;

};



enum st_lsm6dsx_ext_sensor_id {

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+41 −6
Original line number Diff line number Diff line
@@ -68,6 +68,9 @@ enum st_lsm6dsx_fifo_tag { 
	ST_LSM6DSX_GYRO_TAG = 0x01,

	ST_LSM6DSX_ACC_TAG = 0x02,

	ST_LSM6DSX_TS_TAG = 0x04,

	ST_LSM6DSX_EXT0_TAG = 0x0f,

	ST_LSM6DSX_EXT1_TAG = 0x10,

	ST_LSM6DSX_EXT2_TAG = 0x11,

};



static const
@@ -460,6 +463,12 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, 
	struct st_lsm6dsx_sensor *sensor;

	struct iio_dev *iio_dev;



	/*

	 * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG

	 * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG

	 * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled

	 * channel

	 */

	switch (tag) {

	case ST_LSM6DSX_GYRO_TAG:

		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
@@ -467,6 +476,24 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, 
	case ST_LSM6DSX_ACC_TAG:

		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];

		break;

	case ST_LSM6DSX_EXT0_TAG:

		if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))

			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];

		else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))

			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];

		else

			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];

		break;

	case ST_LSM6DSX_EXT1_TAG:

		if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&

		    (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))

			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];

		else

			iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];

		break;

	case ST_LSM6DSX_EXT2_TAG:

		iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];

		break;

	default:

		return -EINVAL;

	}
@@ -593,6 +620,13 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) 
			goto out;

	}



	if (sensor->id == ST_LSM6DSX_ID_EXT0 ||

	    sensor->id == ST_LSM6DSX_ID_EXT1 ||

	    sensor->id == ST_LSM6DSX_ID_EXT2) {

		err = st_lsm6dsx_shub_set_enable(sensor, enable);

		if (err < 0)

			goto out;

	} else {

		err = st_lsm6dsx_sensor_set_enable(sensor, enable);

		if (err < 0)

			goto out;
@@ -600,6 +634,7 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) 
		err = st_lsm6dsx_set_fifo_odr(sensor, enable);

		if (err < 0)

			goto out;

	}



	err = st_lsm6dsx_update_decimators(hw);

	if (err < 0)

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

+5 −0
Original line number Diff line number Diff line
@@ -337,9 +337,14 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.addr = 0x14,

				.mask = GENMASK(1, 0),

			},

			.wr_once = {

				.addr = 0x14,

				.mask = BIT(6),

			},

			.shub_out = 0x02,

			.slv0_addr = 0x15,

			.dw_slv0_addr = 0x21,

			.batch_en = BIT(3),

		}

	},

};

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c

+75 −0
Original line number Diff line number Diff line
@@ -148,6 +148,26 @@ out: 
	return err;

}



static int

st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,

				    u8 mask, u8 val)

{

	int err;



	mutex_lock(&hw->page_lock);

	err = st_lsm6dsx_set_page(hw, true);

	if (err < 0)

		goto out;



	err = regmap_update_bits(hw->regmap, addr, mask, val);



	st_lsm6dsx_set_page(hw, false);

out:

	mutex_unlock(&hw->page_lock);



	return err;

}



static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,

					 bool enable)

{
@@ -238,6 +258,18 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr, 
	int err, i;



	hub_settings = &hw->settings->shub_settings;

	if (hub_settings->wr_once.addr) {

		unsigned int data;



		data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);

		err = st_lsm6dsx_shub_write_reg_with_mask(hw,

			hub_settings->wr_once.addr,

			hub_settings->wr_once.mask,

			data);

		if (err < 0)

			return err;

	}



	slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);

	config[0] = sensor->ext_info.addr << 1;

	for (i = 0 ; i < len; i++) {
@@ -319,11 +351,54 @@ st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) 
					       val);

}



/* use SLV{1,2,3} for FIFO read operations */

static int

st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,

				bool enable)

{

	const struct st_lsm6dsx_shub_settings *hub_settings;

	const struct st_lsm6dsx_ext_dev_settings *settings;

	u8 config[9] = {}, enable_mask, slv_addr;

	struct st_lsm6dsx_hw *hw = sensor->hw;

	struct st_lsm6dsx_sensor *cur_sensor;

	int i, j = 0;



	hub_settings = &hw->settings->shub_settings;

	if (enable)

		enable_mask = hw->enable_mask | BIT(sensor->id);

	else

		enable_mask = hw->enable_mask & ~BIT(sensor->id);



	for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {

		if (!hw->iio_devs[i])

			continue;



		cur_sensor = iio_priv(hw->iio_devs[i]);

		if (!(enable_mask & BIT(cur_sensor->id)))

			continue;



		settings = cur_sensor->ext_info.settings;

		config[j] = (sensor->ext_info.addr << 1) | 1;

		config[j + 1] = settings->out.addr;

		config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |

				hub_settings->batch_en;

		j += 3;

	}



	slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);

	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,

					 sizeof(config));

}



int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)

{

	const struct st_lsm6dsx_ext_dev_settings *settings;

	int err;



	err = st_lsm6dsx_shub_config_channels(sensor, enable);

	if (err < 0)

		return err;



	settings = sensor->ext_info.settings;

	if (enable) {

		err = st_lsm6dsx_shub_set_odr(sensor, sensor->odr);

CRA Git | Maintained and supported by SUSTech CRA and CCSE