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(&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, &counter->events_wait, 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(&q->l);
129			kfree(q);
130		}
131
132		/* Free event node */
133		list_del(&p->l);
134		kfree(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(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
185exit_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
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(&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
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(&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
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->type == COUNTER_COMP_ARRAY) {
251			element = ext->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(&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
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, &comp_node);
375
376err_exit:
377	mutex_unlock(&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(&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(&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(&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
447out_unlock:
448	mutex_unlock(&counter->ops_exist_lock);
449
450	put_device(&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(&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
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, 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 */
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, &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(&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);