[PATCH 3/4] iio: imu: inv_mpu6050: add new interrupt handler for WoM events

[inv.git-commit@tdk.com]

From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>

Add new interrupt handler for generating WoM event from int status
register bits. Launch from interrupt the trigger poll function for
data buffer.

Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 11 ----
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 56 +++++++++++++++++--
 3 files changed, 53 insertions(+), 16 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 519c1eee96ad..9be67cebbd49 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -184,6 +184,7 @@ struct inv_mpu6050_hw {
  *  @magn_orient:       magnetometer sensor chip orientation if available.
  *  @suspended_sensors:	sensors mask of sensors turned off for suspend
  *  @data:		read buffer used for bulk reads.
+ *  @it_timestamp:	interrupt timestamp.
  */
 struct inv_mpu6050_state {
 	struct mutex lock;
@@ -209,6 +210,7 @@ struct inv_mpu6050_state {
 	unsigned int suspended_sensors;
 	bool level_shifter;
 	u8 *data;
+	s64 it_timestamp;
 };
 
 /*register and associated bit definition*/
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 13da6f523ca2..e282378ee2ca 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -51,21 +51,10 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	u32 fifo_period;
 	s64 timestamp;
 	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
-	int int_status;
 	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;
-
 	if (!(st->chip_config.accl_fifo_enable |
 		st->chip_config.gyro_fifo_enable |
 		st->chip_config.magn_fifo_enable))
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index ec2398a87f45..7ffbb9e7c100 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -6,6 +6,7 @@
 #include <linux/pm_runtime.h>
 
 #include <linux/iio/common/inv_sensors_timestamp.h>
+#include <linux/iio/events.h>
 
 #include "inv_mpu_iio.h"
 
@@ -223,6 +224,52 @@ static const struct iio_trigger_ops inv_mpu_trigger_ops = {
 	.set_trigger_state = &inv_mpu_data_rdy_trigger_set_state,
 };
 
+static irqreturn_t inv_mpu6050_interrupt_timestamp(int irq, void *p)
+{
+	struct iio_dev *indio_dev = p;
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	st->it_timestamp = iio_get_time_ns(indio_dev);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t inv_mpu6050_interrupt_handle(int irq, void *p)
+{
+	struct iio_dev *indio_dev = p;
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	unsigned int int_status = 0;
+	int result;
+
+	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 exit_unlock;
+	}
+
+	/* handle WoM event */
+	if (st->chip_config.wom_en && (int_status & INV_MPU6500_BIT_WOM_INT))
+		iio_push_event(indio_dev,
+			       IIO_UNMOD_EVENT_CODE(IIO_ACCEL, 0, IIO_EV_TYPE_MAG_ADAPTIVE,
+						    IIO_EV_DIR_RISING),
+				st->it_timestamp);
+
+exit_unlock:
+	mutex_unlock(&st->lock);
+
+	/* handle raw data interrupt */
+	if (int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT) {
+		indio_dev->pollfunc->timestamp = st->it_timestamp;
+		iio_trigger_poll_nested(st->trig);
+	}
+
+	return IRQ_HANDLED;
+}
+
 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
 {
 	int ret;
@@ -235,11 +282,10 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type)
 	if (!st->trig)
 		return -ENOMEM;
 
-	ret = devm_request_irq(&indio_dev->dev, st->irq,
-			       &iio_trigger_generic_data_rdy_poll,
-			       irq_type,
-			       "inv_mpu",
-			       st->trig);
+	ret = devm_request_threaded_irq(&indio_dev->dev, st->irq,
+					&inv_mpu6050_interrupt_timestamp,
+					&inv_mpu6050_interrupt_handle,
+					irq_type, "inv_mpu", indio_dev);
 	if (ret)
 		return ret;
 
-- 
2.34.1