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