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

Commit 883d4e0d authored by Mario Tesi's avatar Mario Tesi Committed by Gerrit - the friendly Code Review server
Browse files

iio: imu: Add support to ASM330LHH 

Added support to STM ASM330LHH IMU sensor (6-axis acc + gyro)
    - continuous mode support
    - i2c support
    - spi support
    - sw fifo mode support
    - temperature

Git-commit: 258244f33bfbcb658b808057c7d7739f609d268b
Git-repo: https://github.com/STMicroelectronics/STMems_ASM330LHH_IIO_HAL_Driver.git


Change-Id: I33ee074c0678dc76ab52a34f581c7f56c8c23482
Signed-off-by: default avatarPuneet yatnala <puneet@codeaurora.org>
parent 38bc3d98

driver/iio/imu/st_asm330lhh/Kconfig

0 → 100644
+23 −0
Original line number Diff line number Diff line 


config IIO_ST_ASM330LHH

	tristate "STMicroelectronics ASM330LHH sensor"

	depends on (I2C || SPI)

	select IIO_BUFFER

	select IIO_KFIFO_BUF

	select IIO_ST_ASM330LHH_I2C if (I2C)

	select IIO_ST_ASM330LHH_SPI if (SPI_MASTER)

	help

	  Say yes here to build support for STMicroelectronics ASM330LHH imu

	  sensor.



	  To compile this driver as a module, choose M here: the module

	  will be called st_asm330lhh.



config IIO_ST_ASM330LHH_I2C

	tristate

	depends on IIO_ST_ASM330LHH



config IIO_ST_ASM330LHH_SPI

	tristate

	depends on IIO_ST_ASM330LHH

driver/iio/imu/st_asm330lhh/Makefile

0 → 100644
+6 −0
Original line number Diff line number Diff line 
#

# Makefile for ST ASM330 device.

#

obj-$(CONFIG_IIO_ST_ASM330LHH) += st_asm330lhh_core.o st_asm330lhh_buffer.o

obj-$(CONFIG_IIO_ST_ASM330LHH_I2C) += st_asm330lhh_i2c.o

obj-$(CONFIG_IIO_ST_ASM330LHH_SPI) += st_asm330lhh_spi.o

driver/iio/imu/st_asm330lhh/st_asm330lhh.h

0 → 100644
+245 −0
Original line number Diff line number Diff line 
/*

 * STMicroelectronics st_asm330lhh sensor driver

 *

 * Copyright 2018 STMicroelectronics Inc.

 *

 * Lorenzo Bianconi <lorenzo.bianconi@st.com>

 *

 * Licensed under the GPL-2.

 */



#ifndef ST_ASM330LHH_H

#define ST_ASM330LHH_H



#include <linux/device.h>

#include <linux/iio/iio.h>



/*

 * Module version:

 * Version: Major.Minor

 * Commit: last commit in branch

 */

#define ST_ASM330LHH_REVISION		"1.0.1"

#define ST_ASM330LHH_PATCH		"1"



#define ST_ASM330LHH_VERSION		"v"	\

	ST_ASM330LHH_REVISION			\

	"-"					\

	ST_ASM330LHH_PATCH



#define ST_ASM330LHH_DEV_NAME		"asm330lhh"



#define ST_ASM330LHH_SAMPLE_SIZE	6

#define ST_ASM330LHH_TS_SAMPLE_SIZE	4

#define ST_ASM330LHH_TAG_SIZE		1

#define ST_ASM330LHH_FIFO_SAMPLE_SIZE	(ST_ASM330LHH_SAMPLE_SIZE + \

					 ST_ASM330LHH_TAG_SIZE)

#define ST_ASM330LHH_MAX_FIFO_DEPTH	416



#define ST_ASM330LHH_REG_FIFO_BATCH_ADDR	0x09

#define ST_ASM330LHH_REG_FIFO_CTRL4_ADDR	0x0a

#define ST_ASM330LHH_REG_STATUS_ADDR		0x1e

#define ST_ASM330LHH_REG_STATUS_TDA		BIT(2)

#define ST_ASM330LHH_REG_OUT_TEMP_L_ADDR	0x20

