1 // SPDX-License-Identifier: GPL-2.0-only 2 #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 16 struct 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 25 static 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 to 33 * 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 42 static 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 62 static 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 69 static 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 78 static 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 90 static 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 101 static 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 else 112 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 120 static 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 132 static 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 141 static 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 153 static 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 170 static 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 else 180 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 188 static 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 203 struct 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 } 219 EXPORT_SYMBOL(iio_kfifo_allocate); 220 221 void iio_kfifo_free(struct iio_buffer *r) 222 { 223 iio_buffer_put(r); 224 } 225 EXPORT_SYMBOL(iio_kfifo_free); 226 227 static 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 for 235 * 236 * RETURNS: 237 * Pointer to allocated iio_buffer on success, NULL on failure. 238 */ 239 static 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 device 260 * @dev: Device object to which to attach the life-time of this kfifo buffer 261 * @indio_dev: The device the buffer should be attached to 262 * @mode_flags: The mode flags for this buffer (INDIO_BUFFER_SOFTWARE and/or 263 * INDIO_BUFFER_TRIGGERED). 264 * @setup_ops: The setup_ops required to configure the HW part of the buffer (optional) 265 * @buffer_attrs: Extra sysfs buffer attributes for this IIO buffer 266 * 267 * This function allocates a kfifo buffer via devm_iio_kfifo_allocate() and 268 * attaches it to the IIO device via iio_device_attach_buffer(). 269 * This is meant to be a bit of a short-hand/helper function as there are a few 270 * drivers that seem to do this. 271 */ 272 int devm_iio_kfifo_buffer_setup_ext(struct device *dev, 273 struct iio_dev *indio_dev, 274 int mode_flags, 275 const struct iio_buffer_setup_ops *setup_ops, 276 const struct attribute **buffer_attrs) 277 { 278 struct iio_buffer *buffer; 279 280 if (!mode_flags) 281 return -EINVAL; 282 283 buffer = devm_iio_kfifo_allocate(dev); 284 if (!buffer) 285 return -ENOMEM; 286 287 mode_flags &= kfifo_access_funcs.modes; 288 289 indio_dev->modes |= mode_flags; 290 indio_dev->setup_ops = setup_ops; 291 292 buffer->attrs = buffer_attrs; 293 294 return iio_device_attach_buffer(indio_dev, buffer); 295 } 296 EXPORT_SYMBOL_GPL(devm_iio_kfifo_buffer_setup_ext); 297 298 MODULE_LICENSE("GPL"); 299