[linux-block.git] / drivers / iio / imu / st_lsm6dsx / st_lsm6dsx_buffer.c

/*
 * STMicroelectronics st_lsm6dsx FIFO buffer library driver
 *
 * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM: The FIFO buffer can be configured
 * to store data from gyroscope and accelerometer. Samples are queued
 * without any tag according to a specific pattern based on 'FIFO data sets'
 * (6 bytes each):
 *  - 1st data set is reserved for gyroscope data
 *  - 2nd data set is reserved for accelerometer data
 * The FIFO pattern changes depending on the ODRs and decimation factors
 * assigned to the FIFO data sets. The first sequence of data stored in FIFO
 * buffer contains the data of all the enabled FIFO data sets
 * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
 * value of the decimation factor and ODR set for each FIFO data set.
 * FIFO supported modes:
 *  - BYPASS: FIFO disabled
 *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
 *    restarts from the beginning and the oldest sample is overwritten
 *
 * Copyright 2016 STMicroelectronics Inc.
 *
 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
 * Denis Ciocca <denis.ciocca@st.com>
 *
 * Licensed under the GPL-2.
 */
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
#include <linux/regmap.h>
#include <linux/bitfield.h>

#include <linux/platform_data/st_sensors_pdata.h>

#include "st_lsm6dsx.h"

#define ST_LSM6DSX_REG_HLACTIVE_ADDR		0x12
#define ST_LSM6DSX_REG_HLACTIVE_MASK		BIT(5)
#define ST_LSM6DSX_REG_PP_OD_ADDR		0x12
#define ST_LSM6DSX_REG_PP_OD_MASK		BIT(4)
#define ST_LSM6DSX_REG_FIFO_MODE_ADDR		0x0a
#define ST_LSM6DSX_FIFO_MODE_MASK		GENMASK(2, 0)
#define ST_LSM6DSX_FIFO_ODR_MASK		GENMASK(6, 3)
#define ST_LSM6DSX_FIFO_EMPTY_MASK		BIT(12)
#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR		0x3e
#define ST_LSM6DSX_REG_TS_RESET_ADDR		0x42

#define ST_LSM6DSX_MAX_FIFO_ODR_VAL		0x08

#define ST_LSM6DSX_TS_SENSITIVITY		25000UL /* 25us */
#define ST_LSM6DSX_TS_RESET_VAL			0xaa

struct st_lsm6dsx_decimator_entry {
	u8 decimator;
	u8 val;
};

static const
struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
	{  0, 0x0 },
	{  1, 0x1 },
	{  2, 0x2 },
	{  3, 0x3 },
	{  4, 0x4 },
	{  8, 0x5 },
	{ 16, 0x6 },
	{ 32, 0x7 },
};

static int st_lsm6dsx_get_decimator_val(u8 val)
{
	const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
	int i;

	for (i = 0; i < max_size; i++)
		if (st_lsm6dsx_decimator_table[i].decimator == val)
			break;

	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
}

static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
				       u16 *max_odr, u16 *min_odr)
{
	struct st_lsm6dsx_sensor *sensor;
	int i;

	*max_odr = 0, *min_odr = ~0;
	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
		sensor = iio_priv(hw->iio_devs[i]);

		if (!(hw->enable_mask & BIT(sensor->id)))
			continue;

		*max_odr = max_t(u16, *max_odr, sensor->odr);
		*min_odr = min_t(u16, *min_odr, sensor->odr);
	}
}

static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
{
	u16 max_odr, min_odr, sip = 0, ts_sip = 0;
	const struct st_lsm6dsx_reg *ts_dec_reg;
	struct st_lsm6dsx_sensor *sensor;
	int err = 0, i;
	u8 data;

	st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);

	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
		const struct st_lsm6dsx_reg *dec_reg;

		sensor = iio_priv(hw->iio_devs[i]);
		/* update fifo decimators and sample in pattern */
		if (hw->enable_mask & BIT(sensor->id)) {
			sensor->sip = sensor->odr / min_odr;
			sensor->decimator = max_odr / sensor->odr;
			data = st_lsm6dsx_get_decimator_val(sensor->decimator);
		} else {
			sensor->sip = 0;
			sensor->decimator = 0;
			data = 0;
		}
		ts_sip = max_t(u16, ts_sip, sensor->sip);

		dec_reg = &hw->settings->decimator[sensor->id];
		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);
			if (err < 0)
				return err;
		}
		sip += sensor->sip;
	}
	hw->sip = sip + ts_sip;
	hw->ts_sip = ts_sip;

	/*
	 * update hw ts decimator if necessary. Decimator for hw timestamp
	 * is always 1 or 0 in order to have a ts sample for each data
	 * sample in FIFO
	 */
	ts_dec_reg = &hw->settings->ts_settings.decimator;
	if (ts_dec_reg->addr) {
		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,
					 ts_dec_reg->mask, val);
	}
	return err;
}

