[PATCH 2/2] iio: imu: st_lsm6dsx: add iio trigger and buffer support

[Sean Nyekjaer <sean@geanix.com>]

This adds support for using iio triggers, this is needed because
our hardware guys forgot to connect the irq pins from imu device
to the SoC.

Signed-off-by: Sean Nyekjaer <sean@geanix.com>
---
 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 37 ++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 1ca69598678f..65ab853202de 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -40,8 +40,11 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/delay.h>
+#include <linux/iio/buffer.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 #include <linux/pm.h>
 #include <linux/regmap.h>
 #include <linux/bitfield.h>
@@ -945,6 +948,30 @@ static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
 	return 0;
 }
 
+static irqreturn_t st_lsm6dsx_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev);
+	u16 buffer[4 * sizeof(s64)/sizeof(u16)];
+	int tmp, bit;
+
+	memset(buffer, 0, sizeof(buffer));
+
+	for_each_set_bit(bit, indio_dev->active_scan_mask, indio_dev->masklength) {
+		st_lsm6dsx_read_oneshot(sensor,
+					indio_dev->channels[bit].address, &tmp);
+		buffer[bit] = tmp;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
+					   iio_get_time_ns(indio_dev));
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
 static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
 {
 	u8 drdy_int_reg;
@@ -1093,6 +1120,16 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		err = st_lsm6dsx_fifo_setup(hw);
 		if (err < 0)
 			return err;
+	} else {
+		for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+			err = devm_iio_triggered_buffer_setup(hw->dev,
+					hw->iio_devs[i], NULL,
+					st_lsm6dsx_trigger_handler, NULL);
+			if (err < 0) {
+				dev_err(hw->dev, "iio triggered buffer setup failed\n");
+				return err;
+			}
+		}
 	}
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
-- 
2.21.0