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

Commit 70efa4be authored by Linux Build Service Account's avatar Linux Build Service Account Committed by Gerrit - the friendly Code Review server
Browse files

Merge "drivers: iio: imu: Fix cpu latency issue for asm330 irq"

parents eee689c2 ef8d7656

drivers/iio/imu/st_asm330lhh/st_asm330lhh.h

+42 −12
Original line number Diff line number Diff line
@@ -43,6 +43,9 @@ 


#define ST_ASM330LHH_MAX_ODR			416



/* Timestamp Tick 25us/LSB */

#define ST_ASM330LHH_TS_DELTA_NS		25000ULL



/* Define Custom events for FIFO flush */

#define CUSTOM_IIO_EV_DIR_FIFO_EMPTY (IIO_EV_DIR_NONE + 1)

#define CUSTOM_IIO_EV_DIR_FIFO_DATA (IIO_EV_DIR_NONE + 2)
@@ -82,8 +85,8 @@ static const struct iio_event_spec st_asm330lhh_flush_event = { 
	.num_event_specs = 1,				\

}



#define ST_ASM330LHH_RX_MAX_LENGTH	8

#define ST_ASM330LHH_TX_MAX_LENGTH	8

#define ST_ASM330LHH_RX_MAX_LENGTH	64

#define ST_ASM330LHH_TX_MAX_LENGTH	16



#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING

#define ASM_MAXSAMPLE        4000
@@ -197,6 +200,7 @@ struct st_asm330lhh_sensor { 
 * @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.

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

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

 * @state: hw operational state.

 * @enable_mask: Enabled sensor bitmask.
@@ -204,6 +208,13 @@ struct st_asm330lhh_sensor { 
 * @hw_ts: Latest hw timestamp from the sensor.

 * @ts: Latest timestamp from irq handler.

 * @delta_ts: Delta time between two consecutive interrupts.

 * @ts_delta_ns: Calibrate delta time tick.

 * @hw_ts: Latest hw timestamp from the sensor.

 * @val_ts_old: Hold hw timestamp for timer rollover.

 * @hw_ts_high: Save MSB hw timestamp.

 * @tsample: Timestamp for each sensor sample.

 * @delta_ts: Delta time between two consecutive interrupts.

 * @ts: Latest timestamp from irq handler.

 * @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.
@@ -214,34 +225,51 @@ struct st_asm330lhh_hw { 


	struct mutex lock;

	struct mutex fifo_lock;

	struct mutex page_lock;



	enum st_asm330lhh_fifo_mode fifo_mode;

	unsigned long state;

	u8 enable_mask;



	s64 ts_offset;

	u32 hw_val;

	u32 hw_val_old;

	u64 ts_delta_ns;

	s64 hw_ts;

	s64 hw_ts_high;

	u32 val_ts_old;

	u32 hw_ts_high;

	s64 tsample;

	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;

	int enable_gpio;

	int asm330_hrtimer;

	struct hrtimer st_asm330lhh_hrtimer;

};



extern const struct dev_pm_ops st_asm330lhh_pm_ops;



static inline int st_asm330lhh_read_atomic(struct st_asm330lhh_hw *hw, u8 addr,

					 int len, u8 *data)

{

	int err;



	mutex_lock(&hw->page_lock);

	err = hw->tf->read(hw->dev, addr, len, data);

	mutex_unlock(&hw->page_lock);



	return err;

}



static inline s64 st_asm330lhh_get_time_ns(void)

{

	struct timespec ts;



	get_monotonic_boottime(&ts);

	return timespec_to_ns(&ts);

}



int st_asm330lhh_probe(struct device *dev, int irq,

		       const struct st_asm330lhh_transfer_function *tf_ops);

int st_asm330lhh_sensor_set_enable(struct st_asm330lhh_sensor *sensor,
@@ -268,4 +296,6 @@ int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor, 
int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable);

int asm330_check_acc_gyro_early_buff_enable_flag(

		struct st_asm330lhh_sensor *sensor);

void st_asm330lhh_set_cpu_idle_state(bool value);

void st_asm330lhh_hrtimer_reset(struct st_asm330lhh_hw *hw, s64 irq_delta_ts);

#endif /* ST_ASM330LHH_H */

drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c

+134 −142
Original line number Diff line number Diff line 
/*

 * STMicroelectronics st_asm330lhh FIFO buffer library driver

 *

 * Copyright 2018 STMicroelectronics Inc.

 * Copyright 2020 STMicroelectronics Inc.

 *

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

 *
@@ -10,41 +10,41 @@ 
#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/buffer.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_STATUS_DIFF	GENMASK(9, 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_FIFO_STATUS1_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_SAMPLE_DISCHARD		0x7ffd



/* Timestamp convergence filter parameter */

#define ST_ASM330LHH_EWMA_LEVEL			120

#define ST_ASM330LHH_EWMA_DIV			128



enum {

	ST_ASM330LHH_GYRO_TAG = 0x01,

	ST_ASM330LHH_ACC_TAG = 0x02,

	ST_ASM330LHH_TS_TAG = 0x04,

};



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

{

	s64 diff, incr;
@@ -62,10 +62,12 @@ static inline int st_asm330lhh_reset_hwts(struct st_asm330lhh_hw *hw) 


	hw->ts = st_asm330lhh_get_time_ns();

	hw->ts_offset = hw->ts;

	hw->hw_ts_old = 0ull;

	hw->val_ts_old = 0;

	hw->hw_ts_high = 0;

	hw->tsample = 0ull;

	hw->hw_ts_high = 0ull;

	hw->hw_val_old = 0ull;



	if (hw->asm330_hrtimer)

		st_asm330lhh_set_cpu_idle_state(true);



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

			     &data);
@@ -84,6 +86,11 @@ int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw, 


	hw->fifo_mode = fifo_mode;



	if (fifo_mode == ST_ASM330LHH_FIFO_BYPASS)

		clear_bit(ST_ASM330LHH_HW_OPERATIONAL, &hw->state);

	else

		set_bit(ST_ASM330LHH_HW_OPERATIONAL, &hw->state);



	return 0;

}
@@ -105,24 +112,6 @@ static int st_asm330lhh_set_sensor_batching_odr(struct st_asm330lhh_sensor *sens 
					    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;

}



int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,

					 u16 watermark)

