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

iio: imu: st_lsm6dsx: enable sensor-hub support for lsm6dsm 



Enabled i2c master controller support for LSM6DSM sensor. Enable
ext_sensor0 for lsm6dsm. This series has been tested using LIS2MDL as
slave device connected to the i2c controller of the LSM6DSM

Signed-off-by: default avatarLorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 111b0875

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h

+4 −0
Original line number Diff line number Diff line
@@ -176,11 +176,13 @@ struct st_lsm6dsx_hw_ts_settings { 
 * @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).

 * @emb_func:  embedded function register info (addr + mask).

 * @num_ext_dev: max number of slave devices.

 * @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.

 * @pause: controller pause value.

 */

struct st_lsm6dsx_shub_settings {

	struct st_lsm6dsx_reg page_mux;
@@ -196,6 +198,7 @@ struct st_lsm6dsx_shub_settings { 
	} pullup_en;

	struct st_lsm6dsx_reg aux_sens;

	struct st_lsm6dsx_reg wr_once;

	struct st_lsm6dsx_reg emb_func;

	u8 num_ext_dev;

	struct {

		bool sec_page;
@@ -204,6 +207,7 @@ struct st_lsm6dsx_shub_settings { 
	u8 slv0_addr;

	u8 dw_slv0_addr;

	u8 batch_en;

	u8 pause;

};



struct st_lsm6dsx_event_settings {

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+20 −7
Original line number Diff line number Diff line
@@ -332,12 +332,13 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, 
 */

int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)

{

	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;

	int err, 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;

	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;

	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;

	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];

	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];

	u8 ext_buff[ST_LSM6DSX_IIO_BUFF_SIZE];

	bool reset_ts = false;

	__le16 fifo_status;

	s64 ts = 0;
@@ -360,6 +361,8 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 


	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,
@@ -387,12 +390,13 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
		 * 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;



		while (acc_sip > 0 || gyro_sip > 0) {

		while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {

			if (gyro_sip > 0) {

				memcpy(gyro_buff, &hw->buff[offset],

				       ST_LSM6DSX_SAMPLE_SIZE);
@@ -403,6 +407,11 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
				       ST_LSM6DSX_SAMPLE_SIZE);

				offset += ST_LSM6DSX_SAMPLE_SIZE;

			}

			if (ext_sip > 0) {

				memcpy(ext_buff, &hw->buff[offset],

				       ST_LSM6DSX_SAMPLE_SIZE);

				offset += ST_LSM6DSX_SAMPLE_SIZE;

			}



			if (ts_sip-- > 0) {

				u8 data[ST_LSM6DSX_SAMPLE_SIZE];
@@ -436,6 +445,10 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
				iio_push_to_buffers_with_timestamp(

					hw->iio_devs[ST_LSM6DSX_ID_ACC],

					acc_buff, acc_sensor->ts_ref + ts);

			if (ext_sip-- > 0)

				iio_push_to_buffers_with_timestamp(

					hw->iio_devs[ST_LSM6DSX_ID_EXT0],

					ext_buff, ext_sensor->ts_ref + ts);

		}

	}
@@ -628,11 +641,11 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) 
		err = st_lsm6dsx_sensor_set_enable(sensor, enable);

		if (err < 0)

			goto out;

	}



	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

