From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
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@xxxxxxx>
---
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 | 69 +++++++++++++++++--
3 files changed, 66 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 d5b0465d1f74..ca5f7d45a6d4 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -185,6 +185,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;
@@ -210,6 +211,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..2514966f6495 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,65 @@ 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, wom_bits;
+ u64 ev_code;
+ int result;
+
+ switch (st->chip_type) {
+ case INV_MPU6050:
+ case INV_MPU6500:
+ case INV_MPU6515:
+ case INV_MPU6880:
+ case INV_MPU6000:
+ case INV_MPU9150:
+ case INV_MPU9250:
+ case INV_MPU9255:
+ wom_bits = INV_MPU6500_BIT_WOM_INT;
+ break;
+ default:
+ wom_bits = INV_ICM20608_BIT_WOM_INT;
+ break;
+ }
+
+ scoped_guard(mutex, &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");
+ return IRQ_HANDLED;
+ }
+
+ /* handle WoM event */
+ if (st->chip_config.wom_en && (int_status & wom_bits)) {
+ ev_code = IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_X_OR_Y_OR_Z,
+ IIO_EV_TYPE_ROC, IIO_EV_DIR_RISING);
+ iio_push_event(indio_dev, ev_code, st->it_timestamp);
+ }
+ }
+
+ /* 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 +295,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