#define ST_ASM330LHH_REG_OUT_TEMP_H_ADDR	0x21



#define ST_ASM330LHH_MAX_ODR			416



/* Define Custom events for FIFO flush */

#define CUSTOM_IIO_EV_DIR_FIFO_EMPTY (IIO_EV_DIR_FALLING + 1)

#define CUSTOM_IIO_EV_DIR_FIFO_DATA (IIO_EV_DIR_FALLING + 2)

#define CUSTOM_IIO_EV_TYPE_FIFO_FLUSH (IIO_EV_TYPE_MAG_ADAPTIVE + 1)



#define ST_ASM330LHH_CHANNEL(chan_type, addr, mod, ch2, scan_idx,	\

			   rb, sb, sg)					\

{									\

	.type = chan_type,						\

	.address = addr,						\

	.modified = mod,						\

	.channel2 = ch2,						\

	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\

			      BIT(IIO_CHAN_INFO_SCALE),			\

	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),	\

	.scan_index = scan_idx,						\

	.scan_type = {							\

		.sign = sg,						\

		.realbits = rb,						\

		.storagebits = sb,					\

		.endianness = IIO_LE,					\

	},								\

}



static const struct iio_event_spec st_asm330lhh_flush_event = {

	.type = CUSTOM_IIO_EV_TYPE_FIFO_FLUSH,

	.dir = IIO_EV_DIR_EITHER,

};



#define ST_ASM330LHH_FLUSH_CHANNEL(dtype)		\

{							\

	.type = dtype,					\

	.modified = 0,					\

	.scan_index = -1,				\

	.indexed = -1,					\

	.event_spec = &st_asm330lhh_flush_event,	\

	.num_event_specs = 1,				\

}



#define ST_ASM330LHH_RX_MAX_LENGTH	8

#define ST_ASM330LHH_TX_MAX_LENGTH	8



struct st_asm330lhh_transfer_buffer {

	u8 rx_buf[ST_ASM330LHH_RX_MAX_LENGTH];

	u8 tx_buf[ST_ASM330LHH_TX_MAX_LENGTH] ____cacheline_aligned;

};



struct st_asm330lhh_transfer_function {

	int (*read)(struct device *dev, u8 addr, int len, u8 *data);

	int (*write)(struct device *dev, u8 addr, int len, u8 *data);

};



struct st_asm330lhh_reg {

	u8 addr;

	u8 mask;

};



struct st_asm330lhh_odr {

	u16 hz;

	u8 val;

};



#define ST_ASM330LHH_ODR_LIST_SIZE	7

struct st_asm330lhh_odr_table_entry {

	struct st_asm330lhh_reg reg;

	struct st_asm330lhh_odr odr_avl[ST_ASM330LHH_ODR_LIST_SIZE];

};



struct st_asm330lhh_fs {

	u32 gain;

	u8 val;

};



#define ST_ASM330LHH_FS_ACC_LIST_SIZE		4

#define ST_ASM330LHH_FS_GYRO_LIST_SIZE		6

#define ST_ASM330LHH_FS_TEMP_LIST_SIZE		1

#define ST_ASM330LHH_FS_LIST_SIZE		6

struct st_asm330lhh_fs_table_entry {

	u32 size;

	struct st_asm330lhh_reg reg;

	struct st_asm330lhh_fs fs_avl[ST_ASM330LHH_FS_LIST_SIZE];

};



enum st_asm330lhh_sensor_id {

	ST_ASM330LHH_ID_ACC,

	ST_ASM330LHH_ID_GYRO,

	ST_ASM330LHH_ID_TEMP,

	ST_ASM330LHH_ID_MAX,

};



enum st_asm330lhh_fifo_mode {

	ST_ASM330LHH_FIFO_BYPASS = 0x0,

	ST_ASM330LHH_FIFO_CONT = 0x6,

};



enum {

	ST_ASM330LHH_HW_FLUSH,

	ST_ASM330LHH_HW_OPERATIONAL,

};



