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 const struct iio_buffer_access_funcs kfifo_access_funcs = { 142 .store_to = &iio_store_to_kfifo, 143 .read = &iio_read_kfifo, 144 .data_available = iio_kfifo_buf_data_available, 145 .request_update = &iio_request_update_kfifo, 146 .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo, 147 .set_length = &iio_set_length_kfifo, 148 .release = &iio_kfifo_buffer_release, 149 150 .modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED, 151 }; 152 153 struct iio_buffer *iio_kfifo_allocate(void) 154 { 155 struct iio_kfifo *kf; 156 157 kf = kzalloc(sizeof(*kf), GFP_KERNEL); 158 if (!kf) 159 return NULL; 160 161 kf->update_needed = true; 162 iio_buffer_init(&kf->buffer); 163 kf->buffer.access = &kfifo_access_funcs; 164 kf->buffer.length = 2; 165 mutex_init(&kf->user_lock); 166 167 return &kf->buffer; 168 } 169 EXPORT_SYMBOL(iio_kfifo_allocate); 170 171 void iio_kfifo_free(struct iio_buffer *r) 172 { 173 iio_buffer_put(r); 174 } 175 EXPORT_SYMBOL(iio_kfifo_free); 176 177 static void devm_iio_kfifo_release(struct device *dev, void *res) 178 { 179 iio_kfifo_free(*(struct iio_buffer **)res); 180 } 181 182 static int devm_iio_kfifo_match(struct device *dev, void *res, void *data) 183 { 184 struct iio_buffer **r = res; 185 186 if (WARN_ON(!r || !*r)) 187 return 0; 188 189 return *r == data; 190 } 191 192 /** 193 * devm_iio_fifo_allocate - Resource-managed iio_kfifo_allocate() 194 * @dev: Device to allocate kfifo buffer for 195 * 196 * RETURNS: 197 * Pointer to allocated iio_buffer on success, NULL on failure. 198 */ 199 struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev) 200 { 201 struct iio_buffer **ptr, *r; 202 203 ptr = devres_alloc(devm_iio_kfifo_release, sizeof(*ptr), GFP_KERNEL); 204 if (!ptr) 205 return NULL; 206 207 r = iio_kfifo_allocate(); 208 if (r) { 209 *ptr = r; 210 devres_add(dev, ptr); 211 } else { 212 devres_free(ptr); 213 } 214 215 return r; 216 } 217 EXPORT_SYMBOL(devm_iio_kfifo_allocate); 218 219 /** 220 * devm_iio_fifo_free - Resource-managed iio_kfifo_free() 221 * @dev: Device the buffer belongs to 222 * @r: The buffer associated with the device 223 */ 224 void devm_iio_kfifo_free(struct device *dev, struct iio_buffer *r) 225 { 226 WARN_ON(devres_release(dev, devm_iio_kfifo_release, 227 devm_iio_kfifo_match, r)); 228 } 229 EXPORT_SYMBOL(devm_iio_kfifo_free); 230 231 MODULE_LICENSE("GPL"); 232