drivers/staging/greybus/loopback.c

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