/**

 * struct st_asm330lhh_sensor - ST IMU sensor instance

 * @id: Sensor identifier.

 * @hw: Pointer to instance of struct st_asm330lhh_hw.

 * @gain: Configured sensor sensitivity.

 * @odr: Output data rate of the sensor [Hz].

 * @watermark: Sensor watermark level.

 * @batch_mask: Sensor mask for FIFO batching register

 */

struct st_asm330lhh_sensor {

	enum st_asm330lhh_sensor_id id;

	struct st_asm330lhh_hw *hw;



	u32 gain;

	u16 odr;

	u32 offset;



	__le16 old_data;



	u8 std_samples;

	u8 std_level;



	u16 watermark;

	u8 batch_mask;

	u8 batch_addr;

};



/**

 * struct st_asm330lhh_hw - ST IMU MEMS hw instance

 * @dev: Pointer to instance of struct device (I2C or SPI).

 * @irq: Device interrupt line (I2C or SPI).

 * @lock: Mutex to protect read and write operations.

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

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

 * @state: hw operational state.

 * @enable_mask: Enabled sensor bitmask.

 * @ts_offset: Hw timestamp offset.

 * @hw_ts: Latest hw timestamp from the sensor.

 * @ts: Latest timestamp from irq handler.

 * @delta_ts: Delta time between two consecutive interrupts.

 * @iio_devs: Pointers to acc/gyro iio_dev instances.

 * @tf: Transfer function structure used by I/O operations.

 * @tb: Transfer buffers used by SPI I/O operations.

 */

struct st_asm330lhh_hw {

	struct device *dev;

	int irq;



	struct mutex lock;

	struct mutex fifo_lock;



	enum st_asm330lhh_fifo_mode fifo_mode;

	unsigned long state;

	u8 enable_mask;



	s64 ts_offset;

	s64 hw_ts;

	s64 delta_ts;

	s64 ts;

	s64 tsample;

	s64 hw_ts_old;

	s64 delta_hw_ts;



	/* Timestamp sample ODR */

	u16 odr;



	struct iio_dev *iio_devs[ST_ASM330LHH_ID_MAX];



	const struct st_asm330lhh_transfer_function *tf;

	struct st_asm330lhh_transfer_buffer tb;

};



extern const struct dev_pm_ops st_asm330lhh_pm_ops;



int st_asm330lhh_probe(struct device *dev, int irq,

		       const struct st_asm330lhh_transfer_function *tf_ops);

int st_asm330lhh_remove(struct device *dev);

int st_asm330lhh_sensor_set_enable(struct st_asm330lhh_sensor *sensor,

				   bool enable);

int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw);

int st_asm330lhh_deallocate_fifo(struct st_asm330lhh_hw *hw);

int st_asm330lhh_write_with_mask(struct st_asm330lhh_hw *hw, u8 addr, u8 mask,

				 u8 val);

int st_asm330lhh_get_odr_val(enum st_asm330lhh_sensor_id id, u16 odr, u8 *val);

ssize_t st_asm330lhh_flush_fifo(struct device *dev,

				struct device_attribute *attr,

				const char *buf, size_t size);

ssize_t st_asm330lhh_get_max_watermark(struct device *dev,

				       struct device_attribute *attr, char *buf);

ssize_t st_asm330lhh_get_watermark(struct device *dev,

				   struct device_attribute *attr, char *buf);

ssize_t st_asm330lhh_set_watermark(struct device *dev,

				   struct device_attribute *attr,

				   const char *buf, size_t size);

int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,

			       enum st_asm330lhh_fifo_mode fifo_mode);

int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw);

#endif /* ST_ASM330LHH_H */

driver/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c

