[PATCH 3/4] iio: add helper function for reading channel offset in buffer

[Artur Rojek]

This is useful for consumers that wish to parse raw buffer data.

Tested-by: Paul Cercueil <paul@xxxxxxxxxxxxxxx>
Signed-off-by: Artur Rojek <contact@xxxxxxxxxxxxxx>
---
 drivers/iio/industrialio-buffer.c | 28 ++++++++++++++++++++++++++++
 include/linux/iio/buffer.h        |  4 ++++
 2 files changed, 32 insertions(+)

diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 228598b82a2f..cf23736610d9 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -691,6 +691,34 @@ static unsigned int iio_storage_bytes_for_si(struct iio_dev *indio_dev,
 	return bytes;
 }
 
+int iio_find_channel_offset_in_buffer(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      struct iio_buffer *buffer)
+{
+	int length, offset = 0;
+	unsigned int si;
+
+	if (chan->scan_index < 0 ||
+	    !test_bit(chan->scan_index, buffer->scan_mask)) {
+		return -EINVAL;
+	}
+
+	for (si = 0; si < chan->scan_index; ++si) {
+		if (!test_bit(si, buffer->scan_mask))
+			continue;
+
+		length = iio_storage_bytes_for_si(indio_dev, si);
+
+		/* Account for channel alignment. */
+		if (offset % length)
+			offset += length - (offset % length);
+		offset += length;
+	}
+
+	return offset;
+}
+EXPORT_SYMBOL_GPL(iio_find_channel_offset_in_buffer);
+
 static unsigned int iio_storage_bytes_for_timestamp(struct iio_dev *indio_dev)
 {
 	struct iio_dev_opaque *iio_dev_opaque = to_iio_dev_opaque(indio_dev);
diff --git a/include/linux/iio/buffer.h b/include/linux/iio/buffer.h
index 418b1307d3f2..b1db74772e77 100644
--- a/include/linux/iio/buffer.h
+++ b/include/linux/iio/buffer.h
@@ -16,6 +16,10 @@ enum iio_buffer_direction {
 	IIO_BUFFER_DIRECTION_OUT,
 };
 
+int iio_find_channel_offset_in_buffer(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      struct iio_buffer *buffer);
+
 int iio_push_to_buffers(struct iio_dev *indio_dev, const void *data);
 
 int iio_pop_from_buffer(struct iio_buffer *buffer, void *data);
-- 
2.37.2