Orange Pi5 kernel

// SPDX-License-Identifier: GPL-2.0+
/*
 * STMicroelectronics st_lsm6dsr FIFO buffer library driver
 *
 * Copyright 2020 STMicroelectronics Inc.
 *
 * Lorenzo Bianconi <lorenzo.bianconi@st.com>
 */
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/iio/iio.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/events.h>
#include <asm/unaligned.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/buffer.h>
#include "st_lsm6dsr.h"

#define ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE		0x35
#define ST_LSM6DSR_REG_INT_STEP_DET_MASK		BIT(3)
#define ST_LSM6DSR_REG_INT_TILT_MASK			BIT(4)
#define ST_LSM6DSR_REG_INT_SIGMOT_MASK			BIT(5)
#define ST_LSM6DSR_REG_INT_GLANCE_MASK			BIT(0)
#define ST_LSM6DSR_REG_INT_MOTION_MASK			BIT(1)
#define ST_LSM6DSR_REG_INT_NO_MOTION_MASK		BIT(2)
#define ST_LSM6DSR_REG_INT_WAKEUP_MASK			BIT(3)
#define ST_LSM6DSR_REG_INT_PICKUP_MASK			BIT(4)
#define ST_LSM6DSR_REG_INT_ORIENTATION_MASK		BIT(5)
#define ST_LSM6DSR_REG_INT_WRIST_MASK			BIT(6)

#define ST_LSM6DSR_SAMPLE_DISCHARD			0x7ffd

#define ST_LSM6DSR_EWMA_LEVEL				120
#define ST_LSM6DSR_EWMA_DIV				128

enum {
	ST_LSM6DSR_GYRO_TAG = 0x01,
	ST_LSM6DSR_ACC_TAG = 0x02,
	ST_LSM6DSR_TEMP_TAG = 0x03,
	ST_LSM6DSR_TS_TAG = 0x04,
	ST_LSM6DSR_EXT0_TAG = 0x0f,
	ST_LSM6DSR_EXT1_TAG = 0x10,
	ST_LSM6DSR_SC_TAG = 0x12,
};

/**
 * Get Linux timestamp (SW)
 *
 * @return  timestamp in ns
 */
static inline s64 st_lsm6dsr_get_time_ns(void)
{
	return ktime_to_ns(ktime_get_boottime());
}

/**
 * Timestamp low pass filter
 *
 * @param  old: ST IMU MEMS hw instance
 * @param  new: ST IMU MEMS hw instance
 * @param  weight: ST IMU MEMS hw instance
 * @return  estimation of the timestamp average
 */
static inline s64 st_lsm6dsr_ewma(s64 old, s64 new, int weight)
{
	s64 diff, incr;

	diff = new - old;
	incr = div_s64((ST_LSM6DSR_EWMA_DIV - weight) * diff,
			ST_LSM6DSR_EWMA_DIV);

	return old + incr;
}

/**
 * Reset HW Timestamp counter and clear timestamp data structure
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  < 0 if error, 0 otherwise
 */
inline int st_lsm6dsr_reset_hwts(struct st_lsm6dsr_hw *hw)
{
	u8 data = 0xaa;

	hw->ts = st_lsm6dsr_get_time_ns();
	hw->ts_offset = hw->ts;
	hw->val_ts_old = 0;
	hw->hw_ts_high = 0;
	hw->tsample = 0ull;

	return st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_TIMESTAMP2_ADDR,
				       sizeof(data), &data);
}

/**
 * Setting FIFO mode
 *
 * @param  hw: ST IMU MEMS hw instance
 * @param  fifo_mode: ST_LSM6DSR_FIFO_BYPASS or ST_LSM6DSR_FIFO_CONT
 * @return  0 FIFO configured accordingly, non zero otherwise
 */
int st_lsm6dsr_set_fifo_mode(struct st_lsm6dsr_hw *hw,
			     enum st_lsm6dsr_fifo_mode fifo_mode)
{
	int err;

	err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
					 ST_LSM6DSR_REG_FIFO_MODE_MASK,
					 fifo_mode);
	if (err < 0)
		return err;

	hw->fifo_mode = fifo_mode;

	return 0;
}

