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 
26 struct 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 
65 static 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(&counter->events_out_lock);
96 		if (err < 0)
97 			return err;
98 	} while (!copied);
99 
100 	return copied;
101 }
102 
103 static __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, &counter->events_wait, pollt);
113 
114 	if (!kfifo_is_empty(&counter->events))
115 		events = EPOLLIN | EPOLLRDNORM;
116 
117 	return events;
118 }
119 
120 static 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(&q->l);
129 			kfree(q);
130 		}
131 
132 		/* Free event node */
133 		list_del(&p->l);
134 		kfree(p);
135 	}
136 }
137 
138 static 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(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(&event_node->comp_list);
163 		list_add(&event_node->l, &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(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(&comp_node->l, &event_node->comp_list);
184 
185 exit_free_event_node:
186 	/* Free event node if no one else is watching */
187 	if (list_empty(&event_node->comp_list)) {
188 		list_del(&event_node->l);
189 		kfree(event_node);
190 	}
191 
192 	return err;
193 }
194 
195 static 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(&counter->events_list);
204 	list_replace_init(&counter->next_events_list,
205 			  &counter->events_list);
206 
207 	if (counter->ops->events_configure)
208 		err = counter->ops->events_configure(counter);
209 
210 	spin_unlock_irqrestore(&counter->events_list_lock, flags);
211 	mutex_unlock(&counter->n_events_list_lock);
212 
213 	return err;
214 }
215 
216 static 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(&counter->events_list);
224 
225 	if (counter->ops->events_configure)
226 		err = counter->ops->events_configure(counter);
227 
228 	spin_unlock_irqrestore(&counter->events_list_lock, flags);
229 
230 	mutex_lock(&counter->n_events_list_lock);
231 
232 	counter_events_list_free(&counter->next_events_list);
233 
234 	mutex_unlock(&counter->n_events_list_lock);
235 
236 	return err;
237 }
238 
239 static 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 
264 static 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(&watch, uwatch, 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, id, &ext_idx, &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 
363 no_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, &comp_node);
375 
376 err_exit:
377 	mutex_unlock(&counter->n_events_list_lock);
378 
379 	return err;
380 }
381 
382 static 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 
408 out_unlock:
409 	mutex_unlock(&counter->ops_exist_lock);
410 
411 	return ret;
412 }
413 
414 static 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(&counter->dev);
421 	filp->private_data = counter;
422 
423 	return nonseekable_open(inode, filp);
424 }
425 
426 static 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(&counter->events_list);
436 		counter_events_list_free(&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(&counter->ops_exist_lock);
444 		return ret;
445 	}
446 
447 out_unlock:
448 	mutex_unlock(&counter->ops_exist_lock);
449 
450 	put_device(&counter->dev);
451 
452 	return ret;
453 }
454 
455 static 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 
465 int counter_chrdev_add(struct counter_device *const counter)
466 {
467 	/* Initialize Counter events lists */
468 	INIT_LIST_HEAD(&counter->events_list);
469 	INIT_LIST_HEAD(&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 
483 void counter_chrdev_remove(struct counter_device *const counter)
484 {
485 	kfifo_free(&counter->events);
486 }
487 
488 static 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 
524 static 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, id, &ext_idx, &ext_id);
616 		if (ret < 0)
617 			return ret;
618 
619 		return counter_get_array_data(counter, scope, comp_node->parent,
620 					      comp, 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  */
635 void 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, &ev.value);
665 
666 		copied += kfifo_in_spinlocked_noirqsave(&counter->events, &ev,
667 							1, &counter->events_in_lock);
668 	}
669 
670 exit_early:
671 	spin_unlock_irqrestore(&counter->events_list_lock, flags);
672 
673 	if (copied)
674 		wake_up_poll(&counter->events_wait, EPOLLIN);
675 }
676 EXPORT_SYMBOL_NS_GPL(counter_push_event, COUNTER);
677