292 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2#include <linux/slab.h>3#include <linux/kernel.h>4#include <linux/module.h>5#include <linux/device.h>6#include <linux/workqueue.h>7#include <linux/kfifo.h>8#include <linux/mutex.h>9#include <linux/iio/iio.h>10#include <linux/iio/buffer.h>11#include <linux/iio/kfifo_buf.h>12#include <linux/iio/buffer_impl.h>13#include <linux/sched.h>14#include <linux/poll.h>15 16struct iio_kfifo {17 struct iio_buffer buffer;18 struct kfifo kf;19 struct mutex user_lock;20 int update_needed;21};22 23#define iio_to_kfifo(r) container_of(r, struct iio_kfifo, buffer)24 25static inline int __iio_allocate_kfifo(struct iio_kfifo *buf,26 size_t bytes_per_datum, unsigned int length)27{28 if ((length == 0) || (bytes_per_datum == 0))29 return -EINVAL;30 31 /*32 * Make sure we don't overflow an unsigned int after kfifo rounds up to33 * the next power of 2.34 */35 if (roundup_pow_of_two(length) > UINT_MAX / bytes_per_datum)36 return -EINVAL;37 38 return __kfifo_alloc((struct __kfifo *)&buf->kf, length,39 bytes_per_datum, GFP_KERNEL);40}41 42static int iio_request_update_kfifo(struct iio_buffer *r)43{44 int ret = 0;45 struct iio_kfifo *buf = iio_to_kfifo(r);46 47 mutex_lock(&buf->user_lock);48 if (buf->update_needed) {49 kfifo_free(&buf->kf);50 ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum,51 buf->buffer.length);52 if (ret >= 0)53 buf->update_needed = false;54 } else {55 kfifo_reset_out(&buf->kf);56 }57 mutex_unlock(&buf->user_lock);58 59 return ret;60}61 62static int iio_mark_update_needed_kfifo(struct iio_buffer *r)63{64 struct iio_kfifo *kf = iio_to_kfifo(r);65 kf->update_needed = true;66 return 0;67}68 69static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd)70{71 if (r->bytes_per_datum != bpd) {72 r->bytes_per_datum = bpd;73 iio_mark_update_needed_kfifo(r);74 }75 return 0;76}77 78static int iio_set_length_kfifo(struct iio_buffer *r, unsigned int length)79{80 /* Avoid an invalid state */81 if (length < 2)82 length = 2;83 if (r->length != length) {84 r->length = length;85 iio_mark_update_needed_kfifo(r);86 }87 return 0;88}89 90static int iio_store_to_kfifo(struct iio_buffer *r,91 const void *data)92{93 int ret;94 struct iio_kfifo *kf = iio_to_kfifo(r);95 ret = kfifo_in(&kf->kf, data, 1);96 if (ret != 1)97 return -EBUSY;98 return 0;99}100 101static int iio_read_kfifo(struct iio_buffer *r, size_t n, char __user *buf)102{103 int ret, copied;104 struct iio_kfifo *kf = iio_to_kfifo(r);105 106 if (mutex_lock_interruptible(&kf->user_lock))107 return -ERESTARTSYS;108 109 if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf))110 ret = -EINVAL;111 else112 ret = kfifo_to_user(&kf->kf, buf, n, &copied);113 mutex_unlock(&kf->user_lock);114 if (ret < 0)115 return ret;116 117 return copied;118}119 120static size_t iio_kfifo_buf_data_available(struct iio_buffer *r)121{122 struct iio_kfifo *kf = iio_to_kfifo(r);123 size_t samples;124 125 mutex_lock(&kf->user_lock);126 samples = kfifo_len(&kf->kf);127 mutex_unlock(&kf->user_lock);128 129 return samples;130}131 132static void iio_kfifo_buffer_release(struct iio_buffer *buffer)133{134 struct iio_kfifo *kf = iio_to_kfifo(buffer);135 136 mutex_destroy(&kf->user_lock);137 kfifo_free(&kf->kf);138 kfree(kf);139}140 141static size_t iio_kfifo_buf_space_available(struct iio_buffer *r)142{143 struct iio_kfifo *kf = iio_to_kfifo(r);144 size_t avail;145 146 mutex_lock(&kf->user_lock);147 avail = kfifo_avail(&kf->kf);148 mutex_unlock(&kf->user_lock);149 150 return avail;151}152 153static int iio_kfifo_remove_from(struct iio_buffer *r, void *data)154{155 int ret;156 struct iio_kfifo *kf = iio_to_kfifo(r);157 158 if (kfifo_size(&kf->kf) < 1)159 return -EBUSY;160 161 ret = kfifo_out(&kf->kf, data, 1);162 if (ret != 1)163 return -EBUSY;164 165 wake_up_interruptible_poll(&r->pollq, EPOLLOUT | EPOLLWRNORM);166 167 return 0;168}169 170static int iio_kfifo_write(struct iio_buffer *r, size_t n,171 const char __user *buf)172{173 struct iio_kfifo *kf = iio_to_kfifo(r);174 int ret, copied;175 176 mutex_lock(&kf->user_lock);177 if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf))178 ret = -EINVAL;179 else180 ret = kfifo_from_user(&kf->kf, buf, n, &copied);181 mutex_unlock(&kf->user_lock);182 if (ret)183 return ret;184 185 return copied;186}187 188static const struct iio_buffer_access_funcs kfifo_access_funcs = {189 .store_to = &iio_store_to_kfifo,190 .read = &iio_read_kfifo,191 .data_available = iio_kfifo_buf_data_available,192 .remove_from = &iio_kfifo_remove_from,193 .write = &iio_kfifo_write,194 .space_available = &iio_kfifo_buf_space_available,195 .request_update = &iio_request_update_kfifo,196 .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,197 .set_length = &iio_set_length_kfifo,198 .release = &iio_kfifo_buffer_release,199 200 .modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED,201};202 203struct iio_buffer *iio_kfifo_allocate(void)204{205 struct iio_kfifo *kf;206 207 kf = kzalloc(sizeof(*kf), GFP_KERNEL);208 if (!kf)209 return NULL;210 211 kf->update_needed = true;212 iio_buffer_init(&kf->buffer);213 kf->buffer.access = &kfifo_access_funcs;214 kf->buffer.length = 2;215 mutex_init(&kf->user_lock);216 217 return &kf->buffer;218}219EXPORT_SYMBOL(iio_kfifo_allocate);220 221void iio_kfifo_free(struct iio_buffer *r)222{223 iio_buffer_put(r);224}225EXPORT_SYMBOL(iio_kfifo_free);226 227static void devm_iio_kfifo_release(struct device *dev, void *res)228{229 iio_kfifo_free(*(struct iio_buffer **)res);230}231 232/**233 * devm_iio_kfifo_allocate - Resource-managed iio_kfifo_allocate()234 * @dev: Device to allocate kfifo buffer for235 *236 * RETURNS:237 * Pointer to allocated iio_buffer on success, NULL on failure.238 */239static struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev)240{241 struct iio_buffer **ptr, *r;242 243 ptr = devres_alloc(devm_iio_kfifo_release, sizeof(*ptr), GFP_KERNEL);244 if (!ptr)245 return NULL;246 247 r = iio_kfifo_allocate();248 if (r) {249 *ptr = r;250 devres_add(dev, ptr);251 } else {252 devres_free(ptr);253 }254 255 return r;256}257 258/**259 * devm_iio_kfifo_buffer_setup_ext - Allocate a kfifo buffer & attach it to an IIO device260 * @dev: Device object to which to attach the life-time of this kfifo buffer261 * @indio_dev: The device the buffer should be attached to262 * @setup_ops: The setup_ops required to configure the HW part of the buffer (optional)263 * @buffer_attrs: Extra sysfs buffer attributes for this IIO buffer264 *265 * This function allocates a kfifo buffer via devm_iio_kfifo_allocate() and266 * attaches it to the IIO device via iio_device_attach_buffer().267 * This is meant to be a bit of a short-hand/helper function as there are a few268 * drivers that seem to do this.269 */270int devm_iio_kfifo_buffer_setup_ext(struct device *dev,271 struct iio_dev *indio_dev,272 const struct iio_buffer_setup_ops *setup_ops,273 const struct iio_dev_attr **buffer_attrs)274{275 struct iio_buffer *buffer;276 277 buffer = devm_iio_kfifo_allocate(dev);278 if (!buffer)279 return -ENOMEM;280 281 indio_dev->modes |= INDIO_BUFFER_SOFTWARE;282 indio_dev->setup_ops = setup_ops;283 284 buffer->attrs = buffer_attrs;285 286 return iio_device_attach_buffer(indio_dev, buffer);287}288EXPORT_SYMBOL_GPL(devm_iio_kfifo_buffer_setup_ext);289 290MODULE_DESCRIPTION("Industrial I/O buffering based on kfifo");291MODULE_LICENSE("GPL");292