[RFC v2 2/2] iio: ina2xx: add SOFTWARE buffer mode using an iio kfifo.

[Marc Titinger]

Capture the active scan_elements into a kfifo.
The capture thread will compute the remaining time until the next capture
tick, and do an active wait (udelay).

This will produce a stream of up to fours channels plus a 64bits
timestamps (ns).

# iio_readdev ina226 |  od -x
WARNING: High-speed mode not enabled
0000000 042f 0d5a 002e 010c 4be8 4eb4 0013 0000
0000020 0430 0d5a 002e 010c a704 4f3e 0013 0000
0000040 0430 0d5a 002e 010c b477 4fc7 0013 0000
0000060 042f 0d5b 002e 010c 8052 5050 0013 0000
0000100 042f 0d5b 002e 010c 5d92 50d8 0013 0000
0000120 0430 0d5a 002e 010c fa59 515e 0013 0000
0000140 0430 0d5b 002e 010c 95d2 51e5 0013 0000

Signed-off-by: Marc Titinger <mtitinger@xxxxxxxxxxxx>
---
 drivers/iio/adc/Kconfig      |   1 +
 drivers/iio/adc/ina2xx-iio.c | 119 ++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 118 insertions(+), 2 deletions(-)

diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 087e5bd..9b87372 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -174,6 +174,7 @@ config INA2XX_IIO
 	tristate "Texas Instruments INA2xx Power Monitors IIO driver"
 	depends on I2C
 	select REGMAP_I2C
+	select IIO_BUFFER
 	help
 	  Say yes here to build support for TI INA2xx familly Power Monitors.
 
diff --git a/drivers/iio/adc/ina2xx-iio.c b/drivers/iio/adc/ina2xx-iio.c
index 28b7919..4c11c80 100644
--- a/drivers/iio/adc/ina2xx-iio.c
+++ b/drivers/iio/adc/ina2xx-iio.c
@@ -20,7 +20,9 @@
  * Configurable 7-bit I2C slave address from 0x40 to 0x4F
  */
 #include <linux/module.h>
-#include <linux/iio/iio.h>
+#include <linux/kthread.h>
+#include <linux/delay.h>
+#include <linux/iio/kfifo_buf.h>
 #include <linux/i2c.h>
 #include <linux/regmap.h>
 #include <linux/platform_data/ina2xx.h>
@@ -92,6 +94,7 @@ struct ina2xx_config {
 
 struct ina2xx_chip_info {
 	struct iio_dev *indio_dev;
+	struct task_struct *task;
 	const struct ina2xx_config *config;
 	struct mutex state_lock;
 	long rshunt;
@@ -282,6 +285,9 @@ static int ina2xx_write_raw(struct iio_dev *indio_dev,
 	int ret = 0;
 	unsigned int config, tmp;
 
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
 	mutex_lock(&chip->state_lock);
 
 	ret = regmap_read(chip->regmap, INA2XX_CONFIG, &config);
@@ -340,6 +346,106 @@ static const struct iio_chan_spec ina2xx_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(4),
 };
 
+/*
+ * return uS until next due sampling.
+ */
+
+static s64 prev_ns;
+
+static int ina2xx_work_buffer(struct ina2xx_chip_info *chip)
+{
+	unsigned short data[8];
+	struct iio_dev *indio_dev = chip->indio_dev;
+	int bit, ret = 0, i = 0;
+	unsigned long buffer_us = 0, elapsed_us = 0;
+	s64 time_a, time_b;
+
+	time_a = iio_get_time_ns();
+
+	/* Single register reads: bulk_read will not work with ina226
+	 * as there is no auto-increment of the address register for
+	 * data length longer than 16bits.
+	 */
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		unsigned int val;
+
+		ret = regmap_read(chip->regmap,
+				  INA2XX_SHUNT_VOLTAGE + bit, &val);
+		if (ret < 0)
+			goto _err;
+
+		data[i++] = val;
+	}
+
+	time_b = iio_get_time_ns();
+
+	iio_push_to_buffers_with_timestamp(chip->indio_dev,
+					   (unsigned int *)data, time_a);
+
+	buffer_us = (unsigned long)(time_b - time_a) / 1000;
+	elapsed_us = (unsigned long)(time_a - prev_ns) / 1000;
+
+	trace_printk("uS: elapsed: %lu, buf: %lu\n", elapsed_us, buffer_us);
+
+	prev_ns = time_a;
+_err:
+	return buffer_us;
+};
+
+static int ina2xx_capture_thread(void *data)
+{
+	struct ina2xx_chip_info *chip = (struct ina2xx_chip_info *)data;
+	unsigned int sampling_us = chip->period_us * chip->avg;
+	unsigned long buffer_us;
+
+	do {
+		buffer_us = ina2xx_work_buffer(chip);
+
+		if (sampling_us > buffer_us)
+			udelay(sampling_us - buffer_us);
+
+	} while (!kthread_should_stop());
+
+	chip->task = NULL;
+
+	return 0;
+}
+
+int ina2xx_buffer_enable(struct iio_dev *indio_dev)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	unsigned int sampling_us = chip->period_us * chip->avg;
+
+	trace_printk("Enabling buffer w/ scan_mask %02x, freq = %d, avg =%u\n",
+		     (unsigned int)(*indio_dev->active_scan_mask), chip->freq,
+		     chip->avg);
+
+	trace_printk("Expected work period is %u us\n", sampling_us);
+
+	prev_ns = iio_get_time_ns();
+
+	chip->task = kthread_run(ina2xx_capture_thread, (void *)chip,
+				 "ina2xx-%uus", sampling_us);
+
+	return PTR_ERR_OR_ZERO(chip->task);
+}
+
+int ina2xx_buffer_disable(struct iio_dev *indio_dev)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+
+	if (chip->task)
+		kthread_stop(chip->task);
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops ina2xx_setup_ops = {
+	.postenable = &ina2xx_buffer_enable,
+	.postdisable = &ina2xx_buffer_disable,
+};
+
 static int ina2xx_debug_reg(struct iio_dev *indio_dev,
 			    unsigned reg, unsigned writeval, unsigned *readval)
 {
@@ -379,6 +485,7 @@ static int ina2xx_probe(struct i2c_client *client,
 {
 	struct ina2xx_chip_info *chip;
 	struct iio_dev *indio_dev;
+	struct iio_buffer *buffer;
 	int ret;
 	unsigned int val;
 
@@ -421,7 +528,7 @@ static int ina2xx_probe(struct i2c_client *client,
 
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->info = &ina2xx_info;
-	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
 
 	chip->regmap = devm_regmap_init_i2c(client, &ina2xx_regmap_config);
 	if (IS_ERR(chip->regmap)) {
@@ -443,6 +550,14 @@ static int ina2xx_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
+	buffer = devm_iio_kfifo_allocate(&indio_dev->dev);
+	if (!buffer)
+		return -ENOMEM;
+
+	indio_dev->setup_ops = &ina2xx_setup_ops;
+
+	iio_device_attach_buffer(indio_dev, buffer);
+
 	return devm_iio_device_register(&client->dev, indio_dev);
 }
 
-- 
1.9.1

--
To unsubscribe from this list: send the line "unsubscribe linux-kernel" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html
Please read the FAQ at  http://www.tux.org/lkml/