Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

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

iio: imu: st_lsm6dsx: introduce locked read/write utility routines 



Add st_lsm6dsx_update_bits_locked, st_lsm6dsx_read_locked and
st_lsm6dsx_write_locked utility routines in order to guarantee
the bus access is atomic respect to reg page configuration.
This is a preliminary patch to add i2c sensor hub support since
i2c master registers are accessed through a reg page multiplexer

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

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h

+41 −0
Original line number Diff line number Diff line
@@ -148,6 +148,7 @@ struct st_lsm6dsx_sensor { 
 * @irq: Device interrupt line (I2C or SPI).

 * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.

 * @conf_lock: Mutex to prevent concurrent FIFO configuration update.

 * @page_lock: Mutex to prevent concurrent memory page configuration.

 * @fifo_mode: FIFO operating mode supported by the device.

 * @enable_mask: Enabled sensor bitmask.

 * @ts_sip: Total number of timestamp samples in a given pattern.
@@ -163,6 +164,7 @@ struct st_lsm6dsx_hw { 


	struct mutex fifo_lock;

	struct mutex conf_lock;

	struct mutex page_lock;



	enum st_lsm6dsx_fifo_mode fifo_mode;

	u8 enable_mask;
@@ -192,4 +194,43 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw); 
int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw);

int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val);



static inline int

st_lsm6dsx_update_bits_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,

			      unsigned int mask, unsigned int val)

{

	int err;



	mutex_lock(&hw->page_lock);

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

	mutex_unlock(&hw->page_lock);



	return err;

}



static inline int

st_lsm6dsx_read_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,

		       void *val, unsigned int len)

{

	int err;



	mutex_lock(&hw->page_lock);

	err = regmap_bulk_read(hw->regmap, addr, val, len);

	mutex_unlock(&hw->page_lock);



	return err;

}



static inline int

st_lsm6dsx_write_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,

			unsigned int val)

{

	int err;



	mutex_lock(&hw->page_lock);

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

	mutex_unlock(&hw->page_lock);



	return err;

}



#endif /* ST_LSM6DSX_H */

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+34 −29
Original line number Diff line number Diff line
@@ -142,8 +142,9 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) 
		if (dec_reg->addr) {

			int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);



			err = regmap_update_bits(hw->regmap, dec_reg->addr,

						 dec_reg->mask, val);

			err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,

							    dec_reg->mask,

							    val);

			if (err < 0)

				return err;

		}
@@ -162,7 +163,7 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) 
		int val, ts_dec = !!hw->ts_sip;



		val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);

		err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,

		err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,

						    ts_dec_reg->mask, val);

	}

	return err;
@@ -171,12 +172,12 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) 
int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,

			     enum st_lsm6dsx_fifo_mode fifo_mode)

{

	unsigned int data;

	int err;



	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,

				 ST_LSM6DSX_FIFO_MODE_MASK,

				 FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,

					    fifo_mode));

	data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);

	err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,

					    ST_LSM6DSX_FIFO_MODE_MASK, data);

	if (err < 0)

		return err;
@@ -207,11 +208,11 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, 
			data = 0;

		}

		val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);

		return regmap_update_bits(hw->regmap, batch_reg->addr,

		return st_lsm6dsx_update_bits_locked(hw, 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,

		return st_lsm6dsx_update_bits_locked(hw,

					ST_LSM6DSX_REG_FIFO_MODE_ADDR,

					ST_LSM6DSX_FIFO_ODR_MASK,

					FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
@@ -246,19 +247,23 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) 
	fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;

	fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;



	mutex_lock(&hw->page_lock);

	err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,

			  &data);

	if (err < 0)

		return err;

		goto out;



	fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;

	fifo_watermark = ((data << 8) & ~fifo_th_mask) |

			 (fifo_watermark & fifo_th_mask);



	wdata = cpu_to_le16(fifo_watermark);

	return regmap_bulk_write(hw->regmap,

	err = regmap_bulk_write(hw->regmap,

				hw->settings->fifo_ops.fifo_th.addr,

				&wdata, sizeof(wdata));

out:

	mutex_unlock(&hw->page_lock);

	return err;

}



static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
@@ -267,7 +272,7 @@ static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) 
	int i, err;



	/* reset hw ts counter */

	err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,

	err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,

				      ST_LSM6DSX_TS_RESET_VAL);

	if (err < 0)

		return err;
