1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
|
/* The industrial I/O simple minimally locked ring buffer.
*
* Copyright (c) 2008 Jonathan Cameron
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License version 2 as published by
* the Free Software Foundation.
*/
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/workqueue.h>
#include "ring_sw.h"
static inline int __iio_allocate_sw_ring_buffer(struct iio_sw_ring_buffer *ring,
int bytes_per_datum, int length)
{
if ((length == 0) || (bytes_per_datum == 0))
return -EINVAL;
__iio_update_ring_buffer(&ring->buf, bytes_per_datum, length);
ring->data = kmalloc(length*ring->buf.bpd, GFP_KERNEL);
ring->read_p = 0;
ring->write_p = 0;
ring->last_written_p = 0;
ring->half_p = 0;
return ring->data ? 0 : -ENOMEM;
}
static inline void __iio_init_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
{
spin_lock_init(&ring->use_lock);
}
static inline void __iio_free_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
{
kfree(ring->data);
}
void iio_mark_sw_rb_in_use(struct iio_ring_buffer *r)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
spin_lock(&ring->use_lock);
ring->use_count++;
spin_unlock(&ring->use_lock);
}
EXPORT_SYMBOL(iio_mark_sw_rb_in_use);
void iio_unmark_sw_rb_in_use(struct iio_ring_buffer *r)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
spin_lock(&ring->use_lock);
ring->use_count--;
spin_unlock(&ring->use_lock);
}
EXPORT_SYMBOL(iio_unmark_sw_rb_in_use);
/* Ring buffer related functionality */
/* Store to ring is typically called in the bh of a data ready interrupt handler
* in the device driver */
/* Lock always held if their is a chance this may be called */
/* Only one of these per ring may run concurrently - enforced by drivers */
int iio_store_to_sw_ring(struct iio_sw_ring_buffer *ring,
unsigned char *data,
s64 timestamp)
{
int ret = 0;
int code;
unsigned char *temp_ptr, *change_test_ptr;
/* initial store */
if (unlikely(ring->write_p == 0)) {
ring->write_p = ring->data;
/* Doesn't actually matter if this is out of the set
* as long as the read pointer is valid before this
* passes it - guaranteed as set later in this function.
*/
ring->half_p = ring->data - ring->buf.length*ring->buf.bpd/2;
}
/* Copy data to where ever the current write pointer says */
memcpy(ring->write_p, data, ring->buf.bpd);
barrier();
/* Update the pointer used to get most recent value.
* Always valid as either points to latest or second latest value.
* Before this runs it is null and read attempts fail with -EAGAIN.
*/
ring->last_written_p = ring->write_p;
barrier();
/* temp_ptr used to ensure we never have an invalid pointer
* it may be slightly lagging, but never invalid
*/
temp_ptr = ring->write_p + ring->buf.bpd;
/* End of ring, back to the beginning */
if (temp_ptr == ring->data + ring->buf.length*ring->buf.bpd)
temp_ptr = ring->data;
/* Update the write pointer
* always valid as long as this is the only function able to write.
* Care needed with smp systems to ensure more than one ring fill
* is never scheduled.
*/
ring->write_p = temp_ptr;
if (ring->read_p == 0)
ring->read_p = ring->data;
/* Buffer full - move the read pointer and create / escalate
* ring event */
/* Tricky case - if the read pointer moves before we adjust it.
* Handle by not pushing if it has moved - may result in occasional
* unnecessary buffer full events when it wasn't quite true.
*/
else if (ring->write_p == ring->read_p) {
change_test_ptr = ring->read_p;
temp_ptr = change_test_ptr + ring->buf.bpd;
if (temp_ptr
== ring->data + ring->buf.length*ring->buf.bpd) {
temp_ptr = ring->data;
}
/* We are moving pointer on one because the ring is full. Any
* change to the read pointer will be this or greater.
*/
if (change_test_ptr == ring->read_p)
ring->read_p = temp_ptr;
spin_lock(&ring->buf.shared_ev_pointer.lock);
ret = iio_push_or_escallate_ring_event(&ring->buf,
IIO_EVENT_CODE_RING_100_FULL, timestamp);
spin_unlock(&ring->buf.shared_ev_pointer.lock);
if (ret)
goto error_ret;
}
/* investigate if our event barrier has been passed */
/* There are definite 'issues' with this and chances of
* simultaneous read */
/* Also need to use loop count to ensure this only happens once */
ring->half_p += ring->buf.bpd;
if (ring->half_p == ring->data + ring->buf.length*ring->buf.bpd)
ring->half_p = ring->data;
if (ring->half_p == ring->read_p) {
spin_lock(&ring->buf.shared_ev_pointer.lock);
code = IIO_EVENT_CODE_RING_50_FULL;
ret = __iio_push_event(&ring->buf.ev_int,
code,
timestamp,
&ring->buf.shared_ev_pointer);
spin_unlock(&ring->buf.shared_ev_pointer.lock);
}
error_ret:
return ret;
}
int iio_rip_sw_rb(struct iio_ring_buffer *r,
size_t count, u8 **data, int *dead_offset)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
u8 *initial_read_p, *initial_write_p, *current_read_p, *end_read_p;
int ret, max_copied;
int bytes_to_rip;
/* A userspace program has probably made an error if it tries to
* read something that is not a whole number of bpds.
* Return an error.
*/
if (count % ring->buf.bpd) {
ret = -EINVAL;
printk(KERN_INFO "Ring buffer read request not whole number of"
"samples: Request bytes %zd, Current bpd %d\n",
count, ring->buf.bpd);
goto error_ret;
}
/* Limit size to whole of ring buffer */
bytes_to_rip = min((size_t)(ring->buf.bpd*ring->buf.length), count);
*data = kmalloc(bytes_to_rip, GFP_KERNEL);
if (*data == NULL) {
ret = -ENOMEM;
goto error_ret;
}
/* build local copy */
initial_read_p = ring->read_p;
if (unlikely(initial_read_p == 0)) { /* No data here as yet */
ret = 0;
goto error_free_data_cpy;
}
initial_write_p = ring->write_p;
/* Need a consistent pair */
while ((initial_read_p != ring->read_p)
|| (initial_write_p != ring->write_p)) {
initial_read_p = ring->read_p;
initial_write_p = ring->write_p;
}
if (initial_write_p == initial_read_p) {
/* No new data available.*/
ret = 0;
goto error_free_data_cpy;
}
if (initial_write_p >= initial_read_p + bytes_to_rip) {
/* write_p is greater than necessary, all is easy */
max_copied = bytes_to_rip;
memcpy(*data, initial_read_p, max_copied);
end_read_p = initial_read_p + max_copied;
} else if (initial_write_p > initial_read_p) {
/*not enough data to cpy */
max_copied = initial_write_p - initial_read_p;
memcpy(*data, initial_read_p, max_copied);
end_read_p = initial_write_p;
} else {
/* going through 'end' of ring buffer */
max_copied = ring->data
+ ring->buf.length*ring->buf.bpd - initial_read_p;
memcpy(*data, initial_read_p, max_copied);
/* possible we are done if we align precisely with end */
if (max_copied == bytes_to_rip)
end_read_p = ring->data;
else if (initial_write_p
> ring->data + bytes_to_rip - max_copied) {
/* enough data to finish */
memcpy(*data + max_copied, ring->data,
bytes_to_rip - max_copied);
max_copied = bytes_to_rip;
end_read_p = ring->data + (bytes_to_rip - max_copied);
} else { /* not enough data */
memcpy(*data + max_copied, ring->data,
initial_write_p - ring->data);
max_copied += initial_write_p - ring->data;
end_read_p = initial_write_p;
}
}
/* Now to verify which section was cleanly copied - i.e. how far
* read pointer has been pushed */
current_read_p = ring->read_p;
if (initial_read_p <= current_read_p)
*dead_offset = current_read_p - initial_read_p;
else
*dead_offset = ring->buf.length*ring->buf.bpd
- (initial_read_p - current_read_p);
/* possible issue if the initial write has been lapped or indeed
* the point we were reading to has been passed */
/* No valid data read.
* In this case the read pointer is already correct having been
* pushed further than we would look. */
if (max_copied - *dead_offset < 0) {
ret = 0;
goto error_free_data_cpy;
}
/* setup the next read position */
/* Beware, this may fail due to concurrency fun and games.
* Possible that sufficient fill commands have run to push the read
* pointer past where we would be after the rip. If this occurs, leave
* it be.
*/
/* Tricky - deal with loops */
while (ring->read_p != end_read_p)
ring->read_p = end_read_p;
return max_copied - *dead_offset;
error_free_data_cpy:
kfree(*data);
error_ret:
return ret;
}
EXPORT_SYMBOL(iio_rip_sw_rb);
int iio_store_to_sw_rb(struct iio_ring_buffer *r, u8 *data, s64 timestamp)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
return iio_store_to_sw_ring(ring, data, timestamp);
}
EXPORT_SYMBOL(iio_store_to_sw_rb);
int iio_read_last_from_sw_ring(struct iio_sw_ring_buffer *ring,
unsigned char *data)
{
unsigned char *last_written_p_copy;
iio_mark_sw_rb_in_use(&ring->buf);
again:
barrier();
last_written_p_copy = ring->last_written_p;
barrier(); /*unnessecary? */
/* Check there is anything here */
if (last_written_p_copy == 0)
return -EAGAIN;
memcpy(data, last_written_p_copy, ring->buf.bpd);
if (unlikely(ring->last_written_p >= last_written_p_copy))
goto again;
iio_unmark_sw_rb_in_use(&ring->buf);
return 0;
}
int iio_read_last_from_sw_rb(struct iio_ring_buffer *r,
unsigned char *data)
{
return iio_read_last_from_sw_ring(iio_to_sw_ring(r), data);
}
EXPORT_SYMBOL(iio_read_last_from_sw_rb);
int iio_request_update_sw_rb(struct iio_ring_buffer *r)
{
int ret = 0;
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
spin_lock(&ring->use_lock);
if (!ring->update_needed)
goto error_ret;
if (ring->use_count) {
ret = -EAGAIN;
goto error_ret;
}
__iio_free_sw_ring_buffer(ring);
ret = __iio_allocate_sw_ring_buffer(ring, ring->buf.bpd,
ring->buf.length);
error_ret:
spin_unlock(&ring->use_lock);
return ret;
}
EXPORT_SYMBOL(iio_request_update_sw_rb);
int iio_get_bpd_sw_rb(struct iio_ring_buffer *r)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
return ring->buf.bpd;
}
EXPORT_SYMBOL(iio_get_bpd_sw_rb);
int iio_set_bpd_sw_rb(struct iio_ring_buffer *r, size_t bpd)
{
if (r->bpd != bpd) {
r->bpd = bpd;
if (r->access.mark_param_change)
r->access.mark_param_change(r);
}
return 0;
}
EXPORT_SYMBOL(iio_set_bpd_sw_rb);
int iio_get_length_sw_rb(struct iio_ring_buffer *r)
{
return r->length;
}
EXPORT_SYMBOL(iio_get_length_sw_rb);
int iio_set_length_sw_rb(struct iio_ring_buffer *r, int length)
{
if (r->length != length) {
r->length = length;
if (r->access.mark_param_change)
r->access.mark_param_change(r);
}
return 0;
}
EXPORT_SYMBOL(iio_set_length_sw_rb);
int iio_mark_update_needed_sw_rb(struct iio_ring_buffer *r)
{
struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
ring->update_needed = true;
return 0;
}
EXPORT_SYMBOL(iio_mark_update_needed_sw_rb);
static void iio_sw_rb_release(struct device *dev)
{
struct iio_ring_buffer *r = to_iio_ring_buffer(dev);
kfree(iio_to_sw_ring(r));
}
static IIO_RING_ENABLE_ATTR;
static IIO_RING_BPS_ATTR;
static IIO_RING_LENGTH_ATTR;
/* Standard set of ring buffer attributes */
static struct attribute *iio_ring_attributes[] = {
&dev_attr_length.attr,
&dev_attr_bps.attr,
&dev_attr_ring_enable.attr,
NULL,
};
static struct attribute_group iio_ring_attribute_group = {
.attrs = iio_ring_attributes,
};
static const struct attribute_group *iio_ring_attribute_groups[] = {
&iio_ring_attribute_group,
NULL
};
static struct device_type iio_sw_ring_type = {
.release = iio_sw_rb_release,
.groups = iio_ring_attribute_groups,
};
struct iio_ring_buffer *iio_sw_rb_allocate(struct iio_dev *indio_dev)
{
struct iio_ring_buffer *buf;
struct iio_sw_ring_buffer *ring;
ring = kzalloc(sizeof *ring, GFP_KERNEL);
if (!ring)
return 0;
buf = &ring->buf;
iio_ring_buffer_init(buf, indio_dev);
__iio_init_sw_ring_buffer(ring);
buf->dev.type = &iio_sw_ring_type;
device_initialize(&buf->dev);
buf->dev.parent = &indio_dev->dev;
buf->dev.class = &iio_class;
dev_set_drvdata(&buf->dev, (void *)buf);
return buf;
}
EXPORT_SYMBOL(iio_sw_rb_allocate);
void iio_sw_rb_free(struct iio_ring_buffer *r)
{
if (r)
iio_put_ring_buffer(r);
}
EXPORT_SYMBOL(iio_sw_rb_free);
MODULE_DESCRIPTION("Industrialio I/O software ring buffer");
MODULE_LICENSE("GPL");
|