1 /* 2 * Loopback bridge driver for the Greybus loopback module. 3 * 4 * Copyright 2014 Google Inc. 5 * Copyright 2014 Linaro Ltd. 6 * 7 * Released under the GPLv2 only. 8 */ 9 10 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt 11 12 #include <linux/kernel.h> 13 #include <linux/module.h> 14 #include <linux/mutex.h> 15 #include <linux/slab.h> 16 #include <linux/kthread.h> 17 #include <linux/delay.h> 18 #include <linux/random.h> 19 #include <linux/sizes.h> 20 #include <linux/cdev.h> 21 #include <linux/fs.h> 22 #include <linux/kfifo.h> 23 #include <linux/debugfs.h> 24 #include <linux/list_sort.h> 25 #include <linux/spinlock.h> 26 #include <linux/workqueue.h> 27 #include <linux/atomic.h> 28 #include <linux/pm_runtime.h> 29 30 #include <asm/div64.h> 31 32 #include "greybus.h" 33 #include "connection.h" 34 35 #define NSEC_PER_DAY 86400000000000ULL 36 37 struct gb_loopback_stats { 38 u32 min; 39 u32 max; 40 u64 sum; 41 u32 count; 42 }; 43 44 struct gb_loopback_device { 45 struct dentry *root; 46 u32 count; 47 size_t size_max; 48 49 /* We need to take a lock in atomic context */ 50 spinlock_t lock; 51 struct list_head list; 52 struct list_head list_op_async; 53 wait_queue_head_t wq; 54 }; 55 56 static struct gb_loopback_device gb_dev; 57 58 struct gb_loopback_async_operation { 59 struct gb_loopback *gb; 60 struct gb_operation *operation; 61 struct timeval ts; 62 struct timer_list timer; 63 struct list_head entry; 64 struct work_struct work; 65 struct kref kref; 66 bool pending; 67 int (*completion)(struct gb_loopback_async_operation *op_async); 68 }; 69 70 struct gb_loopback { 71 struct gb_connection *connection; 72 73 struct dentry *file; 74 struct kfifo kfifo_lat; 75 struct kfifo kfifo_ts; 76 struct mutex mutex; 77 struct task_struct *task; 78 struct list_head entry; 79 struct device *dev; 80 wait_queue_head_t wq; 81 wait_queue_head_t wq_completion; 82 atomic_t outstanding_operations; 83 84 /* Per connection stats */ 85 struct timeval ts; 86 struct gb_loopback_stats latency; 87 struct gb_loopback_stats throughput; 88 struct gb_loopback_stats requests_per_second; 89 struct gb_loopback_stats apbridge_unipro_latency; 90 struct gb_loopback_stats gbphy_firmware_latency; 91 92 int type; 93 int async; 94 int id; 95 u32 size; 96 u32 iteration_max; 97 u32 iteration_count; 98 int us_wait; 99 u32 error; 100 u32 requests_completed; 101 u32 requests_timedout; 102 u32 timeout; 103 u32 jiffy_timeout; 104 u32 timeout_min; 105 u32 timeout_max; 106 u32 outstanding_operations_max; 107 u32 lbid; 108 u64 elapsed_nsecs; 109 u32 apbridge_latency_ts; 110 u32 gbphy_latency_ts; 111 112 u32 send_count; 113 }; 114 115 static struct class loopback_class = { 116 .name = "gb_loopback", 117 .owner = THIS_MODULE, 118 }; 119 static DEFINE_IDA(loopback_ida); 120 121 /* Min/max values in jiffies */ 122 #define GB_LOOPBACK_TIMEOUT_MIN 1 123 #define GB_LOOPBACK_TIMEOUT_MAX 10000 124 125 #define GB_LOOPBACK_FIFO_DEFAULT 8192 126 127 static unsigned int kfifo_depth = GB_LOOPBACK_FIFO_DEFAULT; 128 module_param(kfifo_depth, uint, 0444); 129 130 /* Maximum size of any one send data buffer we support */ 131 #define MAX_PACKET_SIZE (PAGE_SIZE * 2) 132 133 #define GB_LOOPBACK_US_WAIT_MAX 1000000 134 135 /* interface sysfs attributes */ 136 #define gb_loopback_ro_attr(field) \ 137 static ssize_t field##_show(struct device *dev, \ 138 struct device_attribute *attr, \ 139 char *buf) \ 140 { \ 141 struct gb_loopback *gb = dev_get_drvdata(dev); \ 142 return sprintf(buf, "%u\n", gb->field); \ 143 } \ 144 static DEVICE_ATTR_RO(field) 145 146 #define gb_loopback_ro_stats_attr(name, field, type) \ 147 static ssize_t name##_##field##_show(struct device *dev, \ 148 struct device_attribute *attr, \ 149 char *buf) \ 150 { \ 151 struct gb_loopback *gb = dev_get_drvdata(dev); \ 152 /* Report 0 for min and max if no transfer successed */ \ 153 if (!gb->requests_completed) \ 154 return sprintf(buf, "0\n"); \ 155 return sprintf(buf, "%"#type"\n", gb->name.field); \ 156 } \ 157 static DEVICE_ATTR_RO(name##_##field) 158 159 #define gb_loopback_ro_avg_attr(name) \ 160 static ssize_t name##_avg_show(struct device *dev, \ 161 struct device_attribute *attr, \ 162 char *buf) \ 163 { \ 164 struct gb_loopback_stats *stats; \ 165 struct gb_loopback *gb; \ 166 u64 avg, rem; \ 167 u32 count; \ 168 gb = dev_get_drvdata(dev); \ 169 stats = &gb->name; \ 170 count = stats->count ? stats->count : 1; \ 171 avg = stats->sum + count / 2000000; /* round closest */ \ 172 rem = do_div(avg, count); \ 173 rem *= 1000000; \ 174 do_div(rem, count); \ 175 return sprintf(buf, "%llu.%06u\n", avg, (u32)rem); \ 176 } \ 177 static DEVICE_ATTR_RO(name##_avg) 178 179 #define gb_loopback_stats_attrs(field) \ 180 gb_loopback_ro_stats_attr(field, min, u); \ 181 gb_loopback_ro_stats_attr(field, max, u); \ 182 gb_loopback_ro_avg_attr(field) 183 184 #define gb_loopback_attr(field, type) \ 185 static ssize_t field##_show(struct device *dev, \ 186 struct device_attribute *attr, \ 187 char *buf) \ 188 { \ 189 struct gb_loopback *gb = dev_get_drvdata(dev); \ 190 return sprintf(buf, "%"#type"\n", gb->field); \ 191 } \ 192 static ssize_t field##_store(struct device *dev, \ 193 struct device_attribute *attr, \ 194 const char *buf, \ 195 size_t len) \ 196 { \ 197 int ret; \ 198 struct gb_loopback *gb = dev_get_drvdata(dev); \ 199 mutex_lock(&gb->mutex); \ 200 ret = sscanf(buf, "%"#type, &gb->field); \ 201 if (ret != 1) \ 202 len = -EINVAL; \ 203 else \ 204 gb_loopback_check_attr(gb, bundle); \ 205 mutex_unlock(&gb->mutex); \ 206 return len; \ 207 } \ 208 static DEVICE_ATTR_RW(field) 209 210 #define gb_dev_loopback_ro_attr(field, conn) \ 211 static ssize_t field##_show(struct device *dev, \ 212 struct device_attribute *attr, \ 213 char *buf) \ 214 { \ 215 struct gb_loopback *gb = dev_get_drvdata(dev); \ 216 return sprintf(buf, "%u\n", gb->field); \ 217 } \ 218 static DEVICE_ATTR_RO(field) 219 220 #define gb_dev_loopback_rw_attr(field, type) \ 221 static ssize_t field##_show(struct device *dev, \ 222 struct device_attribute *attr, \ 223 char *buf) \ 224 { \ 225 struct gb_loopback *gb = dev_get_drvdata(dev); \ 226 return sprintf(buf, "%"#type"\n", gb->field); \ 227 } \ 228 static ssize_t field##_store(struct device *dev, \ 229 struct device_attribute *attr, \ 230 const char *buf, \ 231 size_t len) \ 232 { \ 233 int ret; \ 234 struct gb_loopback *gb = dev_get_drvdata(dev); \ 235 mutex_lock(&gb->mutex); \ 236 ret = sscanf(buf, "%"#type, &gb->field); \ 237 if (ret != 1) \ 238 len = -EINVAL; \ 239 else \ 240 gb_loopback_check_attr(gb); \ 241 mutex_unlock(&gb->mutex); \ 242 return len; \ 243 } \ 244 static DEVICE_ATTR_RW(field) 245 246 static void gb_loopback_reset_stats(struct gb_loopback *gb); 247 static void gb_loopback_check_attr(struct gb_loopback *gb) 248 { 249 if (gb->us_wait > GB_LOOPBACK_US_WAIT_MAX) 250 gb->us_wait = GB_LOOPBACK_US_WAIT_MAX; 251 if (gb->size > gb_dev.size_max) 252 gb->size = gb_dev.size_max; 253 gb->requests_timedout = 0; 254 gb->requests_completed = 0; 255 gb->iteration_count = 0; 256 gb->send_count = 0; 257 gb->error = 0; 258 259 if (kfifo_depth < gb->iteration_max) { 260 dev_warn(gb->dev, 261 "cannot log bytes %u kfifo_depth %u\n", 262 gb->iteration_max, kfifo_depth); 263 } 264 kfifo_reset_out(&gb->kfifo_lat); 265 kfifo_reset_out(&gb->kfifo_ts); 266 267 switch (gb->type) { 268 case GB_LOOPBACK_TYPE_PING: 269 case GB_LOOPBACK_TYPE_TRANSFER: 270 case GB_LOOPBACK_TYPE_SINK: 271 gb->jiffy_timeout = usecs_to_jiffies(gb->timeout); 272 if (!gb->jiffy_timeout) 273 gb->jiffy_timeout = GB_LOOPBACK_TIMEOUT_MIN; 274 else if (gb->jiffy_timeout > GB_LOOPBACK_TIMEOUT_MAX) 275 gb->jiffy_timeout = GB_LOOPBACK_TIMEOUT_MAX; 276 gb_loopback_reset_stats(gb); 277 wake_up(&gb->wq); 278 break; 279 default: 280 gb->type = 0; 281 break; 282 } 283 } 284 285 /* Time to send and receive one message */ 286 gb_loopback_stats_attrs(latency); 287 /* Number of requests sent per second on this cport */ 288 gb_loopback_stats_attrs(requests_per_second); 289 /* Quantity of data sent and received on this cport */ 290 gb_loopback_stats_attrs(throughput); 291 /* Latency across the UniPro link from APBridge's perspective */ 292 gb_loopback_stats_attrs(apbridge_unipro_latency); 293 /* Firmware induced overhead in the GPBridge */ 294 gb_loopback_stats_attrs(gbphy_firmware_latency); 295 296 /* Number of errors encountered during loop */ 297 gb_loopback_ro_attr(error); 298 /* Number of requests successfully completed async */ 299 gb_loopback_ro_attr(requests_completed); 300 /* Number of requests timed out async */ 301 gb_loopback_ro_attr(requests_timedout); 302 /* Timeout minimum in useconds */ 303 gb_loopback_ro_attr(timeout_min); 304 /* Timeout minimum in useconds */ 305 gb_loopback_ro_attr(timeout_max); 306 307 /* 308 * Type of loopback message to send based on protocol type definitions 309 * 0 => Don't send message 310 * 2 => Send ping message continuously (message without payload) 311 * 3 => Send transfer message continuously (message with payload, 312 * payload returned in response) 313 * 4 => Send a sink message (message with payload, no payload in response) 314 */ 315 gb_dev_loopback_rw_attr(type, d); 316 /* Size of transfer message payload: 0-4096 bytes */ 317 gb_dev_loopback_rw_attr(size, u); 318 /* Time to wait between two messages: 0-1000 ms */ 319 gb_dev_loopback_rw_attr(us_wait, d); 320 /* Maximum iterations for a given operation: 1-(2^32-1), 0 implies infinite */ 321 gb_dev_loopback_rw_attr(iteration_max, u); 322 /* The current index of the for (i = 0; i < iteration_max; i++) loop */ 323 gb_dev_loopback_ro_attr(iteration_count, false); 324 /* A flag to indicate synchronous or asynchronous operations */ 325 gb_dev_loopback_rw_attr(async, u); 326 /* Timeout of an individual asynchronous request */ 327 gb_dev_loopback_rw_attr(timeout, u); 328 /* Maximum number of in-flight operations before back-off */ 329 gb_dev_loopback_rw_attr(outstanding_operations_max, u); 330 331 static struct attribute *loopback_attrs[] = { 332 &dev_attr_latency_min.attr, 333 &dev_attr_latency_max.attr, 334 &dev_attr_latency_avg.attr, 335 &dev_attr_requests_per_second_min.attr, 336 &dev_attr_requests_per_second_max.attr, 337 &dev_attr_requests_per_second_avg.attr, 338 &dev_attr_throughput_min.attr, 339 &dev_attr_throughput_max.attr, 340 &dev_attr_throughput_avg.attr, 341 &dev_attr_apbridge_unipro_latency_min.attr, 342 &dev_attr_apbridge_unipro_latency_max.attr, 343 &dev_attr_apbridge_unipro_latency_avg.attr, 344 &dev_attr_gbphy_firmware_latency_min.attr, 345 &dev_attr_gbphy_firmware_latency_max.attr, 346 &dev_attr_gbphy_firmware_latency_avg.attr, 347 &dev_attr_type.attr, 348 &dev_attr_size.attr, 349 &dev_attr_us_wait.attr, 350 &dev_attr_iteration_count.attr, 351 &dev_attr_iteration_max.attr, 352 &dev_attr_async.attr, 353 &dev_attr_error.attr, 354 &dev_attr_requests_completed.attr, 355 &dev_attr_requests_timedout.attr, 356 &dev_attr_timeout.attr, 357 &dev_attr_outstanding_operations_max.attr, 358 &dev_attr_timeout_min.attr, 359 &dev_attr_timeout_max.attr, 360 NULL, 361 }; 362 ATTRIBUTE_GROUPS(loopback); 363 364 static void gb_loopback_calculate_stats(struct gb_loopback *gb, bool error); 365 366 static u32 gb_loopback_nsec_to_usec_latency(u64 elapsed_nsecs) 367 { 368 u32 lat; 369 370 do_div(elapsed_nsecs, NSEC_PER_USEC); 371 lat = elapsed_nsecs; 372 return lat; 373 } 374 375 static u64 __gb_loopback_calc_latency(u64 t1, u64 t2) 376 { 377 if (t2 > t1) 378 return t2 - t1; 379 else 380 return NSEC_PER_DAY - t2 + t1; 381 } 382 383 static u64 gb_loopback_calc_latency(struct timeval *ts, struct timeval *te) 384 { 385 u64 t1, t2; 386 387 t1 = timeval_to_ns(ts); 388 t2 = timeval_to_ns(te); 389 390 return __gb_loopback_calc_latency(t1, t2); 391 } 392 393 static void gb_loopback_push_latency_ts(struct gb_loopback *gb, 394 struct timeval *ts, struct timeval *te) 395 { 396 kfifo_in(&gb->kfifo_ts, (unsigned char *)ts, sizeof(*ts)); 397 kfifo_in(&gb->kfifo_ts, (unsigned char *)te, sizeof(*te)); 398 } 399 400 static int gb_loopback_operation_sync(struct gb_loopback *gb, int type, 401 void *request, int request_size, 402 void *response, int response_size) 403 { 404 struct gb_operation *operation; 405 struct timeval ts, te; 406 int ret; 407 408 do_gettimeofday(&ts); 409 operation = gb_operation_create(gb->connection, type, request_size, 410 response_size, GFP_KERNEL); 411 if (!operation) 412 return -ENOMEM; 413 414 if (request_size) 415 memcpy(operation->request->payload, request, request_size); 416 417 ret = gb_operation_request_send_sync(operation); 418 if (ret) { 419 dev_err(&gb->connection->bundle->dev, 420 "synchronous operation failed: %d\n", ret); 421 goto out_put_operation; 422 } else { 423 if (response_size == operation->response->payload_size) { 424 memcpy(response, operation->response->payload, 425 response_size); 426 } else { 427 dev_err(&gb->connection->bundle->dev, 428 "response size %zu expected %d\n", 429 operation->response->payload_size, 430 response_size); 431 ret = -EINVAL; 432 goto out_put_operation; 433 } 434 } 435 436 do_gettimeofday(&te); 437 438 /* Calculate the total time the message took */ 439 gb_loopback_push_latency_ts(gb, &ts, &te); 440 gb->elapsed_nsecs = gb_loopback_calc_latency(&ts, &te); 441 442 out_put_operation: 443 gb_operation_put(operation); 444 445 return ret; 446 } 447 448 static void __gb_loopback_async_operation_destroy(struct kref *kref) 449 { 450 struct gb_loopback_async_operation *op_async; 451 452 op_async = container_of(kref, struct gb_loopback_async_operation, kref); 453 454 list_del(&op_async->entry); 455 if (op_async->operation) 456 gb_operation_put(op_async->operation); 457 atomic_dec(&op_async->gb->outstanding_operations); 458 wake_up(&op_async->gb->wq_completion); 459 kfree(op_async); 460 } 461 462 static void gb_loopback_async_operation_get(struct gb_loopback_async_operation 463 *op_async) 464 { 465 kref_get(&op_async->kref); 466 } 467 468 static void gb_loopback_async_operation_put(struct gb_loopback_async_operation 469 *op_async) 470 { 471 unsigned long flags; 472 473 spin_lock_irqsave(&gb_dev.lock, flags); 474 kref_put(&op_async->kref, __gb_loopback_async_operation_destroy); 475 spin_unlock_irqrestore(&gb_dev.lock, flags); 476 } 477 478 static struct gb_loopback_async_operation * 479 gb_loopback_operation_find(u16 id) 480 { 481 struct gb_loopback_async_operation *op_async; 482 bool found = false; 483 unsigned long flags; 484 485 spin_lock_irqsave(&gb_dev.lock, flags); 486 list_for_each_entry(op_async, &gb_dev.list_op_async, entry) { 487 if (op_async->operation->id == id) { 488 gb_loopback_async_operation_get(op_async); 489 found = true; 490 break; 491 } 492 } 493 spin_unlock_irqrestore(&gb_dev.lock, flags); 494 495 return found ? op_async : NULL; 496 } 497 498 static void gb_loopback_async_wait_all(struct gb_loopback *gb) 499 { 500 wait_event(gb->wq_completion, 501 !atomic_read(&gb->outstanding_operations)); 502 } 503 504 static void gb_loopback_async_operation_callback(struct gb_operation *operation) 505 { 506 struct gb_loopback_async_operation *op_async; 507 struct gb_loopback *gb; 508 struct timeval te; 509 bool err = false; 510 511 do_gettimeofday(&te); 512 op_async = gb_loopback_operation_find(operation->id); 513 if (!op_async) 514 return; 515 516 gb = op_async->gb; 517 mutex_lock(&gb->mutex); 518 519 if (!op_async->pending || gb_operation_result(operation)) { 520 err = true; 521 } else { 522 if (op_async->completion) 523 if (op_async->completion(op_async)) 524 err = true; 525 } 526 527 if (!err) { 528 gb_loopback_push_latency_ts(gb, &op_async->ts, &te); 529 gb->elapsed_nsecs = gb_loopback_calc_latency(&op_async->ts, 530 &te); 531 } 532 533 if (op_async->pending) { 534 if (err) 535 gb->error++; 536 gb->iteration_count++; 537 op_async->pending = false; 538 del_timer_sync(&op_async->timer); 539 gb_loopback_async_operation_put(op_async); 540 gb_loopback_calculate_stats(gb, err); 541 } 542 mutex_unlock(&gb->mutex); 543 544 dev_dbg(&gb->connection->bundle->dev, "complete operation %d\n", 545 operation->id); 546 547 gb_loopback_async_operation_put(op_async); 548 } 549 550 static void gb_loopback_async_operation_work(struct work_struct *work) 551 { 552 struct gb_loopback *gb; 553 struct gb_operation *operation; 554 struct gb_loopback_async_operation *op_async; 555 556 op_async = container_of(work, struct gb_loopback_async_operation, work); 557 gb = op_async->gb; 558 operation = op_async->operation; 559 560 mutex_lock(&gb->mutex); 561 if (op_async->pending) { 562 gb->requests_timedout++; 563 gb->error++; 564 gb->iteration_count++; 565 op_async->pending = false; 566 gb_loopback_async_operation_put(op_async); 567 gb_loopback_calculate_stats(gb, true); 568 } 569 mutex_unlock(&gb->mutex); 570 571 dev_dbg(&gb->connection->bundle->dev, "timeout operation %d\n", 572 operation->id); 573 574 gb_operation_cancel(operation, -ETIMEDOUT); 575 gb_loopback_async_operation_put(op_async); 576 } 577 578 static void gb_loopback_async_operation_timeout(unsigned long data) 579 { 580 struct gb_loopback_async_operation *op_async; 581 u16 id = data; 582 583 op_async = gb_loopback_operation_find(id); 584 if (!op_async) { 585 pr_err("operation %d not found - time out ?\n", id); 586 return; 587 } 588 schedule_work(&op_async->work); 589 } 590 591 static int gb_loopback_async_operation(struct gb_loopback *gb, int type, 592 void *request, int request_size, 593 int response_size, 594 void *completion) 595 { 596 struct gb_loopback_async_operation *op_async; 597 struct gb_operation *operation; 598 int ret; 599 unsigned long flags; 600 601 op_async = kzalloc(sizeof(*op_async), GFP_KERNEL); 602 if (!op_async) 603 return -ENOMEM; 604 605 INIT_WORK(&op_async->work, gb_loopback_async_operation_work); 606 kref_init(&op_async->kref); 607 608 operation = gb_operation_create(gb->connection, type, request_size, 609 response_size, GFP_KERNEL); 610 if (!operation) { 611 kfree(op_async); 612 return -ENOMEM; 613 } 614 615 if (request_size) 616 memcpy(operation->request->payload, request, request_size); 617 618 op_async->gb = gb; 619 op_async->operation = operation; 620 op_async->completion = completion; 621 622 spin_lock_irqsave(&gb_dev.lock, flags); 623 list_add_tail(&op_async->entry, &gb_dev.list_op_async); 624 spin_unlock_irqrestore(&gb_dev.lock, flags); 625 626 do_gettimeofday(&op_async->ts); 627 op_async->pending = true; 628 atomic_inc(&gb->outstanding_operations); 629 mutex_lock(&gb->mutex); 630 ret = gb_operation_request_send(operation, 631 gb_loopback_async_operation_callback, 632 0, 633 GFP_KERNEL); 634 if (ret) 635 goto error; 636 637 setup_timer(&op_async->timer, gb_loopback_async_operation_timeout, 638 (unsigned long)operation->id); 639 op_async->timer.expires = jiffies + gb->jiffy_timeout; 640 add_timer(&op_async->timer); 641 642 goto done; 643 error: 644 gb_loopback_async_operation_put(op_async); 645 done: 646 mutex_unlock(&gb->mutex); 647 return ret; 648 } 649 650 static int gb_loopback_sync_sink(struct gb_loopback *gb, u32 len) 651 { 652 struct gb_loopback_transfer_request *request; 653 int retval; 654 655 request = kmalloc(len + sizeof(*request), GFP_KERNEL); 656 if (!request) 657 return -ENOMEM; 658 659 request->len = cpu_to_le32(len); 660 retval = gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_SINK, 661 request, len + sizeof(*request), 662 NULL, 0); 663 kfree(request); 664 return retval; 665 } 666 667 static int gb_loopback_sync_transfer(struct gb_loopback *gb, u32 len) 668 { 669 struct gb_loopback_transfer_request *request; 670 struct gb_loopback_transfer_response *response; 671 int retval; 672 673 gb->apbridge_latency_ts = 0; 674 gb->gbphy_latency_ts = 0; 675 676 request = kmalloc(len + sizeof(*request), GFP_KERNEL); 677 if (!request) 678 return -ENOMEM; 679 response = kmalloc(len + sizeof(*response), GFP_KERNEL); 680 if (!response) { 681 kfree(request); 682 return -ENOMEM; 683 } 684 685 memset(request->data, 0x5A, len); 686 687 request->len = cpu_to_le32(len); 688 retval = gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_TRANSFER, 689 request, len + sizeof(*request), 690 response, len + sizeof(*response)); 691 if (retval) 692 goto gb_error; 693 694 if (memcmp(request->data, response->data, len)) { 695 dev_err(&gb->connection->bundle->dev, 696 "Loopback Data doesn't match\n"); 697 retval = -EREMOTEIO; 698 } 699 gb->apbridge_latency_ts = (u32)__le32_to_cpu(response->reserved0); 700 gb->gbphy_latency_ts = (u32)__le32_to_cpu(response->reserved1); 701 702 gb_error: 703 kfree(request); 704 kfree(response); 705 706 return retval; 707 } 708 709 static int gb_loopback_sync_ping(struct gb_loopback *gb) 710 { 711 return gb_loopback_operation_sync(gb, GB_LOOPBACK_TYPE_PING, 712 NULL, 0, NULL, 0); 713 } 714 715 static int gb_loopback_async_sink(struct gb_loopback *gb, u32 len) 716 { 717 struct gb_loopback_transfer_request *request; 718 int retval; 719 720 request = kmalloc(len + sizeof(*request), GFP_KERNEL); 721 if (!request) 722 return -ENOMEM; 723 724 request->len = cpu_to_le32(len); 725 retval = gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_SINK, 726 request, len + sizeof(*request), 727 0, NULL); 728 kfree(request); 729 return retval; 730 } 731 732 static int gb_loopback_async_transfer_complete( 733 struct gb_loopback_async_operation *op_async) 734 { 735 struct gb_loopback *gb; 736 struct gb_operation *operation; 737 struct gb_loopback_transfer_request *request; 738 struct gb_loopback_transfer_response *response; 739 size_t len; 740 int retval = 0; 741 742 gb = op_async->gb; 743 operation = op_async->operation; 744 request = operation->request->payload; 745 response = operation->response->payload; 746 len = le32_to_cpu(request->len); 747 748 if (memcmp(request->data, response->data, len)) { 749 dev_err(&gb->connection->bundle->dev, 750 "Loopback Data doesn't match operation id %d\n", 751 operation->id); 752 retval = -EREMOTEIO; 753 } else { 754 gb->apbridge_latency_ts = 755 (u32)__le32_to_cpu(response->reserved0); 756 gb->gbphy_latency_ts = 757 (u32)__le32_to_cpu(response->reserved1); 758 } 759 760 return retval; 761 } 762 763 static int gb_loopback_async_transfer(struct gb_loopback *gb, u32 len) 764 { 765 struct gb_loopback_transfer_request *request; 766 int retval, response_len; 767 768 request = kmalloc(len + sizeof(*request), GFP_KERNEL); 769 if (!request) 770 return -ENOMEM; 771 772 memset(request->data, 0x5A, len); 773 774 request->len = cpu_to_le32(len); 775 response_len = sizeof(struct gb_loopback_transfer_response); 776 retval = gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_TRANSFER, 777 request, len + sizeof(*request), 778 len + response_len, 779 gb_loopback_async_transfer_complete); 780 if (retval) 781 goto gb_error; 782 783 gb_error: 784 kfree(request); 785 return retval; 786 } 787 788 static int gb_loopback_async_ping(struct gb_loopback *gb) 789 { 790 return gb_loopback_async_operation(gb, GB_LOOPBACK_TYPE_PING, 791 NULL, 0, 0, NULL); 792 } 793 794 static int gb_loopback_request_handler(struct gb_operation *operation) 795 { 796 struct gb_connection *connection = operation->connection; 797 struct gb_loopback_transfer_request *request; 798 struct gb_loopback_transfer_response *response; 799 struct device *dev = &connection->bundle->dev; 800 size_t len; 801 802 /* By convention, the AP initiates the version operation */ 803 switch (operation->type) { 804 case GB_LOOPBACK_TYPE_PING: 805 case GB_LOOPBACK_TYPE_SINK: 806 return 0; 807 case GB_LOOPBACK_TYPE_TRANSFER: 808 if (operation->request->payload_size < sizeof(*request)) { 809 dev_err(dev, "transfer request too small (%zu < %zu)\n", 810 operation->request->payload_size, 811 sizeof(*request)); 812 return -EINVAL; /* -EMSGSIZE */ 813 } 814 request = operation->request->payload; 815 len = le32_to_cpu(request->len); 816 if (len > gb_dev.size_max) { 817 dev_err(dev, "transfer request too large (%zu > %zu)\n", 818 len, gb_dev.size_max); 819 return -EINVAL; 820 } 821 822 if (!gb_operation_response_alloc(operation, 823 len + sizeof(*response), GFP_KERNEL)) { 824 dev_err(dev, "error allocating response\n"); 825 return -ENOMEM; 826 } 827 response = operation->response->payload; 828 response->len = cpu_to_le32(len); 829 if (len) 830 memcpy(response->data, request->data, len); 831 832 return 0; 833 default: 834 dev_err(dev, "unsupported request: %u\n", operation->type); 835 return -EINVAL; 836 } 837 } 838 839 static void gb_loopback_reset_stats(struct gb_loopback *gb) 840 { 841 struct gb_loopback_stats reset = { 842 .min = U32_MAX, 843 }; 844 845 /* Reset per-connection stats */ 846 memcpy(&gb->latency, &reset, 847 sizeof(struct gb_loopback_stats)); 848 memcpy(&gb->throughput, &reset, 849 sizeof(struct gb_loopback_stats)); 850 memcpy(&gb->requests_per_second, &reset, 851 sizeof(struct gb_loopback_stats)); 852 memcpy(&gb->apbridge_unipro_latency, &reset, 853 sizeof(struct gb_loopback_stats)); 854 memcpy(&gb->gbphy_firmware_latency, &reset, 855 sizeof(struct gb_loopback_stats)); 856 857 /* Should be initialized at least once per transaction set */ 858 gb->apbridge_latency_ts = 0; 859 gb->gbphy_latency_ts = 0; 860 memset(&gb->ts, 0, sizeof(struct timeval)); 861 } 862 863 static void gb_loopback_update_stats(struct gb_loopback_stats *stats, u32 val) 864 { 865 if (stats->min > val) 866 stats->min = val; 867 if (stats->max < val) 868 stats->max = val; 869 stats->sum += val; 870 stats->count++; 871 } 872 873 static void gb_loopback_update_stats_window(struct gb_loopback_stats *stats, 874 u64 val, u32 count) 875 { 876 stats->sum += val; 877 stats->count += count; 878 879 do_div(val, count); 880 if (stats->min > val) 881 stats->min = val; 882 if (stats->max < val) 883 stats->max = val; 884 } 885 886 static void gb_loopback_requests_update(struct gb_loopback *gb, u32 latency) 887 { 888 u64 req = gb->requests_completed * USEC_PER_SEC; 889 890 gb_loopback_update_stats_window(&gb->requests_per_second, req, latency); 891 } 892 893 static void gb_loopback_throughput_update(struct gb_loopback *gb, u32 latency) 894 { 895 u64 aggregate_size = sizeof(struct gb_operation_msg_hdr) * 2; 896 897 switch (gb->type) { 898 case GB_LOOPBACK_TYPE_PING: 899 break; 900 case GB_LOOPBACK_TYPE_SINK: 901 aggregate_size += sizeof(struct gb_loopback_transfer_request) + 902 gb->size; 903 break; 904 case GB_LOOPBACK_TYPE_TRANSFER: 905 aggregate_size += sizeof(struct gb_loopback_transfer_request) + 906 sizeof(struct gb_loopback_transfer_response) + 907 gb->size * 2; 908 break; 909 default: 910 return; 911 } 912 913 aggregate_size *= gb->requests_completed; 914 aggregate_size *= USEC_PER_SEC; 915 gb_loopback_update_stats_window(&gb->throughput, aggregate_size, 916 latency); 917 } 918 919 static void gb_loopback_calculate_latency_stats(struct gb_loopback *gb) 920 { 921 u32 lat; 922 923 /* Express latency in terms of microseconds */ 924 lat = gb_loopback_nsec_to_usec_latency(gb->elapsed_nsecs); 925 926 /* Log latency stastic */ 927 gb_loopback_update_stats(&gb->latency, lat); 928 929 /* Raw latency log on a per thread basis */ 930 kfifo_in(&gb->kfifo_lat, (unsigned char *)&lat, sizeof(lat)); 931 932 /* Log the firmware supplied latency values */ 933 gb_loopback_update_stats(&gb->apbridge_unipro_latency, 934 gb->apbridge_latency_ts); 935 gb_loopback_update_stats(&gb->gbphy_firmware_latency, 936 gb->gbphy_latency_ts); 937 } 938 939 static void gb_loopback_calculate_stats(struct gb_loopback *gb, bool error) 940 { 941 u64 nlat; 942 u32 lat; 943 struct timeval te; 944 945 if (!error) { 946 gb->requests_completed++; 947 gb_loopback_calculate_latency_stats(gb); 948 } 949 950 do_gettimeofday(&te); 951 nlat = gb_loopback_calc_latency(&gb->ts, &te); 952 if (nlat >= NSEC_PER_SEC || gb->iteration_count == gb->iteration_max) { 953 lat = gb_loopback_nsec_to_usec_latency(nlat); 954 955 gb_loopback_throughput_update(gb, lat); 956 gb_loopback_requests_update(gb, lat); 957 958 if (gb->iteration_count != gb->iteration_max) { 959 gb->ts = te; 960 gb->requests_completed = 0; 961 } 962 } 963 } 964 965 static void gb_loopback_async_wait_to_send(struct gb_loopback *gb) 966 { 967 if (!(gb->async && gb->outstanding_operations_max)) 968 return; 969 wait_event_interruptible(gb->wq_completion, 970 (atomic_read(&gb->outstanding_operations) < 971 gb->outstanding_operations_max) || 972 kthread_should_stop()); 973 } 974 975 static int gb_loopback_fn(void *data) 976 { 977 int error = 0; 978 int us_wait = 0; 979 int type; 980 int ret; 981 u32 size; 982 983 struct gb_loopback *gb = data; 984 struct gb_bundle *bundle = gb->connection->bundle; 985 986 ret = gb_pm_runtime_get_sync(bundle); 987 if (ret) 988 return ret; 989 990 while (1) { 991 if (!gb->type) { 992 gb_pm_runtime_put_autosuspend(bundle); 993 wait_event_interruptible(gb->wq, gb->type || 994 kthread_should_stop()); 995 ret = gb_pm_runtime_get_sync(bundle); 996 if (ret) 997 return ret; 998 } 999 1000 if (kthread_should_stop()) 1001 break; 1002 1003 /* Limit the maximum number of in-flight async operations */ 1004 gb_loopback_async_wait_to_send(gb); 1005 if (kthread_should_stop()) 1006 break; 1007 1008 mutex_lock(&gb->mutex); 1009 1010 /* Optionally terminate */ 1011 if (gb->send_count == gb->iteration_max) { 1012 mutex_unlock(&gb->mutex); 1013 1014 /* Wait for synchronous and asynchronus completion */ 1015 gb_loopback_async_wait_all(gb); 1016 1017 /* Mark complete unless user-space has poked us */ 1018 mutex_lock(&gb->mutex); 1019 if (gb->iteration_count == gb->iteration_max) { 1020 gb->type = 0; 1021 gb->send_count = 0; 1022 sysfs_notify(&gb->dev->kobj, NULL, 1023 "iteration_count"); 1024 dev_dbg(&bundle->dev, "load test complete\n"); 1025 } else { 1026 dev_dbg(&bundle->dev, 1027 "continuing on with new test set\n"); 1028 } 1029 mutex_unlock(&gb->mutex); 1030 continue; 1031 } 1032 size = gb->size; 1033 us_wait = gb->us_wait; 1034 type = gb->type; 1035 if (gb->ts.tv_usec == 0 && gb->ts.tv_sec == 0) 1036 do_gettimeofday(&gb->ts); 1037 mutex_unlock(&gb->mutex); 1038 1039 /* Else operations to perform */ 1040 if (gb->async) { 1041 if (type == GB_LOOPBACK_TYPE_PING) 1042 error = gb_loopback_async_ping(gb); 1043 else if (type == GB_LOOPBACK_TYPE_TRANSFER) 1044 error = gb_loopback_async_transfer(gb, size); 1045 else if (type == GB_LOOPBACK_TYPE_SINK) 1046 error = gb_loopback_async_sink(gb, size); 1047 1048 if (error) 1049 gb->error++; 1050 } else { 1051 /* We are effectively single threaded here */ 1052 if (type == GB_LOOPBACK_TYPE_PING) 1053 error = gb_loopback_sync_ping(gb); 1054 else if (type == GB_LOOPBACK_TYPE_TRANSFER) 1055 error = gb_loopback_sync_transfer(gb, size); 1056 else if (type == GB_LOOPBACK_TYPE_SINK) 1057 error = gb_loopback_sync_sink(gb, size); 1058 1059 if (error) 1060 gb->error++; 1061 gb->iteration_count++; 1062 gb_loopback_calculate_stats(gb, !!error); 1063 } 1064 gb->send_count++; 1065 1066 if (us_wait) { 1067 if (us_wait < 20000) 1068 usleep_range(us_wait, us_wait + 100); 1069 else 1070 msleep(us_wait / 1000); 1071 } 1072 } 1073 1074 gb_pm_runtime_put_autosuspend(bundle); 1075 1076 return 0; 1077 } 1078 1079 static int gb_loopback_dbgfs_latency_show_common(struct seq_file *s, 1080 struct kfifo *kfifo, 1081 struct mutex *mutex) 1082 { 1083 u32 latency; 1084 int retval; 1085 1086 if (kfifo_len(kfifo) == 0) { 1087 retval = -EAGAIN; 1088 goto done; 1089 } 1090 1091 mutex_lock(mutex); 1092 retval = kfifo_out(kfifo, &latency, sizeof(latency)); 1093 if (retval > 0) { 1094 seq_printf(s, "%u", latency); 1095 retval = 0; 1096 } 1097 mutex_unlock(mutex); 1098 done: 1099 return retval; 1100 } 1101 1102 static int gb_loopback_dbgfs_latency_show(struct seq_file *s, void *unused) 1103 { 1104 struct gb_loopback *gb = s->private; 1105 1106 return gb_loopback_dbgfs_latency_show_common(s, &gb->kfifo_lat, 1107 &gb->mutex); 1108 } 1109 1110 static int gb_loopback_latency_open(struct inode *inode, struct file *file) 1111 { 1112 return single_open(file, gb_loopback_dbgfs_latency_show, 1113 inode->i_private); 1114 } 1115 1116 static const struct file_operations gb_loopback_debugfs_latency_ops = { 1117 .open = gb_loopback_latency_open, 1118 .read = seq_read, 1119 .llseek = seq_lseek, 1120 .release = single_release, 1121 }; 1122 1123 static int gb_loopback_bus_id_compare(void *priv, struct list_head *lha, 1124 struct list_head *lhb) 1125 { 1126 struct gb_loopback *a = list_entry(lha, struct gb_loopback, entry); 1127 struct gb_loopback *b = list_entry(lhb, struct gb_loopback, entry); 1128 struct gb_connection *ca = a->connection; 1129 struct gb_connection *cb = b->connection; 1130 1131 if (ca->bundle->intf->interface_id < cb->bundle->intf->interface_id) 1132 return -1; 1133 if (cb->bundle->intf->interface_id < ca->bundle->intf->interface_id) 1134 return 1; 1135 if (ca->bundle->id < cb->bundle->id) 1136 return -1; 1137 if (cb->bundle->id < ca->bundle->id) 1138 return 1; 1139 if (ca->intf_cport_id < cb->intf_cport_id) 1140 return -1; 1141 else if (cb->intf_cport_id < ca->intf_cport_id) 1142 return 1; 1143 1144 return 0; 1145 } 1146 1147 static void gb_loopback_insert_id(struct gb_loopback *gb) 1148 { 1149 struct gb_loopback *gb_list; 1150 u32 new_lbid = 0; 1151 1152 /* perform an insertion sort */ 1153 list_add_tail(&gb->entry, &gb_dev.list); 1154 list_sort(NULL, &gb_dev.list, gb_loopback_bus_id_compare); 1155 list_for_each_entry(gb_list, &gb_dev.list, entry) { 1156 gb_list->lbid = 1 << new_lbid; 1157 new_lbid++; 1158 } 1159 } 1160 1161 #define DEBUGFS_NAMELEN 32 1162 1163 static int gb_loopback_probe(struct gb_bundle *bundle, 1164 const struct greybus_bundle_id *id) 1165 { 1166 struct greybus_descriptor_cport *cport_desc; 1167 struct gb_connection *connection; 1168 struct gb_loopback *gb; 1169 struct device *dev; 1170 int retval; 1171 char name[DEBUGFS_NAMELEN]; 1172 unsigned long flags; 1173 1174 if (bundle->num_cports != 1) 1175 return -ENODEV; 1176 1177 cport_desc = &bundle->cport_desc[0]; 1178 if (cport_desc->protocol_id != GREYBUS_PROTOCOL_LOOPBACK) 1179 return -ENODEV; 1180 1181 gb = kzalloc(sizeof(*gb), GFP_KERNEL); 1182 if (!gb) 1183 return -ENOMEM; 1184 1185 connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id), 1186 gb_loopback_request_handler); 1187 if (IS_ERR(connection)) { 1188 retval = PTR_ERR(connection); 1189 goto out_kzalloc; 1190 } 1191 1192 gb->connection = connection; 1193 greybus_set_drvdata(bundle, gb); 1194 1195 init_waitqueue_head(&gb->wq); 1196 init_waitqueue_head(&gb->wq_completion); 1197 atomic_set(&gb->outstanding_operations, 0); 1198 gb_loopback_reset_stats(gb); 1199 1200 /* Reported values to user-space for min/max timeouts */ 1201 gb->timeout_min = jiffies_to_usecs(GB_LOOPBACK_TIMEOUT_MIN); 1202 gb->timeout_max = jiffies_to_usecs(GB_LOOPBACK_TIMEOUT_MAX); 1203 1204 if (!gb_dev.count) { 1205 /* Calculate maximum payload */ 1206 gb_dev.size_max = gb_operation_get_payload_size_max(connection); 1207 if (gb_dev.size_max <= 1208 sizeof(struct gb_loopback_transfer_request)) { 1209 retval = -EINVAL; 1210 goto out_connection_destroy; 1211 } 1212 gb_dev.size_max -= sizeof(struct gb_loopback_transfer_request); 1213 } 1214 1215 /* Create per-connection sysfs and debugfs data-points */ 1216 snprintf(name, sizeof(name), "raw_latency_%s", 1217 dev_name(&connection->bundle->dev)); 1218 gb->file = debugfs_create_file(name, S_IFREG | 0444, gb_dev.root, gb, 1219 &gb_loopback_debugfs_latency_ops); 1220 1221 gb->id = ida_simple_get(&loopback_ida, 0, 0, GFP_KERNEL); 1222 if (gb->id < 0) { 1223 retval = gb->id; 1224 goto out_debugfs_remove; 1225 } 1226 1227 retval = gb_connection_enable(connection); 1228 if (retval) 1229 goto out_ida_remove; 1230 1231 dev = device_create_with_groups(&loopback_class, 1232 &connection->bundle->dev, 1233 MKDEV(0, 0), gb, loopback_groups, 1234 "gb_loopback%d", gb->id); 1235 if (IS_ERR(dev)) { 1236 retval = PTR_ERR(dev); 1237 goto out_connection_disable; 1238 } 1239 gb->dev = dev; 1240 1241 /* Allocate kfifo */ 1242 if (kfifo_alloc(&gb->kfifo_lat, kfifo_depth * sizeof(u32), 1243 GFP_KERNEL)) { 1244 retval = -ENOMEM; 1245 goto out_conn; 1246 } 1247 if (kfifo_alloc(&gb->kfifo_ts, kfifo_depth * sizeof(struct timeval) * 2, 1248 GFP_KERNEL)) { 1249 retval = -ENOMEM; 1250 goto out_kfifo0; 1251 } 1252 1253 /* Fork worker thread */ 1254 mutex_init(&gb->mutex); 1255 gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback"); 1256 if (IS_ERR(gb->task)) { 1257 retval = PTR_ERR(gb->task); 1258 goto out_kfifo1; 1259 } 1260 1261 spin_lock_irqsave(&gb_dev.lock, flags); 1262 gb_loopback_insert_id(gb); 1263 gb_dev.count++; 1264 spin_unlock_irqrestore(&gb_dev.lock, flags); 1265 1266 gb_connection_latency_tag_enable(connection); 1267 1268 gb_pm_runtime_put_autosuspend(bundle); 1269 1270 return 0; 1271 1272 out_kfifo1: 1273 kfifo_free(&gb->kfifo_ts); 1274 out_kfifo0: 1275 kfifo_free(&gb->kfifo_lat); 1276 out_conn: 1277 device_unregister(dev); 1278 out_connection_disable: 1279 gb_connection_disable(connection); 1280 out_ida_remove: 1281 ida_simple_remove(&loopback_ida, gb->id); 1282 out_debugfs_remove: 1283 debugfs_remove(gb->file); 1284 out_connection_destroy: 1285 gb_connection_destroy(connection); 1286 out_kzalloc: 1287 kfree(gb); 1288 1289 return retval; 1290 } 1291 1292 static void gb_loopback_disconnect(struct gb_bundle *bundle) 1293 { 1294 struct gb_loopback *gb = greybus_get_drvdata(bundle); 1295 unsigned long flags; 1296 int ret; 1297 1298 ret = gb_pm_runtime_get_sync(bundle); 1299 if (ret) 1300 gb_pm_runtime_get_noresume(bundle); 1301 1302 gb_connection_disable(gb->connection); 1303 1304 if (!IS_ERR_OR_NULL(gb->task)) 1305 kthread_stop(gb->task); 1306 1307 kfifo_free(&gb->kfifo_lat); 1308 kfifo_free(&gb->kfifo_ts); 1309 gb_connection_latency_tag_disable(gb->connection); 1310 debugfs_remove(gb->file); 1311 1312 /* 1313 * FIXME: gb_loopback_async_wait_all() is redundant now, as connection 1314 * is disabled at the beginning and so we can't have any more 1315 * incoming/outgoing requests. 1316 */ 1317 gb_loopback_async_wait_all(gb); 1318 1319 spin_lock_irqsave(&gb_dev.lock, flags); 1320 gb_dev.count--; 1321 list_del(&gb->entry); 1322 spin_unlock_irqrestore(&gb_dev.lock, flags); 1323 1324 device_unregister(gb->dev); 1325 ida_simple_remove(&loopback_ida, gb->id); 1326 1327 gb_connection_destroy(gb->connection); 1328 kfree(gb); 1329 } 1330 1331 static const struct greybus_bundle_id gb_loopback_id_table[] = { 1332 { GREYBUS_DEVICE_CLASS(GREYBUS_CLASS_LOOPBACK) }, 1333 { } 1334 }; 1335 MODULE_DEVICE_TABLE(greybus, gb_loopback_id_table); 1336 1337 static struct greybus_driver gb_loopback_driver = { 1338 .name = "loopback", 1339 .probe = gb_loopback_probe, 1340 .disconnect = gb_loopback_disconnect, 1341 .id_table = gb_loopback_id_table, 1342 }; 1343 1344 static int loopback_init(void) 1345 { 1346 int retval; 1347 1348 INIT_LIST_HEAD(&gb_dev.list); 1349 INIT_LIST_HEAD(&gb_dev.list_op_async); 1350 spin_lock_init(&gb_dev.lock); 1351 gb_dev.root = debugfs_create_dir("gb_loopback", NULL); 1352 1353 retval = class_register(&loopback_class); 1354 if (retval) 1355 goto err; 1356 1357 retval = greybus_register(&gb_loopback_driver); 1358 if (retval) 1359 goto err_unregister; 1360 1361 return 0; 1362 1363 err_unregister: 1364 class_unregister(&loopback_class); 1365 err: 1366 debugfs_remove_recursive(gb_dev.root); 1367 return retval; 1368 } 1369 module_init(loopback_init); 1370 1371 static void __exit loopback_exit(void) 1372 { 1373 debugfs_remove_recursive(gb_dev.root); 1374 greybus_deregister(&gb_loopback_driver); 1375 class_unregister(&loopback_class); 1376 ida_destroy(&loopback_ida); 1377 } 1378 module_exit(loopback_exit); 1379 1380 MODULE_LICENSE("GPL v2"); 1381