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 } 481 *value = value_u8; 482 return ret; 483 case COUNTER_COMP_SIGNAL_LEVEL: 484 case COUNTER_COMP_FUNCTION: 485 case COUNTER_COMP_ENUM: 486 case COUNTER_COMP_COUNT_DIRECTION: 487 case COUNTER_COMP_COUNT_MODE: 488 switch (comp_node->component.scope) { 489 case COUNTER_SCOPE_DEVICE: 490 ret = comp->device_u32_read(counter, &value_u32); 491 break; 492 case COUNTER_SCOPE_SIGNAL: 493 ret = comp->signal_u32_read(counter, parent, 494 &value_u32); 495 break; 496 case COUNTER_SCOPE_COUNT: 497 ret = comp->count_u32_read(counter, parent, &value_u32); 498 break; 499 } 500 *value = value_u32; 501 return ret; 502 case COUNTER_COMP_U64: 503 switch (comp_node->component.scope) { 504 case COUNTER_SCOPE_DEVICE: 505 return comp->device_u64_read(counter, value); 506 case COUNTER_SCOPE_SIGNAL: 507 return comp->signal_u64_read(counter, parent, value); 508 case COUNTER_SCOPE_COUNT: 509 return comp->count_u64_read(counter, parent, value); 510 default: 511 return -EINVAL; 512 } 513 case COUNTER_COMP_SYNAPSE_ACTION: 514 ret = comp->action_read(counter, parent, comp->priv, 515 &value_u32); 516 *value = value_u32; 517 return ret; 518 default: 519 return -EINVAL; 520 } 521 } 522 523 /** 524 * counter_push_event - queue event for userspace reading 525 * @counter: pointer to Counter structure 526 * @event: triggered event 527 * @channel: event channel 528 * 529 * Note: If no one is watching for the respective event, it is silently 530 * discarded. 531 */ 532 void counter_push_event(struct counter_device *const counter, const u8 event, 533 const u8 channel) 534 { 535 struct counter_event ev; 536 unsigned int copied = 0; 537 unsigned long flags; 538 struct counter_event_node *event_node; 539 struct counter_comp_node *comp_node; 540 541 ev.timestamp = ktime_get_ns(); 542 ev.watch.event = event; 543 ev.watch.channel = channel; 544 545 /* Could be in an interrupt context, so use a spin lock */ 546 spin_lock_irqsave(&counter->events_list_lock, flags); 547 548 /* Search for event in the list */ 549 list_for_each_entry(event_node, &counter->events_list, l) 550 if (event_node->event == event && 551 event_node->channel == channel) 552 break; 553 554 /* If event is not in the list */ 555 if (&event_node->l == &counter->events_list) 556 goto exit_early; 557 558 /* Read and queue relevant comp for userspace */ 559 list_for_each_entry(comp_node, &event_node->comp_list, l) { 560 ev.watch.component = comp_node->component; 561 ev.status = -counter_get_data(counter, comp_node, &ev.value); 562 563 copied += kfifo_in_spinlocked_noirqsave(&counter->events, &ev, 564 1, &counter->events_in_lock); 565 } 566 567 exit_early: 568 spin_unlock_irqrestore(&counter->events_list_lock, flags); 569 570 if (copied) 571 wake_up_poll(&counter->events_wait, EPOLLIN); 572 } 573 EXPORT_SYMBOL_GPL(counter_push_event); 574