Commit | Line | Data |
---|---|---|
b174baf4 JC |
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 | ||
9 | #include "kfifo_buf.h" | |
10 | ||
a710cc77 JC |
11 | struct iio_kfifo { |
12 | struct iio_ring_buffer ring; | |
13 | struct kfifo kf; | |
14 | int use_count; | |
15 | int update_needed; | |
16 | struct mutex use_lock; | |
17 | }; | |
18 | ||
5565a450 JC |
19 | #define iio_to_kfifo(r) container_of(r, struct iio_kfifo, ring) |
20 | ||
b174baf4 JC |
21 | static inline int __iio_allocate_kfifo(struct iio_kfifo *buf, |
22 | int bytes_per_datum, int length) | |
23 | { | |
24 | if ((length == 0) || (bytes_per_datum == 0)) | |
25 | return -EINVAL; | |
26 | ||
27 | __iio_update_ring_buffer(&buf->ring, bytes_per_datum, length); | |
28 | return kfifo_alloc(&buf->kf, bytes_per_datum*length, GFP_KERNEL); | |
29 | } | |
30 | ||
5565a450 | 31 | static int iio_request_update_kfifo(struct iio_ring_buffer *r) |
b174baf4 JC |
32 | { |
33 | int ret = 0; | |
34 | struct iio_kfifo *buf = iio_to_kfifo(r); | |
35 | ||
36 | mutex_lock(&buf->use_lock); | |
37 | if (!buf->update_needed) | |
38 | goto error_ret; | |
39 | if (buf->use_count) { | |
40 | ret = -EAGAIN; | |
41 | goto error_ret; | |
42 | } | |
43 | kfifo_free(&buf->kf); | |
44 | ret = __iio_allocate_kfifo(buf, buf->ring.bytes_per_datum, | |
45 | buf->ring.length); | |
46 | error_ret: | |
47 | mutex_unlock(&buf->use_lock); | |
48 | return ret; | |
49 | } | |
b174baf4 | 50 | |
5565a450 | 51 | static void iio_mark_kfifo_in_use(struct iio_ring_buffer *r) |
b174baf4 JC |
52 | { |
53 | struct iio_kfifo *buf = iio_to_kfifo(r); | |
54 | mutex_lock(&buf->use_lock); | |
55 | buf->use_count++; | |
56 | mutex_unlock(&buf->use_lock); | |
57 | } | |
b174baf4 | 58 | |
5565a450 | 59 | static void iio_unmark_kfifo_in_use(struct iio_ring_buffer *r) |
b174baf4 JC |
60 | { |
61 | struct iio_kfifo *buf = iio_to_kfifo(r); | |
62 | mutex_lock(&buf->use_lock); | |
63 | buf->use_count--; | |
64 | mutex_unlock(&buf->use_lock); | |
65 | } | |
b174baf4 | 66 | |
5565a450 | 67 | static int iio_get_length_kfifo(struct iio_ring_buffer *r) |
b174baf4 JC |
68 | { |
69 | return r->length; | |
70 | } | |
b174baf4 JC |
71 | |
72 | static inline void __iio_init_kfifo(struct iio_kfifo *kf) | |
73 | { | |
74 | mutex_init(&kf->use_lock); | |
75 | } | |
76 | ||
77 | static IIO_RING_ENABLE_ATTR; | |
78 | static IIO_RING_BYTES_PER_DATUM_ATTR; | |
79 | static IIO_RING_LENGTH_ATTR; | |
80 | ||
81 | static struct attribute *iio_kfifo_attributes[] = { | |
82 | &dev_attr_length.attr, | |
83 | &dev_attr_bytes_per_datum.attr, | |
84 | &dev_attr_enable.attr, | |
85 | NULL, | |
86 | }; | |
87 | ||
88 | static struct attribute_group iio_kfifo_attribute_group = { | |
89 | .attrs = iio_kfifo_attributes, | |
1aa04278 | 90 | .name = "buffer", |
b174baf4 JC |
91 | }; |
92 | ||
93 | struct iio_ring_buffer *iio_kfifo_allocate(struct iio_dev *indio_dev) | |
94 | { | |
95 | struct iio_kfifo *kf; | |
96 | ||
97 | kf = kzalloc(sizeof *kf, GFP_KERNEL); | |
98 | if (!kf) | |
99 | return NULL; | |
5565a450 | 100 | kf->update_needed = true; |
b174baf4 | 101 | iio_ring_buffer_init(&kf->ring, indio_dev); |
1aa04278 | 102 | kf->ring.attrs = &iio_kfifo_attribute_group; |
b174baf4 | 103 | __iio_init_kfifo(kf); |
b174baf4 JC |
104 | |
105 | return &kf->ring; | |
106 | } | |
107 | EXPORT_SYMBOL(iio_kfifo_allocate); | |
108 | ||
5565a450 | 109 | static int iio_get_bytes_per_datum_kfifo(struct iio_ring_buffer *r) |
b174baf4 JC |
110 | { |
111 | return r->bytes_per_datum; | |
112 | } | |
b174baf4 | 113 | |
5565a450 | 114 | static int iio_set_bytes_per_datum_kfifo(struct iio_ring_buffer *r, size_t bpd) |
b174baf4 JC |
115 | { |
116 | if (r->bytes_per_datum != bpd) { | |
117 | r->bytes_per_datum = bpd; | |
5565a450 JC |
118 | if (r->access->mark_param_change) |
119 | r->access->mark_param_change(r); | |
b174baf4 JC |
120 | } |
121 | return 0; | |
122 | } | |
b174baf4 | 123 | |
5565a450 | 124 | static int iio_mark_update_needed_kfifo(struct iio_ring_buffer *r) |
b174baf4 JC |
125 | { |
126 | struct iio_kfifo *kf = iio_to_kfifo(r); | |
127 | kf->update_needed = true; | |
128 | return 0; | |
129 | } | |
b174baf4 | 130 | |
5565a450 | 131 | static int iio_set_length_kfifo(struct iio_ring_buffer *r, int length) |
b174baf4 JC |
132 | { |
133 | if (r->length != length) { | |
134 | r->length = length; | |
5565a450 JC |
135 | if (r->access->mark_param_change) |
136 | r->access->mark_param_change(r); | |
b174baf4 JC |
137 | } |
138 | return 0; | |
139 | } | |
b174baf4 JC |
140 | |
141 | void iio_kfifo_free(struct iio_ring_buffer *r) | |
142 | { | |
1aa04278 | 143 | kfree(iio_to_kfifo(r)); |
b174baf4 JC |
144 | } |
145 | EXPORT_SYMBOL(iio_kfifo_free); | |
146 | ||
5565a450 JC |
147 | static int iio_store_to_kfifo(struct iio_ring_buffer *r, |
148 | u8 *data, | |
149 | s64 timestamp) | |
b174baf4 JC |
150 | { |
151 | int ret; | |
152 | struct iio_kfifo *kf = iio_to_kfifo(r); | |
153 | u8 *datal = kmalloc(r->bytes_per_datum, GFP_KERNEL); | |
154 | memcpy(datal, data, r->bytes_per_datum - sizeof(timestamp)); | |
155 | memcpy(datal + r->bytes_per_datum - sizeof(timestamp), | |
156 | ×tamp, sizeof(timestamp)); | |
157 | ret = kfifo_in(&kf->kf, data, r->bytes_per_datum); | |
158 | if (ret != r->bytes_per_datum) { | |
159 | kfree(datal); | |
160 | return -EBUSY; | |
161 | } | |
162 | kfree(datal); | |
163 | return 0; | |
164 | } | |
b174baf4 | 165 | |
5565a450 | 166 | static int iio_read_first_n_kfifo(struct iio_ring_buffer *r, |
b26a2188 | 167 | size_t n, char __user *buf) |
b174baf4 JC |
168 | { |
169 | int ret, copied; | |
170 | struct iio_kfifo *kf = iio_to_kfifo(r); | |
171 | ||
b4281733 | 172 | ret = kfifo_to_user(&kf->kf, buf, r->bytes_per_datum*n, &copied); |
b174baf4 JC |
173 | |
174 | return copied; | |
175 | } | |
5565a450 JC |
176 | |
177 | const struct iio_ring_access_funcs kfifo_access_funcs = { | |
178 | .mark_in_use = &iio_mark_kfifo_in_use, | |
179 | .unmark_in_use = &iio_unmark_kfifo_in_use, | |
180 | .store_to = &iio_store_to_kfifo, | |
181 | .read_first_n = &iio_read_first_n_kfifo, | |
182 | .mark_param_change = &iio_mark_update_needed_kfifo, | |
183 | .request_update = &iio_request_update_kfifo, | |
184 | .get_bytes_per_datum = &iio_get_bytes_per_datum_kfifo, | |
185 | .set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo, | |
186 | .get_length = &iio_get_length_kfifo, | |
187 | .set_length = &iio_set_length_kfifo, | |
188 | }; | |
189 | EXPORT_SYMBOL(kfifo_access_funcs); | |
190 | ||
b174baf4 | 191 | MODULE_LICENSE("GPL"); |