int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
			     enum st_lsm6dsx_fifo_mode fifo_mode)
{
	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));
	if (err < 0)
		return err;

	hw->fifo_mode = fifo_mode;

	return 0;
}

static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
				   bool enable)
{
	struct st_lsm6dsx_hw *hw = sensor->hw;
	u8 data;

	data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
	return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
				 ST_LSM6DSX_FIFO_ODR_MASK,
				 FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
}

int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
{
	u16 fifo_watermark = ~0, cur_watermark, sip = 0, fifo_th_mask;
	struct st_lsm6dsx_hw *hw = sensor->hw;
	struct st_lsm6dsx_sensor *cur_sensor;
	int i, err, data;
	__le16 wdata;

	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
		cur_sensor = iio_priv(hw->iio_devs[i]);

		if (!(hw->enable_mask & BIT(cur_sensor->id)))
			continue;

		cur_watermark = (cur_sensor == sensor) ? watermark
						       : cur_sensor->watermark;

		fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
		sip += cur_sensor->sip;
	}

	if (!sip)
		return 0;

	fifo_watermark = max_t(u16, fifo_watermark, sip);
	fifo_watermark = (fifo_watermark / sip) * sip;
	fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;

	err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
			  &data);
	if (err < 0)
		return err;

	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,
				 hw->settings->fifo_ops.fifo_th.addr,
				 &wdata, sizeof(wdata));
}

static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
{
	struct st_lsm6dsx_sensor *sensor;
	int i, err;

	/* reset hw ts counter */
	err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,
			   ST_LSM6DSX_TS_RESET_VAL);
	if (err < 0)
		return err;

	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
		sensor = iio_priv(hw->iio_devs[i]);
		/*
		 * store enable buffer timestamp as reference for
		 * hw timestamp
		 */
		sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
	}
	return 0;
}

/*
 * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
 * a kmalloc for each bus access
 */
static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 *data,
					unsigned int data_len)
{
	unsigned int word_len, read_len = 0;
	int err;

	while (read_len < data_len) {
		word_len = min_t(unsigned int, data_len - read_len,
				 ST_LSM6DSX_MAX_WORD_LEN);
		err = regmap_bulk_read(hw->regmap,
				       ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
				       data + read_len, word_len);
		if (err < 0)
			return err;
		read_len += word_len;
	}
	return 0;
}

#define ST_LSM6DSX_IIO_BUFF_SIZE	(ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
					       sizeof(s64)) + sizeof(s64))
/**
 * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
 * @hw: Pointer to instance of struct st_lsm6dsx_hw.
 *
 * Read samples from the hw FIFO and push them to IIO buffers.
 *
 * Return: Number of bytes read from the FIFO
 */
static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
	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];
	bool reset_ts = false;
	__le16 fifo_status;
	s64 ts = 0;

	err = regmap_bulk_read(hw->regmap,
			       hw->settings->fifo_ops.fifo_diff.addr,
			       &fifo_status, sizeof(fifo_status));
	if (err < 0)
		return err;

	if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
		return 0;

	fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
		   ST_LSM6DSX_CHAN_SIZE;
	fifo_len = (fifo_len / pattern_len) * pattern_len;

	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);

	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
		err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len);
		if (err < 0)
			return err;

		/*
		 * Data are written to the FIFO with a specific pattern
		 * depending on the configured ODRs. The first sequence of data
		 * stored in FIFO contains the data of all enabled sensors
		 * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
		 * depending on the value of the decimation factor set for each
		 * sensor.
		 *
		 * Supposing the FIFO is storing data from gyroscope and
		 * accelerometer at different ODRs:
		 *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
		 * Since the gyroscope ODR is twice the accelerometer one, the
		 * following pattern is repeated every 9 samples:
		 *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
		 */
		gyro_sip = gyro_sensor->sip;
		acc_sip = acc_sensor->sip;
		ts_sip = hw->ts_sip;
		offset = 0;

		while (acc_sip > 0 || gyro_sip > 0) {
			if (gyro_sip > 0) {
				memcpy(gyro_buff, &hw->buff[offset],
				       ST_LSM6DSX_SAMPLE_SIZE);
				offset += ST_LSM6DSX_SAMPLE_SIZE;
			}
			if (acc_sip > 0) {
				memcpy(acc_buff, &hw->buff[offset],
				       ST_LSM6DSX_SAMPLE_SIZE);
				offset += ST_LSM6DSX_SAMPLE_SIZE;
			}

			if (ts_sip-- > 0) {
				u8 data[ST_LSM6DSX_SAMPLE_SIZE];

				memcpy(data, &hw->buff[offset], sizeof(data));
				/*
				 * hw timestamp is 3B long and it is stored
				 * in FIFO using 6B as 4th FIFO data set
				 * according to this schema:
				 * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
				 */
				ts = data[1] << 16 | data[0] << 8 | data[3];
				/*
				 * check if hw timestamp engine is going to
				 * reset (the sensor generates an interrupt
				 * to signal the hw timestamp will reset in
				 * 1.638s)
				 */
				if (!reset_ts && ts >= 0xff0000)
					reset_ts = true;
				ts *= ST_LSM6DSX_TS_SENSITIVITY;

				offset += ST_LSM6DSX_SAMPLE_SIZE;
			}

			if (gyro_sip-- > 0)
				iio_push_to_buffers_with_timestamp(
					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
					gyro_buff, gyro_sensor->ts_ref + ts);
			if (acc_sip-- > 0)
				iio_push_to_buffers_with_timestamp(
					hw->iio_devs[ST_LSM6DSX_ID_ACC],
					acc_buff, acc_sensor->ts_ref + ts);
		}
	}

	if (unlikely(reset_ts)) {
		err = st_lsm6dsx_reset_hw_ts(hw);
		if (err < 0)
			return err;
	}
	return read_len;
}

int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
{
	int err;

	mutex_lock(&hw->fifo_lock);

	st_lsm6dsx_read_fifo(hw);
	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);

	mutex_unlock(&hw->fifo_lock);

	return err;
}

static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
{
	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
	struct st_lsm6dsx_hw *hw = sensor->hw;
	int err;

	mutex_lock(&hw->conf_lock);

	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
		err = st_lsm6dsx_flush_fifo(hw);
		if (err < 0)
			goto out;
	}

	if (enable) {
		err = st_lsm6dsx_sensor_enable(sensor);
		if (err < 0)
			goto out;
	} else {
		err = st_lsm6dsx_sensor_disable(sensor);
		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)
		goto out;

	err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
	if (err < 0)
		goto out;

	if (hw->enable_mask) {
		/* reset hw ts counter */
		err = st_lsm6dsx_reset_hw_ts(hw);
		if (err < 0)
			goto out;

		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
	}

out:
	mutex_unlock(&hw->conf_lock);

	return err;
}

static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
{
	struct st_lsm6dsx_hw *hw = private;

	return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
}

static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
{
	struct st_lsm6dsx_hw *hw = private;
	int count;

	mutex_lock(&hw->fifo_lock);
	count = st_lsm6dsx_read_fifo(hw);
	mutex_unlock(&hw->fifo_lock);

	return !count ? IRQ_NONE : IRQ_HANDLED;
}

static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
{
	return st_lsm6dsx_update_fifo(iio_dev, true);
}

static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
{
	return st_lsm6dsx_update_fifo(iio_dev, false);
}

static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
	.preenable = st_lsm6dsx_buffer_preenable,
	.postdisable = st_lsm6dsx_buffer_postdisable,
};