@@ -297,7 +302,7 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, 
	while (read_len < data_len) {

		word_len = min_t(unsigned int, data_len - read_len,

				 max_word_len);

		err = regmap_bulk_read(hw->regmap, addr, data + read_len,

		err = st_lsm6dsx_read_locked(hw, addr, data + read_len,

					     word_len);

		if (err < 0)

			return err;
@@ -328,7 +333,7 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
	__le16 fifo_status;

	s64 ts = 0;



	err = regmap_bulk_read(hw->regmap,

	err = st_lsm6dsx_read_locked(hw,

				     hw->settings->fifo_ops.fifo_diff.addr,

				     &fifo_status, sizeof(fifo_status));

	if (err < 0) {
@@ -455,7 +460,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) 
	__le16 fifo_status;

	s64 ts = 0;



	err = regmap_bulk_read(hw->regmap,

	err = st_lsm6dsx_read_locked(hw,

				     hw->settings->fifo_ops.fifo_diff.addr,

				     &fifo_status, sizeof(fifo_status));

	if (err < 0) {

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

+15 −9
Original line number Diff line number Diff line
@@ -421,6 +421,7 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, 
{

	struct st_lsm6dsx_hw *hw = sensor->hw;

	const struct st_lsm6dsx_reg *reg;

	unsigned int data;

	int i, err;

	u8 val;
@@ -433,8 +434,8 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, 


	val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;

	reg = &st_lsm6dsx_fs_table[sensor->id].reg;

	err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,

				 ST_LSM6DSX_SHIFT_VAL(val, reg->mask));

	data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);

	err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);

	if (err < 0)

		return err;
@@ -463,6 +464,7 @@ static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) 
{

	struct st_lsm6dsx_hw *hw = sensor->hw;

	const struct st_lsm6dsx_reg *reg;

	unsigned int data;

	int err;

	u8 val;
@@ -471,8 +473,8 @@ static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) 
		return err;



	reg = &st_lsm6dsx_odr_table[sensor->id].reg;

	return regmap_update_bits(hw->regmap, reg->addr, reg->mask,

				  ST_LSM6DSX_SHIFT_VAL(val, reg->mask));

	data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);

	return st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);

}



int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
@@ -492,11 +494,12 @@ int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor) 
{

	struct st_lsm6dsx_hw *hw = sensor->hw;

	const struct st_lsm6dsx_reg *reg;

	unsigned int data;

	int err;



	reg = &st_lsm6dsx_odr_table[sensor->id].reg;

	err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,

				 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));

	data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);

	err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);

	if (err < 0)

		return err;
@@ -519,7 +522,7 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor, 
	delay = 1000000 / sensor->odr;

	usleep_range(delay, 2 * delay);



	err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data));

	err = st_lsm6dsx_read_locked(hw, addr, &data, sizeof(data));

	if (err < 0)

		return err;
@@ -865,6 +868,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, 


	mutex_init(&hw->fifo_lock);

	mutex_init(&hw->conf_lock);

	mutex_init(&hw->page_lock);



	hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL);

	if (!hw->buff)
@@ -909,6 +913,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev) 
	struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);

	struct st_lsm6dsx_sensor *sensor;

	const struct st_lsm6dsx_reg *reg;

	unsigned int data;

	int i, err = 0;



	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
@@ -917,8 +922,9 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev) 
			continue;



		reg = &st_lsm6dsx_odr_table[sensor->id].reg;

		err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,

					 ST_LSM6DSX_SHIFT_VAL(0, reg->mask));

		data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);

		err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask,

						    data);

		if (err < 0)

			return err;

	}