/**
 * Setting sensor ODR in batching mode
 *
 * @param  sensor: ST IMU sensor instance
 * @param  enable: enable or disable batching mode
 * @return  0 FIFO configured accordingly, non zero otherwise
 */
int __st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
					 bool enable)
{
	struct st_lsm6dsr_hw *hw = sensor->hw;
	u8 data = 0;
	int err;
	int podr, puodr;

	if (enable) {
		err = st_lsm6dsr_get_odr_val(sensor->id, sensor->odr,
					     sensor->uodr, &podr, &puodr,
					     &data);
		if (err < 0)
			return err;
	}

	err = __st_lsm6dsr_write_with_mask(hw, sensor->batch_reg.addr,
					   sensor->batch_reg.mask, data);
	return err < 0 ? err : 0;
}

/**
 * Setting timestamp ODR in batching mode
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  Timestamp ODR
 */
static int st_lsm6dsr_ts_odr(struct st_lsm6dsr_hw *hw)
{
	struct st_lsm6dsr_sensor *sensor;
	int odr = 0;
	u8 i;

	for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_EXT1; i++) {
		if (!hw->iio_devs[i])
			continue;

		sensor = iio_priv(hw->iio_devs[i]);
		if (hw->enable_mask & BIT(sensor->id))
			odr = max_t(int, odr, sensor->odr);
	}

	return odr;
}

/**
 * Setting sensor ODR in batching mode
 *
 * @param  sensor: ST IMU sensor instance
 * @param  enable: enable or disable batching mode
 * @return  0 FIFO configured accordingly, non zero otherwise
 */
static inline int
st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
				   bool enable)
{
	struct st_lsm6dsr_hw *hw = sensor->hw;
	int err;

	mutex_lock(&hw->page_lock);
	err = __st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
	mutex_unlock(&hw->page_lock);

	return err;
}

/**
 * Update watermark level in FIFO
 *
 * @param  sensor: ST IMU sensor instance
 * @param  watermark: New watermark level
 * @return  0 if FIFO configured, non zero for error
 */
int st_lsm6dsr_update_watermark(struct st_lsm6dsr_sensor *sensor,
				u16 watermark)
{
	u16 fifo_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH, cur_watermark = 0;
	struct st_lsm6dsr_hw *hw = sensor->hw;
	struct st_lsm6dsr_sensor *cur_sensor;
	__le16 wdata;
	int i, err;
	u8 data;

	for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_STEP_COUNTER; i++) {
		if (!hw->iio_devs[i])
			continue;

		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);
	}

	fifo_watermark = max_t(u16, fifo_watermark, 2);

	err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR + 1,
				     sizeof(data), &data);
	if (err < 0)
		goto out;

	fifo_watermark = ((data << 8) & ~ST_LSM6DSR_REG_FIFO_WTM_MASK) |
			 (fifo_watermark & ST_LSM6DSR_REG_FIFO_WTM_MASK);
	wdata = cpu_to_le16(fifo_watermark);
	err = st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR,
				      sizeof(wdata), (u8 *)&wdata);
out:
	return err < 0 ? err : 0;
}

/**
 * Timestamp correlation finction
 *
 * @param  hw: ST IMU MEMS hw instance
 * @param  ts: New timestamp
 */
static inline void st_lsm6dsr_sync_hw_ts(struct st_lsm6dsr_hw *hw, s64 ts)
{
	s64 delta = ts - hw->hw_ts;

	hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset, delta,
					ST_LSM6DSR_EWMA_LEVEL);
}

/**
 * Return the iio device structure based on FIFO TAG ID
 *
 * @param  hw: ST IMU MEMS hw instance
 * @param  tag: FIFO sample TAG ID
 * @return  0 if FIFO configured, non zero for error
 */
static struct
iio_dev *st_lsm6dsr_get_iiodev_from_tag(struct st_lsm6dsr_hw *hw,
					u8 tag)
{
	struct iio_dev *iio_dev;

	switch (tag) {
	case ST_LSM6DSR_GYRO_TAG:
		iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GYRO];
		break;
	case ST_LSM6DSR_ACC_TAG:
		iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ACC];
		break;
	case ST_LSM6DSR_TEMP_TAG:
		iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TEMP];
		break;
	case ST_LSM6DSR_EXT0_TAG:
		if (hw->enable_mask & BIT(ST_LSM6DSR_ID_EXT0))
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT0];
		else
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
		break;
	case ST_LSM6DSR_EXT1_TAG:
		iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
		break;
	case ST_LSM6DSR_SC_TAG:
		iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_COUNTER];
		break;
	default:
		iio_dev = NULL;
		break;
	}

	return iio_dev;
}

