8 files changed, 80 insertions, 48 deletions

diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 0121501dc634..833dd6b73bc3 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -148,6 +148,7 @@ static const struct iio_chan_spec adis16201_channels[] = {
 static const struct iio_info adis16201_info = {
 	.read_raw = &adis16201_read_raw,
 	.write_raw = &adis16201_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 326b1067543e..f631e578fbd1 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -112,6 +112,7 @@ static const struct iio_chan_spec adis16203_channels[] = {
 static const struct iio_info adis16203_info = {
 	.read_raw = &adis16203_read_raw,
 	.write_raw = &adis16203_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 4f69ead1ae81..dbec841ce30c 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -153,6 +153,7 @@ static const struct iio_chan_spec adis16204_channels[] = {
 static const struct iio_info adis16204_info = {
 	.read_raw = &adis16204_read_raw,
 	.write_raw = &adis16204_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index e203e96a5663..f9f9d582b32d 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -146,6 +146,7 @@ static const struct iio_chan_spec adis16209_channels[] = {
 static const struct iio_info adis16209_info = {
 	.read_raw = &adis16209_read_raw,
 	.write_raw = &adis16209_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 019a31e22038..3d1a8a9921ad 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -202,6 +202,7 @@ static const struct iio_info adis16240_info = {
 	.attrs = &adis16240_attribute_group,
 	.read_raw = &adis16240_read_raw,
 	.write_raw = &adis16240_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index 820547baae30..b988b4f5bdda 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -267,6 +267,7 @@ static const struct iio_info adis16260_info = {
 	.attrs = &adis16260_attribute_group,
 	.read_raw = &adis16260_read_raw,
 	.write_raw = &adis16260_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/imu/adis.h b/drivers/staging/iio/imu/adis.h
index c84da7afe146..8c3304d44b97 100644
--- a/drivers/staging/iio/imu/adis.h
+++ b/drivers/staging/iio/imu/adis.h
@@ -87,6 +87,7 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 		.sign = 'u', \
 		.realbits = (bits), \
 		.storagebits = 16, \
+		.endianness = IIO_BE, \
 	}, \
 }
 
@@ -109,6 +110,7 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 		.sign = 'u', \
 		.realbits = (bits), \
 		.storagebits = 16, \
+		.endianness = IIO_BE, \
 	}, \
 }
 
@@ -125,6 +127,7 @@ int adis_single_conversion(struct iio_dev *indio_dev,
 		.sign = 's', \
 		.realbits = (bits), \
 		.storagebits = 16, \
+		.endianness = IIO_BE, \
 	}, \
 }
 
@@ -150,6 +153,9 @@ void adis_cleanup_buffer_and_trigger(struct adis *adis,
 int adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev);
 void adis_remove_trigger(struct adis *adis);
 
+int adis_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *scan_mask);
+
 #else /* CONFIG_IIO_BUFFER */
 
 static inline int adis_setup_buffer_and_trigger(struct adis *adis,
@@ -173,6 +179,8 @@ static inline void adis_remove_trigger(struct adis *adis)
 {
 }
 
+#define adis_update_scan_mode NULL
+
 #endif /* CONFIG_IIO_BUFFER */
 
 #endif
diff --git a/drivers/staging/iio/imu/adis_buffer.c b/drivers/staging/iio/imu/adis_buffer.c
index 0fa8e80a54b3..342758c14d2b 100644
--- a/drivers/staging/iio/imu/adis_buffer.c
+++ b/drivers/staging/iio/imu/adis_buffer.c
@@ -1,3 +1,12 @@
+/*
+ * Common library for ADIS16XXX devices
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *   Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
 #include <linux/export.h>
 #include <linux/interrupt.h>
 #include <linux/mutex.h>
@@ -12,73 +21,80 @@
 
 #include  "adis.h"
 
-#define ADIS_MAX_OUTPUTS 12
-
-static int adis_read_buffer_data(struct adis *adis, struct iio_dev *indio_dev)
+int adis_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *scan_mask)
 {
-	int n_outputs = indio_dev->num_channels;
-	struct spi_transfer xfers[ADIS_MAX_OUTPUTS + 1];
-	struct spi_message msg;
-	int ret;
-	int i;
-
-	mutex_lock(&adis->txrx_lock);
-
-	spi_message_init(&msg);
-
-	memset(xfers, 0, sizeof(xfers));
-	for (i = 0; i <= n_outputs; i++) {
-		xfers[i].bits_per_word = 8;
-		xfers[i].cs_change = 1;
-		xfers[i].len = 2;
-		xfers[i].delay_usecs = adis->data->read_delay;
-		if (i < n_outputs) {
-			xfers[i].tx_buf = adis->tx + 2 * i;
-			adis->tx[2 * i] = indio_dev->channels[i].address;
-			adis->tx[2 * i + 1] = 0;
-		}
-		if (i >= 1)
-			xfers[i].rx_buf = adis->rx + 2 * (i - 1);
-		spi_message_add_tail(&xfers[i], &msg);
-	}
+	struct adis *adis = iio_device_get_drvdata(indio_dev);
+	const struct iio_chan_spec *chan;
+	unsigned int scan_count;
+	unsigned int i, j;
+	__be16 *tx, *rx;
 
-	ret = spi_sync(adis->spi, &msg);
-	if (ret)
-		dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
+	kfree(adis->xfer);
+	kfree(adis->buffer);
 
-	mutex_unlock(&adis->txrx_lock);
+	scan_count = indio_dev->scan_bytes / 2;
 
-	return ret;
+	adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL);
+	if (!adis->xfer)
+		return -ENOMEM;
+
+	adis->buffer = kzalloc(indio_dev->scan_bytes * 2, GFP_KERNEL);
+	if (!adis->buffer)
+		return -ENOMEM;
+
+	rx = adis->buffer;
+	tx = rx + indio_dev->scan_bytes;
+
+	spi_message_init(&adis->msg);
+
+	for (j = 0; j <= scan_count; j++) {
+		adis->xfer[j].bits_per_word = 8;
+		if (j != scan_count)
+			adis->xfer[j].cs_change = 1;
+		adis->xfer[j].len = 2;
+		adis->xfer[j].delay_usecs = adis->data->read_delay;
+		if (j < scan_count)
+			adis->xfer[j].tx_buf = &tx[j];
+		if (j >= 1)
+			adis->xfer[j].rx_buf = &rx[j - 1];
+		spi_message_add_tail(&adis->xfer[j], &adis->msg);
+	}
+
+	chan = indio_dev->channels;
+	for (i = 0; i < indio_dev->num_channels; i++, chan++) {
+		if (!test_bit(chan->scan_index, scan_mask))
+			continue;
+		*tx++ = cpu_to_be16(chan->address << 8);
+	}
+
+	return 0;
 }
+EXPORT_SYMBOL_GPL(adis_update_scan_mode);
 
 static irqreturn_t adis_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct adis *adis = iio_device_get_drvdata(indio_dev);
-	u16 *data;
-	int i = 0;
+	int ret;
 
-	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
-	if (data == NULL) {
-		dev_err(&adis->spi->dev, "Failed to allocate memory.");
+	if (!adis->buffer)
 		return -ENOMEM;
-	}
 
-	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)
-	    && adis_read_buffer_data(adis, indio_dev) >= 0)
-		for (; i < bitmap_weight(indio_dev->active_scan_mask,
-					 indio_dev->masklength); i++)
-			data[i] = be16_to_cpup((__be16 *)&(adis->rx[i*2]));
+	ret = spi_sync(adis->spi, &adis->msg);
+	if (ret)
+		dev_err(&adis->spi->dev, "Failed to read data: %d", ret);
 
 	/* Guaranteed to be aligned with 8 byte boundary */
-	if (indio_dev->scan_timestamp)
-		*((s64 *)(PTR_ALIGN(data, sizeof(s64)))) = pf->timestamp;
+	if (indio_dev->scan_timestamp) {
+		void *b = adis->buffer + indio_dev->scan_bytes - sizeof(s64);
+		*(s64 *)b = pf->timestamp;
+	}
 
-	iio_push_to_buffers(indio_dev, (u8 *)data);
+	iio_push_to_buffers(indio_dev, adis->buffer);
 
 	iio_trigger_notify_done(indio_dev->trig);
-	kfree(data);
 
 	return IRQ_HANDLED;
 }
@@ -137,6 +153,8 @@ void adis_cleanup_buffer_and_trigger(struct adis *adis,
 {
 	if (adis->spi->irq)
 		adis_remove_trigger(adis);
+	kfree(adis->buffer);
+	kfree(adis->xfer);
 	iio_triggered_buffer_cleanup(indio_dev);
 }
 EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger);