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(u: 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(t1: ktime_to_ns(kt: ts), t2: ktime_to_ns(kt: 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(connection: 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(ts: 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, error: result); |
449 | |
450 | mutex_unlock(lock: &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(v: &op_async->gb->outstanding_operations); |
457 | wake_up(&gb->wq_completion); |
458 | |
459 | /* Release resources */ |
460 | gb_operation_put(operation); |
461 | kfree(objp: 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(size: sizeof(*op_async), GFP_KERNEL); |
474 | if (!op_async) |
475 | return -ENOMEM; |
476 | |
477 | operation = gb_operation_create(connection: gb->connection, type, request_size, |
478 | response_size, GFP_KERNEL); |
479 | if (!operation) { |
480 | kfree(objp: 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, data: 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(v: &gb->outstanding_operations); |
496 | ret = gb_operation_request_send(operation, |
497 | callback: gb_loopback_async_operation_callback, |
498 | timeout: jiffies_to_msecs(j: gb->jiffy_timeout), |
499 | GFP_KERNEL); |
500 | if (ret) { |
501 | atomic_dec(v: &gb->outstanding_operations); |
502 | gb_operation_put(operation); |
503 | kfree(objp: 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(size: 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, request_size: len + sizeof(*request), |
520 | NULL, response_size: 0); |
521 | kfree(objp: 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(size: len + sizeof(*request), GFP_KERNEL); |
535 | if (!request) |
536 | return -ENOMEM; |
537 | response = kmalloc(size: len + sizeof(*response), GFP_KERNEL); |
538 | if (!response) { |
539 | kfree(objp: 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, request_size: len + sizeof(*request), |
548 | response, response_size: len + sizeof(*response)); |
549 | if (retval) |
550 | goto gb_error; |
551 | |
552 | if (memcmp(p: request->data, q: response->data, size: 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(objp: request); |
562 | kfree(objp: 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, request_size: 0, NULL, response_size: 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(size: 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, request_size: len + sizeof(*request), |
585 | response_size: 0, NULL); |
586 | kfree(objp: 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(p: request->data, q: response->data, size: 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(size: 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, request_size: len + sizeof(*request), |
636 | response_size: len + response_len, |
637 | completion: gb_loopback_async_transfer_complete); |
638 | if (retval) |
639 | goto gb_error; |
640 | |
641 | gb_error: |
642 | kfree(objp: 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, request_size: 0, response_size: 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 | response_size: 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(secs: 0, nsecs: 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(stats: &gb->requests_per_second, val: req, count: 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(stats: &gb->throughput, val: aggregate_size, |
774 | count: 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(elapsed_nsecs: gb->elapsed_nsecs); |
783 | |
784 | /* Log latency stastic */ |
785 | gb_loopback_update_stats(stats: &gb->latency, val: 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(stats: &gb->apbridge_unipro_latency, |
792 | val: gb->apbridge_latency_ts); |
793 | gb_loopback_update_stats(stats: &gb->gbphy_firmware_latency, |
794 | val: 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(ts: gb->ts, te); |
810 | if (nlat >= NSEC_PER_SEC || gb->iteration_count == gb->iteration_max) { |
811 | lat = gb_loopback_nsec_to_usec_latency(elapsed_nsecs: nlat); |
812 | |
813 | gb_loopback_throughput_update(gb, latency: lat); |
814 | gb_loopback_requests_update(gb, latency: 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(lock: &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(kobj: &gb->dev->kobj, NULL, |
881 | attr: "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(lock: &gb->mutex); |
888 | continue; |
889 | } |
890 | size = gb->size; |
891 | us_wait = gb->us_wait; |
892 | type = gb->type; |
893 | if (ktime_to_ns(kt: 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, len: size); |
902 | else if (type == GB_LOOPBACK_TYPE_SINK) |
903 | error = gb_loopback_async_sink(gb, len: 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, len: size); |
915 | else if (type == GB_LOOPBACK_TYPE_SINK) |
916 | error = gb_loopback_sync_sink(gb, len: size); |
917 | |
918 | if (error) |
919 | gb->error++; |
920 | gb->iteration_count++; |
921 | gb_loopback_calculate_stats(gb, error: !!error); |
922 | } |
923 | gb->send_count++; |
924 | mutex_unlock(lock: &gb->mutex); |
925 | |
926 | if (us_wait) { |
927 | if (us_wait < 20000) |
928 | usleep_range(min: us_wait, max: us_wait + 100); |
929 | else |
930 | msleep(msecs: 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(m: s, fmt: "%u" , latency); |
955 | retval = 0; |
956 | } |
957 | mutex_unlock(lock: 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, kfifo: &gb->kfifo_lat, |
967 | mutex: &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(size: sizeof(*gb), GFP_KERNEL); |
992 | if (!gb) |
993 | return -ENOMEM; |
994 | |
995 | connection = gb_connection_create(bundle, le16_to_cpu(cport_desc->id), |
996 | handler: gb_loopback_request_handler); |
997 | if (IS_ERR(ptr: connection)) { |
998 | retval = PTR_ERR(ptr: connection); |
999 | goto out_kzalloc; |
1000 | } |
1001 | |
1002 | gb->connection = connection; |
1003 | greybus_set_drvdata(bundle, data: gb); |
1004 | |
1005 | init_waitqueue_head(&gb->wq); |
1006 | init_waitqueue_head(&gb->wq_completion); |
1007 | atomic_set(v: &gb->outstanding_operations, i: 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(buf: name, size: sizeof(name), fmt: "raw_latency_%s" , |
1027 | dev_name(dev: &connection->bundle->dev)); |
1028 | gb->file = debugfs_create_file(name, S_IFREG | 0444, parent: gb_dev.root, data: gb, |
1029 | fops: &gb_loopback_dbgfs_latency_fops); |
1030 | |
1031 | gb->id = ida_alloc(ida: &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(cls: &loopback_class, |
1042 | parent: &connection->bundle->dev, |
1043 | MKDEV(0, 0), drvdata: gb, groups: loopback_groups, |
1044 | fmt: "gb_loopback%d" , gb->id); |
1045 | if (IS_ERR(ptr: dev)) { |
1046 | retval = PTR_ERR(ptr: 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(ptr: gb->task)) { |
1061 | retval = PTR_ERR(ptr: gb->task); |
1062 | goto out_kfifo; |
1063 | } |
1064 | |
1065 | spin_lock_irqsave(&gb_dev.lock, flags); |
1066 | gb_dev.count++; |
1067 | spin_unlock_irqrestore(lock: &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, id: gb->id); |
1083 | out_debugfs_remove: |
1084 | debugfs_remove(dentry: gb->file); |
1085 | out_connection_destroy: |
1086 | gb_connection_destroy(connection); |
1087 | out_kzalloc: |
1088 | kfree(objp: 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(connection: gb->connection); |
1104 | |
1105 | if (!IS_ERR_OR_NULL(ptr: gb->task)) |
1106 | kthread_stop(k: gb->task); |
1107 | |
1108 | kfifo_free(&gb->kfifo_lat); |
1109 | gb_connection_latency_tag_disable(connection: gb->connection); |
1110 | debugfs_remove(dentry: 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(lock: &gb_dev.lock, flags); |
1122 | |
1123 | device_unregister(dev: gb->dev); |
1124 | ida_free(&loopback_ida, id: gb->id); |
1125 | |
1126 | gb_connection_destroy(connection: gb->connection); |
1127 | kfree(objp: 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(name: "gb_loopback" , NULL); |
1149 | |
1150 | retval = class_register(class: &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(class: &loopback_class); |
1162 | err: |
1163 | debugfs_remove_recursive(dentry: 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(dentry: gb_dev.root); |
1171 | greybus_deregister(&gb_loopback_driver); |
1172 | class_unregister(class: &loopback_class); |
1173 | ida_destroy(ida: &loopback_ida); |
1174 | } |
1175 | module_exit(loopback_exit); |
1176 | |
1177 | MODULE_LICENSE("GPL v2" ); |
1178 | |