0 → 100644
+559 −0
Original line number Diff line number Diff line 
/*

 * STMicroelectronics st_asm330lhh FIFO buffer library driver

 *

 * Copyright 2018 STMicroelectronics Inc.

 *

 * Lorenzo Bianconi <lorenzo.bianconi@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/events.h>

#include <asm/unaligned.h>

#include <linux/of.h>



#include "st_asm330lhh.h"



#define ST_ASM330LHH_REG_FIFO_THL_ADDR		0x07

#define ST_ASM330LHH_REG_FIFO_LEN_MASK		GENMASK(8, 0)

#define ST_ASM330LHH_REG_FIFO_MODE_MASK		GENMASK(2, 0)

#define ST_ASM330LHH_REG_DEC_TS_MASK		GENMASK(7, 6)

#define ST_ASM330LHH_REG_HLACTIVE_ADDR		0x12

#define ST_ASM330LHH_REG_HLACTIVE_MASK		BIT(5)

#define ST_ASM330LHH_REG_PP_OD_ADDR		0x12

#define ST_ASM330LHH_REG_PP_OD_MASK		BIT(4)

#define ST_ASM330LHH_REG_FIFO_DIFFL_ADDR	0x3a

#define ST_ASM330LHH_REG_TS0_ADDR		0x40

#define ST_ASM330LHH_REG_TS2_ADDR		0x42

#define ST_ASM330LHH_REG_FIFO_OUT_TAG_ADDR	0x78

#define ST_ASM330LHH_GYRO_TAG			0x01

#define ST_ASM330LHH_ACC_TAG			0x02

#define ST_ASM330LHH_TS_TAG			0x04



#define ST_ASM330LHH_TS_DELTA_NS		25000ULL /* 25us/LSB */



static inline s64 st_asm330lhh_get_time_ns(void)

{

	struct timespec ts;



	get_monotonic_boottime(&ts);

	return timespec_to_ns(&ts);

}



#define ST_ASM330LHH_EWMA_LEVEL			120

#define ST_ASM330LHH_EWMA_DIV			128

static inline s64 st_asm330lhh_ewma(s64 old, s64 new, int weight)

{

	s64 diff, incr;



	diff = new - old;

	incr = div_s64((ST_ASM330LHH_EWMA_DIV - weight) * diff,

		       ST_ASM330LHH_EWMA_DIV);



	return old + incr;

}



static inline int st_asm330lhh_reset_hwts(struct st_asm330lhh_hw *hw)

{

	u8 data = 0xaa;



	hw->ts = st_asm330lhh_get_time_ns();

	hw->ts_offset = hw->ts;

	hw->hw_ts_old = 0ull;

	hw->tsample = 0ull;



	return hw->tf->write(hw->dev, ST_ASM330LHH_REG_TS2_ADDR, sizeof(data),

			     &data);

}



int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,

			       enum st_asm330lhh_fifo_mode fifo_mode)

{

	int err;



	err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,

					   ST_ASM330LHH_REG_FIFO_MODE_MASK,

					   fifo_mode);

	if (err < 0)

		return err;



	hw->fifo_mode = fifo_mode;



	return 0;

}



static int st_asm330lhh_set_sensor_batching_odr(struct st_asm330lhh_sensor *sensor,

						bool enable)

{

	struct st_asm330lhh_hw *hw = sensor->hw;

	u8 data = 0;

	int err;



	if (enable) {

		err = st_asm330lhh_get_odr_val(sensor->id, sensor->odr, &data);

		if (err < 0)

			return err;

	}



	return st_asm330lhh_write_with_mask(hw,

					    sensor->batch_addr,

					    sensor->batch_mask, data);

}



static u16 st_asm330lhh_ts_odr(struct st_asm330lhh_hw *hw)

{

	struct st_asm330lhh_sensor *sensor;

	u16 odr = 0;

	u8 i;



	for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {

		if (!hw->iio_devs[i])

			continue;



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

		if (hw->enable_mask & BIT(sensor->id))

			odr = max_t(u16, odr, sensor->odr);

	}



	return odr;

}



static int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,

					 u16 watermark)

{

	u16 fifo_watermark = ST_ASM330LHH_MAX_FIFO_DEPTH, cur_watermark = 0;

	struct st_asm330lhh_hw *hw = sensor->hw;

	struct st_asm330lhh_sensor *cur_sensor;

	__le16 wdata;

	int i, err;

	u8 data;



	for (i = 0; i < ST_ASM330LHH_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);

	}



	fifo_watermark = max_t(u16, fifo_watermark, 2);

	mutex_lock(&hw->lock);



	err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_THL_ADDR + 1,

			   sizeof(data), &data);

	if (err < 0)

		goto out;



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

			 (fifo_watermark & ST_ASM330LHH_REG_FIFO_LEN_MASK);

	wdata = cpu_to_le16(fifo_watermark);

	err = hw->tf->write(hw->dev, ST_ASM330LHH_REG_FIFO_THL_ADDR,

			    sizeof(wdata), (u8 *)&wdata);



