diff mbox series

[v2] iio: imu: mpu6050: add watermark support for icm20602/20690

Message ID 20210924123124.25831-1-bmansuy@invensense.com (mailing list archive)
State Changes Requested
Headers show
Series [v2] iio: imu: mpu6050: add watermark support for icm20602/20690 | expand

Commit Message

Baptiste Mansuy Sept. 24, 2021, 12:31 p.m. UTC
Add watermark support using FIFO interrupt for chips having this
feature. This allows the use of the watermark interrupt with the
posibility to change it's size. Change the timestamp computation
to be used with the watermark.

Add the set_watermark and flush callbacks according to the iio
description.

Signed-off-by: Baptiste Mansuy <bmansuy@invensense.com>
---
 V1 -> V2: Changed watermark computation following the latest ABI
 
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    | 195 +++++++++++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  23 ++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 138 ++++++++++---
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c |  24 ++-
 4 files changed, 339 insertions(+), 41 deletions(-)

Comments

Jonathan Cameron Sept. 26, 2021, 11:57 a.m. UTC | #1
On Fri, 24 Sep 2021 14:31:24 +0200
Baptiste Mansuy <bmansuy@invensense.com> wrote:

> Add watermark support using FIFO interrupt for chips having this
> feature. This allows the use of the watermark interrupt with the
> posibility to change it's size. Change the timestamp computation
> to be used with the watermark.
> 
> Add the set_watermark and flush callbacks according to the iio
> description.
> 
> Signed-off-by: Baptiste Mansuy <bmansuy@invensense.com>

Hi Baptiste,

From discussion on v1 and the changes made here I think we've ended up
inconsistent and we should avoid that even if it means that in some corner
cases perhaps we aren't operating in the best possible mode.
Probably my fault that I wasn't clear in v1 discussion.

So I think you need to just pretend the hwfifo limit is always 7/8 of the true
value  and clamp to that.

I don't really like the 'hack' nature of clamping the value but without
some rather more sophisticated code taking into account 99.9999% latency
or similar perhaps it's the best we can do :(

Thanks,

Jonathan
> ---
>  V1 -> V2: Changed watermark computation following the latest ABI
>  
...


> +int inv_mpu6050_compute_hw_watermark(struct inv_mpu6050_state *st, unsigned int val)
> +{
> +	unsigned int nb_byte, new_size_byte;
> +	int ret;
> +
> +	nb_byte = inv_mpu6050_get_sample_size(st);
> +
> +	/*
> +	 * Compute watermark size.
> +	 *
> +	 * Use a threshold when watermark size is not supported by the chip
> +	 * to prevent overwriting the first samples of the fifo.
> +	 * The threshold creates latency for the computing of the interrupt.
> +	 * When using a sampling frequency of 500Hz, the max latency is 12ms.
> +	 */
> +	new_size_byte = val * nb_byte;
> +	if (new_size_byte > st->hw->fifo_size) {

This is a little odd. You report a max size of st->hw->fifo_size/nb_byte, and indeed
allow the value to go up to that.  However if the value set is larger than that value
you reduce the hw fifo down to 7/8 of that value.

I'm not against enforcing this lower maximum fifo size but I think you should
report it as the max possible for consistency and clamp to that.


> +		val = ((st->hw->fifo_size * 7) / 8) / nb_byte;
> +		new_size_byte = val * nb_byte;
> +	}
> +
> +	ret = inv_mpu6050_set_hw_watermark(st, new_size_byte);
> +	if (ret)
> +		return ret;
> +
> +	st->chip_config.wm_val = val;
> +
> +	return 0;
> +}
> +
>  static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,
>  					int clock, int temp_dis)
>  {
> @@ -1018,6 +1096,70 @@ static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
>  	}
>  }
>  
> +/*
> + * inv_mpu6050_get_fifo_watermark_max() - Get the max watermark size of current chip.
> + */
> +
> +static ssize_t
> +inv_mpu6050_get_fifo_watermark_max(struct device *dev, struct device_attribute *attr,
> +		   char *buf)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
> +	unsigned int nb_byte;
> +	size_t watermark_max_size;
> +
> +	mutex_lock(&st->lock);
> +	nb_byte = inv_mpu6050_get_sample_size(st);
> +
> +	/* Use default value of accel + gyro + temp sample */
> +	if (nb_byte == 0)
> +		nb_byte = 2 * INV_MPU6050_BYTES_PER_3AXIS_SENSOR
> +				+ INV_MPU6050_BYTES_PER_TEMP_SENSOR;
> +	watermark_max_size = st->hw->fifo_size / nb_byte;

