iio/buffer/kfifo_buf.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 #include <linux/slab.h>
0003 #include <linux/kernel.h>
0004 #include <linux/module.h>
0005 #include <linux/device.h>
0006 #include <linux/workqueue.h>
0007 #include <linux/kfifo.h>
0008 #include <linux/mutex.h>
0009 #include <linux/iio/iio.h>
0010 #include <linux/iio/buffer.h>
0011 #include <linux/iio/kfifo_buf.h>
0012 #include <linux/iio/buffer_impl.h>
0013 #include <linux/sched.h>
0014 #include <linux/poll.h>
0015
0016 struct iio_kfifo {
0017     struct iio_buffer buffer;
0018     struct kfifo kf;
0019     struct mutex user_lock;
0020     int update_needed;
0021 };
0022
0023 #define iio_to_kfifo(r) container_of(r, struct iio_kfifo, buffer)
0024
0025 static inline int __iio_allocate_kfifo(struct iio_kfifo *buf,
0026             size_t bytes_per_datum, unsigned int length)
0027 {
0028     if ((length == 0) || (bytes_per_datum == 0))
0029         return -EINVAL;
0030
0031     /*
0032      * Make sure we don't overflow an unsigned int after kfifo rounds up to
0033      * the next power of 2.
0034      */
0035     if (roundup_pow_of_two(length) > UINT_MAX / bytes_per_datum)
0036         return -EINVAL;
0037
0038     return __kfifo_alloc((struct __kfifo *)&buf->kf, length,
0039                  bytes_per_datum, GFP_KERNEL);
0040 }
0041
0042 static int iio_request_update_kfifo(struct iio_buffer *r)
0043 {
0044     int ret = 0;
0045     struct iio_kfifo *buf = iio_to_kfifo(r);
0046
0047     mutex_lock(&buf->user_lock);
0048     if (buf->update_needed) {
0049         kfifo_free(&buf->kf);
0050         ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum,
0051                    buf->buffer.length);
0052         if (ret >= 0)
0053             buf->update_needed = false;
0054     } else {
0055         kfifo_reset_out(&buf->kf);
0056     }
0057     mutex_unlock(&buf->user_lock);
0058
0059     return ret;
0060 }
0061
0062 static int iio_mark_update_needed_kfifo(struct iio_buffer *r)
0063 {
0064     struct iio_kfifo *kf = iio_to_kfifo(r);
0065     kf->update_needed = true;
0066     return 0;
0067 }
0068
0069 static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd)
0070 {
0071     if (r->bytes_per_datum != bpd) {
0072         r->bytes_per_datum = bpd;
0073         iio_mark_update_needed_kfifo(r);
0074     }
0075     return 0;
0076 }
0077
0078 static int iio_set_length_kfifo(struct iio_buffer *r, unsigned int length)
0079 {
0080     /* Avoid an invalid state */
0081     if (length < 2)
0082         length = 2;
0083     if (r->length != length) {
0084         r->length = length;
0085         iio_mark_update_needed_kfifo(r);
0086     }
0087     return 0;
0088 }
0089
0090 static int iio_store_to_kfifo(struct iio_buffer *r,
0091                   const void *data)
0092 {
0093     int ret;
0094     struct iio_kfifo *kf = iio_to_kfifo(r);
0095     ret = kfifo_in(&kf->kf, data, 1);
0096     if (ret != 1)
0097         return -EBUSY;
0098     return 0;
0099 }
0100
0101 static int iio_read_kfifo(struct iio_buffer *r, size_t n, char __user *buf)
0102 {
0103     int ret, copied;
0104     struct iio_kfifo *kf = iio_to_kfifo(r);
0105
0106     if (mutex_lock_interruptible(&kf->user_lock))
0107         return -ERESTARTSYS;
0108
0109     if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf))
0110         ret = -EINVAL;
0111     else
0112         ret = kfifo_to_user(&kf->kf, buf, n, &copied);
0113     mutex_unlock(&kf->user_lock);
0114     if (ret < 0)
0115         return ret;
0116
0117     return copied;
0118 }
0119
0120 static size_t iio_kfifo_buf_data_available(struct iio_buffer *r)
0121 {
0122     struct iio_kfifo *kf = iio_to_kfifo(r);
0123     size_t samples;
0124
0125     mutex_lock(&kf->user_lock);
0126     samples = kfifo_len(&kf->kf);
0127     mutex_unlock(&kf->user_lock);
0128
0129     return samples;
0130 }
0131
0132 static void iio_kfifo_buffer_release(struct iio_buffer *buffer)
0133 {
0134     struct iio_kfifo *kf = iio_to_kfifo(buffer);
0135
0136     mutex_destroy(&kf->user_lock);
0137     kfifo_free(&kf->kf);
0138     kfree(kf);
0139 }
0140
0141 static size_t iio_kfifo_buf_space_available(struct iio_buffer *r)
0142 {
0143     struct iio_kfifo *kf = iio_to_kfifo(r);
0144     size_t avail;
0145
0146     mutex_lock(&kf->user_lock);
0147     avail = kfifo_avail(&kf->kf);
0148     mutex_unlock(&kf->user_lock);
0149
0150     return avail;
0151 }
0152
0153 static int iio_kfifo_remove_from(struct iio_buffer *r, void *data)
0154 {
0155     int ret;
0156     struct iio_kfifo *kf = iio_to_kfifo(r);
0157
0158     if (kfifo_size(&kf->kf) < 1)
0159         return -EBUSY;
0160
0161     ret = kfifo_out(&kf->kf, data, 1);
0162     if (ret != 1)
0163         return -EBUSY;
0164
0165     wake_up_interruptible_poll(&r->pollq, EPOLLOUT | EPOLLWRNORM);
0166
0167     return 0;
0168 }
0169
0170 static int iio_kfifo_write(struct iio_buffer *r, size_t n,
0171     const char __user *buf)
0172 {
0173     struct iio_kfifo *kf = iio_to_kfifo(r);
0174     int ret, copied;
0175
0176     mutex_lock(&kf->user_lock);
0177     if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf))
0178         ret = -EINVAL;
0179     else
0180         ret = kfifo_from_user(&kf->kf, buf, n, &copied);
0181     mutex_unlock(&kf->user_lock);
0182     if (ret)
0183         return ret;
0184
0185     return copied;
0186 }
0187
0188 static const struct iio_buffer_access_funcs kfifo_access_funcs = {
0189     .store_to = &iio_store_to_kfifo,
0190     .read = &iio_read_kfifo,
0191     .data_available = iio_kfifo_buf_data_available,
0192     .remove_from = &iio_kfifo_remove_from,
0193     .write = &iio_kfifo_write,
0194     .space_available = &iio_kfifo_buf_space_available,
0195     .request_update = &iio_request_update_kfifo,
0196     .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,
0197     .set_length = &iio_set_length_kfifo,
0198     .release = &iio_kfifo_buffer_release,
0199
0200     .modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED,
0201 };
0202
0203 struct iio_buffer *iio_kfifo_allocate(void)
0204 {
0205     struct iio_kfifo *kf;
0206
0207     kf = kzalloc(sizeof(*kf), GFP_KERNEL);
0208     if (!kf)
0209         return NULL;
0210
0211     kf->update_needed = true;
0212     iio_buffer_init(&kf->buffer);
0213     kf->buffer.access = &kfifo_access_funcs;
0214     kf->buffer.length = 2;
0215     mutex_init(&kf->user_lock);
0216
0217     return &kf->buffer;
0218 }
0219 EXPORT_SYMBOL(iio_kfifo_allocate);
0220
0221 void iio_kfifo_free(struct iio_buffer *r)
0222 {
0223     iio_buffer_put(r);
0224 }
0225 EXPORT_SYMBOL(iio_kfifo_free);
0226
0227 static void devm_iio_kfifo_release(struct device *dev, void *res)
0228 {
0229     iio_kfifo_free(*(struct iio_buffer **)res);
0230 }
0231
0232 /**
0233  * devm_iio_kfifo_allocate - Resource-managed iio_kfifo_allocate()
0234  * @dev:        Device to allocate kfifo buffer for
0235  *
0236  * RETURNS:
0237  * Pointer to allocated iio_buffer on success, NULL on failure.
0238  */
0239 static struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev)
0240 {
0241     struct iio_buffer **ptr, *r;
0242
0243     ptr = devres_alloc(devm_iio_kfifo_release, sizeof(*ptr), GFP_KERNEL);
0244     if (!ptr)
0245         return NULL;
0246
0247     r = iio_kfifo_allocate();
0248     if (r) {
0249         *ptr = r;
0250         devres_add(dev, ptr);
0251     } else {
0252         devres_free(ptr);
0253     }
0254
0255     return r;
0256 }
0257
0258 /**
0259  * devm_iio_kfifo_buffer_setup_ext - Allocate a kfifo buffer & attach it to an IIO device
0260  * @dev: Device object to which to attach the life-time of this kfifo buffer
0261  * @indio_dev: The device the buffer should be attached to
0262  * @setup_ops: The setup_ops required to configure the HW part of the buffer (optional)
0263  * @buffer_attrs: Extra sysfs buffer attributes for this IIO buffer
0264  *
0265  * This function allocates a kfifo buffer via devm_iio_kfifo_allocate() and
0266  * attaches it to the IIO device via iio_device_attach_buffer().
0267  * This is meant to be a bit of a short-hand/helper function as there are a few
0268  * drivers that seem to do this.
0269  */
0270 int devm_iio_kfifo_buffer_setup_ext(struct device *dev,
0271                     struct iio_dev *indio_dev,
0272                     const struct iio_buffer_setup_ops *setup_ops,
0273                     const struct attribute **buffer_attrs)
0274 {
0275     struct iio_buffer *buffer;
0276
0277     buffer = devm_iio_kfifo_allocate(dev);
0278     if (!buffer)
0279         return -ENOMEM;
0280
0281     indio_dev->modes |= INDIO_BUFFER_SOFTWARE;
0282     indio_dev->setup_ops = setup_ops;
0283
0284     buffer->attrs = buffer_attrs;
0285
0286     return iio_device_attach_buffer(indio_dev, buffer);
0287 }
0288 EXPORT_SYMBOL_GPL(devm_iio_kfifo_buffer_setup_ext);
0289
0290 MODULE_LICENSE("GPL");