out:

	mutex_unlock(&hw->lock);



	return err < 0 ? err : 0;

}



static inline void st_asm330lhh_sync_hw_ts(struct st_asm330lhh_hw *hw, s64 ts)

{

	s64 delta = ts - hw->hw_ts;



	hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, delta,

					  ST_ASM330LHH_EWMA_LEVEL);

}



static struct iio_dev *st_asm330lhh_get_iiodev_from_tag(struct st_asm330lhh_hw *hw,

							u8 tag)

{

	struct iio_dev *iio_dev;



	switch (tag) {

	case ST_ASM330LHH_GYRO_TAG:

		iio_dev = hw->iio_devs[ST_ASM330LHH_ID_GYRO];

		break;

	case ST_ASM330LHH_ACC_TAG:

		iio_dev = hw->iio_devs[ST_ASM330LHH_ID_ACC];

		break;

	default:

		iio_dev = NULL;

		break;

	}



	return iio_dev;

}



static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)

{

	u8 iio_buf[ALIGN(ST_ASM330LHH_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];

	u8 buf[6 * ST_ASM330LHH_FIFO_SAMPLE_SIZE], tag, *ptr;

	s64 ts_delta_hw_ts = 0, ts_irq;

	s64 ts_delta_offs;

	int i, err, read_len, word_len, fifo_len;

	struct st_asm330lhh_sensor *sensor;

	struct iio_dev *iio_dev;

	__le16 fifo_status;

	u16 fifo_depth;

	u32 val;

	int ts_processed = 0;

	s64 hw_ts = 0ull, delta_hw_ts, cpu_timestamp;



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



	do

	{

		err = hw->tf->read(hw->dev, ST_ASM330LHH_REG_FIFO_DIFFL_ADDR,

				   sizeof(fifo_status), (u8 *)&fifo_status);

		if (err < 0)

			return err;



		fifo_depth = le16_to_cpu(fifo_status) & ST_ASM330LHH_REG_FIFO_LEN_MASK;

		if (!fifo_depth)

			return 0;



		read_len = 0;

		fifo_len = fifo_depth * ST_ASM330LHH_FIFO_SAMPLE_SIZE;

		while (read_len < fifo_len) {

			word_len = min_t(int, fifo_len - read_len, sizeof(buf));

			err = hw->tf->read(hw->dev,

					   ST_ASM330LHH_REG_FIFO_OUT_TAG_ADDR,

					   word_len, buf);

			if (err < 0)

				return err;



			for (i = 0; i < word_len; i += ST_ASM330LHH_FIFO_SAMPLE_SIZE) {

				ptr = &buf[i + ST_ASM330LHH_TAG_SIZE];

				tag = buf[i] >> 3;



				if (tag == ST_ASM330LHH_TS_TAG) {

					val = get_unaligned_le32(ptr);

					hw->hw_ts = val * ST_ASM330LHH_TS_DELTA_NS;

					ts_delta_hw_ts = hw->hw_ts - hw->hw_ts_old;

					hw_ts += ts_delta_hw_ts;

					ts_delta_offs =

						div_s64(hw->delta_hw_ts * ST_ASM330LHH_MAX_ODR, hw->odr);



					hw->ts_offset = st_asm330lhh_ewma(hw->ts_offset, ts_irq -

						hw->hw_ts + ts_delta_offs, ST_ASM330LHH_EWMA_LEVEL);



					ts_irq += (hw->hw_ts + ts_delta_offs);

					hw->hw_ts_old = hw->hw_ts;

					ts_processed++;



					if (!hw->tsample)

						hw->tsample =

							hw->ts_offset + (hw->hw_ts + ts_delta_offs);

					else

						hw->tsample =

							hw->tsample + (ts_delta_hw_ts + ts_delta_offs);

				} else {

					iio_dev = st_asm330lhh_get_iiodev_from_tag(hw, tag);

					if (!iio_dev)

						continue;



					sensor = iio_priv(iio_dev);

					if (sensor->std_samples < sensor->std_level) {

						sensor->std_samples++;

						continue;

					}



					sensor = iio_priv(iio_dev);



					/* Check if timestamp is in the future. */

					cpu_timestamp = st_asm330lhh_get_time_ns();



					/* Avoid samples in the future. */

					if (hw->tsample > cpu_timestamp)

						hw->tsample = cpu_timestamp;



					memcpy(iio_buf, ptr, ST_ASM330LHH_SAMPLE_SIZE);

					iio_push_to_buffers_with_timestamp(iio_dev,

									   iio_buf,

									   hw->tsample);

				}

			}

			read_len += word_len;

		}



		delta_hw_ts = div_s64(hw->delta_ts - hw_ts, ts_processed);

		delta_hw_ts = div_s64(delta_hw_ts * hw->odr, ST_ASM330LHH_MAX_ODR);

		hw->delta_hw_ts = st_asm330lhh_ewma(hw->delta_hw_ts,

							delta_hw_ts,

							ST_ASM330LHH_EWMA_LEVEL);

	} while(read_len);



	return read_len;

}



ssize_t st_asm330lhh_get_max_watermark(struct device *dev,

				       struct device_attribute *attr, char *buf)

{

	return sprintf(buf, "%d\n", ST_ASM330LHH_MAX_FIFO_DEPTH);

}



ssize_t st_asm330lhh_get_watermark(struct device *dev,

				   struct device_attribute *attr, char *buf)

{

	struct iio_dev *iio_dev = dev_get_drvdata(dev);

	struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);



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

}



