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(fifo: (struct __kfifo *)&buf->kf, size: length, |
39 | esize: 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, bytes_per_datum: buf->buffer.bytes_per_datum, |
51 | length: buf->buffer.length); |
52 | if (ret >= 0) |
53 | buf->update_needed = false; |
54 | } else { |
55 | kfifo_reset_out(&buf->kf); |
56 | } |
57 | mutex_unlock(lock: &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(lock: &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(lock: &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(lock: &kf->user_lock); |
137 | kfifo_free(&kf->kf); |
138 | kfree(objp: 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(lock: &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(lock: &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(size: sizeof(*kf), GFP_KERNEL); |
208 | if (!kf) |
209 | return NULL; |
210 | |
211 | kf->update_needed = true; |
212 | iio_buffer_init(buffer: &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(buffer: 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, res: ptr); |
251 | } else { |
252 | devres_free(res: 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 | * @setup_ops: The setup_ops required to configure the HW part of the buffer (optional) |
263 | * @buffer_attrs: Extra sysfs buffer attributes for this IIO buffer |
264 | * |
265 | * This function allocates a kfifo buffer via devm_iio_kfifo_allocate() and |
266 | * attaches it to the IIO device via iio_device_attach_buffer(). |
267 | * This is meant to be a bit of a short-hand/helper function as there are a few |
268 | * drivers that seem to do this. |
269 | */ |
270 | int devm_iio_kfifo_buffer_setup_ext(struct device *dev, |
271 | struct iio_dev *indio_dev, |
272 | const struct iio_buffer_setup_ops *setup_ops, |
273 | const struct iio_dev_attr **buffer_attrs) |
274 | { |
275 | struct iio_buffer *buffer; |
276 | |
277 | buffer = devm_iio_kfifo_allocate(dev); |
278 | if (!buffer) |
279 | return -ENOMEM; |
280 | |
281 | indio_dev->modes |= INDIO_BUFFER_SOFTWARE; |
282 | indio_dev->setup_ops = setup_ops; |
283 | |
284 | buffer->attrs = buffer_attrs; |
285 | |
286 | return iio_device_attach_buffer(indio_dev, buffer); |
287 | } |
288 | EXPORT_SYMBOL_GPL(devm_iio_kfifo_buffer_setup_ext); |
289 | |
290 | MODULE_LICENSE("GPL" ); |
291 | |