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