/**
 * Read all FIFO data stored after WTM FIFO irq fired interrupt
 *
 * @param hw: ST IMU MEMS hw instance
 * @return Number of read bytes in FIFO or error if negative
 */
static int st_lsm6dsr_read_fifo(struct st_lsm6dsr_hw *hw)
{
	u8 iio_buf[ALIGN(ST_LSM6DSR_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
	/* acc + gyro + 2 ext + ts + sc */
	u8 buf[6 * ST_LSM6DSR_FIFO_SAMPLE_SIZE], tag, *ptr;
	int i, err, word_len, fifo_len, read_len;
	struct st_lsm6dsr_sensor *sensor;
	struct iio_dev *iio_dev;
	s64 ts_irq, hw_ts_old;
	__le16 fifo_status;
	u16 fifo_depth;
	s16 drdymask;
	u32 val;

	ts_irq = hw->ts - hw->delta_ts;

	err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_STATUS1_ADDR,
				     sizeof(fifo_status), (u8 *)&fifo_status);
	if (err < 0)
		return err;

	fifo_depth = le16_to_cpu(fifo_status) & ST_LSM6DSR_REG_FIFO_STATUS_DIFF;
	if (!fifo_depth)
		return 0;

	fifo_len = fifo_depth * ST_LSM6DSR_FIFO_SAMPLE_SIZE;
	read_len = 0;

	while (read_len < fifo_len) {
		word_len = min_t(int, fifo_len - read_len, sizeof(buf));
		err = st_lsm6dsr_read_atomic(hw,
					ST_LSM6DSR_REG_FIFO_DATA_OUT_TAG_ADDR,
					word_len, buf);
		if (err < 0)
			return err;

		for (i = 0; i < word_len; i += ST_LSM6DSR_FIFO_SAMPLE_SIZE) {
			ptr = &buf[i + ST_LSM6DSR_TAG_SIZE];
			tag = buf[i] >> 3;

			if (tag == ST_LSM6DSR_TS_TAG) {
				val = get_unaligned_le32(ptr);

				if (hw->val_ts_old > val)
					hw->hw_ts_high++;

				hw_ts_old = hw->hw_ts;

				/* check hw rollover */
				hw->val_ts_old = val;
				hw->hw_ts = (val +
					     ((s64)hw->hw_ts_high << 32)) *
					     hw->ts_delta_ns;
				hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset,
						ts_irq - hw->hw_ts,
						ST_LSM6DSR_EWMA_LEVEL);

				if (!test_bit(ST_LSM6DSR_HW_FLUSH, &hw->state))
					/* sync ap timestamp and sensor one */
					st_lsm6dsr_sync_hw_ts(hw, ts_irq);

				ts_irq += hw->hw_ts;

				if (!hw->tsample)
					hw->tsample = hw->ts_offset + hw->hw_ts;
				else
					hw->tsample = hw->tsample +
						      hw->hw_ts - hw_ts_old;
			} else {
				iio_dev =
					st_lsm6dsr_get_iiodev_from_tag(hw, tag);
				if (!iio_dev)
					continue;

				sensor = iio_priv(iio_dev);

				/* skip samples if not ready */
				drdymask =
				      (s16)le16_to_cpu(get_unaligned_le16(ptr));
				if (unlikely(drdymask >=
						ST_LSM6DSR_SAMPLE_DISCHARD)) {
					continue;
				}

				/*
				 * hw ts in not queued in FIFO if only step
				 * counter enabled
				 */
				if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
					val = get_unaligned_le32(ptr + 2);
					hw->tsample = (val +
						((s64)hw->hw_ts_high << 32)) *
						hw->ts_delta_ns;

					/* avoid samples in the future */
					hw->tsample = min_t(s64,
							    st_lsm6dsr_get_time_ns(),
							    hw->tsample);
				} else {
					hw->tsample = st_lsm6dsr_get_time_ns();
				}

				memcpy(iio_buf, ptr, ST_LSM6DSR_SAMPLE_SIZE);

				sensor->last_fifo_timestamp = hw->tsample;

				/* support decimation for ODR < 12.5 Hz */
				if (sensor->dec_counter > 0) {
					sensor->dec_counter--;
				} else {
					sensor->dec_counter = sensor->decimator;
					iio_push_to_buffers_with_timestamp(iio_dev,
								   iio_buf,
								   hw->tsample);
				}
			}
		}
		read_len += word_len;
	}

	return read_len;
}

