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 counter_chrdev_read(struct file * filp,char __user * buf,size_t len,loff_t * f_ps)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 counter_chrdev_poll(struct file * filp,struct poll_table_struct * pollt)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 counter_events_list_free(struct list_head * const events_list)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 counter_set_event_node(struct counter_device * const counter,struct counter_watch * const watch,const struct counter_comp_node * const cfg)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 counter_enable_events(struct counter_device * const counter)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 counter_disable_events(struct counter_device * const counter)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 counter_get_ext(const struct counter_comp * const ext,const size_t num_ext,const size_t component_id,size_t * const ext_idx,size_t * const id)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 counter_add_watch(struct counter_device * const counter,const unsigned long arg)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 counter_chrdev_ioctl(struct file * filp,unsigned int cmd,unsigned long arg)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 counter_chrdev_open(struct inode * inode,struct file * filp)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 counter_chrdev_release(struct inode * inode,struct file * filp)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 .read = counter_chrdev_read, 458 .poll = counter_chrdev_poll, 459 .unlocked_ioctl = counter_chrdev_ioctl, 460 .open = counter_chrdev_open, 461 .release = counter_chrdev_release, 462 }; 463 counter_chrdev_add(struct counter_device * const counter)464 int counter_chrdev_add(struct counter_device *const counter) 465 { 466 /* Initialize Counter events lists */ 467 INIT_LIST_HEAD(&counter->events_list); 468 INIT_LIST_HEAD(&counter->next_events_list); 469 spin_lock_init(&counter->events_list_lock); 470 mutex_init(&counter->n_events_list_lock); 471 init_waitqueue_head(&counter->events_wait); 472 spin_lock_init(&counter->events_in_lock); 473 mutex_init(&counter->events_out_lock); 474 475 /* Initialize character device */ 476 cdev_init(&counter->chrdev, &counter_fops); 477 478 /* Allocate Counter events queue */ 479 return kfifo_alloc(&counter->events, 64, GFP_KERNEL); 480 } 481 counter_chrdev_remove(struct counter_device * const counter)482 void counter_chrdev_remove(struct counter_device *const counter) 483 { 484 kfifo_free(&counter->events); 485 } 486 counter_get_array_data(struct counter_device * const counter,const enum counter_scope scope,void * const parent,const struct counter_comp * const comp,const size_t idx,u64 * const value)487 static int counter_get_array_data(struct counter_device *const counter, 488 const enum counter_scope scope, 489 void *const parent, 490 const struct counter_comp *const comp, 491 const size_t idx, u64 *const value) 492 { 493 const struct counter_array *const element = comp->priv; 494 u32 value_u32 = 0; 495 int ret; 496 497 switch (element->type) { 498 case COUNTER_COMP_SIGNAL_POLARITY: 499 if (scope != COUNTER_SCOPE_SIGNAL) 500 return -EINVAL; 501 ret = comp->signal_array_u32_read(counter, parent, idx, 502 &value_u32); 503 *value = value_u32; 504 return ret; 505 case COUNTER_COMP_U64: 506 switch (scope) { 507 case COUNTER_SCOPE_DEVICE: 508 return comp->device_array_u64_read(counter, idx, value); 509 case COUNTER_SCOPE_SIGNAL: 510 return comp->signal_array_u64_read(counter, parent, idx, 511 value); 512 case COUNTER_SCOPE_COUNT: 513 return comp->count_array_u64_read(counter, parent, idx, 514 value); 515 default: 516 return -EINVAL; 517 } 518 default: 519 return -EINVAL; 520 } 521 } 522 counter_get_data(struct counter_device * const counter,const struct counter_comp_node * const comp_node,u64 * const value)523 static int counter_get_data(struct counter_device *const counter, 524 const struct counter_comp_node *const comp_node, 525 u64 *const value) 526 { 527 const struct counter_comp *const comp = &comp_node->comp; 528 const enum counter_scope scope = comp_node->component.scope; 529 const size_t id = comp_node->component.id; 530 struct counter_signal *const signal = comp_node->parent; 531 struct counter_count *const count = comp_node->parent; 532 u8 value_u8 = 0; 533 u32 value_u32 = 0; 534 const struct counter_comp *ext; 535 size_t num_ext; 536 size_t ext_idx, ext_id; 537 int ret; 538 539 if (comp_node->component.type == COUNTER_COMPONENT_NONE) 540 return 0; 541 542 switch (comp->type) { 543 case COUNTER_COMP_U8: 544 case COUNTER_COMP_BOOL: 545 switch (scope) { 546 case COUNTER_SCOPE_DEVICE: 547 ret = comp->device_u8_read(counter, &value_u8); 548 break; 549 case COUNTER_SCOPE_SIGNAL: 550 ret = comp->signal_u8_read(counter, signal, &value_u8); 551 break; 552 case COUNTER_SCOPE_COUNT: 553 ret = comp->count_u8_read(counter, count, &value_u8); 554 break; 555 default: 556 return -EINVAL; 557 } 558 *value = value_u8; 559 return ret; 560 case COUNTER_COMP_SIGNAL_LEVEL: 561 case COUNTER_COMP_FUNCTION: 562 case COUNTER_COMP_ENUM: 563 case COUNTER_COMP_COUNT_DIRECTION: 564 case COUNTER_COMP_COUNT_MODE: 565 case COUNTER_COMP_SIGNAL_POLARITY: 566 switch (scope) { 567 case COUNTER_SCOPE_DEVICE: 568 ret = comp->device_u32_read(counter, &value_u32); 569 break; 570 case COUNTER_SCOPE_SIGNAL: 571 ret = comp->signal_u32_read(counter, signal, 572 &value_u32); 573 break; 574 case COUNTER_SCOPE_COUNT: 575 ret = comp->count_u32_read(counter, count, &value_u32); 576 break; 577 default: 578 return -EINVAL; 579 } 580 *value = value_u32; 581 return ret; 582 case COUNTER_COMP_U64: 583 switch (scope) { 584 case COUNTER_SCOPE_DEVICE: 585 return comp->device_u64_read(counter, value); 586 case COUNTER_SCOPE_SIGNAL: 587 return comp->signal_u64_read(counter, signal, value); 588 case COUNTER_SCOPE_COUNT: 589 return comp->count_u64_read(counter, count, value); 590 default: 591 return -EINVAL; 592 } 593 case COUNTER_COMP_SYNAPSE_ACTION: 594 ret = comp->action_read(counter, count, comp->priv, &value_u32); 595 *value = value_u32; 596 return ret; 597 case COUNTER_COMP_ARRAY: 598 switch (scope) { 599 case COUNTER_SCOPE_DEVICE: 600 ext = counter->ext; 601 num_ext = counter->num_ext; 602 break; 603 case COUNTER_SCOPE_SIGNAL: 604 ext = signal->ext; 605 num_ext = signal->num_ext; 606 break; 607 case COUNTER_SCOPE_COUNT: 608 ext = count->ext; 609 num_ext = count->num_ext; 610 break; 611 default: 612 return -EINVAL; 613 } 614 ret = counter_get_ext(ext, num_ext, id, &ext_idx, &ext_id); 615 if (ret < 0) 616 return ret; 617 618 return counter_get_array_data(counter, scope, comp_node->parent, 619 comp, id - ext_id, value); 620 default: 621 return -EINVAL; 622 } 623 } 624 625 /** 626 * counter_push_event - queue event for userspace reading 627 * @counter: pointer to Counter structure 628 * @event: triggered event 629 * @channel: event channel 630 * 631 * Note: If no one is watching for the respective event, it is silently 632 * discarded. 633 */ counter_push_event(struct counter_device * const counter,const u8 event,const u8 channel)634 void counter_push_event(struct counter_device *const counter, const u8 event, 635 const u8 channel) 636 { 637 struct counter_event ev; 638 unsigned int copied = 0; 639 unsigned long flags; 640 struct counter_event_node *event_node; 641 struct counter_comp_node *comp_node; 642 643 ev.timestamp = ktime_get_ns(); 644 ev.watch.event = event; 645 ev.watch.channel = channel; 646 647 /* Could be in an interrupt context, so use a spin lock */ 648 spin_lock_irqsave(&counter->events_list_lock, flags); 649 650 /* Search for event in the list */ 651 list_for_each_entry(event_node, &counter->events_list, l) 652 if (event_node->event == event && 653 event_node->channel == channel) 654 break; 655 656 /* If event is not in the list */ 657 if (&event_node->l == &counter->events_list) 658 goto exit_early; 659 660 /* Read and queue relevant comp for userspace */ 661 list_for_each_entry(comp_node, &event_node->comp_list, l) { 662 ev.watch.component = comp_node->component; 663 ev.status = -counter_get_data(counter, comp_node, &ev.value); 664 665 copied += kfifo_in_spinlocked_noirqsave(&counter->events, &ev, 666 1, &counter->events_in_lock); 667 } 668 669 exit_early: 670 spin_unlock_irqrestore(&counter->events_list_lock, flags); 671 672 if (copied) 673 wake_up_poll(&counter->events_wait, EPOLLIN); 674 } 675 EXPORT_SYMBOL_NS_GPL(counter_push_event, COUNTER); 676