{
@@ -134,6 +123,9 @@ int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor, 
	u8 data;



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

		if (!hw->iio_devs[i])

			continue;



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



		if (!(hw->enable_mask & BIT(cur_sensor->id)))
@@ -146,6 +138,7 @@ int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor, 
	}



	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,
@@ -165,14 +158,6 @@ int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor, 
	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)

{
@@ -248,37 +233,51 @@ static void store_acc_gyro_boot_sample(struct st_asm330lhh_sensor *sensor, 
}

#endif



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 int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)

{

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

	u8 buf[30 * 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;

	int i, err, word_len, fifo_len, read_len;

	struct iio_dev *iio_dev;

	struct st_asm330lhh_sensor *sensor;

	s64 ts_irq, hw_ts_old;

	__le16 fifo_status;

	u16 fifo_depth;

	int ts_processed = 0;

	s64 hw_ts = 0ull, delta_hw_ts, cpu_timestamp;

	s16 drdymask;

	u32 val;



	/* return if FIFO is already disabled */

	if (!test_bit(ST_ASM330LHH_HW_OPERATIONAL, &hw->state)) {

		dev_warn(hw->dev, "%s: FIFO in bypass mode\n", __func__);



		return 0;

	}



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



	do {

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

	err = st_asm330lhh_read_atomic(hw, ST_ASM330LHH_REG_FIFO_STATUS1_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;

	fifo_depth = le16_to_cpu(fifo_status) &

		ST_ASM330LHH_REG_FIFO_STATUS_DIFF;

	if (!fifo_depth)

		return 0;



		read_len = 0;

	fifo_len = fifo_depth * ST_ASM330LHH_FIFO_SAMPLE_SIZE;

	read_len = 0;

	while (read_len < fifo_len) {

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

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

		err = st_asm330lhh_read_atomic(hw,

					     ST_ASM330LHH_REG_FIFO_OUT_TAG_ADDR,

					     word_len, buf);

		if (err < 0)
@@ -289,54 +288,54 @@ static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw) 
			tag = buf[i] >> 3;



			if (tag == ST_ASM330LHH_TS_TAG) {

					hw->hw_val = get_unaligned_le32(ptr);

				val = get_unaligned_le32(ptr);



					/* check for timer rollover */

					if (hw->hw_val < hw->hw_val_old)

				if (hw->val_ts_old > val)

					hw->hw_ts_high++;

					hw->hw_ts =

					(hw->hw_val + (hw->hw_ts_high << 32))

						* 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;

					hw->hw_val_old = hw->hw_val;

					ts_processed++;



				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_asm330lhh_ewma(hw->ts_offset,

						ts_irq - hw->hw_ts,

						ST_ASM330LHH_EWMA_LEVEL);



				if (!test_bit(ST_ASM330LHH_HW_FLUSH,

							&hw->state))

					/* sync ap timestamp and sensor one */

					st_asm330lhh_sync_hw_ts(hw, ts_irq);



				ts_irq += hw->hw_ts;



				if (!hw->tsample)

						hw->tsample =

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

					hw->tsample = hw->ts_offset + hw->hw_ts;

				else

						hw->tsample =

							hw->tsample + (ts_delta_hw_ts + ts_delta_offs);

					hw->tsample = hw->tsample +

						hw->hw_ts - hw_ts_old;

			} else {

					iio_dev = st_asm330lhh_get_iiodev_from_tag(hw, tag);

				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++;



				/* skip samples if not ready */

				drdymask = (s16)le16_to_cpu(

						get_unaligned_le16(ptr));

				if (unlikely(drdymask

					>= ST_ASM330LHH_SAMPLE_DISCHARD)) {

					continue;

				}



					sensor = iio_priv(iio_dev);



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

					cpu_timestamp = st_asm330lhh_get_time_ns();

				memcpy(iio_buf, ptr, ST_ASM330LHH_SAMPLE_SIZE);



					/* Avoid samples in the future. */

					if (hw->tsample > cpu_timestamp)

						hw->tsample = cpu_timestamp;

				hw->tsample = min_t(s64,

						    hw->ts,

						    hw->tsample);



					memcpy(iio_buf, ptr, ST_ASM330LHH_SAMPLE_SIZE);

				iio_push_to_buffers_with_timestamp(iio_dev,

								   iio_buf,

								   hw->tsample);
@@ -347,13 +346,6 @@ static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw) 
		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;

}
@@ -412,8 +404,9 @@ ssize_t st_asm330lhh_flush_fifo(struct device *dev, 
	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;

	s64 event;

	int count;

	s64 type;

	s64 ts;



	mutex_lock(&hw->fifo_lock);
@@ -421,9 +414,7 @@ ssize_t st_asm330lhh_flush_fifo(struct device *dev, 
	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;
@@ -439,10 +430,8 @@ 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;
@@ -468,8 +457,6 @@ int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable) 
	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);
@@ -491,6 +478,9 @@ static irqreturn_t st_asm330lhh_handler_irq(int irq, void *private) 
	hw->delta_ts = ts - hw->ts;

	hw->ts = ts;



	if (hw->asm330_hrtimer)

		st_asm330lhh_hrtimer_reset(hw, hw->delta_ts);



	return IRQ_WAKE_THREAD;

}
@@ -499,12 +489,13 @@ 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);



	if (hw->asm330_hrtimer)

		st_asm330lhh_set_cpu_idle_state(false);



	return IRQ_HANDLED;

}
@@ -548,6 +539,8 @@ int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw) 
	int i, 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:
@@ -605,4 +598,3 @@ int st_asm330lhh_fifo_setup(struct st_asm330lhh_hw *hw) 


	return st_asm330lhh_fifo_init(hw);

}

drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c

+81 −2
Original line number Diff line number Diff line
@@ -17,6 +17,7 @@ 
#include <linux/version.h>

#include <linux/of.h>

#include <linux/of_gpio.h>

#include <linux/cpu.h>



#include <linux/platform_data/st_sensors_pdata.h>
@@ -26,18 +27,24 @@ 
#define ST_ASM330LHH_REG_INT2_ADDR		0x0e

#define ST_ASM330LHH_REG_FIFO_CTRL4_ADDR	0x0a

#define ST_ASM330LHH_REG_FIFO_FTH_IRQ_MASK	BIT(3)



#define ST_ASM330LHH_REG_WHOAMI_ADDR		0x0f

#define ST_ASM330LHH_WHOAMI_VAL			0x6b

#define ST_ASM330LHH_REG_CTRL1_XL_ADDR		0x10

#define ST_ASM330LHH_REG_CTRL2_G_ADDR		0x11



#define ST_ASM330LHH_REG_RESET_ADDR		0x12

#define ST_ASM330LHH_REG_RESET_MASK		BIT(0)



#define ST_ASM330LHH_REG_BDU_ADDR		0x12

#define ST_ASM330LHH_REG_BDU_MASK		BIT(6)

#define ST_ASM330LHH_REG_INT2_ON_INT1_ADDR	0x13

#define ST_ASM330LHH_REG_INT2_ON_INT1_MASK	BIT(5)



#define ST_ASM330LHH_REG_CTRL4_C_ADDR		0x13

#define ST_ASM330LHH_REG_DRDY_MASK		BIT(3)



#define ST_ASM330LHH_REG_ROUNDING_ADDR		0x14

#define ST_ASM330LHH_REG_ROUNDING_MASK		GENMASK(6, 5)



#define ST_ASM330LHH_REG_TIMESTAMP_EN_ADDR	0x19

#define ST_ASM330LHH_REG_TIMESTAMP_EN_MASK	BIT(5)
@@ -64,6 +71,8 @@ 
#define ST_ASM330LHH_GYRO_FS_2000_GAIN		IIO_DEGREE_TO_RAD(70000)

#define ST_ASM330LHH_GYRO_FS_4000_GAIN		IIO_DEGREE_TO_RAD(140000)