int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
{
	struct device_node *np = hw->dev->of_node;
	struct st_sensors_platform_data *pdata;
	struct iio_buffer *buffer;
	unsigned long irq_type;
	bool irq_active_low;
	int i, err;

	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));

	switch (irq_type) {
	case IRQF_TRIGGER_HIGH:
	case IRQF_TRIGGER_RISING:
		irq_active_low = false;
		break;
	case IRQF_TRIGGER_LOW:
	case IRQF_TRIGGER_FALLING:
		irq_active_low = true;
		break;
	default:
		dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
		return -EINVAL;
	}

	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
				 ST_LSM6DSX_REG_HLACTIVE_MASK,
				 FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
					    irq_active_low));
	if (err < 0)
		return err;

	pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
	if ((np && of_property_read_bool(np, "drive-open-drain")) ||
	    (pdata && pdata->open_drain)) {
		err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
					 ST_LSM6DSX_REG_PP_OD_MASK,
					 FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
						    1));
		if (err < 0)
			return err;

		irq_type |= IRQF_SHARED;
	}

	err = devm_request_threaded_irq(hw->dev, hw->irq,
					st_lsm6dsx_handler_irq,
					st_lsm6dsx_handler_thread,
					irq_type | IRQF_ONESHOT,
					"lsm6dsx", hw);
	if (err) {
		dev_err(hw->dev, "failed to request trigger irq %d\n",
			hw->irq);
		return err;
	}

	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
		buffer = devm_iio_kfifo_allocate(hw->dev);
		if (!buffer)
			return -ENOMEM;

		iio_device_attach_buffer(hw->iio_devs[i], buffer);
		hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
		hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
	}

	return 0;
}
Commit	Line	Data
290a6ce1 LB	1	/*
	2	* STMicroelectronics st_lsm6dsx FIFO buffer library driver
	3	*
df47710a LB	4	* LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM: The FIFO buffer can be configured
	5	* to store data from gyroscope and accelerometer. Samples are queued
	6	* without any tag according to a specific pattern based on 'FIFO data sets'
	7	* (6 bytes each):
290a6ce1 LB	8	* - 1st data set is reserved for gyroscope data
	9	* - 2nd data set is reserved for accelerometer data
	10	* The FIFO pattern changes depending on the ODRs and decimation factors
	11	* assigned to the FIFO data sets. The first sequence of data stored in FIFO
	12	* buffer contains the data of all the enabled FIFO data sets
	13	* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
	14	* value of the decimation factor and ODR set for each FIFO data set.
	15	* FIFO supported modes:
	16	* - BYPASS: FIFO disabled
	17	* - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
	18	* restarts from the beginning and the oldest sample is overwritten
	19	*
	20	* Copyright 2016 STMicroelectronics Inc.
	21	*
	22	* Lorenzo Bianconi <lorenzo.bianconi@st.com>
	23	* Denis Ciocca <denis.ciocca@st.com>
	24	*
	25	* Licensed under the GPL-2.
	26	*/
	27	#include <linux/module.h>
	28	#include <linux/interrupt.h>
	29	#include <linux/irq.h>
	30	#include <linux/iio/kfifo_buf.h>
	31	#include <linux/iio/iio.h>
	32	#include <linux/iio/buffer.h>
51a8b707 LB	33	#include <linux/regmap.h>
51a8b707 LB	34	#include <linux/bitfield.h>
290a6ce1	35
ff5fff4a LB	36	#include <linux/platform_data/st_sensors_pdata.h>
ff5fff4a LB	37
290a6ce1 LB	38	#include "st_lsm6dsx.h"
290a6ce1 LB	39
89ca88a7 LB	40	#define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12
89ca88a7 LB	41	#define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5)
ff5fff4a LB	42	#define ST_LSM6DSX_REG_PP_OD_ADDR 0x12
ff5fff4a LB	43	#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)
290a6ce1 LB	44	#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
	45	#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
	46	#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
290a6ce1 LB	47	#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
290a6ce1 LB	48	#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
21345107	49	#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
290a6ce1 LB	50
	51	#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
	52
21345107 LB	53	#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */
	54	#define ST_LSM6DSX_TS_RESET_VAL 0xaa
	55
