1// SPDX-License-Identifier: GPL-2.0
2/*
3 * Generic Counter character device interface
4 * Copyright (C) 2020 William Breathitt Gray
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 a.signal_array_u32_read == b.signal_array_u32_read || \
45 a.device_array_u64_read == b.device_array_u64_read || \
46 a.count_array_u64_read == b.count_array_u64_read || \
47 a.signal_array_u64_read == b.signal_array_u64_read)
48
49#define counter_comp_read_is_set(comp) \
50 (comp.action_read || \
51 comp.device_u8_read || \
52 comp.count_u8_read || \
53 comp.signal_u8_read || \
54 comp.device_u32_read || \
55 comp.count_u32_read || \
56 comp.signal_u32_read || \
57 comp.device_u64_read || \
58 comp.count_u64_read || \
59 comp.signal_u64_read || \
60 comp.signal_array_u32_read || \
61 comp.device_array_u64_read || \
62 comp.count_array_u64_read || \
63 comp.signal_array_u64_read)
64
65static ssize_t counter_chrdev_read(struct file *filp, char __user *buf,
66 size_t len, loff_t *f_ps)
67{
68 struct counter_device *const counter = filp->private_data;
69 int err;
70 unsigned int copied;
71
72 if (!counter->ops)
73 return -ENODEV;
74
75 if (len < sizeof(struct counter_event))
76 return -EINVAL;
77
78 do {
79 if (kfifo_is_empty(&counter->events)) {
80 if (filp->f_flags & O_NONBLOCK)
81 return -EAGAIN;
82
83 err = wait_event_interruptible(counter->events_wait,
84 !kfifo_is_empty(&counter->events) ||
85 !counter->ops);
86 if (err < 0)
87 return err;
88 if (!counter->ops)
89 return -ENODEV;
90 }
91
92 if (mutex_lock_interruptible(&counter->events_out_lock))
93 return -ERESTARTSYS;
94 err = kfifo_to_user(&counter->events, buf, len, &copied);
95 mutex_unlock(lock: &counter->events_out_lock);
96 if (err < 0)
97 return err;
98 } while (!copied);
99
100 return copied;
101}
102
103static __poll_t counter_chrdev_poll(struct file *filp,
104 struct poll_table_struct *pollt)
105{
106 struct counter_device *const counter = filp->private_data;
107 __poll_t events = 0;
108
109 if (!counter->ops)
110 return events;
111
112 poll_wait(filp, wait_address: &counter->events_wait, p: pollt);
113
114 if (!kfifo_is_empty(&counter->events))
115 events = EPOLLIN | EPOLLRDNORM;
116
117 return events;
118}
119
120static void counter_events_list_free(struct list_head *const events_list)
121{
122 struct counter_event_node *p, *n;
123 struct counter_comp_node *q, *o;
124
125 list_for_each_entry_safe(p, n, events_list, l) {
126 /* Free associated component nodes */
127 list_for_each_entry_safe(q, o, &p->comp_list, l) {
128 list_del(entry: &q->l);
129 kfree(objp: q);
130 }
131
132 /* Free event node */
133 list_del(entry: &p->l);
134 kfree(objp: p);
135 }
136}
137
138static int counter_set_event_node(struct counter_device *const counter,
139 struct counter_watch *const watch,
140 const struct counter_comp_node *const cfg)
141{
142 struct counter_event_node *event_node;
143 int err = 0;
144 struct counter_comp_node *comp_node;
145
146 /* Search for event in the list */
147 list_for_each_entry(event_node, &counter->next_events_list, l)
148 if (event_node->event == watch->event &&
149 event_node->channel == watch->channel)
150 break;
151
152 /* If event is not already in the list */
153 if (&event_node->l == &counter->next_events_list) {
154 /* Allocate new event node */
155 event_node = kmalloc(size: sizeof(*event_node), GFP_KERNEL);
156 if (!event_node)
157 return -ENOMEM;
158
159 /* Configure event node and add to the list */
160 event_node->event = watch->event;
161 event_node->channel = watch->channel;
162 INIT_LIST_HEAD(list: &event_node->comp_list);
163 list_add(new: &event_node->l, head: &counter->next_events_list);
164 }
165
166 /* Check if component watch has already been set before */
167 list_for_each_entry(comp_node, &event_node->comp_list, l)
168 if (comp_node->parent == cfg->parent &&
169 counter_comp_read_is_equal(comp_node->comp, cfg->comp)) {
170 err = -EINVAL;
171 goto exit_free_event_node;
172 }
173
174 /* Allocate component node */
175 comp_node = kmalloc(size: sizeof(*comp_node), GFP_KERNEL);
176 if (!comp_node) {
177 err = -ENOMEM;
178 goto exit_free_event_node;
179 }
180 *comp_node = *cfg;
181
182 /* Add component node to event node */
183 list_add_tail(new: &comp_node->l, head: &event_node->comp_list);
184
185exit_free_event_node:
186 /* Free event node if no one else is watching */
187 if (list_empty(head: &event_node->comp_list)) {
188 list_del(entry: &event_node->l);
189 kfree(objp: event_node);
190 }
191
192 return err;
193}
194
195static int counter_enable_events(struct counter_device *const counter)
196{
197 unsigned long flags;
198 int err = 0;
199
200 mutex_lock(&counter->n_events_list_lock);
201 spin_lock_irqsave(&counter->events_list_lock, flags);
202
203 counter_events_list_free(events_list: &counter->events_list);
204 list_replace_init(old: &counter->next_events_list,
205 new: &counter->events_list);
206
207 if (counter->ops->events_configure)
208 err = counter->ops->events_configure(counter);
209
210 spin_unlock_irqrestore(lock: &counter->events_list_lock, flags);
211 mutex_unlock(lock: &counter->n_events_list_lock);
212
213 return err;
214}
215
216static int counter_disable_events(struct counter_device *const counter)
217{
218 unsigned long flags;
219 int err = 0;
220
221 spin_lock_irqsave(&counter->events_list_lock, flags);
222
223 counter_events_list_free(events_list: &counter->events_list);
224
225 if (counter->ops->events_configure)
226 err = counter->ops->events_configure(counter);
227
228 spin_unlock_irqrestore(lock: &counter->events_list_lock, flags);
229
230 mutex_lock(&counter->n_events_list_lock);
231
232 counter_events_list_free(events_list: &counter->next_events_list);
233
234 mutex_unlock(lock: &counter->n_events_list_lock);
235
236 return err;
237}
238
239static int counter_get_ext(const struct counter_comp *const ext,
240 const size_t num_ext, const size_t component_id,
241 size_t *const ext_idx, size_t *const id)
242{
243 struct counter_array *element;
244
245 *id = 0;
246 for (*ext_idx = 0; *ext_idx < num_ext; (*ext_idx)++) {
247 if (*id == component_id)
248 return 0;
249
250 if (ext[*ext_idx].type == COUNTER_COMP_ARRAY) {
251 element = ext[*ext_idx].priv;
252
253 if (component_id - *id < element->length)
254 return 0;
255
256 *id += element->length;
257 } else
258 (*id)++;
259 }
260
261 return -EINVAL;
262}
263
264static int counter_add_watch(struct counter_device *const counter,
265 const unsigned long arg)
266{
267 void __user *const uwatch = (void __user *)arg;
268 struct counter_watch watch;
269 struct counter_comp_node comp_node = {};
270 size_t parent, id;
271 struct counter_comp *ext;
272 size_t num_ext;
273 size_t ext_idx, ext_id;
274 int err = 0;
275
276 if (copy_from_user(to: &watch, from: uwatch, n: sizeof(watch)))
277 return -EFAULT;
278
279 if (watch.component.type == COUNTER_COMPONENT_NONE)
280 goto no_component;
281
282 parent = watch.component.parent;
283
284 /* Configure parent component info for comp node */
285 switch (watch.component.scope) {
286 case COUNTER_SCOPE_DEVICE:
287 ext = counter->ext;
288 num_ext = counter->num_ext;
289 break;
290 case COUNTER_SCOPE_SIGNAL:
291 if (parent >= counter->num_signals)
292 return -EINVAL;
293 parent = array_index_nospec(parent, counter->num_signals);
294
295 comp_node.parent = counter->signals + parent;
296
297 ext = counter->signals[parent].ext;
298 num_ext = counter->signals[parent].num_ext;
299 break;
300 case COUNTER_SCOPE_COUNT:
301 if (parent >= counter->num_counts)
302 return -EINVAL;
303 parent = array_index_nospec(parent, counter->num_counts);
304
305 comp_node.parent = counter->counts + parent;
306
307 ext = counter->counts[parent].ext;
308 num_ext = counter->counts[parent].num_ext;
309 break;
310 default:
311 return -EINVAL;
312 }
313
314 id = watch.component.id;
315
316 /* Configure component info for comp node */
317 switch (watch.component.type) {
318 case COUNTER_COMPONENT_SIGNAL:
319 if (watch.component.scope != COUNTER_SCOPE_SIGNAL)
320 return -EINVAL;
321
322 comp_node.comp.type = COUNTER_COMP_SIGNAL_LEVEL;
323 comp_node.comp.signal_u32_read = counter->ops->signal_read;
324 break;
325 case COUNTER_COMPONENT_COUNT:
326 if (watch.component.scope != COUNTER_SCOPE_COUNT)
327 return -EINVAL;
328
329 comp_node.comp.type = COUNTER_COMP_U64;
330 comp_node.comp.count_u64_read = counter->ops->count_read;
331 break;
332 case COUNTER_COMPONENT_FUNCTION:
333 if (watch.component.scope != COUNTER_SCOPE_COUNT)
334 return -EINVAL;
335
336 comp_node.comp.type = COUNTER_COMP_FUNCTION;
337 comp_node.comp.count_u32_read = counter->ops->function_read;
338 break;
339 case COUNTER_COMPONENT_SYNAPSE_ACTION:
340 if (watch.component.scope != COUNTER_SCOPE_COUNT)
341 return -EINVAL;
342 if (id >= counter->counts[parent].num_synapses)
343 return -EINVAL;
344 id = array_index_nospec(id, counter->counts[parent].num_synapses);
345
346 comp_node.comp.type = COUNTER_COMP_SYNAPSE_ACTION;
347 comp_node.comp.action_read = counter->ops->action_read;
348 comp_node.comp.priv = counter->counts[parent].synapses + id;
349 break;
350 case COUNTER_COMPONENT_EXTENSION:
351 err = counter_get_ext(ext, num_ext, component_id: id, ext_idx: &ext_idx, id: &ext_id);
352 if (err < 0)
353 return err;
354
355 comp_node.comp = ext[ext_idx];
356 break;
357 default:
358 return -EINVAL;
359 }
360 if (!counter_comp_read_is_set(comp_node.comp))
361 return -EOPNOTSUPP;
362
363no_component:
364 mutex_lock(&counter->n_events_list_lock);
365
366 if (counter->ops->watch_validate) {
367 err = counter->ops->watch_validate(counter, &watch);
368 if (err < 0)
369 goto err_exit;
370 }
371
372 comp_node.component = watch.component;
373
374 err = counter_set_event_node(counter, watch: &watch, cfg: &comp_node);
375
376err_exit:
377 mutex_unlock(lock: &counter->n_events_list_lock);
378
379 return err;
380}
381
382static long counter_chrdev_ioctl(struct file *filp, unsigned int cmd,
383 unsigned long arg)
384{
385 struct counter_device *const counter = filp->private_data;
386 int ret = -ENODEV;
387
388 mutex_lock(&counter->ops_exist_lock);
389
390 if (!counter->ops)
391 goto out_unlock;
392
393 switch (cmd) {
394 case COUNTER_ADD_WATCH_IOCTL:
395 ret = counter_add_watch(counter, arg);
396 break;
397 case COUNTER_ENABLE_EVENTS_IOCTL:
398 ret = counter_enable_events(counter);
399 break;
400 case COUNTER_DISABLE_EVENTS_IOCTL:
401 ret = counter_disable_events(counter);
402 break;
403 default:
404 ret = -ENOIOCTLCMD;
405 break;
406 }
407
408out_unlock:
409 mutex_unlock(lock: &counter->ops_exist_lock);
410
411 return ret;
412}
413
414static int counter_chrdev_open(struct inode *inode, struct file *filp)
415{
416 struct counter_device *const counter = container_of(inode->i_cdev,
417 typeof(*counter),
418 chrdev);
419
420 get_device(dev: &counter->dev);
421 filp->private_data = counter;
422
423 return nonseekable_open(inode, filp);
424}
425
426static int counter_chrdev_release(struct inode *inode, struct file *filp)
427{
428 struct counter_device *const counter = filp->private_data;
429 int ret = 0;
430
431 mutex_lock(&counter->ops_exist_lock);
432
433 if (!counter->ops) {
434 /* Free any lingering held memory */
435 counter_events_list_free(events_list: &counter->events_list);
436 counter_events_list_free(events_list: &counter->next_events_list);
437 ret = -ENODEV;
438 goto out_unlock;
439 }
440
441 ret = counter_disable_events(counter);
442 if (ret < 0) {
443 mutex_unlock(lock: &counter->ops_exist_lock);
444 return ret;
445 }
446
447out_unlock:
448 mutex_unlock(lock: &counter->ops_exist_lock);
449
450 put_device(dev: &counter->dev);
451
452 return ret;
453}
454
455static const struct file_operations counter_fops = {
456 .owner = THIS_MODULE,
457 .llseek = no_llseek,
458 .read = counter_chrdev_read,
459 .poll = counter_chrdev_poll,
460 .unlocked_ioctl = counter_chrdev_ioctl,
461 .open = counter_chrdev_open,
462 .release = counter_chrdev_release,
463};
464
465int counter_chrdev_add(struct counter_device *const counter)
466{
467 /* Initialize Counter events lists */
468 INIT_LIST_HEAD(list: &counter->events_list);
469 INIT_LIST_HEAD(list: &counter->next_events_list);
470 spin_lock_init(&counter->events_list_lock);
471 mutex_init(&counter->n_events_list_lock);
472 init_waitqueue_head(&counter->events_wait);
473 spin_lock_init(&counter->events_in_lock);
474 mutex_init(&counter->events_out_lock);
475
476 /* Initialize character device */
477 cdev_init(&counter->chrdev, &counter_fops);
478
479 /* Allocate Counter events queue */
480 return kfifo_alloc(&counter->events, 64, GFP_KERNEL);
481}
482
483void counter_chrdev_remove(struct counter_device *const counter)
484{
485 kfifo_free(&counter->events);
486}
487
488static int counter_get_array_data(struct counter_device *const counter,
489 const enum counter_scope scope,
490 void *const parent,
491 const struct counter_comp *const comp,
492 const size_t idx, u64 *const value)
493{
494 const struct counter_array *const element = comp->priv;
495 u32 value_u32 = 0;
496 int ret;
497
498 switch (element->type) {
499 case COUNTER_COMP_SIGNAL_POLARITY:
500 if (scope != COUNTER_SCOPE_SIGNAL)
501 return -EINVAL;
502 ret = comp->signal_array_u32_read(counter, parent, idx,
503 &value_u32);
504 *value = value_u32;
505 return ret;
506 case COUNTER_COMP_U64:
507 switch (scope) {
508 case COUNTER_SCOPE_DEVICE:
509 return comp->device_array_u64_read(counter, idx, value);
510 case COUNTER_SCOPE_SIGNAL:
511 return comp->signal_array_u64_read(counter, parent, idx,
512 value);
513 case COUNTER_SCOPE_COUNT:
514 return comp->count_array_u64_read(counter, parent, idx,
515 value);
516 default:
517 return -EINVAL;
518 }
519 default:
520 return -EINVAL;
521 }
522}
523
524static int counter_get_data(struct counter_device *const counter,
525 const struct counter_comp_node *const comp_node,
526 u64 *const value)
527{
528 const struct counter_comp *const comp = &comp_node->comp;
529 const enum counter_scope scope = comp_node->component.scope;
530 const size_t id = comp_node->component.id;
531 struct counter_signal *const signal = comp_node->parent;
532 struct counter_count *const count = comp_node->parent;
533 u8 value_u8 = 0;
534 u32 value_u32 = 0;
535 const struct counter_comp *ext;
536 size_t num_ext;
537 size_t ext_idx, ext_id;
538 int ret;
539
540 if (comp_node->component.type == COUNTER_COMPONENT_NONE)
541 return 0;
542
543 switch (comp->type) {
544 case COUNTER_COMP_U8:
545 case COUNTER_COMP_BOOL:
546 switch (scope) {
547 case COUNTER_SCOPE_DEVICE:
548 ret = comp->device_u8_read(counter, &value_u8);
549 break;
550 case COUNTER_SCOPE_SIGNAL:
551 ret = comp->signal_u8_read(counter, signal, &value_u8);
552 break;
553 case COUNTER_SCOPE_COUNT:
554 ret = comp->count_u8_read(counter, count, &value_u8);
555 break;
556 default:
557 return -EINVAL;
558 }
559 *value = value_u8;
560 return ret;
561 case COUNTER_COMP_SIGNAL_LEVEL:
562 case COUNTER_COMP_FUNCTION:
563 case COUNTER_COMP_ENUM:
564 case COUNTER_COMP_COUNT_DIRECTION:
565 case COUNTER_COMP_COUNT_MODE:
566 case COUNTER_COMP_SIGNAL_POLARITY:
567 switch (scope) {
568 case COUNTER_SCOPE_DEVICE:
569 ret = comp->device_u32_read(counter, &value_u32);
570 break;
571 case COUNTER_SCOPE_SIGNAL:
572 ret = comp->signal_u32_read(counter, signal,
573 &value_u32);
574 break;
575 case COUNTER_SCOPE_COUNT:
576 ret = comp->count_u32_read(counter, count, &value_u32);
577 break;
578 default:
579 return -EINVAL;
580 }
581 *value = value_u32;
582 return ret;
583 case COUNTER_COMP_U64:
584 switch (scope) {
585 case COUNTER_SCOPE_DEVICE:
586 return comp->device_u64_read(counter, value);
587 case COUNTER_SCOPE_SIGNAL:
588 return comp->signal_u64_read(counter, signal, value);
589 case COUNTER_SCOPE_COUNT:
590 return comp->count_u64_read(counter, count, value);
591 default:
592 return -EINVAL;
593 }
594 case COUNTER_COMP_SYNAPSE_ACTION:
595 ret = comp->action_read(counter, count, comp->priv, &value_u32);
596 *value = value_u32;
597 return ret;
598 case COUNTER_COMP_ARRAY:
599 switch (scope) {
600 case COUNTER_SCOPE_DEVICE:
601 ext = counter->ext;
602 num_ext = counter->num_ext;
603 break;
604 case COUNTER_SCOPE_SIGNAL:
605 ext = signal->ext;
606 num_ext = signal->num_ext;
607 break;
608 case COUNTER_SCOPE_COUNT:
609 ext = count->ext;
610 num_ext = count->num_ext;
611 break;
612 default:
613 return -EINVAL;
614 }
615 ret = counter_get_ext(ext, num_ext, component_id: id, ext_idx: &ext_idx, id: &ext_id);
616 if (ret < 0)
617 return ret;
618
619 return counter_get_array_data(counter, scope, parent: comp_node->parent,
620 comp, idx: id - ext_id, value);
621 default:
622 return -EINVAL;
623 }
624}
625
626/**
627 * counter_push_event - queue event for userspace reading
628 * @counter: pointer to Counter structure
629 * @event: triggered event
630 * @channel: event channel
631 *
632 * Note: If no one is watching for the respective event, it is silently
633 * discarded.
634 */
635void counter_push_event(struct counter_device *const counter, const u8 event,
636 const u8 channel)
637{
638 struct counter_event ev;
639 unsigned int copied = 0;
640 unsigned long flags;
641 struct counter_event_node *event_node;
642 struct counter_comp_node *comp_node;
643
644 ev.timestamp = ktime_get_ns();
645 ev.watch.event = event;
646 ev.watch.channel = channel;
647
648 /* Could be in an interrupt context, so use a spin lock */
649 spin_lock_irqsave(&counter->events_list_lock, flags);
650
651 /* Search for event in the list */
652 list_for_each_entry(event_node, &counter->events_list, l)
653 if (event_node->event == event &&
654 event_node->channel == channel)
655 break;
656
657 /* If event is not in the list */
658 if (&event_node->l == &counter->events_list)
659 goto exit_early;
660
661 /* Read and queue relevant comp for userspace */
662 list_for_each_entry(comp_node, &event_node->comp_list, l) {
663 ev.watch.component = comp_node->component;
664 ev.status = -counter_get_data(counter, comp_node, value: &ev.value);
665
666 copied += kfifo_in_spinlocked_noirqsave(&counter->events, &ev,
667 1, &counter->events_in_lock);
668 }
669
670exit_early:
671 spin_unlock_irqrestore(lock: &counter->events_list_lock, flags);
672
673 if (copied)
674 wake_up_poll(&counter->events_wait, EPOLLIN);
675}
676EXPORT_SYMBOL_NS_GPL(counter_push_event, COUNTER);
677

source code of linux/drivers/counter/counter-chrdev.c