As above - I think you should have a consistent max rather than one that is dependent
on whether the kfifo size is larger than the hw fifo can handle or not.

> +	mutex_unlock(&st->lock);
> +
> +	return scnprintf(buf, PAGE_SIZE, "%zu\n", watermark_max_size);
> +}
> +
> +/*
> + * inv_mpu6050_get_fifo_state() - Get the state of the hwfifo.
> + *
> + * Watermark is disabled when the watermark size is set to 0
> + */
> +
> +static ssize_t
> +inv_mpu6050_get_fifo_state(struct device *dev, struct device_attribute *attr,
> +		   char *buf)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
> +	unsigned int fifo_state = 0;
> +
> +	mutex_lock(&st->lock);
> +	if (st->chip_config.wm_size > 0)
> +		fifo_state = 1;
> +	mutex_unlock(&st->lock);
> +
> +	return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_state);
> +}
> +
> +/*
> + * inv_mpu6050_get_fifo_watermark() - Get the current watermark size.
> + */
> +
> +static ssize_t
> +inv_mpu6050_get_fifo_watermark(struct device *dev, struct device_attribute *attr,
> +		   char *buf)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
> +	unsigned int watermark;
> +
> +	mutex_lock(&st->lock);
> +	watermark = st->chip_config.wm_size;
> +	mutex_unlock(&st->lock);
> +
> +	return scnprintf(buf, PAGE_SIZE, "%u\n", watermark);
> +}
> +
>  /**
>   * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense
>   *                                  MPU6050 device.
> @@ -1258,6 +1400,14 @@ static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL,
>  static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL,
>  	ATTR_ACCL_MATRIX);
>  
> +static IIO_CONST_ATTR(hwfifo_watermark_min, "1");
> +static IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO,
> +	inv_mpu6050_get_fifo_watermark_max, NULL, 0);
> +static IIO_DEVICE_ATTR(hwfifo_enabled, S_IRUGO,
> +	inv_mpu6050_get_fifo_state, NULL, 0);
> +static IIO_DEVICE_ATTR(hwfifo_watermark, S_IRUGO,
> +	inv_mpu6050_get_fifo_watermark, NULL, 0);
> +
>  static struct attribute *inv_attributes[] = {
>  	&iio_dev_attr_in_gyro_matrix.dev_attr.attr,  /* deprecated */
>  	&iio_dev_attr_in_accel_matrix.dev_attr.attr, /* deprecated */
> @@ -1265,6 +1415,10 @@ static struct attribute *inv_attributes[] = {
>  	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
>  	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
>  	&iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
> +	&iio_const_attr_hwfifo_watermark_min.dev_attr.attr,
> +	&iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
> +	&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
> +	&iio_dev_attr_hwfifo_enabled.dev_attr.attr,
>  	NULL,
>  };
diff mbox series

