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_first_n_kfifo(struct iio_buffer *r, 102 size_t n, char __user *buf) 103 { 104 int ret, copied; 105 struct iio_kfifo *kf = iio_to_kfifo(r); 106 107 if (mutex_lock_interruptible(&kf->user_lock)) 108 return -ERESTARTSYS; 109 110 if (!kfifo_initialized(&kf->kf) || n < kfifo_esize(&kf->kf)) 111 ret = -EINVAL; 112 else 113 ret = kfifo_to_user(&kf->kf, buf, n, &copied); 114 mutex_unlock(&kf->user_lock); 115 if (ret < 0) 116 return ret; 117 118 return copied; 119 } 120 121 static size_t iio_kfifo_buf_data_available(struct iio_buffer *r) 122 { 123 struct iio_kfifo *kf = iio_to_kfifo(r); 124 size_t samples; 125 126 mutex_lock(&kf->user_lock); 127 samples = kfifo_len(&kf->kf); 128 mutex_unlock(&kf->user_lock); 129 130 return samples; 131 } 132 133 static void iio_kfifo_buffer_release(struct iio_buffer *buffer) 134 { 135 struct iio_kfifo *kf = iio_to_kfifo(buffer); 136 137 mutex_destroy(&kf->user_lock); 138 kfifo_free(&kf->kf); 139 kfree(kf); 140 } 141 142 static const struct iio_buffer_access_funcs kfifo_access_funcs = { 143 .store_to = &iio_store_to_kfifo, 144 .read_first_n = &iio_read_first_n_kfifo, 145 .data_available = iio_kfifo_buf_data_available, 146 .request_update = &iio_request_update_kfifo, 147 .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo, 148 .set_length = &iio_set_length_kfifo, 149 .release = &iio_kfifo_buffer_release, 150 151 .modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED, 152 }; 153 154 struct iio_buffer *iio_kfifo_allocate(void) 155 { 156 struct iio_kfifo *kf; 157 158 kf = kzalloc(sizeof(*kf), GFP_KERNEL); 159 if (!kf) 160 return NULL; 161 162 kf->update_needed = true; 163 iio_buffer_init(&kf->buffer); 164 kf->buffer.access = &kfifo_access_funcs; 165 kf->buffer.length = 2; 166 mutex_init(&kf->user_lock); 167 168 return &kf->buffer; 169 } 170 EXPORT_SYMBOL(iio_kfifo_allocate); 171 172 void iio_kfifo_free(struct iio_buffer *r) 173 { 174 iio_buffer_put(r); 175 } 176 EXPORT_SYMBOL(iio_kfifo_free); 177 178 static void devm_iio_kfifo_release(struct device *dev, void *res) 179 { 180 iio_kfifo_free(*(struct iio_buffer **)res); 181 } 182 183 static int devm_iio_kfifo_match(struct device *dev, void *res, void *data) 184 { 185 struct iio_buffer **r = res; 186 187 if (WARN_ON(!r || !*r)) 188 return 0; 189 190 return *r == data; 191 } 192 193 /** 194 * devm_iio_fifo_allocate - Resource-managed iio_kfifo_allocate() 195 * @dev: Device to allocate kfifo buffer for 196 * 197 * RETURNS: 198 * Pointer to allocated iio_buffer on success, NULL on failure. 199 */ 200 struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev) 201 { 202 struct iio_buffer **ptr, *r; 203 204 ptr = devres_alloc(devm_iio_kfifo_release, sizeof(*ptr), GFP_KERNEL); 205 if (!ptr) 206 return NULL; 207 208 r = iio_kfifo_allocate(); 209 if (r) { 210 *ptr = r; 211 devres_add(dev, ptr); 212 } else { 213 devres_free(ptr); 214 } 215 216 return r; 217 } 218 EXPORT_SYMBOL(devm_iio_kfifo_allocate); 219 220 /** 221 * devm_iio_fifo_free - Resource-managed iio_kfifo_free() 222 * @dev: Device the buffer belongs to 223 * @r: The buffer associated with the device 224 */ 225 void devm_iio_kfifo_free(struct device *dev, struct iio_buffer *r) 226 { 227 WARN_ON(devres_release(dev, devm_iio_kfifo_release, 228 devm_iio_kfifo_match, r)); 229 } 230 EXPORT_SYMBOL(devm_iio_kfifo_free); 231 232 MODULE_LICENSE("GPL"); 233