/**
 * Report events after WTM FIFO irq fired interrupt
 *
 * @param hw: ST IMU MEMS hw instance
 * @return 0 if OK, non zero for error
 */
static int st_lsm6dsr_report_events(struct st_lsm6dsr_hw *hw)
{
	struct iio_dev *iio_dev;
	u8 status[3];
	s64 event;
	int err;

	if (hw->enable_mask & (BIT(ST_LSM6DSR_ID_STEP_DETECTOR) |
			       BIT(ST_LSM6DSR_ID_SIGN_MOTION) |
			       BIT(ST_LSM6DSR_ID_TILT) |
			       BIT(ST_LSM6DSR_ID_MOTION) |
			       BIT(ST_LSM6DSR_ID_NO_MOTION) |
			       BIT(ST_LSM6DSR_ID_WAKEUP) |
			       BIT(ST_LSM6DSR_ID_PICKUP) |
			       BIT(ST_LSM6DSR_ID_ORIENTATION) |
			       BIT(ST_LSM6DSR_ID_WRIST_TILT) |
			       BIT(ST_LSM6DSR_ID_GLANCE))) {

		err = hw->tf->read(hw->dev,
				   ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE,
				   sizeof(status), status);
		if (err < 0)
			return err;

		/* embedded function sensors */
		if (status[0] & ST_LSM6DSR_REG_INT_STEP_DET_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_DETECTOR];
			event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[0] & ST_LSM6DSR_REG_INT_SIGMOT_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_SIGN_MOTION];
			event = IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[0] & ST_LSM6DSR_REG_INT_TILT_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TILT];
			event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		/*  fsm sensors */
		if (status[1] & ST_LSM6DSR_REG_INT_GLANCE_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GLANCE];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[1] & ST_LSM6DSR_REG_INT_MOTION_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_MOTION];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[1] & ST_LSM6DSR_REG_INT_NO_MOTION_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_NO_MOTION];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[1] & ST_LSM6DSR_REG_INT_WAKEUP_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WAKEUP];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[1] & ST_LSM6DSR_REG_INT_PICKUP_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_PICKUP];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
		if (status[1] & ST_LSM6DSR_REG_INT_ORIENTATION_MASK) {
			struct st_lsm6dsr_sensor *sensor;

			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
			sensor = iio_priv(iio_dev);
			iio_trigger_poll_chained(sensor->trig);
		}
		if (status[1] & ST_LSM6DSR_REG_INT_WRIST_MASK) {
			iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WRIST_TILT];
			event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
						     IIO_EV_TYPE_THRESH,
						     IIO_EV_DIR_RISING);
			iio_push_event(iio_dev, event,
				       st_lsm6dsr_get_time_ns());
		}
	}

	return 0;
}

/**
 * Return the max FIFO watermark level accepted
 *
 * @param  dev: Linux Device
 * @param  attr: Device Attribute
 * @param  buf: User Buffer
 * @return  Number of chars printed into the buffer
 */
ssize_t st_lsm6dsr_get_max_watermark(struct device *dev,
				     struct device_attribute *attr,
				     char *buf)
{
	struct iio_dev *iio_dev = dev_get_drvdata(dev);
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);

	return sprintf(buf, "%d\n", sensor->max_watermark);
}

/**
 * Return the FIFO watermark level
 *
 * @param  dev: Linux Device
 * @param  attr: Device Attribute
 * @param  buf: User Buffer
 * @return  Number of chars printed into the buffer
 */
ssize_t st_lsm6dsr_get_watermark(struct device *dev,
				 struct device_attribute *attr,
				 char *buf)
{
	struct iio_dev *iio_dev = dev_get_drvdata(dev);
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);

	return sprintf(buf, "%d\n", sensor->watermark);
}