290a6ce1 LB	56	struct st_lsm6dsx_decimator_entry {
	57	u8 decimator;
	58	u8 val;
	59	};
	60
	61	static const
	62	struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
	63	{ 0, 0x0 },
	64	{ 1, 0x1 },
	65	{ 2, 0x2 },
	66	{ 3, 0x3 },
	67	{ 4, 0x4 },
	68	{ 8, 0x5 },
	69	{ 16, 0x6 },
	70	{ 32, 0x7 },
	71	};
	72
	73	static int st_lsm6dsx_get_decimator_val(u8 val)
	74	{
	75	const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
	76	int i;
	77
	78	for (i = 0; i < max_size; i++)
	79	if (st_lsm6dsx_decimator_table[i].decimator == val)
	80	break;
	81
	82	return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
	83	}
	84
	85	static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
	86	u16 max_odr, u16 min_odr)
	87	{
	88	struct st_lsm6dsx_sensor *sensor;
	89	int i;
	90
	91	max_odr = 0, min_odr = ~0;
	92	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	93	sensor = iio_priv(hw->iio_devs[i]);
	94
	95	if (!(hw->enable_mask & BIT(sensor->id)))
	96	continue;
	97
	98	max_odr = max_t(u16, max_odr, sensor->odr);
	99	min_odr = min_t(u16, min_odr, sensor->odr);
	100	}
	101	}
	102
	103	static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
	104	{
21345107 LB	105	u16 max_odr, min_odr, sip = 0, ts_sip = 0;
21345107 LB	106	const struct st_lsm6dsx_reg *ts_dec_reg;
290a6ce1	107	struct st_lsm6dsx_sensor *sensor;
21345107	108	int err = 0, i;
290a6ce1 LB	109	u8 data;
	110
	111	st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
	112
	113	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
7ca3ac9e	114	const struct st_lsm6dsx_reg *dec_reg;
290a6ce1	115
7ca3ac9e	116	sensor = iio_priv(hw->iio_devs[i]);
290a6ce1 LB	117	/* update fifo decimators and sample in pattern */
	118	if (hw->enable_mask & BIT(sensor->id)) {
	119	sensor->sip = sensor->odr / min_odr;
	120	sensor->decimator = max_odr / sensor->odr;
	121	data = st_lsm6dsx_get_decimator_val(sensor->decimator);
	122	} else {
	123	sensor->sip = 0;
	124	sensor->decimator = 0;
	125	data = 0;
	126	}
21345107	127	ts_sip = max_t(u16, ts_sip, sensor->sip);
290a6ce1	128
7ca3ac9e LB	129	dec_reg = &hw->settings->decimator[sensor->id];
7ca3ac9e LB	130	if (dec_reg->addr) {
51a8b707 LB	131	int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
	132
	133	err = regmap_update_bits(hw->regmap, dec_reg->addr,
	134	dec_reg->mask, val);
7ca3ac9e LB	135	if (err < 0)
	136	return err;
	137	}
290a6ce1 LB	138	sip += sensor->sip;
290a6ce1 LB	139	}
21345107 LB	140	hw->sip = sip + ts_sip;
21345107 LB	141	hw->ts_sip = ts_sip;
290a6ce1	142
21345107 LB	143	/*
	144	* update hw ts decimator if necessary. Decimator for hw timestamp
	145	* is always 1 or 0 in order to have a ts sample for each data
	146	* sample in FIFO
	147	*/
	148	ts_dec_reg = &hw->settings->ts_settings.decimator;
	149	if (ts_dec_reg->addr) {
	150	int val, ts_dec = !!hw->ts_sip;
	151
	152	val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
	153	err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,
	154	ts_dec_reg->mask, val);
	155	}
	156	return err;
290a6ce1 LB	157	}
290a6ce1 LB	158
535de397 LB	159	int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
535de397 LB	160	enum st_lsm6dsx_fifo_mode fifo_mode)
290a6ce1	161	{
290a6ce1 LB	162	int err;
290a6ce1 LB	163
51a8b707 LB	164	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
	165	ST_LSM6DSX_FIFO_MODE_MASK,
	166	FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
	167	fifo_mode));
290a6ce1 LB	168	if (err < 0)
	169	return err;
	170
	171	hw->fifo_mode = fifo_mode;
	172
	173	return 0;
	174	}
	175
ff81a933 LB	176	static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
	177	bool enable)
	178	{
	179	struct st_lsm6dsx_hw *hw = sensor->hw;
	180	u8 data;
	181
	182	data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
51a8b707 LB	183	return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
	184	ST_LSM6DSX_FIFO_ODR_MASK,
	185	FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
ff81a933 LB	186	}
ff81a933 LB	187
290a6ce1 LB	188	int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
290a6ce1 LB	189	{
92617c15	190	u16 fifo_watermark = ~0, cur_watermark, sip = 0, fifo_th_mask;
290a6ce1 LB	191	struct st_lsm6dsx_hw *hw = sensor->hw;
290a6ce1 LB	192	struct st_lsm6dsx_sensor *cur_sensor;
51a8b707	193	int i, err, data;
290a6ce1	194	__le16 wdata;
290a6ce1 LB	195
	196	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	197	cur_sensor = iio_priv(hw->iio_devs[i]);
	198
	199	if (!(hw->enable_mask & BIT(cur_sensor->id)))
	200	continue;
	201
	202	cur_watermark = (cur_sensor == sensor) ? watermark
	203	: cur_sensor->watermark;
	204
	205	fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
	206	sip += cur_sensor->sip;
	207	}
	208
	209	if (!sip)
	210	return 0;
	211
	212	fifo_watermark = max_t(u16, fifo_watermark, sip);
	213	fifo_watermark = (fifo_watermark / sip) * sip;
