1 files changed, 32 insertions, 65 deletions

diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index e18bc6782256..c55898543a47 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -24,81 +24,30 @@
 
 int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
 {
-	u8 *addr;
-	int i, n = 0, len;
+	int i, len;
+	int total = 0;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 	unsigned int num_data_channels = sdata->num_data_channels;
-	unsigned int byte_for_channel =
-			indio_dev->channels[0].scan_type.storagebits >> 3;
-
-	addr = kmalloc(num_data_channels, GFP_KERNEL);
-	if (!addr) {
-		len = -ENOMEM;
-		goto st_sensors_get_buffer_element_error;
-	}
 
 	for (i = 0; i < num_data_channels; i++) {
+		unsigned int bytes_to_read;
+
 		if (test_bit(i, indio_dev->active_scan_mask)) {
-			addr[n] = indio_dev->channels[i].address;
-			n++;
-		}
-	}
-	switch (n) {
-	case 1:
-		len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
-			addr[0], byte_for_channel, buf, sdata->multiread_bit);
-		break;
-	case 2:
-		if ((addr[1] - addr[0]) == byte_for_channel) {
+			bytes_to_read = indio_dev->channels[i].scan_type.storagebits >> 3;
 			len = sdata->tf->read_multiple_byte(&sdata->tb,
-				sdata->dev, addr[0], byte_for_channel * n,
-				buf, sdata->multiread_bit);
-		} else {
-			u8 *rx_array;
-			rx_array = kmalloc(byte_for_channel * num_data_channels,
-					   GFP_KERNEL);
-			if (!rx_array) {
-				len = -ENOMEM;
-				goto st_sensors_free_memory;
-			}
+				sdata->dev, indio_dev->channels[i].address,
+				bytes_to_read,
+				buf + total, sdata->multiread_bit);
 
-			len = sdata->tf->read_multiple_byte(&sdata->tb,
-				sdata->dev, addr[0],
-				byte_for_channel * num_data_channels,
-				rx_array, sdata->multiread_bit);
-			if (len < 0) {
-				kfree(rx_array);
-				goto st_sensors_free_memory;
-			}
-
-			for (i = 0; i < n * byte_for_channel; i++) {
-				if (i < n)
-					buf[i] = rx_array[i];
-				else
-					buf[i] = rx_array[n + i];
-			}
-			kfree(rx_array);
-			len = byte_for_channel * n;
+			if (len < bytes_to_read)
+				return -EIO;
+
+			/* Advance the buffer pointer */
+			total += len;
 		}
-		break;
-	case 3:
-		len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
-			addr[0], byte_for_channel * num_data_channels,
-			buf, sdata->multiread_bit);
-		break;
-	default:
-		len = -EINVAL;
-		goto st_sensors_free_memory;
-	}
-	if (len != byte_for_channel * n) {
-		len = -EIO;
-		goto st_sensors_free_memory;
 	}
 
-st_sensors_free_memory:
-	kfree(addr);
-st_sensors_get_buffer_element_error:
-	return len;
+	return total;
 }
 EXPORT_SYMBOL(st_sensors_get_buffer_element);
 
@@ -109,6 +58,24 @@ irqreturn_t st_sensors_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
+	/* If we have a status register, check if this IRQ came from us */
+	if (sdata->sensor_settings->drdy_irq.addr_stat_drdy) {
+		u8 status;
+
+		len = sdata->tf->read_byte(&sdata->tb, sdata->dev,
+			   sdata->sensor_settings->drdy_irq.addr_stat_drdy,
+			   &status);
+		if (len < 0)
+			dev_err(sdata->dev, "could not read channel status\n");
+
+		/*
+		 * If this was not caused by any channels on this sensor,
+		 * return IRQ_NONE
+		 */
+		if (!(status & (u8)indio_dev->active_scan_mask[0]))
+			return IRQ_NONE;
+	}
+
 	len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data);
 	if (len < 0)
 		goto st_sensors_get_buffer_element_error;