/**
 * Set the FIFO watermark level
 *
 * @param  dev: Linux Device
 * @param  attr: Device Attribute
 * @param  buf: User Buffer
 * @param  size: New FIFO watermark level
 * @return  Watermark level if >= 0, error otherwise
 */
ssize_t st_lsm6dsr_set_watermark(struct device *dev,
				 struct device_attribute *attr,
				 const char *buf, size_t size)
{
	struct iio_dev *iio_dev = dev_get_drvdata(dev);
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
	int err, val;

	mutex_lock(&iio_dev->mlock);

	err = kstrtoint(buf, 10, &val);
	if (err < 0)
		goto out;

	err = st_lsm6dsr_update_watermark(sensor, val);
	if (err < 0)
		goto out;

	sensor->watermark = val;

out:
	mutex_unlock(&iio_dev->mlock);

	return err < 0 ? err : size;
}

/**
 * Flush internal HW FIFO
 *
 * @param  dev: Linux Device
 * @param  attr: Device Attribute
 * @param  buf: User Buffer
 * @param  size: unused
 * @return  Watermark level if >= 0, error otherwise
 */
ssize_t st_lsm6dsr_flush_fifo(struct device *dev,
			      struct device_attribute *attr,
			      const char *buf, size_t size)
{
	struct iio_dev *iio_dev = dev_get_drvdata(dev);
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
	struct st_lsm6dsr_hw *hw = sensor->hw;
	int count;
	s64 ts;
#ifdef CONFIG_NO_GKI
	s64 type;
	s64 event;
	s64 fts;
#endif

	mutex_lock(&hw->fifo_lock);
	ts = st_lsm6dsr_get_time_ns();
	hw->delta_ts = ts - hw->ts;
	hw->ts = ts;
	set_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
	count = st_lsm6dsr_read_fifo(hw);
	sensor->dec_counter = 0;
	mutex_unlock(&hw->fifo_lock);

#ifdef CONFIG_NO_GKI
	if (count > 0)
		fts = sensor->last_fifo_timestamp;
	else
		fts = ts;

	type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY;
	event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1,
				     IIO_EV_TYPE_FIFO_FLUSH, type);
	iio_push_event(iio_dev, event, fts);
#endif

	return size;
}

/**
 * Empty FIFO and set HW FIFO in Bypass mode
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  Watermark level if >= 0, error otherwise
 */
int st_lsm6dsr_suspend_fifo(struct st_lsm6dsr_hw *hw)
{
	int err;

	mutex_lock(&hw->fifo_lock);
	st_lsm6dsr_read_fifo(hw);
	err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
	mutex_unlock(&hw->fifo_lock);

	return err;
}

/**
 * Update ODR batching in FIFO and Timestamp
 *
 * @param  iio_dev: Linux IIO device
 * @param  enable: enable/disable batcing in FIFO
 * @return  < 0 if error, 0 otherwise
 */
int st_lsm6dsr_update_batching(struct iio_dev *iio_dev, bool enable)
{
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
	struct st_lsm6dsr_hw *hw = sensor->hw;
	int err;

	disable_irq(hw->irq);

	err = st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
	if (err < 0)
		goto out;

	/* Calc TS ODR */
	hw->odr = st_lsm6dsr_ts_odr(hw);

out:
	enable_irq(hw->irq);

	return err;
}

/**
 * Update FIFO watermark value based to the enabled sensors
 *
 * @param  iio_dev: Linux IIO device
 * @param  enable: enable/disable batcing in FIFO
 * @return  < 0 if error, 0 otherwise
 */
static int st_lsm6dsr_update_fifo(struct iio_dev *iio_dev, bool enable)
{
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
	struct st_lsm6dsr_hw *hw = sensor->hw;
	int err;
	int podr, puodr;

	disable_irq(hw->irq);

	if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
	    sensor->id == ST_LSM6DSR_ID_EXT1) {
		err = st_lsm6dsr_shub_set_enable(sensor, enable);
		if (err < 0)
			goto out;
	} else {
		if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
			err = st_lsm6dsr_step_counter_set_enable(sensor,
								 enable);
			if (err < 0)
				goto out;
		} else {
			err = st_lsm6dsr_sensor_set_enable(sensor, enable);
			if (err < 0)
				goto out;

			err = st_lsm6dsr_set_sensor_batching_odr(sensor,
								 enable);
			if (err < 0)
				goto out;
		}
	}

	/*
	 * this is an auxiliary sensor, it need to get batched
	 * together at least with a primary sensor (Acc/Gyro)
	 */
	if (sensor->id == ST_LSM6DSR_ID_TEMP) {
		if (!(hw->enable_mask & (BIT(ST_LSM6DSR_ID_ACC) |
					 BIT(ST_LSM6DSR_ID_GYRO)))) {
			struct st_lsm6dsr_sensor *acc_sensor;
			u8 data = 0;

			acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
			if (enable) {
				err = st_lsm6dsr_get_odr_val(ST_LSM6DSR_ID_ACC,
						sensor->odr, sensor->uodr,
						&podr, &puodr, &data);
				if (err < 0)
					goto out;
			}

			err = st_lsm6dsr_write_with_mask(hw,
					acc_sensor->batch_reg.addr,
					acc_sensor->batch_reg.mask,
					data);
			if (err < 0)
				goto out;
		}
	}

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

	/* Calc TS ODR */
	hw->odr = st_lsm6dsr_ts_odr(hw);

	if (enable && hw->fifo_mode == ST_LSM6DSR_FIFO_BYPASS) {
		st_lsm6dsr_reset_hwts(hw);
		err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_CONT);
	} else if (!hw->enable_mask) {
		err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
	}

out:
	enable_irq(hw->irq);

	return err;
}

/**
 * Bottom handler for FSM Orientation sensor event generation
 *
 * @param  irq: IIO trigger irq number
 * @param  p: iio poll function environment
 * @return  IRQ_HANDLED or < 0 for error
 */
static irqreturn_t st_lsm6dsr_buffer_handler_thread(int irq, void *p)
{
	struct iio_poll_func *pf = p;
	struct iio_dev *iio_dev = pf->indio_dev;
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
	u8 buffer[sizeof(u8) + sizeof(s64)];
	int err;

	err = st_lsm6dsr_fsm_get_orientation(sensor->hw, buffer);
	if (err < 0)
		goto out;

	iio_push_to_buffers_with_timestamp(iio_dev, buffer,
					   st_lsm6dsr_get_time_ns());
out:
	iio_trigger_notify_done(sensor->trig);

	return IRQ_HANDLED;
}

/**
 * Top handler for sensor event generation + FIFO management
 *
 * @param  irq: IIO trigger irq number
 * @param  private: iio poll function environment
 * @return  IRQ_HANDLED or < 0 for error
 */
static irqreturn_t st_lsm6dsr_handler_irq(int irq, void *private)
{
	struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;
	s64 ts = st_lsm6dsr_get_time_ns();

	hw->delta_ts = ts - hw->ts;
	hw->ts = ts;

	return IRQ_WAKE_THREAD;
}

/**
 * Bottom handler for sensor event generation + FIFO management
 *
 * @param  irq: irq line number
 * @param  private: device private environment pointer
 * @return  IRQ_HANDLED or < 0 for error
 */
static irqreturn_t st_lsm6dsr_handler_thread(int irq, void *private)
{
	struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;

	mutex_lock(&hw->fifo_lock);
	st_lsm6dsr_read_fifo(hw);
	clear_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
	mutex_unlock(&hw->fifo_lock);

	st_lsm6dsr_report_events(hw);

	return IRQ_HANDLED;
}

/**
 * IIO fifo pre enabled callback function
 *
 * @param  iio_dev: IIO device
 * @return  < 0 if error, 0 otherwise
 */
static int st_lsm6dsr_fifo_preenable(struct iio_dev *iio_dev)
{
	return st_lsm6dsr_update_fifo(iio_dev, true);
}

/**
 * IIO fifo post disable callback function
 *
 * @param  iio_dev: IIO device
 * @return  < 0 if error, 0 otherwise
 */
static int st_lsm6dsr_fifo_postdisable(struct iio_dev *iio_dev)
{
	return st_lsm6dsr_update_fifo(iio_dev, false);
}

