1 #include <linux/slab.h> 2 #include <linux/kernel.h> 3 #include <linux/module.h> 4 #include <linux/device.h> 5 #include <linux/workqueue.h> 6 #include <linux/kfifo.h> 7 #include <linux/mutex.h> 8 #include <linux/iio/iio.h> 9 #include <linux/iio/buffer.h> 10 #include <linux/iio/kfifo_buf.h> 11 #include <linux/iio/buffer_impl.h> 12 #include <linux/sched.h> 13 #include <linux/poll.h> 14 15 struct iio_kfifo { 16 struct iio_buffer buffer; 17 struct kfifo kf; 18 struct mutex user_lock; 19 int update_needed; 20 }; 21 22 #define iio_to_kfifo(r) container_of(r, struct iio_kfifo, buffer) 23 24 static inline int __iio_allocate_kfifo(struct iio_kfifo *buf, 25 size_t bytes_per_datum, unsigned int length) 26 { 27 if ((length == 0) || (bytes_per_datum == 0)) 28 return -EINVAL; 29 30 /* 31 * Make sure we don't overflow an unsigned int after kfifo rounds up to 32 * the next power of 2. 33 */ 34 if (roundup_pow_of_two(length) > UINT_MAX / bytes_per_datum) 35 return -EINVAL; 36 37 return __kfifo_alloc((struct __kfifo *)&buf->kf, length, 38 bytes_per_datum, GFP_KERNEL); 39 } 40 41 static int iio_request_update_kfifo(struct iio_buffer *r) 42 { 43 int ret = 0; 44 struct iio_kfifo *buf = iio_to_kfifo(r); 45 46 mutex_lock(&buf->user_lock); 47 if (buf->update_needed) { 48 kfifo_free(&buf->kf); 49 ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum, 50 buf->buffer.length); 51 if (ret >= 0) 52 buf->update_needed = false; 53 } else { 54 kfifo_reset_out(&buf->kf); 55 } 56 mutex_unlock(&buf->user_lock); 57 58 return ret; 59 } 60 61 static int iio_mark_update_needed_kfifo(struct iio_buffer *r) 62 { 63 struct iio_kfifo *kf = iio_to_kfifo(r); 64 kf->update_needed = true; 65 return 0; 66 } 67 68 static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd) 69 { 70 if (r->bytes_per_datum != bpd) { 71 r->bytes_per_datum = bpd; 72 iio_mark_update_needed_kfifo(r); 73 } 74 return 0; 75 } 76 77 static int iio_set_length_kfifo(struct iio_buffer *r, unsigned int length) 78 { 79 /* Avoid an invalid state */ 80 if (length < 2) 81 length = 2; 82 if (r->length != length) { 83 r->length = length; 84 iio_mark_update_needed_kfifo(r); 85 } 86 return 0; 87 } 88 89 static int iio_store_to_kfifo(struct iio_buffer *r, 90 const void *data) 91 { 92 int ret; 93 struct iio_kfifo *kf = iio_to_kfifo(r); 94 ret = kfifo_in(&kf->kf, data, 1); 95 if (ret != 1) 96 return -EBUSY; 97 return 0; 98 } 99 100 static int iio_read_first_n_kfifo(struct iio_buffer *r, 101 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_first_n = &iio_read_first_n_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