92617c15	214	fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
290a6ce1	215
51a8b707 LB	216	err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
51a8b707 LB	217	&data);
290a6ce1	218	if (err < 0)
51a8b707	219	return err;
290a6ce1	220
92617c15 LB	221	fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
	222	fifo_watermark = ((data << 8) & ~fifo_th_mask) \|
	223	(fifo_watermark & fifo_th_mask);
290a6ce1 LB	224
290a6ce1 LB	225	wdata = cpu_to_le16(fifo_watermark);
51a8b707 LB	226	return regmap_bulk_write(hw->regmap,
	227	hw->settings->fifo_ops.fifo_th.addr,
	228	&wdata, sizeof(wdata));
	229	}
290a6ce1	230
21345107 LB	231	static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
	232	{
	233	struct st_lsm6dsx_sensor *sensor;
	234	int i, err;
	235
	236	/* reset hw ts counter */
	237	err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,
	238	ST_LSM6DSX_TS_RESET_VAL);
	239	if (err < 0)
	240	return err;
	241
	242	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	243	sensor = iio_priv(hw->iio_devs[i]);
	244	/*
	245	* store enable buffer timestamp as reference for
	246	* hw timestamp
	247	*/
	248	sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
	249	}
	250	return 0;
	251	}
	252
51a8b707 LB	253	/*
	254	* Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
	255	* a kmalloc for each bus access
	256	*/
	257	static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw hw, u8 data,
	258	unsigned int data_len)
	259	{
	260	unsigned int word_len, read_len = 0;
	261	int err;
	262
	263	while (read_len < data_len) {
	264	word_len = min_t(unsigned int, data_len - read_len,
	265	ST_LSM6DSX_MAX_WORD_LEN);
	266	err = regmap_bulk_read(hw->regmap,
	267	ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
	268	data + read_len, word_len);
	269	if (err < 0)
	270	return err;
	271	read_len += word_len;
	272	}
	273	return 0;
290a6ce1 LB	274	}
290a6ce1 LB	275
21345107 LB	276	#define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
21345107 LB	277	sizeof(s64)) + sizeof(s64))
290a6ce1	278	/**
df47710a	279	* st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
290a6ce1 LB	280	* @hw: Pointer to instance of struct st_lsm6dsx_hw.
	281	*
	282	* Read samples from the hw FIFO and push them to IIO buffers.
	283	*
	284	* Return: Number of bytes read from the FIFO
	285	*/
	286	static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
	287	{
	288	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
92617c15	289	u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
21345107	290	int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
290a6ce1	291	struct st_lsm6dsx_sensor acc_sensor, gyro_sensor;
21345107 LB	292	u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
	293	u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
	294	bool reset_ts = false;
290a6ce1	295	__le16 fifo_status;
21345107	296	s64 ts = 0;
290a6ce1	297
51a8b707 LB	298	err = regmap_bulk_read(hw->regmap,
	299	hw->settings->fifo_ops.fifo_diff.addr,
	300	&fifo_status, sizeof(fifo_status));
290a6ce1 LB	301	if (err < 0)
	302	return err;
	303
	304	if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
	305	return 0;
	306
92617c15	307	fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
290a6ce1	308	ST_LSM6DSX_CHAN_SIZE;
290a6ce1 LB	309	fifo_len = (fifo_len / pattern_len) * pattern_len;
290a6ce1 LB	310
290a6ce1	311	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
290a6ce1	312	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
290a6ce1 LB	313
290a6ce1 LB	314	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
91a6b841	315	err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len);
290a6ce1 LB	316	if (err < 0)
	317	return err;
	318
	319	/*
	320	* Data are written to the FIFO with a specific pattern
	321	* depending on the configured ODRs. The first sequence of data
	322	* stored in FIFO contains the data of all enabled sensors
21345107	323	* (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
290a6ce1 LB	324	* depending on the value of the decimation factor set for each
	325	* sensor.
	326	*
	327	* Supposing the FIFO is storing data from gyroscope and
	328	* accelerometer at different ODRs:
	329	* - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
	330	* Since the gyroscope ODR is twice the accelerometer one, the
	331	* following pattern is repeated every 9 samples:
21345107	332	* - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
290a6ce1 LB	333	*/
	334	gyro_sip = gyro_sensor->sip;
	335	acc_sip = acc_sensor->sip;