/**
 * IIO fifo callback registruction structure
 */
static const struct iio_buffer_setup_ops st_lsm6dsr_fifo_ops = {
	.preenable = st_lsm6dsr_fifo_preenable,
	.postdisable = st_lsm6dsr_fifo_postdisable,
};

/**
 * Enable HW FIFO
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  < 0 if error, 0 otherwise
 */
static int st_lsm6dsr_fifo_init(struct st_lsm6dsr_hw *hw)
{
	return st_lsm6dsr_write_with_mask(hw,
					  ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
					  ST_LSM6DSR_REG_DEC_TS_MASK, 1);
}

static const struct iio_trigger_ops st_lsm6dsr_trigger_ops;

static int st_lsm6dsr_buffer_preenable(struct iio_dev *iio_dev)
{
	return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), true);
}

static int st_lsm6dsr_buffer_postdisable(struct iio_dev *iio_dev)
{
	return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), false);
}

static const struct iio_buffer_setup_ops st_lsm6dsr_buffer_ops = {
	.preenable = st_lsm6dsr_buffer_preenable,
	.postenable = NULL,
	.predisable = NULL,
	.postdisable = st_lsm6dsr_buffer_postdisable,
};


/**
 * Init IRQ
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  < 0 if error, 0 otherwise
 */
int st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw *hw)
{
	unsigned long irq_type;
	bool irq_active_low;
	int err;

	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
	if (irq_type == IRQF_TRIGGER_NONE)
		irq_type = IRQF_TRIGGER_HIGH;

	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 = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_CTRL3_C_ADDR,
					 ST_LSM6DSR_REG_H_LACTIVE_MASK,
					 irq_active_low);
	if (err < 0)
		return err;

	err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_INT1_CTRL_ADDR,
					 ST_LSM6DSR_REG_INT_FIFO_TH_MASK, 1);
	if (err < 0)
		return err;

	return 0;
}

/**
 * Init IIO buffers and triggers
 *
 * @param  hw: ST IMU MEMS hw instance
 * @return  < 0 if error, 0 otherwise
 */
int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw)
{
	struct device_node *np = hw->dev->of_node;
	struct st_lsm6dsr_sensor *sensor;
	struct iio_buffer *buffer;
	struct iio_dev *iio_dev;
	unsigned long irq_type;
	int i, err;

	err = st_lsm6dsr_irq_setup(hw);
	if (err < 0)
		return err;

	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
	if (irq_type == IRQF_TRIGGER_NONE)
		irq_type = IRQF_TRIGGER_HIGH;

	if (np && of_property_read_bool(np, "drive-open-drain")) {
		err = st_lsm6dsr_write_with_mask(hw,
						 ST_LSM6DSR_REG_CTRL3_C_ADDR,
						 ST_LSM6DSR_REG_PP_OD_MASK, 1);
		if (err < 0)
			return err;

		irq_type |= IRQF_SHARED;
	}

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

	for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_SIGN_MOTION; i++) {
		if (!hw->iio_devs[i])
			continue;

		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_lsm6dsr_fifo_ops;
	}

	err = st_lsm6dsr_fifo_init(hw);
	if (err < 0)
		return err;

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

	err = iio_triggered_buffer_setup(iio_dev,
				NULL, st_lsm6dsr_buffer_handler_thread,
				&st_lsm6dsr_buffer_ops);
	if (err < 0)
		return err;

	sensor->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger",
					      iio_dev->name);
	if (!sensor->trig)
		return -ENOMEM;

	iio_trigger_set_drvdata(sensor->trig, iio_dev);
	sensor->trig->ops = &st_lsm6dsr_trigger_ops;
	sensor->trig->dev.parent = hw->dev;
	sensor->trig->owner = THIS_MODULE;
	iio_dev->trig = iio_trigger_get(sensor->trig);

	err = iio_trigger_register(sensor->trig);
	if (err < 0)
		iio_triggered_buffer_cleanup(iio_dev);

	return err;
}

int st_lsm6dsr_deallocate_buffers(struct st_lsm6dsr_hw *hw)
{
	struct iio_dev *iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
	struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);

	iio_trigger_unregister(sensor->trig);
	iio_triggered_buffer_cleanup(iio_dev);

	return 0;
}