Staging: IIO: Ring buffer: Initial pass at rarely locked ring buffer

Please note this ring buffer implementation is very much a
work in progress (and hence RFC).  In it's current form
it is stable and reasonably efficient.  There are a couple
of unlikely cases that will lead to more data being lost
that is strictly necessary. The target was for the case
of requiring regular sampling even during user space reads.

All comments welcome.

The intention is to make this only one of several
implementations with run time selection.  For now there
is only one, so it is hard coded into the drivers using it.

Signed-off-by: Jonathan Cameron <jic23@cam.ac.uk>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

1 files changed, 433 insertions, 0 deletions

diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
new file mode 100644
index 0000000..c04ca4c
--- /dev/null
+++ b/drivers/staging/iio/ring_sw.c
@@ -0,0 +1,433 @@
+/* The industrial I/O simple minimally locked ring buffer.
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/workqueue.h>
+#include "ring_sw.h"
+
+static inline int __iio_init_sw_ring_buffer(struct iio_sw_ring_buffer *ring,
+					    int bytes_per_datum, int length)
+{
+	if ((length == 0) || (bytes_per_datum == 0))
+		return -EINVAL;
+
+	__iio_init_ring_buffer(&ring->buf, bytes_per_datum, length);
+	ring->use_lock = __SPIN_LOCK_UNLOCKED((ring)->use_lock);
+	ring->data = kmalloc(length*ring->buf.bpd, GFP_KERNEL);
+	ring->read_p = 0;
+	ring->write_p = 0;
+	ring->last_written_p = 0;
+	ring->half_p = 0;
+	return ring->data ? 0 : -ENOMEM;
+}
+
+static inline void __iio_free_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
+{
+	kfree(ring->data);
+}
+
+void iio_mark_sw_rb_in_use(struct iio_ring_buffer *r)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	spin_lock(&ring->use_lock);
+	ring->use_count++;
+	spin_unlock(&ring->use_lock);
+}
+EXPORT_SYMBOL(iio_mark_sw_rb_in_use);
+
+void iio_unmark_sw_rb_in_use(struct iio_ring_buffer *r)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	spin_lock(&ring->use_lock);
+	ring->use_count--;
+	spin_unlock(&ring->use_lock);
+}
+EXPORT_SYMBOL(iio_unmark_sw_rb_in_use);
+
+
+/* Ring buffer related functionality */
+/* Store to ring is typically called in the bh of a data ready interrupt handler
+ * in the device driver */
+/* Lock always held if their is a chance this may be called */
+/* Only one of these per ring may run concurrently - enforced by drivers */
+int iio_store_to_sw_ring(struct iio_sw_ring_buffer *ring,
+			 unsigned char *data,
+			 s64 timestamp)
+{
+	int ret = 0;
+	int code;
+	unsigned char *temp_ptr, *change_test_ptr;
+
+	/* initial store */
+	if (unlikely(ring->write_p == 0)) {
+		ring->write_p = ring->data;
+		/* Doesn't actually matter if this is out of the set
+		 * as long as the read pointer is valid before this
+		 * passes it - guaranteed as set later in this function.
+		 */
+		ring->half_p = ring->data - ring->buf.length*ring->buf.bpd/2;
+	}
+	/* Copy data to where ever the current write pointer says */
+	memcpy(ring->write_p, data, ring->buf.bpd);
+	barrier();
+	/* Update the pointer used to get most recent value.
+	 * Always valid as either points to latest or second latest value.
+	 * Before this runs it is null and read attempts fail with -EAGAIN.
+	 */
+	ring->last_written_p = ring->write_p;
+	barrier();
+	/* temp_ptr used to ensure we never have an invalid pointer
+	 * it may be slightly lagging, but never invalid
+	 */
+	temp_ptr = ring->write_p + ring->buf.bpd;
+	/* End of ring, back to the beginning */
+	if (temp_ptr == ring->data + ring->buf.length*ring->buf.bpd)
+		temp_ptr = ring->data;
+	/* Update the write pointer
+	 * always valid as long as this is the only function able to write.
+	 * Care needed with smp systems to ensure more than one ring fill
+	 * is never scheduled.
+	 */
+	ring->write_p = temp_ptr;
+
+	if (ring->read_p == 0)
+		ring->read_p = ring->data;
+	/* Buffer full - move the read pointer and create / escalate
+	 * ring event */
+	/* Tricky case - if the read pointer moves before we adjust it.
+	 * Handle by not pushing if it has moved - may result in occasional
+	 * unnecessary buffer full events when it wasn't quite true.
+	 */
+	else if (ring->write_p == ring->read_p) {
+		change_test_ptr = ring->read_p;
+		temp_ptr = change_test_ptr + ring->buf.bpd;
+		if (temp_ptr
+		    == ring->data + ring->buf.length*ring->buf.bpd) {
+			temp_ptr = ring->data;
+		}
+		/* We are moving pointer on one because the ring is full.  Any
+		 * change to the read pointer will be this or greater.
+		 */
+		if (change_test_ptr == ring->read_p)
+			ring->read_p = temp_ptr;
+
+		spin_lock(&ring->buf.shared_ev_pointer.lock);
+
+		ret = iio_push_or_escallate_ring_event(&ring->buf,
+						       IIO_EVENT_CODE_RING_100_FULL,
+						       timestamp);
+		spin_unlock(&ring->buf.shared_ev_pointer.lock);
+		if (ret)
+			goto error_ret;
+	}
+	/* investigate if our event barrier has been passed */
+	/* There are definite 'issues' with this and chances of
+	 * simultaneous read */
+	/* Also need to use loop count to ensure this only happens once */
+	ring->half_p += ring->buf.bpd;
+	if (ring->half_p == ring->data + ring->buf.length*ring->buf.bpd)
+		ring->half_p = ring->data;
+	if (ring->half_p == ring->read_p) {
+		spin_lock(&ring->buf.shared_ev_pointer.lock);
+		code = IIO_EVENT_CODE_RING_50_FULL;
+		ret = __iio_push_event(&ring->buf.ev_int,
+				       code,
+				       timestamp,
+				       &ring->buf.shared_ev_pointer);
+		spin_unlock(&ring->buf.shared_ev_pointer.lock);
+	}
+error_ret:
+	return ret;
+}
+
+int iio_rip_sw_rb(struct iio_ring_buffer *r,
+		  size_t count, u8 **data, int *dead_offset)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+
+	u8 *initial_read_p, *initial_write_p, *current_read_p, *end_read_p;
+	int ret, max_copied;
+	int bytes_to_rip;
+
+	/* A userspace program has probably made an error if it tries to
+	 *  read something that is not a whole number of bpds.
+	 * Return an error.
+	 */
+	if (count % ring->buf.bpd) {
+		ret = -EINVAL;
+		printk(KERN_INFO "Ring buffer read request not whole number of"
+		       "samples: Request bytes %zd, Current bpd %d\n",
+		       count, ring->buf.bpd);
+		goto error_ret;
+	}
+	/* Limit size to whole of ring buffer */
+	bytes_to_rip = min((size_t)(ring->buf.bpd*ring->buf.length), count);
+
+	*data = kmalloc(bytes_to_rip, GFP_KERNEL);
+	if (*data == NULL) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	/* build local copy */
+	initial_read_p = ring->read_p;
+	if (unlikely(initial_read_p == 0)) { /* No data here as yet */
+		ret = 0;
+		goto error_free_data_cpy;
+	}
+
+	initial_write_p = ring->write_p;
+
+	/* Need a consistent pair */
+	while ((initial_read_p != ring->read_p)
+	       || (initial_write_p != ring->write_p)) {
+		initial_read_p = ring->read_p;
+		initial_write_p = ring->write_p;
+	}
+	if (initial_write_p == initial_read_p) {
+		/* No new data available.*/
+		ret = 0;
+		goto error_free_data_cpy;
+	}
+
+	if (initial_write_p >= initial_read_p + bytes_to_rip) {
+		/* write_p is greater than necessary, all is easy */
+		max_copied = bytes_to_rip;
+		memcpy(*data, initial_read_p, max_copied);
+		end_read_p = initial_read_p + max_copied;
+	} else if (initial_write_p > initial_read_p) {
+		/*not enough data to cpy */
+		max_copied = initial_write_p - initial_read_p;
+		memcpy(*data, initial_read_p, max_copied);
+		end_read_p = initial_write_p;
+	} else {
+		/* going through 'end' of ring buffer */
+		max_copied = ring->data
+			+ ring->buf.length*ring->buf.bpd - initial_read_p;
+		memcpy(*data, initial_read_p, max_copied);
+		/* possible we are done if we align precisely with end */
+		if (max_copied == bytes_to_rip)
+			end_read_p = ring->data;
+		else if (initial_write_p
+			 > ring->data + bytes_to_rip - max_copied) {
+			/* enough data to finish */
+			memcpy(*data + max_copied, ring->data,
+			       bytes_to_rip - max_copied);
+			max_copied = bytes_to_rip;
+			end_read_p = ring->data + (bytes_to_rip - max_copied);
+		} else {  /* not enough data */
+			memcpy(*data + max_copied, ring->data,
+			       initial_write_p - ring->data);
+			max_copied += initial_write_p - ring->data;
+			end_read_p = initial_write_p;
+		}
+	}
+	/* Now to verify which section was cleanly copied - i.e. how far
+	 * read pointer has been pushed */
+	current_read_p = ring->read_p;
+
+	if (initial_read_p <= current_read_p)
+		*dead_offset = current_read_p - initial_read_p;
+	else
+		*dead_offset = ring->buf.length*ring->buf.bpd
+			- (initial_read_p - current_read_p);
+
+	/* possible issue if the initial write has been lapped or indeed
+	 * the point we were reading to has been passed */
+	/* No valid data read.
+	 * In this case the read pointer is already correct having been
+	 * pushed further than we would look. */
+	if (max_copied - *dead_offset < 0) {
+		ret = 0;
+		goto error_free_data_cpy;
+	}
+
+	/* setup the next read position */
+	/* Beware, this may fail due to concurrency fun and games.
+	 *  Possible that sufficient fill commands have run to push the read
+	 * pointer past where we would be after the rip. If this occurs, leave
+	 * it be.
+	 */
+	/* Tricky - deal with loops */
+
+	while (ring->read_p != end_read_p)
+		ring->read_p = end_read_p;
+
+	return max_copied - *dead_offset;
+
+error_free_data_cpy:
+	kfree(*data);
+error_ret:
+	return ret;
+}
+EXPORT_SYMBOL(iio_rip_sw_rb);
+
+int iio_store_to_sw_rb(struct iio_ring_buffer *r, u8 *data, s64 timestamp)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	return iio_store_to_sw_ring(ring, data, timestamp);
+}
+EXPORT_SYMBOL(iio_store_to_sw_rb);
+
+int iio_read_last_from_sw_ring(struct iio_sw_ring_buffer *ring,
+			       unsigned char *data)
+{
+	unsigned char *last_written_p_copy;
+
+	iio_mark_sw_rb_in_use(&ring->buf);
+again:
+	barrier();
+	last_written_p_copy = ring->last_written_p;
+	barrier(); /*unnessecary? */
+	/* Check there is anything here */
+	if (last_written_p_copy == 0)
+		return -EAGAIN;
+	memcpy(data, last_written_p_copy, ring->buf.bpd);
+
+	if (unlikely(ring->last_written_p >= last_written_p_copy))
+		goto again;
+
+	iio_unmark_sw_rb_in_use(&ring->buf);
+	return 0;
+}
+
+int iio_read_last_from_sw_rb(struct iio_ring_buffer *r,
+			     unsigned char *data)
+{
+	return iio_read_last_from_sw_ring(iio_to_sw_ring(r), data);
+}
+EXPORT_SYMBOL(iio_read_last_from_sw_rb);
+
+int iio_request_update_sw_rb(struct iio_ring_buffer *r)
+{
+	int ret = 0;
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+
+	spin_lock(&ring->use_lock);
+	if (!ring->update_needed)
+		goto error_ret;
+	if (ring->use_count) {
+		ret = -EAGAIN;
+		goto error_ret;
+	}
+	__iio_free_sw_ring_buffer(ring);
+	ret = __iio_init_sw_ring_buffer(ring, ring->buf.bpd, ring->buf.length);
+error_ret:
+	spin_unlock(&ring->use_lock);
+	return ret;
+}
+EXPORT_SYMBOL(iio_request_update_sw_rb);
+
+int iio_get_bpd_sw_rb(struct iio_ring_buffer *r)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	return ring->buf.bpd;
+}
+EXPORT_SYMBOL(iio_get_bpd_sw_rb);
+
+int iio_set_bpd_sw_rb(struct iio_ring_buffer *r, size_t bpd)
+{
+	if (r->bpd != bpd) {
+		r->bpd = bpd;
+		if (r->access.mark_param_change)
+			r->access.mark_param_change(r);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iio_set_bpd_sw_rb);
+
+int iio_get_length_sw_rb(struct iio_ring_buffer *r)
+{
+	return r->length;
+}
+EXPORT_SYMBOL(iio_get_length_sw_rb);
+
+int iio_set_length_sw_rb(struct iio_ring_buffer *r, int length)
+{
+	if (r->length != length) {
+		r->length = length;
+		if (r->access.mark_param_change)
+			r->access.mark_param_change(r);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iio_set_length_sw_rb);
+
+int iio_mark_update_needed_sw_rb(struct iio_ring_buffer *r)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	ring->update_needed = true;
+	return 0;
+}
+EXPORT_SYMBOL(iio_mark_update_needed_sw_rb);
+
+static void iio_sw_rb_release(struct device *dev)
+{
+	struct iio_ring_buffer *r = to_iio_ring_buffer(dev);
+	kfree(iio_to_sw_ring(r));
+}
+
+static IIO_RING_ENABLE_ATTR;
+static IIO_RING_BPS_ATTR;
+static IIO_RING_LENGTH_ATTR;
+
+/* Standard set of ring buffer attributes */
+static struct attribute *iio_ring_attributes[] = {
+	&dev_attr_length.attr,
+	&dev_attr_bps.attr,
+	&dev_attr_ring_enable.attr,
+	NULL,
+};
+
+static struct attribute_group iio_ring_attribute_group = {
+	.attrs = iio_ring_attributes,
+};
+
+static struct attribute_group *iio_ring_attribute_groups[] = {
+	&iio_ring_attribute_group,
+	NULL
+};
+
+static struct device_type iio_sw_ring_type = {
+	.release = iio_sw_rb_release,
+	.groups = iio_ring_attribute_groups,
+};
+
+struct iio_ring_buffer *iio_sw_rb_allocate(struct iio_dev *indio_dev)
+{
+	struct iio_ring_buffer *buf;
+	struct iio_sw_ring_buffer *ring;
+
+	ring = kzalloc(sizeof *ring, GFP_KERNEL);
+	if (!ring)
+		return 0;
+	buf = &ring->buf;
+
+	iio_ring_buffer_init(buf, indio_dev);
+	buf->dev.type = &iio_sw_ring_type;
+	device_initialize(&buf->dev);
+	buf->dev.parent = &indio_dev->dev;
+	buf->dev.class = &iio_class;
+	dev_set_drvdata(&buf->dev, (void *)buf);
+
+	return buf;
+}
+EXPORT_SYMBOL(iio_sw_rb_allocate);
+
+void iio_sw_rb_free(struct iio_ring_buffer *r)
+{
+	if (r)
+		iio_put_ring_buffer(r);
+}
+EXPORT_SYMBOL(iio_sw_rb_free);
+MODULE_DESCRIPTION("Industrialio I/O software ring buffer");
+MODULE_LICENSE("GPL");