ssize_t st_asm330lhh_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_asm330lhh_sensor *sensor = iio_priv(iio_dev);

	int err, val;



	mutex_lock(&iio_dev->mlock);

	if (iio_buffer_enabled(iio_dev)) {

		err = -EBUSY;

		goto out;

	}



	err = kstrtoint(buf, 10, &val);

	if (err < 0)

		goto out;



	err = st_asm330lhh_update_watermark(sensor, val);

	if (err < 0)

		goto out;



	sensor->watermark = val;



out:

	mutex_unlock(&iio_dev->mlock);



	return err < 0 ? err : size;

}



ssize_t st_asm330lhh_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_asm330lhh_sensor *sensor = iio_priv(iio_dev);

	struct st_asm330lhh_hw *hw = sensor->hw;

	s64 type, event;

	int count;

	s64 ts;



	mutex_lock(&hw->fifo_lock);

	ts = st_asm330lhh_get_time_ns();

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

	hw->ts = ts;

	set_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);



	count = st_asm330lhh_read_fifo(hw);



	mutex_unlock(&hw->fifo_lock);



	type = count > 0 ? CUSTOM_IIO_EV_DIR_FIFO_DATA : CUSTOM_IIO_EV_DIR_FIFO_EMPTY;

	event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1,

				     CUSTOM_IIO_EV_TYPE_FIFO_FLUSH, type);

	iio_push_event(iio_dev, event, st_asm330lhh_get_time_ns());



	return size;

}



int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw)

{

	int err;



	mutex_lock(&hw->fifo_lock);



	st_asm330lhh_read_fifo(hw);

	err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_BYPASS);



	mutex_unlock(&hw->fifo_lock);



	return err;

}



static int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable)

{

	struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);

	struct st_asm330lhh_hw *hw = sensor->hw;

	int err;



	mutex_lock(&hw->fifo_lock);



	err = st_asm330lhh_sensor_set_enable(sensor, enable);

	if (err < 0)

		goto out;



	err = st_asm330lhh_set_sensor_batching_odr(sensor, enable);

	if (err < 0)

		goto out;



	err = st_asm330lhh_update_watermark(sensor, sensor->watermark);

	if (err < 0)

		goto out;



	hw->odr = st_asm330lhh_ts_odr(hw);



	if (enable && hw->fifo_mode == ST_ASM330LHH_FIFO_BYPASS) {

		st_asm330lhh_reset_hwts(hw);

		err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_CONT);

	} else if (!hw->enable_mask) {

		err = st_asm330lhh_set_fifo_mode(hw, ST_ASM330LHH_FIFO_BYPASS);

	}



