1
2
3
4
5
6#include <linux/cdev.h>
7#include <linux/counter.h>
8#include <linux/err.h>
9#include <linux/errno.h>
10#include <linux/export.h>
11#include <linux/fs.h>
12#include <linux/kfifo.h>
13#include <linux/list.h>
14#include <linux/mutex.h>
15#include <linux/nospec.h>
16#include <linux/poll.h>
17#include <linux/slab.h>
18#include <linux/spinlock.h>
19#include <linux/timekeeping.h>
20#include <linux/types.h>
21#include <linux/uaccess.h>
22#include <linux/wait.h>
23
24#include "counter-chrdev.h"
25
26struct counter_comp_node {
27 struct list_head l;
28 struct counter_component component;
29 struct counter_comp comp;
30 void *parent;
31};
32
33#define counter_comp_read_is_equal(a, b) \
34 (a.action_read == b.action_read || \
35 a.device_u8_read == b.device_u8_read || \
36 a.count_u8_read == b.count_u8_read || \
37 a.signal_u8_read == b.signal_u8_read || \
38 a.device_u32_read == b.device_u32_read || \
39 a.count_u32_read == b.count_u32_read || \
40 a.signal_u32_read == b.signal_u32_read || \
41 a.device_u64_read == b.device_u64_read || \
42 a.count_u64_read == b.count_u64_read || \
43 a.signal_u64_read == b.signal_u64_read)
44
45#define counter_comp_read_is_set(comp) \
46 (comp.action_read || \
47 comp.device_u8_read || \
48 comp.count_u8_read || \
49 comp.signal_u8_read || \
50 comp.device_u32_read || \
51 comp.count_u32_read || \
52 comp.signal_u32_read || \
53 comp.device_u64_read || \
54 comp.count_u64_read || \
55 comp.signal_u64_read)
56
57static ssize_t counter_chrdev_read(struct file *filp, char __user *buf,
58 size_t len, loff_t *f_ps)
59{
60 struct counter_device *const counter = filp->private_data;
61 int err;
62 unsigned int copied;
63
64 if (!counter->ops)
65 return -ENODEV;
66
67 if (len < sizeof(struct counter_event))
68 return -EINVAL;
69
70 do {
71 if (kfifo_is_empty(&counter->events)) {
72 if (filp->f_flags & O_NONBLOCK)
73 return -EAGAIN;
74
75 err = wait_event_interruptible(counter->events_wait,
76 !kfifo_is_empty(&counter->events) ||
77 !counter->ops);
78 if (err < 0)
79 return err;
80 if (!counter->ops)
81 return -ENODEV;
82 }
83
84 if (mutex_lock_interruptible(&counter->events_out_lock))
85 return -ERESTARTSYS;
86 err = kfifo_to_user(&counter->events, buf, len, &copied);
87 mutex_unlock(&counter->events_out_lock);
88 if (err < 0)
89 return err;
90 } while (!copied);
91
92 return copied;
93}
94
95static __poll_t counter_chrdev_poll(struct file *filp,
96 struct poll_table_struct *pollt)
97{
98 struct counter_device *const counter = filp->private_data;
99 __poll_t events = 0;
100
101 if (!counter->ops)
102 return events;
103
104 poll_wait(filp, &counter->events_wait, pollt);
105
106 if (!kfifo_is_empty(&counter->events))
107 events = EPOLLIN | EPOLLRDNORM;
108
109 return events;
110}
111
112static void counter_events_list_free(struct list_head *const events_list)
113{
114 struct counter_event_node *p, *n;
115 struct counter_comp_node *q, *o;
116
117 list_for_each_entry_safe(p, n, events_list, l) {
118
119 list_for_each_entry_safe(q, o, &p->comp_list, l) {
120 list_del(&q->l);
121 kfree(q);
122 }
123
124
125 list_del(&p->l);
126 kfree(p);
127 }
128}
129
130static int counter_set_event_node(struct counter_device *const counter,
131 struct counter_watch *const watch,
132 const struct counter_comp_node *const cfg)
133{
134 struct counter_event_node *event_node;
135 int err = 0;
136 struct counter_comp_node *comp_node;
137
138
139 list_for_each_entry(event_node, &counter->next_events_list, l)
140 if (event_node->event == watch->event &&
141 event_node->channel == watch->channel)
142 break;
143
144
145 if (&event_node->l == &counter->next_events_list) {
146
147 event_node = kmalloc(sizeof(*event_node), GFP_KERNEL);
148 if (!event_node)
149 return -ENOMEM;
150
151
152 event_node->event = watch->event;
153 event_node->channel = watch->channel;
154 INIT_LIST_HEAD(&event_node->comp_list);
155 list_add(&event_node->l, &counter->next_events_list);
156 }
157
158
159 list_for_each_entry(comp_node, &event_node->comp_list, l)
160 if (comp_node->parent == cfg->parent &&
161 counter_comp_read_is_equal(comp_node->comp, cfg->comp)) {
162 err = -EINVAL;
163 goto exit_free_event_node;
164 }
165
166
167 comp_node = kmalloc(sizeof(*comp_node), GFP_KERNEL);
168 if (!comp_node) {
169 err = -ENOMEM;
170 goto exit_free_event_node;
171 }
172 *comp_node = *cfg;
173
174
175 list_add_tail(&comp_node->l, &event_node->comp_list);
176
177exit_free_event_node:
178
179 if (list_empty(&event_node->comp_list)) {
180 list_del(&event_node->l);
181 kfree(event_node);
182 }
183
184 return err;
185}
186
187static int counter_enable_events(struct counter_device *const counter)
188{
189 unsigned long flags;
190 int err = 0;
191
192 mutex_lock(&counter->n_events_list_lock);
193 spin_lock_irqsave(&counter->events_list_lock, flags);
194
195 counter_events_list_free(&counter->events_list);
196 list_replace_init(&counter->next_events_list,
197 &counter->events_list);
198
199 if (counter->ops->events_configure)
200 err = counter->ops->events_configure(counter);
201
202 spin_unlock_irqrestore(&counter->events_list_lock, flags);
203 mutex_unlock(&counter->n_events_list_lock);
204
205 return err;
206}
207
208static int counter_disable_events(struct counter_device *const counter)
209{
210 unsigned long flags;
211 int err = 0;
212
213 spin_lock_irqsave(&counter->events_list_lock, flags);
214
215 counter_events_list_free(&counter->events_list);
216
217 if (counter->ops->events_configure)
218 err = counter->ops->events_configure(counter);
219
220 spin_unlock_irqrestore(&counter->events_list_lock, flags);
221
222 mutex_lock(&counter->n_events_list_lock);
223
224 counter_events_list_free(&counter->next_events_list);
225
226 mutex_unlock(&counter->n_events_list_lock);
227
228 return err;
229}
230
231static int counter_add_watch(struct counter_device *const counter,
232 const unsigned long arg)
233{
234 void __user *const uwatch = (void __user *)arg;
235 struct counter_watch watch;
236 struct counter_comp_node comp_node = {};
237 size_t parent, id;
238 struct counter_comp *ext;
239 size_t num_ext;
240 int err = 0;
241
242 if (copy_from_user(&watch, uwatch, sizeof(watch)))
243 return -EFAULT;
244
245 if (watch.component.type == COUNTER_COMPONENT_NONE)
246 goto no_component;
247
248 parent = watch.component.parent;
249
250
251 switch (watch.component.scope) {
252 case COUNTER_SCOPE_DEVICE:
253 ext = counter->ext;
254 num_ext = counter->num_ext;
255 break;
256 case COUNTER_SCOPE_SIGNAL:
257 if (parent >= counter->num_signals)
258 return -EINVAL;
259 parent = array_index_nospec(parent, counter->num_signals);
260
261 comp_node.parent = counter->signals + parent;
262
263 ext = counter->signals[parent].ext;
264 num_ext = counter->signals[parent].num_ext;
265 break;
266 case COUNTER_SCOPE_COUNT:
267 if (parent >= counter->num_counts)
268 return -EINVAL;
269 parent = array_index_nospec(parent, counter->num_counts);
270
271 comp_node.parent = counter->counts + parent;
272
273 ext = counter->counts[parent].ext;
274 num_ext = counter->counts[parent].num_ext;
275 break;
276 default:
277 return -EINVAL;
278 }
279
280 id = watch.component.id;
281
282
283 switch (watch.component.type) {
284 case COUNTER_COMPONENT_SIGNAL:
285 if (watch.component.scope != COUNTER_SCOPE_SIGNAL)
286 return -EINVAL;
287
288 comp_node.comp.type = COUNTER_COMP_SIGNAL_LEVEL;
289 comp_node.comp.signal_u32_read = counter->ops->signal_read;
290 break;
291 case COUNTER_COMPONENT_COUNT:
292 if (watch.component.scope != COUNTER_SCOPE_COUNT)
293 return -EINVAL;
294
295 comp_node.comp.type = COUNTER_COMP_U64;
296 comp_node.comp.count_u64_read = counter->ops->count_read;
297 break;
298 case COUNTER_COMPONENT_FUNCTION:
299 if (watch.component.scope != COUNTER_SCOPE_COUNT)
300 return -EINVAL;
301
302 comp_node.comp.type = COUNTER_COMP_FUNCTION;
303 comp_node.comp.count_u32_read = counter->ops->function_read;
304 break;
305 case COUNTER_COMPONENT_SYNAPSE_ACTION:
306 if (watch.component.scope != COUNTER_SCOPE_COUNT)
307 return -EINVAL;
308 if (id >= counter->counts[parent].num_synapses)
309 return -EINVAL;
310 id = array_index_nospec(id, counter->counts[parent].num_synapses);
311
312 comp_node.comp.type = COUNTER_COMP_SYNAPSE_ACTION;
313 comp_node.comp.action_read = counter->ops->action_read;
314 comp_node.comp.priv = counter->counts[parent].synapses + id;
315 break;
316 case COUNTER_COMPONENT_EXTENSION:
317 if (id >= num_ext)
318 return -EINVAL;
319 id = array_index_nospec(id, num_ext);
320
321 comp_node.comp = ext[id];
322 break;
323 default:
324 return -EINVAL;
325 }
326 if (!counter_comp_read_is_set(comp_node.comp))
327 return -EOPNOTSUPP;
328
329no_component:
330 mutex_lock(&counter->n_events_list_lock);
331
332 if (counter->ops->watch_validate) {
333 err = counter->ops->watch_validate(counter, &watch);
334 if (err < 0)
335 goto err_exit;
336 }
337
338 comp_node.component = watch.component;
339
340 err = counter_set_event_node(counter, &watch, &comp_node);
341
342err_exit:
343 mutex_unlock(&counter->n_events_list_lock);
344
345 return err;
346}
347
348static long counter_chrdev_ioctl(struct file *filp, unsigned int cmd,
349 unsigned long arg)
350{
351 struct counter_device *const counter = filp->private_data;
352 int ret = -ENODEV;
353
354 mutex_lock(&counter->ops_exist_lock);
355
356 if (!counter->ops)
357 goto out_unlock;
358
359 switch (cmd) {
360 case COUNTER_ADD_WATCH_IOCTL:
361 ret = counter_add_watch(counter, arg);
362 break;
363 case COUNTER_ENABLE_EVENTS_IOCTL:
364 ret = counter_enable_events(counter);
365 break;
366 case COUNTER_DISABLE_EVENTS_IOCTL:
367 ret = counter_disable_events(counter);
368 break;
369 default:
370 ret = -ENOIOCTLCMD;
371 break;
372 }
373
374out_unlock:
375 mutex_unlock(&counter->ops_exist_lock);
376
377 return ret;
378}
379
380static int counter_chrdev_open(struct inode *inode, struct file *filp)
381{
382 struct counter_device *const counter = container_of(inode->i_cdev,
383 typeof(*counter),
384 chrdev);
385
386 get_device(&counter->dev);
387 filp->private_data = counter;
388
389 return nonseekable_open(inode, filp);
390}
391
392static int counter_chrdev_release(struct inode *inode, struct file *filp)
393{
394 struct counter_device *const counter = filp->private_data;
395 int ret = 0;
396
397 mutex_lock(&counter->ops_exist_lock);
398
399 if (!counter->ops) {
400
401 counter_events_list_free(&counter->events_list);
402 counter_events_list_free(&counter->next_events_list);
403 ret = -ENODEV;
404 goto out_unlock;
405 }
406
407 ret = counter_disable_events(counter);
408 if (ret < 0) {
409 mutex_unlock(&counter->ops_exist_lock);
410 return ret;
411 }
412
413out_unlock:
414 mutex_unlock(&counter->ops_exist_lock);
415
416 put_device(&counter->dev);
417
418 return ret;
419}
420
421static const struct file_operations counter_fops = {
422 .owner = THIS_MODULE,
423 .llseek = no_llseek,
424 .read = counter_chrdev_read,
425 .poll = counter_chrdev_poll,
426 .unlocked_ioctl = counter_chrdev_ioctl,
427 .open = counter_chrdev_open,
428 .release = counter_chrdev_release,
429};
430
431int counter_chrdev_add(struct counter_device *const counter)
432{
433
434 INIT_LIST_HEAD(&counter->events_list);
435 INIT_LIST_HEAD(&counter->next_events_list);
436 spin_lock_init(&counter->events_list_lock);
437 mutex_init(&counter->n_events_list_lock);
438 init_waitqueue_head(&counter->events_wait);
439 spin_lock_init(&counter->events_in_lock);
440 mutex_init(&counter->events_out_lock);
441
442
443 cdev_init(&counter->chrdev, &counter_fops);
444
445
446 return kfifo_alloc(&counter->events, 64, GFP_KERNEL);
447}
448
449void counter_chrdev_remove(struct counter_device *const counter)
450{
451 kfifo_free(&counter->events);
452}
453
454static int counter_get_data(struct counter_device *const counter,
455 const struct counter_comp_node *const comp_node,
456 u64 *const value)
457{
458 const struct counter_comp *const comp = &comp_node->comp;
459 void *const parent = comp_node->parent;
460 u8 value_u8 = 0;
461 u32 value_u32 = 0;
462 int ret;
463
464 if (comp_node->component.type == COUNTER_COMPONENT_NONE)
465 return 0;
466
467 switch (comp->type) {
468 case COUNTER_COMP_U8:
469 case COUNTER_COMP_BOOL:
470 switch (comp_node->component.scope) {
471 case COUNTER_SCOPE_DEVICE:
472 ret = comp->device_u8_read(counter, &value_u8);
473 break;
474 case COUNTER_SCOPE_SIGNAL:
475 ret = comp->signal_u8_read(counter, parent, &value_u8);
476 break;
477 case COUNTER_SCOPE_COUNT:
478 ret = comp->count_u8_read(counter, parent, &value_u8);
479 break;
480 default:
481 return -EINVAL;
482 }
483 *value = value_u8;
484 return ret;
485 case COUNTER_COMP_SIGNAL_LEVEL:
486 case COUNTER_COMP_FUNCTION:
487 case COUNTER_COMP_ENUM:
488 case COUNTER_COMP_COUNT_DIRECTION:
489 case COUNTER_COMP_COUNT_MODE:
490 switch (comp_node->component.scope) {
491 case COUNTER_SCOPE_DEVICE:
492 ret = comp->device_u32_read(counter, &value_u32);
493 break;
494 case COUNTER_SCOPE_SIGNAL:
495 ret = comp->signal_u32_read(counter, parent,
496 &value_u32);
497 break;
498 case COUNTER_SCOPE_COUNT:
499 ret = comp->count_u32_read(counter, parent, &value_u32);
500 break;
501 default:
502 return -EINVAL;
503 }
504 *value = value_u32;
505 return ret;
506 case COUNTER_COMP_U64:
507 switch (comp_node->component.scope) {
508 case COUNTER_SCOPE_DEVICE:
509 return comp->device_u64_read(counter, value);
510 case COUNTER_SCOPE_SIGNAL:
511 return comp->signal_u64_read(counter, parent, value);
512 case COUNTER_SCOPE_COUNT:
513 return comp->count_u64_read(counter, parent, value);
514 default:
515 return -EINVAL;
516 }
517 case COUNTER_COMP_SYNAPSE_ACTION:
518 ret = comp->action_read(counter, parent, comp->priv,
519 &value_u32);
520 *value = value_u32;
521 return ret;
522 default:
523 return -EINVAL;
524 }
525}
526
527
528
529
530
531
532
533
534
535
536void counter_push_event(struct counter_device *const counter, const u8 event,
537 const u8 channel)
538{
539 struct counter_event ev;
540 unsigned int copied = 0;
541 unsigned long flags;
542 struct counter_event_node *event_node;
543 struct counter_comp_node *comp_node;
544
545 ev.timestamp = ktime_get_ns();
546 ev.watch.event = event;
547 ev.watch.channel = channel;
548
549
550 spin_lock_irqsave(&counter->events_list_lock, flags);
551
552
553 list_for_each_entry(event_node, &counter->events_list, l)
554 if (event_node->event == event &&
555 event_node->channel == channel)
556 break;
557
558
559 if (&event_node->l == &counter->events_list)
560 goto exit_early;
561
562
563 list_for_each_entry(comp_node, &event_node->comp_list, l) {
564 ev.watch.component = comp_node->component;
565 ev.status = -counter_get_data(counter, comp_node, &ev.value);
566
567 copied += kfifo_in_spinlocked_noirqsave(&counter->events, &ev,
568 1, &counter->events_in_lock);
569 }
570
571exit_early:
572 spin_unlock_irqrestore(&counter->events_list_lock, flags);
573
574 if (copied)
575 wake_up_poll(&counter->events_wait, EPOLLIN);
576}
577EXPORT_SYMBOL_GPL(counter_push_event);
578