21345107	336	ts_sip = hw->ts_sip;
290a6ce1 LB	337	offset = 0;
	338
	339	while (acc_sip > 0 \|\| gyro_sip > 0) {
21345107 LB	340	if (gyro_sip > 0) {
21345107 LB	341	memcpy(gyro_buff, &hw->buff[offset],
290a6ce1	342	ST_LSM6DSX_SAMPLE_SIZE);
290a6ce1	343	offset += ST_LSM6DSX_SAMPLE_SIZE;
290a6ce1	344	}
21345107 LB	345	if (acc_sip > 0) {
21345107 LB	346	memcpy(acc_buff, &hw->buff[offset],
290a6ce1	347	ST_LSM6DSX_SAMPLE_SIZE);
290a6ce1	348	offset += ST_LSM6DSX_SAMPLE_SIZE;
290a6ce1	349	}
21345107 LB	350
	351	if (ts_sip-- > 0) {
	352	u8 data[ST_LSM6DSX_SAMPLE_SIZE];
	353
	354	memcpy(data, &hw->buff[offset], sizeof(data));
	355	/*
	356	* hw timestamp is 3B long and it is stored
	357	* in FIFO using 6B as 4th FIFO data set
	358	* according to this schema:
	359	* B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
	360	*/
	361	ts = data[1] << 16 \| data[0] << 8 \| data[3];
	362	/*
	363	* check if hw timestamp engine is going to
	364	* reset (the sensor generates an interrupt
	365	* to signal the hw timestamp will reset in
	366	* 1.638s)
	367	*/
	368	if (!reset_ts && ts >= 0xff0000)
	369	reset_ts = true;
	370	ts *= ST_LSM6DSX_TS_SENSITIVITY;
	371
	372	offset += ST_LSM6DSX_SAMPLE_SIZE;
	373	}
	374
	375	if (gyro_sip-- > 0)
	376	iio_push_to_buffers_with_timestamp(
	377	hw->iio_devs[ST_LSM6DSX_ID_GYRO],
	378	gyro_buff, gyro_sensor->ts_ref + ts);
	379	if (acc_sip-- > 0)
	380	iio_push_to_buffers_with_timestamp(
	381	hw->iio_devs[ST_LSM6DSX_ID_ACC],
	382	acc_buff, acc_sensor->ts_ref + ts);
290a6ce1 LB	383	}
	384	}
	385
21345107 LB	386	if (unlikely(reset_ts)) {
	387	err = st_lsm6dsx_reset_hw_ts(hw);
	388	if (err < 0)
	389	return err;
	390	}
290a6ce1 LB	391	return read_len;
	392	}
	393
535de397	394	int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
290a6ce1 LB	395	{
	396	int err;
	397
	398	mutex_lock(&hw->fifo_lock);
	399
	400	st_lsm6dsx_read_fifo(hw);
	401	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
	402
	403	mutex_unlock(&hw->fifo_lock);
	404
	405	return err;
	406	}
	407
	408	static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
	409	{
	410	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
	411	struct st_lsm6dsx_hw *hw = sensor->hw;
	412	int err;
	413
335eaedc LB	414	mutex_lock(&hw->conf_lock);
335eaedc LB	415
290a6ce1 LB	416	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
	417	err = st_lsm6dsx_flush_fifo(hw);
	418	if (err < 0)
335eaedc	419	goto out;
290a6ce1 LB	420	}
	421
	422	if (enable) {
	423	err = st_lsm6dsx_sensor_enable(sensor);
	424	if (err < 0)
335eaedc	425	goto out;
290a6ce1 LB	426	} else {
	427	err = st_lsm6dsx_sensor_disable(sensor);
	428	if (err < 0)
335eaedc	429	goto out;
290a6ce1 LB	430	}
290a6ce1 LB	431
ff81a933 LB	432	err = st_lsm6dsx_set_fifo_odr(sensor, enable);
ff81a933 LB	433	if (err < 0)
335eaedc	434	goto out;
ff81a933	435
290a6ce1 LB	436	err = st_lsm6dsx_update_decimators(hw);
290a6ce1 LB	437	if (err < 0)
335eaedc	438	goto out;
290a6ce1 LB	439
	440	err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
	441	if (err < 0)