out:

	mutex_unlock(&hw->fifo_lock);



	return err;

}



static irqreturn_t st_asm330lhh_handler_irq(int irq, void *private)

{

	struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;

	s64 ts = st_asm330lhh_get_time_ns();



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

	hw->ts = ts;



	return IRQ_WAKE_THREAD;

}



static irqreturn_t st_asm330lhh_handler_thread(int irq, void *private)

{

	struct st_asm330lhh_hw *hw = (struct st_asm330lhh_hw *)private;



	mutex_lock(&hw->fifo_lock);



	st_asm330lhh_read_fifo(hw);

	clear_bit(ST_ASM330LHH_HW_FLUSH, &hw->state);



	mutex_unlock(&hw->fifo_lock);



	return IRQ_HANDLED;

}



static int st_asm330lhh_buffer_preenable(struct iio_dev *iio_dev)

{

	return st_asm330lhh_update_fifo(iio_dev, true);

}



static int st_asm330lhh_buffer_postdisable(struct iio_dev *iio_dev)

{

	return st_asm330lhh_update_fifo(iio_dev, false);

}



static const struct iio_buffer_setup_ops st_asm330lhh_buffer_ops = {

	.preenable = st_asm330lhh_buffer_preenable,

	.postdisable = st_asm330lhh_buffer_postdisable,

};



static int st_asm330lhh_fifo_init(struct st_asm330lhh_hw *hw)

{

	return st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_FIFO_CTRL4_ADDR,

					    ST_ASM330LHH_REG_DEC_TS_MASK, 1);

}



int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw)

{

	struct device_node *np = hw->dev->of_node;

	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 = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_HLACTIVE_ADDR,

					   ST_ASM330LHH_REG_HLACTIVE_MASK,

					   irq_active_low);

	if (err < 0)

		return err;



	if (np && of_property_read_bool(np, "drive-open-drain")) {

		err = st_asm330lhh_write_with_mask(hw,

					ST_ASM330LHH_REG_PP_OD_ADDR,

					ST_ASM330LHH_REG_PP_OD_MASK, 1);

		if (err < 0)

			return err;



		irq_type |= IRQF_SHARED;

	}



	err = devm_request_threaded_irq(hw->dev, hw->irq,

					st_asm330lhh_handler_irq,

					st_asm330lhh_handler_thread,

					irq_type | IRQF_ONESHOT,

					"asm330lhh", hw);

	if (err) {

		dev_err(hw->dev, "failed to request trigger irq %d\n",

			hw->irq);

		return err;

	}



	for (i = ST_ASM330LHH_ID_ACC; i < ST_ASM330LHH_ID_MAX; i++) {

		if (!hw->iio_devs[i])

			continue;



		buffer = iio_kfifo_allocate(hw->iio_devs[i]);

		if (!buffer)

			return -ENOMEM;



		iio_device_attach_buffer(hw->iio_devs[i], buffer);

		hw->iio_devs[i]->modes |= INDIO_BUFFER_HARDWARE;

		hw->iio_devs[i]->setup_ops = &st_asm330lhh_buffer_ops;

		err = iio_buffer_register(hw->iio_devs[i],

					  hw->iio_devs[i]->channels,

					  hw->iio_devs[i]->num_channels);

		if (err)

			goto fifo_allocate_error;

	}



	return st_asm330lhh_fifo_init(hw);



fifo_allocate_error:

	for (i--; i >= ST_ASM330LHH_ID_GYRO; i--)

		iio_buffer_unregister(hw->iio_devs[i]);



	for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {

		if (!hw->iio_devs[i]->buffer)

			continue;

		iio_kfifo_free(hw->iio_devs[i]->buffer);

	}



	return err;

}



int st_asm330lhh_deallocate_fifo(struct st_asm330lhh_hw *hw)

{

	int i;



	for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {

		iio_buffer_unregister(hw->iio_devs[i]);

		iio_kfifo_free(hw->iio_devs[i]->buffer);

	}



	return 0;

}