From 2c3c1048746a4622d8c89a29670120dc8fab93c4 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Sun, 7 Apr 2024 20:49:45 +0200 Subject: Adding upstream version 6.1.76. Signed-off-by: Daniel Baumann --- drivers/iio/buffer/kfifo_buf.c | 290 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 290 insertions(+) create mode 100644 drivers/iio/buffer/kfifo_buf.c (limited to 'drivers/iio/buffer/kfifo_buf.c') diff --git a/drivers/iio/buffer/kfifo_buf.c b/drivers/iio/buffer/kfifo_buf.c new file mode 100644 index 000000000..35d8b4077 --- /dev/null +++ b/drivers/iio/buffer/kfifo_buf.c @@ -0,0 +1,290 @@ +// SPDX-License-Identifier: GPL-2.0-only +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct iio_kfifo { + struct iio_buffer buffer; + struct kfifo kf; + struct mutex user_lock; + int update_needed; +}; + +#define iio_to_kfifo(r) container_of(r, struct iio_kfifo, buffer) + +static inline int __iio_allocate_kfifo(struct iio_kfifo *buf, + size_t bytes_per_datum, unsigned int length) +{ + if ((length == 0) || (bytes_per_datum == 0)) + return -EINVAL; + + /* + * Make sure we don't overflow an unsigned int after kfifo rounds up to + * the next power of 2. + */ + if (roundup_pow_of_two(length) > UINT_MAX / bytes_per_datum) + return -EINVAL; + + return __kfifo_alloc((struct __kfifo *)&buf->kf, length, + bytes_per_datum, GFP_KERNEL); +} + +static int iio_request_update_kfifo(struct iio_buffer *r) +{ + int ret = 0; + struct iio_kfifo *buf = iio_to_kfifo(r); + + mutex_lock(&buf->user_lock); + if (buf->update_needed) { + kfifo_free(&buf->kf); + ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum, + buf->buffer.length); + if (ret >= 0) + buf->update_needed = false; + } else { + kfifo_reset_out(&buf->kf); + } + mutex_unlock(&buf->user_lock); + + return ret; +} + +static int iio_mark_update_needed_kfifo(struct iio_buffer *r) +{ + struct iio_kfifo *kf = iio_to_kfifo(r); + kf->update_needed = true; + return 0; +} + +static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd) +{ + if (r->bytes_per_datum != bpd) { + r->bytes_per_datum = bpd; + iio_mark_update_needed_kfifo(r); + } + return 0; +} + +static int iio_set_length_kfifo(struct iio_buffer *r, unsigned int length) +{ + /* Avoid an invalid state */ + if (length < 2) + length = 2; + if (r->length != length) { + r->length = length; + iio_mark_update_needed_kfifo(r); + } + return 0; +} + +static int iio_store_to_kfifo(struct iio_buffer *r, + const void *data) +{ + int ret; + struct iio_kfifo *kf = iio_to_kfifo(r); + ret = kfifo_in(&kf->kf, data, 1); + if (ret != 1) + return -EBUSY; + return 0; +} + +static int iio_read_kfifo(struct iio_buffer *r, size_t n, char __user *buf) +{ + int ret, copied; + struct iio_kfifo *kf = iio_to_kfifo(r); + + if (mutex_lock_interruptible(&kf->user_lock)) + return -ERESTARTSYS; + + if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf)) + ret = -EINVAL; + else + ret = kfifo_to_user(&kf->kf, buf, n, &copied); + mutex_unlock(&kf->user_lock); + if (ret < 0) + return ret; + + return copied; +} + +static size_t iio_kfifo_buf_data_available(struct iio_buffer *r) +{ + struct iio_kfifo *kf = iio_to_kfifo(r); + size_t samples; + + mutex_lock(&kf->user_lock); + samples = kfifo_len(&kf->kf); + mutex_unlock(&kf->user_lock); + + return samples; +} + +static void iio_kfifo_buffer_release(struct iio_buffer *buffer) +{ + struct iio_kfifo *kf = iio_to_kfifo(buffer); + + mutex_destroy(&kf->user_lock); + kfifo_free(&kf->kf); + kfree(kf); +} + +static size_t iio_kfifo_buf_space_available(struct iio_buffer *r) +{ + struct iio_kfifo *kf = iio_to_kfifo(r); + size_t avail; + + mutex_lock(&kf->user_lock); + avail = kfifo_avail(&kf->kf); + mutex_unlock(&kf->user_lock); + + return avail; +} + +static int iio_kfifo_remove_from(struct iio_buffer *r, void *data) +{ + int ret; + struct iio_kfifo *kf = iio_to_kfifo(r); + + if (kfifo_size(&kf->kf) < 1) + return -EBUSY; + + ret = kfifo_out(&kf->kf, data, 1); + if (ret != 1) + return -EBUSY; + + wake_up_interruptible_poll(&r->pollq, EPOLLOUT | EPOLLWRNORM); + + return 0; +} + +static int iio_kfifo_write(struct iio_buffer *r, size_t n, + const char __user *buf) +{ + struct iio_kfifo *kf = iio_to_kfifo(r); + int ret, copied; + + mutex_lock(&kf->user_lock); + if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf)) + ret = -EINVAL; + else + ret = kfifo_from_user(&kf->kf, buf, n, &copied); + mutex_unlock(&kf->user_lock); + if (ret) + return ret; + + return copied; +} + +static const struct iio_buffer_access_funcs kfifo_access_funcs = { + .store_to = &iio_store_to_kfifo, + .read = &iio_read_kfifo, + .data_available = iio_kfifo_buf_data_available, + .remove_from = &iio_kfifo_remove_from, + .write = &iio_kfifo_write, + .space_available = &iio_kfifo_buf_space_available, + .request_update = &iio_request_update_kfifo, + .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo, + .set_length = &iio_set_length_kfifo, + .release = &iio_kfifo_buffer_release, + + .modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED, +}; + +struct iio_buffer *iio_kfifo_allocate(void) +{ + struct iio_kfifo *kf; + + kf = kzalloc(sizeof(*kf), GFP_KERNEL); + if (!kf) + return NULL; + + kf->update_needed = true; + iio_buffer_init(&kf->buffer); + kf->buffer.access = &kfifo_access_funcs; + kf->buffer.length = 2; + mutex_init(&kf->user_lock); + + return &kf->buffer; +} +EXPORT_SYMBOL(iio_kfifo_allocate); + +void iio_kfifo_free(struct iio_buffer *r) +{ + iio_buffer_put(r); +} +EXPORT_SYMBOL(iio_kfifo_free); + +static void devm_iio_kfifo_release(struct device *dev, void *res) +{ + iio_kfifo_free(*(struct iio_buffer **)res); +} + +/** + * devm_iio_kfifo_allocate - Resource-managed iio_kfifo_allocate() + * @dev: Device to allocate kfifo buffer for + * + * RETURNS: + * Pointer to allocated iio_buffer on success, NULL on failure. + */ +static struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev) +{ + struct iio_buffer **ptr, *r; + + ptr = devres_alloc(devm_iio_kfifo_release, sizeof(*ptr), GFP_KERNEL); + if (!ptr) + return NULL; + + r = iio_kfifo_allocate(); + if (r) { + *ptr = r; + devres_add(dev, ptr); + } else { + devres_free(ptr); + } + + return r; +} + +/** + * devm_iio_kfifo_buffer_setup_ext - Allocate a kfifo buffer & attach it to an IIO device + * @dev: Device object to which to attach the life-time of this kfifo buffer + * @indio_dev: The device the buffer should be attached to + * @setup_ops: The setup_ops required to configure the HW part of the buffer (optional) + * @buffer_attrs: Extra sysfs buffer attributes for this IIO buffer + * + * This function allocates a kfifo buffer via devm_iio_kfifo_allocate() and + * attaches it to the IIO device via iio_device_attach_buffer(). + * This is meant to be a bit of a short-hand/helper function as there are a few + * drivers that seem to do this. + */ +int devm_iio_kfifo_buffer_setup_ext(struct device *dev, + struct iio_dev *indio_dev, + const struct iio_buffer_setup_ops *setup_ops, + const struct attribute **buffer_attrs) +{ + struct iio_buffer *buffer; + + buffer = devm_iio_kfifo_allocate(dev); + if (!buffer) + return -ENOMEM; + + indio_dev->modes |= INDIO_BUFFER_SOFTWARE; + indio_dev->setup_ops = setup_ops; + + buffer->attrs = buffer_attrs; + + return iio_device_attach_buffer(indio_dev, buffer); +} +EXPORT_SYMBOL_GPL(devm_iio_kfifo_buffer_setup_ext); + +MODULE_LICENSE("GPL"); -- cgit v1.2.3