335eaedc	442	goto out;
290a6ce1 LB	443
290a6ce1 LB	444	if (hw->enable_mask) {
21345107 LB	445	/* reset hw ts counter */
21345107 LB	446	err = st_lsm6dsx_reset_hw_ts(hw);
290a6ce1	447	if (err < 0)
335eaedc	448	goto out;
290a6ce1	449
21345107	450	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
290a6ce1 LB	451	}
290a6ce1 LB	452
335eaedc LB	453	out:
	454	mutex_unlock(&hw->conf_lock);
	455
	456	return err;
290a6ce1 LB	457	}
	458
	459	static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
	460	{
407e0b53	461	struct st_lsm6dsx_hw *hw = private;
290a6ce1	462
21345107	463	return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
290a6ce1 LB	464	}
	465
	466	static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
	467	{
407e0b53	468	struct st_lsm6dsx_hw *hw = private;
290a6ce1 LB	469	int count;
	470
	471	mutex_lock(&hw->fifo_lock);
	472	count = st_lsm6dsx_read_fifo(hw);
	473	mutex_unlock(&hw->fifo_lock);
	474
	475	return !count ? IRQ_NONE : IRQ_HANDLED;
	476	}
	477
	478	static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
	479	{
	480	return st_lsm6dsx_update_fifo(iio_dev, true);
	481	}
	482
	483	static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
	484	{
	485	return st_lsm6dsx_update_fifo(iio_dev, false);
	486	}
	487
	488	static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
	489	.preenable = st_lsm6dsx_buffer_preenable,
	490	.postdisable = st_lsm6dsx_buffer_postdisable,
	491	};
	492
	493	int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
	494	{
ff5fff4a LB	495	struct device_node *np = hw->dev->of_node;
ff5fff4a LB	496	struct st_sensors_platform_data *pdata;
290a6ce1 LB	497	struct iio_buffer *buffer;
290a6ce1 LB	498	unsigned long irq_type;
89ca88a7	499	bool irq_active_low;
290a6ce1 LB	500	int i, err;
	501
	502	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
	503
	504	switch (irq_type) {
	505	case IRQF_TRIGGER_HIGH:
	506	case IRQF_TRIGGER_RISING:
89ca88a7 LB	507	irq_active_low = false;
	508	break;
	509	case IRQF_TRIGGER_LOW:
	510	case IRQF_TRIGGER_FALLING:
	511	irq_active_low = true;
290a6ce1 LB	512	break;
	513	default:
	514	dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
	515	return -EINVAL;
	516	}
	517
51a8b707 LB	518	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
	519	ST_LSM6DSX_REG_HLACTIVE_MASK,
	520	FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
	521	irq_active_low));
89ca88a7 LB	522	if (err < 0)
	523	return err;
	524
ff5fff4a LB	525	pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
	526	if ((np && of_property_read_bool(np, "drive-open-drain")) \|\|
	527	(pdata && pdata->open_drain)) {
51a8b707 LB	528	err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
	529	ST_LSM6DSX_REG_PP_OD_MASK,
	530	FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
	531	1));
ff5fff4a LB	532	if (err < 0)
	533	return err;
	534
	535	irq_type \|= IRQF_SHARED;
	536	}
	537
290a6ce1 LB	538	err = devm_request_threaded_irq(hw->dev, hw->irq,
	539	st_lsm6dsx_handler_irq,
	540	st_lsm6dsx_handler_thread,
	541	irq_type \| IRQF_ONESHOT,
	542	"lsm6dsx", hw);
	543	if (err) {
	544	dev_err(hw->dev, "failed to request trigger irq %d\n",
	545	hw->irq);
	546	return err;
	547	}
	548
	549	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
	550	buffer = devm_iio_kfifo_allocate(hw->dev);
	551	if (!buffer)
	552	return -ENOMEM;
	553
	554	iio_device_attach_buffer(hw->iio_devs[i], buffer);
	555	hw->iio_devs[i]->modes \|= INDIO_BUFFER_SOFTWARE;
	556	hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
	557	}
	558
	559	return 0;
	560	}