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