Patch

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 597768c..2f70991 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -47,7 +47,9 @@  static const struct inv_mpu6050_reg_map reg_set_icm20602 = {
 	.raw_accl               = INV_MPU6050_REG_RAW_ACCEL,
 	.temperature            = INV_MPU6050_REG_TEMPERATURE,
 	.int_enable             = INV_MPU6050_REG_INT_ENABLE,
-	.int_status             = INV_MPU6050_REG_INT_STATUS,
+	.int_status             = INV_ICM20602_REG_FIFO_WM_INT_STATUS,
+	.wm_th_hb               = INV_ICM20602_FIFO_WM_TH_HB,
+	.wm_th_lb               = INV_ICM20602_FIFO_WM_TH_LB,
 	.pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,
 	.pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,
 	.int_pin_cfg            = INV_MPU6050_REG_INT_PIN_CFG,
@@ -56,6 +58,31 @@  static const struct inv_mpu6050_reg_map reg_set_icm20602 = {
 	.i2c_if                 = INV_ICM20602_REG_I2C_IF,
 };
 
+static const struct inv_mpu6050_reg_map reg_set_icm20690 = {
+	.sample_rate_div	= INV_MPU6050_REG_SAMPLE_RATE_DIV,
+	.lpf                    = INV_MPU6050_REG_CONFIG,
+	.accel_lpf              = INV_MPU6500_REG_ACCEL_CONFIG_2,
+	.user_ctrl              = INV_MPU6050_REG_USER_CTRL,
+	.fifo_en                = INV_MPU6050_REG_FIFO_EN,
+	.gyro_config            = INV_MPU6050_REG_GYRO_CONFIG,
+	.accl_config            = INV_MPU6050_REG_ACCEL_CONFIG,
+	.fifo_count_h           = INV_MPU6050_REG_FIFO_COUNT_H,
+	.fifo_r_w               = INV_MPU6050_REG_FIFO_R_W,
+	.raw_gyro               = INV_MPU6050_REG_RAW_GYRO,
+	.raw_accl               = INV_MPU6050_REG_RAW_ACCEL,
+	.temperature            = INV_MPU6050_REG_TEMPERATURE,
+	.int_enable             = INV_MPU6050_REG_INT_ENABLE,
+	.int_status             = INV_ICM20602_REG_FIFO_WM_INT_STATUS,
+	.wm_th_hb               = INV_ICM20690_FIFO_WM_TH_HB,
+	.wm_th_lb               = INV_ICM20690_FIFO_WM_TH_LB,
+	.pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,
+	.pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,
+	.int_pin_cfg		= INV_MPU6050_REG_INT_PIN_CFG,
+	.accl_offset		= INV_MPU6500_REG_ACCEL_OFFSET,
+	.gyro_offset		= INV_MPU6050_REG_GYRO_OFFSET,
+	.i2c_if                 = 0,
+};
+
 static const struct inv_mpu6050_reg_map reg_set_6500 = {
 	.sample_rate_div	= INV_MPU6050_REG_SAMPLE_RATE_DIV,
 	.lpf                    = INV_MPU6050_REG_CONFIG,
@@ -115,6 +142,8 @@  static const struct inv_mpu6050_chip_config chip_config_6050 = {
 	.magn_fifo_enable = false,
 	.accl_fs = INV_MPU6050_FS_02G,
 	.user_ctrl = 0,
+	.wm_val = 1,
+	.wm_size = 0,
 };
 
 static const struct inv_mpu6050_chip_config chip_config_6500 = {
@@ -132,6 +161,8 @@  static const struct inv_mpu6050_chip_config chip_config_6500 = {
 	.magn_fifo_enable = false,
 	.accl_fs = INV_MPU6050_FS_02G,
 	.user_ctrl = 0,
+	.wm_val = 1,
+	.wm_size = 0,
 };
 
 /* Indexed by enum inv_devices */
@@ -247,7 +278,7 @@  static const struct inv_mpu6050_hw hw_info[] = {
 	{
 		.whoami = INV_ICM20690_WHOAMI_VALUE,
 		.name = "ICM20690",
-		.reg = &reg_set_6500,
+		.reg = &reg_set_icm20690,
 		.config = &chip_config_6500,
 		.fifo_size = 1024,
 		.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
@@ -264,6 +295,53 @@  static const struct inv_mpu6050_hw hw_info[] = {
 	},
 };
 
+unsigned int inv_mpu6050_get_sample_size(struct inv_mpu6050_state *st)
+{
+	unsigned int nb_byte;
+
+	nb_byte = 0;
+	if (st->chip_config.gyro_en)
+		nb_byte += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
+	if (st->chip_config.accl_en)
+		nb_byte += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
+	if (st->chip_config.magn_en)
+		nb_byte += INV_MPU9X50_BYTES_MAGN;
+	if (st->chip_config.temp_en)
+		nb_byte += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
+
+	return nb_byte;
+}
+
+int inv_mpu6050_compute_hw_watermark(struct inv_mpu6050_state *st, unsigned int val)
+{
+	unsigned int nb_byte, new_size_byte;
+	int ret;
+
+	nb_byte = inv_mpu6050_get_sample_size(st);
+
+	/*
+	 * Compute watermark size.
+	 *
+	 * Use a threshold when watermark size is not supported by the chip
+	 * to prevent overwriting the first samples of the fifo.
+	 * The threshold creates latency for the computing of the interrupt.
+	 * When using a sampling frequency of 500Hz, the max latency is 12ms.
+	 */
+	new_size_byte = val * nb_byte;
+	if (new_size_byte > st->hw->fifo_size) {
+		val = ((st->hw->fifo_size * 7) / 8) / nb_byte;
+		new_size_byte = val * nb_byte;
+	}
+
+	ret = inv_mpu6050_set_hw_watermark(st, new_size_byte);
+	if (ret)
+		return ret;
+
+	st->chip_config.wm_val = val;
+
+	return 0;
+}
+
 static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,
 					int clock, int temp_dis)
 {
@@ -1018,6 +1096,70 @@  static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
 	}
 }
 
+/*
+ * inv_mpu6050_get_fifo_watermark_max() - Get the max watermark size of current chip.
+ */
+
+static ssize_t
+inv_mpu6050_get_fifo_watermark_max(struct device *dev, struct device_attribute *attr,
+		   char *buf)
+{
+	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned int nb_byte;
+	size_t watermark_max_size;
+
+	mutex_lock(&st->lock);
+	nb_byte = inv_mpu6050_get_sample_size(st);
+
+	/* Use default value of accel + gyro + temp sample */
+	if (nb_byte == 0)
+		nb_byte = 2 * INV_MPU6050_BYTES_PER_3AXIS_SENSOR
+				+ INV_MPU6050_BYTES_PER_TEMP_SENSOR;
+	watermark_max_size = st->hw->fifo_size / nb_byte;
+	mutex_unlock(&st->lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%zu\n", watermark_max_size);
+}
+
+/*
+ * inv_mpu6050_get_fifo_state() - Get the state of the hwfifo.
+ *
+ * Watermark is disabled when the watermark size is set to 0
+ */
+
+static ssize_t
+inv_mpu6050_get_fifo_state(struct device *dev, struct device_attribute *attr,
+		   char *buf)
+{
+	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned int fifo_state = 0;
+
+	mutex_lock(&st->lock);
+	if (st->chip_config.wm_size > 0)
+		fifo_state = 1;
+	mutex_unlock(&st->lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_state);
+}
+
+/*
+ * inv_mpu6050_get_fifo_watermark() - Get the current watermark size.
+ */
+
+static ssize_t
+inv_mpu6050_get_fifo_watermark(struct device *dev, struct device_attribute *attr,
+		   char *buf)
+{
+	struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned int watermark;
+
+	mutex_lock(&st->lock);
+	watermark = st->chip_config.wm_size;
+	mutex_unlock(&st->lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", watermark);
+}
+
 /**
  * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense
  *                                  MPU6050 device.
@@ -1258,6 +1400,14 @@  static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL,
 static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL,
 	ATTR_ACCL_MATRIX);
 
+static IIO_CONST_ATTR(hwfifo_watermark_min, "1");
+static IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO,
+	inv_mpu6050_get_fifo_watermark_max, NULL, 0);
+static IIO_DEVICE_ATTR(hwfifo_enabled, S_IRUGO,
+	inv_mpu6050_get_fifo_state, NULL, 0);
+static IIO_DEVICE_ATTR(hwfifo_watermark, S_IRUGO,
+	inv_mpu6050_get_fifo_watermark, NULL, 0);
+
 static struct attribute *inv_attributes[] = {
 	&iio_dev_attr_in_gyro_matrix.dev_attr.attr,  /* deprecated */
 	&iio_dev_attr_in_accel_matrix.dev_attr.attr, /* deprecated */
@@ -1265,6 +1415,10 @@  static struct attribute *inv_attributes[] = {
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
 	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
 	&iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
+	&iio_const_attr_hwfifo_watermark_min.dev_attr.attr,
+	&iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
+	&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
+	&iio_dev_attr_hwfifo_enabled.dev_attr.attr,
 	NULL,
 };
 
@@ -1290,6 +1444,39 @@  static int inv_mpu6050_reg_access(struct iio_dev *indio_dev,
 	return ret;
 }
 
+static int inv_mpu6050_set_watermark(struct iio_dev *indio_dev, unsigned int val)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int ret;
+
+	switch (st->chip_type) {
+	case INV_ICM20602:
+	case INV_ICM20690:
+		mutex_lock(&st->lock);
+		ret = inv_mpu6050_compute_hw_watermark(st, val);
+		mutex_unlock(&st->lock);
+		break;
+	default:
+		ret = 0;
+		break;
+	}
+
+	return ret;
+}
+
+static int inv_mpu6050_hw_flush_to_buffer(struct iio_dev *indio_dev,
+				      unsigned int count)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&st->lock);
+	ret = inv_mpu6050_flush_fifo(indio_dev, 0, count);
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
 static const struct iio_info mpu_info = {
 	.read_raw = &inv_mpu6050_read_raw,
 	.write_raw = &inv_mpu6050_write_raw,
@@ -1297,6 +1484,8 @@  static const struct iio_info mpu_info = {
 	.attrs = &inv_attribute_group,
 	.validate_trigger = inv_mpu6050_validate_trigger,
 	.debugfs_reg_access = &inv_mpu6050_reg_access,
+	.hwfifo_set_watermark = &inv_mpu6050_set_watermark,
+	.hwfifo_flush_to_buffer = &inv_mpu6050_hw_flush_to_buffer,
 };
 
 /*
@@ -1618,7 +1807,7 @@  int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		 */
 		result = devm_iio_triggered_buffer_setup(dev, indio_dev,
 							 iio_pollfunc_store_time,
-							 inv_mpu6050_read_fifo,
+							 inv_mpu6050_interrupt_handler,
 							 NULL);
 		if (result) {
 			dev_err(dev, "configure buffer fail %d\n", result);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index c6aa36e..660ebda 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -57,6 +57,8 @@  struct inv_mpu6050_reg_map {
 	u8 temperature;
 	u8 int_enable;
 	u8 int_status;
+	u8 wm_th_hb;
+	u8 wm_th_lb;
 	u8 pwr_mgmt_1;
 	u8 pwr_mgmt_2;
 	u8 int_pin_cfg;
@@ -105,6 +107,8 @@  enum inv_devices {
  *  @temp_fifo_enable:	enable temp data output
  *  @magn_fifo_enable:	enable magn data output
  *  @divider:		chip sample rate divider (sample rate divider - 1)
+ *  @wm_val:		watermark size in number of data
+ *  @wm_size:		watermark size in bytes
  */
 struct inv_mpu6050_chip_config {
 	unsigned int clk:3;
@@ -121,6 +125,8 @@  struct inv_mpu6050_chip_config {
 	unsigned int magn_fifo_enable:1;
 	u8 divider;
 	u8 user_ctrl;
+	unsigned int wm_val;
+	unsigned int wm_size;
 };
 
 /*
@@ -264,8 +270,19 @@  struct inv_mpu6050_state {
 #define INV_MPU6050_BIT_FIFO_OVERFLOW_INT   0x10
 #define INV_MPU6050_BIT_RAW_DATA_RDY_INT    0x01
 
+#define INV_ICM20602_REG_FIFO_WM_INT_STATUS 0x39
+#define INV_ICM20602_BIT_FIFO_WM_INT        0x40
+
 #define INV_MPU6050_REG_EXT_SENS_DATA       0x49
 
+#define INV_ICM20602_FIFO_WM_TH_HB          0x60
+#define INV_ICM20602_BITS_FIFO_WM_TH_HB     0x03
+#define INV_ICM20602_FIFO_WM_TH_LB          0x61
+
+#define INV_ICM20690_FIFO_WM_TH_HB          0x5E
+#define INV_ICM20690_BITS_FIFO_WM_TH_HB     0x03
+#define INV_ICM20690_FIFO_WM_TH_LB          0x61
+
 /* I2C slaves data output from 0 to 3 */
 #define INV_MPU6050_REG_I2C_SLV_DO(_x)      (0x63 + (_x))
 
@@ -338,7 +355,6 @@  struct inv_mpu6050_state {
 #define INV_ICM20690_GYRO_STARTUP_TIME       80
 #define INV_ICM20690_ACCEL_STARTUP_TIME      10
 
-
 /* delay time in microseconds */
 #define INV_MPU6050_REG_UP_TIME_MIN          5000
 #define INV_MPU6050_REG_UP_TIME_MAX          10000
@@ -457,7 +473,7 @@  enum inv_mpu6050_clock_sel_e {
 	NUM_CLK
 };
 
-irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
+irqreturn_t inv_mpu6050_interrupt_handler(int irq, void *p);
 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
 int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable);
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
@@ -468,5 +484,8 @@  void inv_mpu_acpi_delete_mux_client(struct i2c_client *client);
 int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type);
 extern const struct dev_pm_ops inv_mpu_pmops;
+int inv_mpu6050_set_hw_watermark(struct inv_mpu6050_state *st, unsigned int wm_size_byte);
+int inv_mpu6050_compute_hw_watermark(struct inv_mpu6050_state *st, unsigned int wm_size_byte);
+int inv_mpu6050_flush_fifo(struct iio_dev *indio_dev, s64 timestamp_val, unsigned int count);
 
 #endif
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 45c3752..4c558cf 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -40,15 +40,16 @@  static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
 	if (st->it_timestamp == 0) {
 		/* not initialized, forced to use it_timestamp */
 		use_it_timestamp = true;
-	} else if (nb == 1) {
+	} else if (nb == st->chip_config.wm_val) {
 		/*
 		 * Validate the use of it timestamp by checking if interrupt
 		 * has been delayed.
-		 * nb > 1 means interrupt was delayed for more than 1 sample,
+		 * nb > wm_val means interrupt was delayed for more than 1 sample,
 		 * so it's obviously not good.
+		 * If watermark does not exist for the chip, wm_val = 1.
 		 * Compute the chip period between 2 interrupts for validating.
 		 */
-		delta = div_s64(timestamp - st->it_timestamp, divider);
+		delta = div_s64(timestamp - st->it_timestamp, divider) / st->chip_config.wm_val;
 		if (delta > period_min && delta < period_max) {
 			/* update chip period and use it timestamp */
 			st->chip_period = (st->chip_period + delta) / 2;
@@ -89,6 +90,25 @@  static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
 	return ts;
 }
 
+int inv_mpu6050_set_hw_watermark(struct inv_mpu6050_state *st, unsigned int size)
+{
+	int ret;
+
+	if (size == st->chip_config.wm_size)
+		return 0;
+
+	ret = regmap_write(st->map, st->reg->wm_th_hb, (size >> 8) & 0xFF);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, st->reg->wm_th_lb, size & 0xFF);
+	if (ret)
+		return ret;
+
+	st->chip_config.wm_size = size;
+	return 0;
+}
+
 static int inv_reset_fifo(struct iio_dev *indio_dev)
 {
 	int result;
@@ -104,43 +124,44 @@  static int inv_reset_fifo(struct iio_dev *indio_dev)
 
 reset_fifo_fail:
 	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
-	result = regmap_write(st->map, st->reg->int_enable,
-			      INV_MPU6050_BIT_DATA_RDY_EN);
-
+	switch (st->chip_type) {
+	case INV_ICM20602:
+	case INV_ICM20690:
+		return inv_mpu6050_set_hw_watermark(st, st->chip_config.wm_size);
+	default:
+		return regmap_write(st->map, st->reg->int_enable,
+				INV_MPU6050_BIT_DATA_RDY_EN);
+	}
 	return result;
 }
 
-/*
- * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
+/**
+ * inv_mpu6050_flush_fifo() - Flush fifo
+ *
+ * @indio_dev:		Device driver instance.
+ * @timestamp_val:	the interrupt timestamp
+ * @count:		Number of data to flush
+ *
+ * This function flushes the fifo using count as the number of data to flush.
+ * As fifo_size may differ according to the chip unsigned
+ * count = 0 is used to set the max size of the fifo for count.
  */
-irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
+
+int inv_mpu6050_flush_fifo(struct iio_dev *indio_dev,
+				      s64 timestamp_val, unsigned int count)
 {
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
-	size_t bytes_per_datum;
-	int result;
 	u16 fifo_count;
 	s64 timestamp;
-	int int_status;
+	int result;
 	size_t i, nb;
-
-	mutex_lock(&st->lock);
-
-	/* ack interrupt and check status */
-	result = regmap_read(st->map, st->reg->int_status, &int_status);
-	if (result) {
-		dev_err(regmap_get_device(st->map),
-			"failed to ack interrupt\n");
-		goto flush_fifo;
-	}
-	if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
-		goto end_session;
+	size_t bytes_per_datum;
 
 	if (!(st->chip_config.accl_fifo_enable |
 		st->chip_config.gyro_fifo_enable |
 		st->chip_config.magn_fifo_enable))
-		goto end_session;
+		return 0;
+
 	bytes_per_datum = 0;
 	if (st->chip_config.accl_fifo_enable)
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
@@ -161,7 +182,7 @@  irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
 				  st->data, INV_MPU6050_FIFO_COUNT_BYTE);
 	if (result)
-		goto end_session;
+		return 0;
 	fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
 
 	/*
@@ -171,18 +192,30 @@  irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	 */
 	nb = 3 * bytes_per_datum;
 	if (fifo_count >= st->hw->fifo_size - nb) {
-		dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
-		goto flush_fifo;
+		dev_warn(regmap_get_device(st->map), "fifo overflow\n");
+		inv_reset_fifo(indio_dev);
+		return -1;
 	}
 
 	/* compute and process all complete datum */
 	nb = fifo_count / bytes_per_datum;
-	inv_mpu6050_update_period(st, pf->timestamp, nb);
+	/*
+	 * As fifo_size may differ according to the chip unsigned
+	 * count = 0 is used to choose the max value for count
+	 */
+	if (count == 0)
+		count = st->hw->fifo_size / bytes_per_datum;
+	if (timestamp_val != 0)
+		inv_mpu6050_update_period(st, timestamp_val, nb);
+	nb = min(nb, count);
 	for (i = 0; i < nb; ++i) {
 		result = regmap_noinc_read(st->map, st->reg->fifo_r_w,
 					   st->data, bytes_per_datum);
-		if (result)
-			goto flush_fifo;
+		if (result) {
+			inv_reset_fifo(indio_dev);
+			return result;
+		}
+
 		/* skip first samples if needed */
 		if (st->skip_samples) {
 			st->skip_samples--;
@@ -192,6 +225,45 @@  irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 		iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
 	}
 
+	return i;
+}
+
+/*
+ * inv_mpu6050_interrupt_handler() - Transfer data from hardware FIFO to KFIFO.
+ */
+irqreturn_t inv_mpu6050_interrupt_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int result;
+	int int_status;
+	int int_status_bit;
+
+	mutex_lock(&st->lock);
+
+	/* ack interrupt and check status */
+	result = regmap_read(st->map, st->reg->int_status, &int_status);
+	if (result) {
+		dev_err(regmap_get_device(st->map),
+			"failed to ack interrupt\n");
+		goto flush_fifo;
+	}
+
+	switch (st->chip_type) {
+	case INV_ICM20602:
+	case INV_ICM20690:
+		int_status_bit = INV_ICM20602_BIT_FIFO_WM_INT;
+		break;
+	default:
+		int_status_bit = INV_MPU6050_BIT_RAW_DATA_RDY_INT;
+		break;
+	}
+	if (!(int_status & int_status_bit))
+		goto end_session;
+
+	inv_mpu6050_flush_fifo(indio_dev, pf->timestamp, 0);
+
 end_session:
 	mutex_unlock(&st->lock);
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 8825468..c685ed4 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -131,10 +131,28 @@  int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
 		if (ret)
 			return ret;
 		/* enable interrupt */
-		ret = regmap_write(st->map, st->reg->int_enable,
-				   INV_MPU6050_BIT_DATA_RDY_EN);
+		switch (st->chip_type) {
+		case INV_ICM20602:
+		case INV_ICM20690:
+			/* watermark interrupt is enabled by setting the watermark value */
+			ret = inv_mpu6050_compute_hw_watermark(st, st->chip_config.wm_val);
+			break;
+		default:
+			ret = regmap_write(st->map, st->reg->int_enable,
+					INV_MPU6050_BIT_DATA_RDY_EN);
+			break;
+		}
 	} else {
-		ret = regmap_write(st->map, st->reg->int_enable, 0);
+		switch (st->chip_type) {
+		case INV_ICM20602:
+		case INV_ICM20690:
+			/* watermark interrupt is disabled by setting the watermark value to 0 */
+			ret = inv_mpu6050_set_hw_watermark(st, 0);
+			break;
+		default:
+			ret = regmap_write(st->map, st->reg->int_enable, 0);
+			break;
+		}
 		if (ret)
 			return ret;
 		ret = regmap_write(st->map, st->reg->fifo_en, 0);