drivers/counter/counter-chrdev.c

   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         .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
 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
 482 void counter_chrdev_remove(struct counter_device *const counter)
 483 {
 484         kfifo_free(&counter->events);
 485 }
 486
 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
 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  */
 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);