+47 −0
Original line number Diff line number Diff line
@@ -656,6 +656,10 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.addr = 0x08,

				.mask = GENMASK(5, 3),

			},

			[ST_LSM6DSX_ID_EXT0] = {

				.addr = 0x09,

				.mask = GENMASK(2, 0),

			},

		},

		.fifo_ops = {

			.update_fifo = st_lsm6dsx_update_fifo,
@@ -688,6 +692,39 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.mask = GENMASK(5, 3),

			},

		},

		.shub_settings = {

			.page_mux = {

				.addr = 0x01,

				.mask = BIT(7),

			},

			.master_en = {

				.addr = 0x1a,

				.mask = BIT(0),

			},

			.pullup_en = {

				.addr = 0x1a,

				.mask = BIT(3),

			},

			.aux_sens = {

				.addr = 0x04,

				.mask = GENMASK(5, 4),

			},

			.wr_once = {

				.addr = 0x07,

				.mask = BIT(5),

			},

			.emb_func = {

				.addr = 0x19,

				.mask = BIT(2),

			},

			.num_ext_dev = 1,

			.shub_out = {

				.addr = 0x2e,

			},

			.slv0_addr = 0x02,

			.dw_slv0_addr = 0x0e,

			.pause = 0x7,

		},

		.event_settings = {

			.enable_reg = {

				.addr = 0x58,
@@ -1902,6 +1939,16 @@ static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw) 
					 hub_settings->aux_sens.mask, data);



		st_lsm6dsx_set_page(hw, false);



		if (err < 0)

			return err;

	}



	if (hub_settings->emb_func.addr) {

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

		err = regmap_update_bits(hw->regmap,

					 hub_settings->emb_func.addr,

					 hub_settings->emb_func.mask, data);

	}



	return err;

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c

+23 −8
Original line number Diff line number Diff line
@@ -221,16 +221,21 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr, 
		     u8 *data, int len)

{

	const struct st_lsm6dsx_shub_settings *hub_settings;

	u8 config[3], slv_addr, slv_config = 0;

	struct st_lsm6dsx_hw *hw = sensor->hw;

	u8 config[3], slv_addr;

	const struct st_lsm6dsx_reg *aux_sens;

	int err;



	hub_settings = &hw->settings->shub_settings;

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

	aux_sens = &hw->settings->shub_settings.aux_sens;

	/* do not overwrite aux_sens */

	if (slv_addr + 2 == aux_sens->addr)

		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);



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

	config[1] = addr;

	config[2] = len & ST_LS6DSX_READ_OP_MASK;

	config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;



	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,

					sizeof(config));
@@ -248,7 +253,9 @@ st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr, 


	st_lsm6dsx_shub_master_enable(sensor, false);



	memset(config, 0, sizeof(config));

	config[0] = hub_settings->pause;

	config[1] = 0;

	config[2] = slv_config;

	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,

					 sizeof(config));

}
@@ -305,7 +312,8 @@ st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr, 
		st_lsm6dsx_shub_master_enable(sensor, false);

	}



	memset(config, 0, sizeof(config));

	config[0] = hub_settings->pause;

	config[1] = 0;

	return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));

}
@@ -697,14 +705,19 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, 
			  const struct st_lsm6dsx_ext_dev_settings *settings)

{

	const struct st_lsm6dsx_shub_settings *hub_settings;

	u8 config[3], data, slv_addr, slv_config = 0;

	const struct st_lsm6dsx_reg *aux_sens;

	struct st_lsm6dsx_sensor *sensor;

	u8 config[3], data, slv_addr;

	bool found = false;

	int i, err;



	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);

	hub_settings = &hw->settings->shub_settings;

	aux_sens = &hw->settings->shub_settings.aux_sens;

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

	sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);

	/* do not overwrite aux_sens */

	if (slv_addr + 2 == aux_sens->addr)

		slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);



	for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {

		if (!settings->i2c_addr[i])
@@ -713,7 +726,7 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, 
		/* read wai slave register */

		config[0] = (settings->i2c_addr[i] << 1) | 0x1;

		config[1] = settings->wai.addr;

		config[2] = 0x1;

		config[2] = 0x1 | slv_config;



		err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,

						sizeof(config));
@@ -742,7 +755,9 @@ st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, 
	}



	/* reset SLV0 channel */

	memset(config, 0, sizeof(config));

	config[0] = hub_settings->pause;

	config[1] = 0;

	config[2] = slv_config;

	err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,

					sizeof(config));

	if (err < 0)

CRA Git | Maintained and supported by SUSTech CRA and CCSE