#define ST_ASM330LHH_INTERNAL_FREQ_FINE		0x63



/* Temperature in uC */

#define ST_ASM330LHH_TEMP_GAIN			256

#define ST_ASM330LHH_TEMP_FS_GAIN		(1000000 / ST_ASM330LHH_TEMP_GAIN)
@@ -184,6 +193,33 @@ static const struct iio_chan_spec st_asm330lhh_temp_channels[] = { 
	},

};



void st_asm330lhh_set_cpu_idle_state(bool value)

{

	cpu_idle_poll_ctrl(value);

}

static enum hrtimer_restart st_asm330lhh_timer_function(

		struct hrtimer *timer)

{

	st_asm330lhh_set_cpu_idle_state(true);



	return HRTIMER_NORESTART;

}

void st_asm330lhh_hrtimer_reset(struct st_asm330lhh_hw *hw,

		s64 irq_delta_ts)

{

	hrtimer_cancel(&hw->st_asm330lhh_hrtimer);

	/*forward HRTIMER just before 1ms of irq arrival*/

	hrtimer_forward(&hw->st_asm330lhh_hrtimer, ktime_get(),

			ns_to_ktime(irq_delta_ts - 1000000));

	hrtimer_restart(&hw->st_asm330lhh_hrtimer);

}

static void st_asm330lhh_hrtimer_init(struct st_asm330lhh_hw *hw)

{

	hrtimer_init(&hw->st_asm330lhh_hrtimer, CLOCK_MONOTONIC,

			HRTIMER_MODE_REL);

	hw->st_asm330lhh_hrtimer.function = st_asm330lhh_timer_function;

}



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

				 u8 val)

{
@@ -230,6 +266,30 @@ static int st_asm330lhh_check_whoami(struct st_asm330lhh_hw *hw) 
	return 0;

}



static int st_asm330lhh_get_odr_calibration(struct st_asm330lhh_hw *hw)

{

	s64 odr_calib;

	int err;

	s8 data;



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

			   ST_ASM330LHH_INTERNAL_FREQ_FINE,

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

	if (err < 0) {

		dev_err(hw->dev, "failed to read %d register\n",

				ST_ASM330LHH_INTERNAL_FREQ_FINE);

		return err;

	}



	odr_calib = (data * 37500) / 1000;

	hw->ts_delta_ns = ST_ASM330LHH_TS_DELTA_NS - odr_calib;



	dev_info(hw->dev, "Freq Fine %lld (ts %lld)\n",

			odr_calib, hw->ts_delta_ns);



	return 0;

}



static int st_asm330lhh_set_full_scale(struct st_asm330lhh_sensor *sensor,

				       u32 gain)

{
@@ -775,6 +835,12 @@ static int st_asm330lhh_init_device(struct st_asm330lhh_hw *hw) 
	if (err < 0)

		return err;



	/* enable DRDY MASK for filters settling time */

	err = st_asm330lhh_write_with_mask(hw, ST_ASM330LHH_REG_CTRL4_C_ADDR,

					 ST_ASM330LHH_REG_DRDY_MASK, 1);

	if (err < 0)

		return err;



	/* enable FIFO watermak interrupt */

	err = st_asm330lhh_get_drdy_reg(hw, &drdy_int_reg);

	if (err < 0)
@@ -1043,6 +1109,7 @@ int st_asm330lhh_probe(struct device *dev, int irq, 


	mutex_init(&hw->lock);

	mutex_init(&hw->fifo_lock);

	mutex_init(&hw->page_lock);



	hw->dev = dev;

	hw->irq = irq;
@@ -1062,11 +1129,20 @@ int st_asm330lhh_probe(struct device *dev, int irq, 
		msleep(ST_ASM330LHH_TURN_ON_TIME);

	}



	/* use hrtimer if property is enabled */

	if (of_property_read_u32(np, "qcom,asm330_hrtimer",

				&hw->asm330_hrtimer))

		hw->asm330_hrtimer = 0; //force to 0 if not in dt



	dev_info(hw->dev, "Ver: %s\n", ST_ASM330LHH_VERSION);

	err = st_asm330lhh_check_whoami(hw);

	if (err < 0)

		return err;



	err = st_asm330lhh_get_odr_calibration(hw);

	if (err < 0)

		return err;



	err = st_asm330lhh_init_device(hw);

	if (err < 0)

		return err;
@@ -1096,6 +1172,9 @@ int st_asm330lhh_probe(struct device *dev, int irq, 
	if (!err)

		return err;



	if (hw->asm330_hrtimer)

		st_asm330lhh_hrtimer_init(hw);



	st_asm330lhh_enable_acc_gyro(hw);



	dev_info(hw->dev, "probe ok\n");