1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
3 
4 #include "tsnep.h"
5 
6 #include <net/pkt_sched.h>
7 
8 enum tsnep_test {
9 	TSNEP_TEST_ENABLE = 0,
10 	TSNEP_TEST_TAPRIO,
11 	TSNEP_TEST_TAPRIO_CHANGE,
12 	TSNEP_TEST_TAPRIO_EXTENSION,
13 };
14 
15 static const char tsnep_test_strings[][ETH_GSTRING_LEN] = {
16 	"Enable timeout        (offline)",
17 	"TAPRIO                (offline)",
18 	"TAPRIO change         (offline)",
19 	"TAPRIO extension      (offline)",
20 };
21 
22 #define TSNEP_TEST_COUNT (sizeof(tsnep_test_strings) / ETH_GSTRING_LEN)
23 
enable_gc_timeout(struct tsnep_adapter * adapter)24 static bool enable_gc_timeout(struct tsnep_adapter *adapter)
25 {
26 	iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
27 	if (!(ioread32(adapter->addr + TSNEP_GC) & TSNEP_GC_TIMEOUT_ACTIVE))
28 		return false;
29 
30 	return true;
31 }
32 
gc_timeout_signaled(struct tsnep_adapter * adapter)33 static bool gc_timeout_signaled(struct tsnep_adapter *adapter)
34 {
35 	if (ioread32(adapter->addr + TSNEP_GC) & TSNEP_GC_TIMEOUT_SIGNAL)
36 		return true;
37 
38 	return false;
39 }
40 
ack_gc_timeout(struct tsnep_adapter * adapter)41 static bool ack_gc_timeout(struct tsnep_adapter *adapter)
42 {
43 	iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
44 	if (ioread32(adapter->addr + TSNEP_GC) &
45 	    (TSNEP_GC_TIMEOUT_ACTIVE | TSNEP_GC_TIMEOUT_SIGNAL))
46 		return false;
47 	return true;
48 }
49 
enable_gc(struct tsnep_adapter * adapter,bool a)50 static bool enable_gc(struct tsnep_adapter *adapter, bool a)
51 {
52 	u8 enable;
53 	u8 active;
54 
55 	if (a) {
56 		enable = TSNEP_GC_ENABLE_A;
57 		active = TSNEP_GC_ACTIVE_A;
58 	} else {
59 		enable = TSNEP_GC_ENABLE_B;
60 		active = TSNEP_GC_ACTIVE_B;
61 	}
62 
63 	iowrite8(enable, adapter->addr + TSNEP_GC);
64 	if (!(ioread32(adapter->addr + TSNEP_GC) & active))
65 		return false;
66 
67 	return true;
68 }
69 
disable_gc(struct tsnep_adapter * adapter)70 static bool disable_gc(struct tsnep_adapter *adapter)
71 {
72 	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
73 	if (ioread32(adapter->addr + TSNEP_GC) &
74 	    (TSNEP_GC_ACTIVE_A | TSNEP_GC_ACTIVE_B))
75 		return false;
76 
77 	return true;
78 }
79 
gc_delayed_enable(struct tsnep_adapter * adapter,bool a,int delay)80 static bool gc_delayed_enable(struct tsnep_adapter *adapter, bool a, int delay)
81 {
82 	u64 before, after;
83 	u32 time;
84 	bool enabled;
85 
86 	if (!disable_gc(adapter))
87 		return false;
88 
89 	before = ktime_get_ns();
90 
91 	if (!enable_gc_timeout(adapter))
92 		return false;
93 
94 	/* for start time after timeout, the timeout can guarantee, that enable
95 	 * is blocked if too late
96 	 */
97 	time = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW);
98 	time += TSNEP_GC_TIMEOUT;
99 	iowrite32(time, adapter->addr + TSNEP_GC_TIME);
100 
101 	ndelay(delay);
102 
103 	enabled = enable_gc(adapter, a);
104 	after = ktime_get_ns();
105 
106 	if (delay > TSNEP_GC_TIMEOUT) {
107 		/* timeout must have blocked enable */
108 		if (enabled)
109 			return false;
110 	} else if ((after - before) < TSNEP_GC_TIMEOUT * 14 / 16) {
111 		/* timeout must not have blocked enable */
112 		if (!enabled)
113 			return false;
114 	}
115 
116 	if (enabled) {
117 		if (gc_timeout_signaled(adapter))
118 			return false;
119 	} else {
120 		if (!gc_timeout_signaled(adapter))
121 			return false;
122 		if (!ack_gc_timeout(adapter))
123 			return false;
124 	}
125 
126 	if (!disable_gc(adapter))
127 		return false;
128 
129 	return true;
130 }
131 
tsnep_test_gc_enable(struct tsnep_adapter * adapter)132 static bool tsnep_test_gc_enable(struct tsnep_adapter *adapter)
133 {
134 	int i;
135 
136 	iowrite32(0x80000001, adapter->addr + TSNEP_GCL_A + 0);
137 	iowrite32(100000, adapter->addr + TSNEP_GCL_A + 4);
138 
139 	for (i = 0; i < 200000; i += 100) {
140 		if (!gc_delayed_enable(adapter, true, i))
141 			return false;
142 	}
143 
144 	iowrite32(0x80000001, adapter->addr + TSNEP_GCL_B + 0);
145 	iowrite32(100000, adapter->addr + TSNEP_GCL_B + 4);
146 
147 	for (i = 0; i < 200000; i += 100) {
148 		if (!gc_delayed_enable(adapter, false, i))
149 			return false;
150 	}
151 
152 	return true;
153 }
154 
delay_base_time(struct tsnep_adapter * adapter,struct tc_taprio_qopt_offload * qopt,s64 ms)155 static void delay_base_time(struct tsnep_adapter *adapter,
156 			    struct tc_taprio_qopt_offload *qopt, s64 ms)
157 {
158 	u64 system_time;
159 	u64 base_time = ktime_to_ns(qopt->base_time);
160 	u64 n;
161 
162 	tsnep_get_system_time(adapter, &system_time);
163 	system_time += ms * 1000000;
164 	n = div64_u64(system_time - base_time, qopt->cycle_time);
165 
166 	qopt->base_time = ktime_add_ns(qopt->base_time,
167 				       (n + 1) * qopt->cycle_time);
168 }
169 
get_gate_state(struct tsnep_adapter * adapter,u32 * gc,u32 * gc_time,u64 * system_time)170 static void get_gate_state(struct tsnep_adapter *adapter, u32 *gc, u32 *gc_time,
171 			   u64 *system_time)
172 {
173 	u32 time_high_before;
174 	u32 time_low;
175 	u32 time_high;
176 	u32 gc_time_before;
177 
178 	time_high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
179 	*gc_time = ioread32(adapter->addr + TSNEP_GC_TIME);
180 	do {
181 		time_low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW);
182 		*gc = ioread32(adapter->addr + TSNEP_GC);
183 
184 		gc_time_before = *gc_time;
185 		*gc_time = ioread32(adapter->addr + TSNEP_GC_TIME);
186 		time_high_before = time_high;
187 		time_high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
188 	} while ((time_high != time_high_before) ||
189 		 (*gc_time != gc_time_before));
190 
191 	*system_time = (((u64)time_high) << 32) | ((u64)time_low);
192 }
193 
get_operation(struct tsnep_gcl * gcl,u64 system_time,u64 * next)194 static int get_operation(struct tsnep_gcl *gcl, u64 system_time, u64 *next)
195 {
196 	u64 n = div64_u64(system_time - gcl->base_time, gcl->cycle_time);
197 	u64 cycle_start = gcl->base_time + gcl->cycle_time * n;
198 	int i;
199 
200 	*next = cycle_start;
201 	for (i = 0; i < gcl->count; i++) {
202 		*next += gcl->operation[i].interval;
203 		if (*next > system_time)
204 			break;
205 	}
206 
207 	return i;
208 }
209 
check_gate(struct tsnep_adapter * adapter)210 static bool check_gate(struct tsnep_adapter *adapter)
211 {
212 	u32 gc_time;
213 	u32 gc;
214 	u64 system_time;
215 	struct tsnep_gcl *curr;
216 	struct tsnep_gcl *prev;
217 	u64 next_time;
218 	u8 gate_open;
219 	u8 next_gate_open;
220 
221 	get_gate_state(adapter, &gc, &gc_time, &system_time);
222 
223 	if (gc & TSNEP_GC_ACTIVE_A) {
224 		curr = &adapter->gcl[0];
225 		prev = &adapter->gcl[1];
226 	} else if (gc & TSNEP_GC_ACTIVE_B) {
227 		curr = &adapter->gcl[1];
228 		prev = &adapter->gcl[0];
229 	} else {
230 		return false;
231 	}
232 	if (curr->start_time <= system_time) {
233 		/* GCL is already active */
234 		int index;
235 
236 		index = get_operation(curr, system_time, &next_time);
237 		gate_open = curr->operation[index].properties & TSNEP_GCL_MASK;
238 		if (index == curr->count - 1)
239 			index = 0;
240 		else
241 			index++;
242 		next_gate_open =
243 			curr->operation[index].properties & TSNEP_GCL_MASK;
244 	} else if (curr->change) {
245 		/* operation of previous GCL is active */
246 		int index;
247 		u64 start_before;
248 		u64 n;
249 
250 		index = get_operation(prev, system_time, &next_time);
251 		next_time = curr->start_time;
252 		start_before = prev->base_time;
253 		n = div64_u64(curr->start_time - start_before,
254 			      prev->cycle_time);
255 		start_before += n * prev->cycle_time;
256 		if (curr->start_time == start_before)
257 			start_before -= prev->cycle_time;
258 		if (((start_before + prev->cycle_time_extension) >=
259 		     curr->start_time) &&
260 		    (curr->start_time - prev->cycle_time_extension <=
261 		     system_time)) {
262 			/* extend */
263 			index = prev->count - 1;
264 		}
265 		gate_open = prev->operation[index].properties & TSNEP_GCL_MASK;
266 		next_gate_open =
267 			curr->operation[0].properties & TSNEP_GCL_MASK;
268 	} else {
269 		/* GCL is waiting for start */
270 		next_time = curr->start_time;
271 		gate_open = 0xFF;
272 		next_gate_open = curr->operation[0].properties & TSNEP_GCL_MASK;
273 	}
274 
275 	if (gc_time != (next_time & 0xFFFFFFFF)) {
276 		dev_err(&adapter->pdev->dev, "gate control time 0x%x!=0x%llx\n",
277 			gc_time, next_time);
278 		return false;
279 	}
280 	if (((gc & TSNEP_GC_OPEN) >> TSNEP_GC_OPEN_SHIFT) != gate_open) {
281 		dev_err(&adapter->pdev->dev,
282 			"gate control open 0x%02x!=0x%02x\n",
283 			((gc & TSNEP_GC_OPEN) >> TSNEP_GC_OPEN_SHIFT),
284 			gate_open);
285 		return false;
286 	}
287 	if (((gc & TSNEP_GC_NEXT_OPEN) >> TSNEP_GC_NEXT_OPEN_SHIFT) !=
288 	    next_gate_open) {
289 		dev_err(&adapter->pdev->dev,
290 			"gate control next open 0x%02x!=0x%02x\n",
291 			((gc & TSNEP_GC_NEXT_OPEN) >> TSNEP_GC_NEXT_OPEN_SHIFT),
292 			next_gate_open);
293 		return false;
294 	}
295 
296 	return true;
297 }
298 
check_gate_duration(struct tsnep_adapter * adapter,s64 ms)299 static bool check_gate_duration(struct tsnep_adapter *adapter, s64 ms)
300 {
301 	ktime_t start = ktime_get();
302 
303 	do {
304 		if (!check_gate(adapter))
305 			return false;
306 	} while (ktime_ms_delta(ktime_get(), start) < ms);
307 
308 	return true;
309 }
310 
enable_check_taprio(struct tsnep_adapter * adapter,struct tc_taprio_qopt_offload * qopt,s64 ms)311 static bool enable_check_taprio(struct tsnep_adapter *adapter,
312 				struct tc_taprio_qopt_offload *qopt, s64 ms)
313 {
314 	int retval;
315 
316 	retval = tsnep_tc_setup(adapter->netdev, TC_SETUP_QDISC_TAPRIO, qopt);
317 	if (retval)
318 		return false;
319 
320 	if (!check_gate_duration(adapter, ms))
321 		return false;
322 
323 	return true;
324 }
325 
disable_taprio(struct tsnep_adapter * adapter)326 static bool disable_taprio(struct tsnep_adapter *adapter)
327 {
328 	struct tc_taprio_qopt_offload qopt;
329 	int retval;
330 
331 	memset(&qopt, 0, sizeof(qopt));
332 	qopt.cmd = TAPRIO_CMD_DESTROY;
333 	retval = tsnep_tc_setup(adapter->netdev, TC_SETUP_QDISC_TAPRIO, &qopt);
334 	if (retval)
335 		return false;
336 
337 	return true;
338 }
339 
run_taprio(struct tsnep_adapter * adapter,struct tc_taprio_qopt_offload * qopt,s64 ms)340 static bool run_taprio(struct tsnep_adapter *adapter,
341 		       struct tc_taprio_qopt_offload *qopt, s64 ms)
342 {
343 	if (!enable_check_taprio(adapter, qopt, ms))
344 		return false;
345 
346 	if (!disable_taprio(adapter))
347 		return false;
348 
349 	return true;
350 }
351 
tsnep_test_taprio(struct tsnep_adapter * adapter)352 static bool tsnep_test_taprio(struct tsnep_adapter *adapter)
353 {
354 	struct tc_taprio_qopt_offload *qopt;
355 	int i;
356 
357 	qopt = kzalloc(struct_size(qopt, entries, 255), GFP_KERNEL);
358 	if (!qopt)
359 		return false;
360 	for (i = 0; i < 255; i++)
361 		qopt->entries[i].command = TC_TAPRIO_CMD_SET_GATES;
362 
363 	qopt->cmd = TAPRIO_CMD_REPLACE;
364 	qopt->base_time = ktime_set(0, 0);
365 	qopt->cycle_time = 1500000;
366 	qopt->cycle_time_extension = 0;
367 	qopt->entries[0].gate_mask = 0x02;
368 	qopt->entries[0].interval = 200000;
369 	qopt->entries[1].gate_mask = 0x03;
370 	qopt->entries[1].interval = 800000;
371 	qopt->entries[2].gate_mask = 0x07;
372 	qopt->entries[2].interval = 240000;
373 	qopt->entries[3].gate_mask = 0x01;
374 	qopt->entries[3].interval = 80000;
375 	qopt->entries[4].gate_mask = 0x04;
376 	qopt->entries[4].interval = 70000;
377 	qopt->entries[5].gate_mask = 0x06;
378 	qopt->entries[5].interval = 60000;
379 	qopt->entries[6].gate_mask = 0x0F;
380 	qopt->entries[6].interval = 50000;
381 	qopt->num_entries = 7;
382 	if (!run_taprio(adapter, qopt, 100))
383 		goto failed;
384 
385 	qopt->cmd = TAPRIO_CMD_REPLACE;
386 	qopt->base_time = ktime_set(0, 0);
387 	qopt->cycle_time = 411854;
388 	qopt->cycle_time_extension = 0;
389 	qopt->entries[0].gate_mask = 0x17;
390 	qopt->entries[0].interval = 23842;
391 	qopt->entries[1].gate_mask = 0x16;
392 	qopt->entries[1].interval = 13482;
393 	qopt->entries[2].gate_mask = 0x15;
394 	qopt->entries[2].interval = 49428;
395 	qopt->entries[3].gate_mask = 0x14;
396 	qopt->entries[3].interval = 38189;
397 	qopt->entries[4].gate_mask = 0x13;
398 	qopt->entries[4].interval = 92321;
399 	qopt->entries[5].gate_mask = 0x12;
400 	qopt->entries[5].interval = 71239;
401 	qopt->entries[6].gate_mask = 0x11;
402 	qopt->entries[6].interval = 69932;
403 	qopt->entries[7].gate_mask = 0x10;
404 	qopt->entries[7].interval = 53421;
405 	qopt->num_entries = 8;
406 	if (!run_taprio(adapter, qopt, 100))
407 		goto failed;
408 
409 	qopt->cmd = TAPRIO_CMD_REPLACE;
410 	qopt->base_time = ktime_set(0, 0);
411 	delay_base_time(adapter, qopt, 12);
412 	qopt->cycle_time = 125000;
413 	qopt->cycle_time_extension = 0;
414 	qopt->entries[0].gate_mask = 0x27;
415 	qopt->entries[0].interval = 15000;
416 	qopt->entries[1].gate_mask = 0x26;
417 	qopt->entries[1].interval = 15000;
418 	qopt->entries[2].gate_mask = 0x25;
419 	qopt->entries[2].interval = 12500;
420 	qopt->entries[3].gate_mask = 0x24;
421 	qopt->entries[3].interval = 17500;
422 	qopt->entries[4].gate_mask = 0x23;
423 	qopt->entries[4].interval = 10000;
424 	qopt->entries[5].gate_mask = 0x22;
425 	qopt->entries[5].interval = 11000;
426 	qopt->entries[6].gate_mask = 0x21;
427 	qopt->entries[6].interval = 9000;
428 	qopt->entries[7].gate_mask = 0x20;
429 	qopt->entries[7].interval = 10000;
430 	qopt->entries[8].gate_mask = 0x20;
431 	qopt->entries[8].interval = 12500;
432 	qopt->entries[9].gate_mask = 0x20;
433 	qopt->entries[9].interval = 12500;
434 	qopt->num_entries = 10;
435 	if (!run_taprio(adapter, qopt, 100))
436 		goto failed;
437 
438 	kfree(qopt);
439 
440 	return true;
441 
442 failed:
443 	disable_taprio(adapter);
444 	kfree(qopt);
445 
446 	return false;
447 }
448 
tsnep_test_taprio_change(struct tsnep_adapter * adapter)449 static bool tsnep_test_taprio_change(struct tsnep_adapter *adapter)
450 {
451 	struct tc_taprio_qopt_offload *qopt;
452 	int i;
453 
454 	qopt = kzalloc(struct_size(qopt, entries, 255), GFP_KERNEL);
455 	if (!qopt)
456 		return false;
457 	for (i = 0; i < 255; i++)
458 		qopt->entries[i].command = TC_TAPRIO_CMD_SET_GATES;
459 
460 	qopt->cmd = TAPRIO_CMD_REPLACE;
461 	qopt->base_time = ktime_set(0, 0);
462 	qopt->cycle_time = 100000;
463 	qopt->cycle_time_extension = 0;
464 	qopt->entries[0].gate_mask = 0x30;
465 	qopt->entries[0].interval = 20000;
466 	qopt->entries[1].gate_mask = 0x31;
467 	qopt->entries[1].interval = 80000;
468 	qopt->num_entries = 2;
469 	if (!enable_check_taprio(adapter, qopt, 100))
470 		goto failed;
471 
472 	/* change to identical */
473 	if (!enable_check_taprio(adapter, qopt, 100))
474 		goto failed;
475 	delay_base_time(adapter, qopt, 17);
476 	if (!enable_check_taprio(adapter, qopt, 100))
477 		goto failed;
478 
479 	/* change to same cycle time */
480 	qopt->base_time = ktime_set(0, 0);
481 	qopt->entries[0].gate_mask = 0x42;
482 	qopt->entries[1].gate_mask = 0x43;
483 	delay_base_time(adapter, qopt, 2);
484 	if (!enable_check_taprio(adapter, qopt, 100))
485 		goto failed;
486 	qopt->base_time = ktime_set(0, 0);
487 	qopt->entries[0].gate_mask = 0x54;
488 	qopt->entries[0].interval = 33333;
489 	qopt->entries[1].gate_mask = 0x55;
490 	qopt->entries[1].interval = 66667;
491 	delay_base_time(adapter, qopt, 23);
492 	if (!enable_check_taprio(adapter, qopt, 100))
493 		goto failed;
494 	qopt->base_time = ktime_set(0, 0);
495 	qopt->entries[0].gate_mask = 0x66;
496 	qopt->entries[0].interval = 50000;
497 	qopt->entries[1].gate_mask = 0x67;
498 	qopt->entries[1].interval = 25000;
499 	qopt->entries[2].gate_mask = 0x68;
500 	qopt->entries[2].interval = 25000;
501 	qopt->num_entries = 3;
502 	delay_base_time(adapter, qopt, 11);
503 	if (!enable_check_taprio(adapter, qopt, 100))
504 		goto failed;
505 
506 	/* change to multiple of cycle time */
507 	qopt->base_time = ktime_set(0, 0);
508 	qopt->cycle_time = 200000;
509 	qopt->entries[0].gate_mask = 0x79;
510 	qopt->entries[0].interval = 50000;
511 	qopt->entries[1].gate_mask = 0x7A;
512 	qopt->entries[1].interval = 150000;
513 	qopt->num_entries = 2;
514 	delay_base_time(adapter, qopt, 11);
515 	if (!enable_check_taprio(adapter, qopt, 100))
516 		goto failed;
517 	qopt->base_time = ktime_set(0, 0);
518 	qopt->cycle_time = 1000000;
519 	qopt->entries[0].gate_mask = 0x7B;
520 	qopt->entries[0].interval = 125000;
521 	qopt->entries[1].gate_mask = 0x7C;
522 	qopt->entries[1].interval = 250000;
523 	qopt->entries[2].gate_mask = 0x7D;
524 	qopt->entries[2].interval = 375000;
525 	qopt->entries[3].gate_mask = 0x7E;
526 	qopt->entries[3].interval = 250000;
527 	qopt->num_entries = 4;
528 	delay_base_time(adapter, qopt, 3);
529 	if (!enable_check_taprio(adapter, qopt, 100))
530 		goto failed;
531 
532 	/* change to shorter cycle time */
533 	qopt->base_time = ktime_set(0, 0);
534 	qopt->cycle_time = 333333;
535 	qopt->entries[0].gate_mask = 0x8F;
536 	qopt->entries[0].interval = 166666;
537 	qopt->entries[1].gate_mask = 0x80;
538 	qopt->entries[1].interval = 166667;
539 	qopt->num_entries = 2;
540 	delay_base_time(adapter, qopt, 11);
541 	if (!enable_check_taprio(adapter, qopt, 100))
542 		goto failed;
543 	qopt->base_time = ktime_set(0, 0);
544 	qopt->cycle_time = 62500;
545 	qopt->entries[0].gate_mask = 0x81;
546 	qopt->entries[0].interval = 31250;
547 	qopt->entries[1].gate_mask = 0x82;
548 	qopt->entries[1].interval = 15625;
549 	qopt->entries[2].gate_mask = 0x83;
550 	qopt->entries[2].interval = 15625;
551 	qopt->num_entries = 3;
552 	delay_base_time(adapter, qopt, 1);
553 	if (!enable_check_taprio(adapter, qopt, 100))
554 		goto failed;
555 
556 	/* change to longer cycle time */
557 	qopt->base_time = ktime_set(0, 0);
558 	qopt->cycle_time = 400000;
559 	qopt->entries[0].gate_mask = 0x84;
560 	qopt->entries[0].interval = 100000;
561 	qopt->entries[1].gate_mask = 0x85;
562 	qopt->entries[1].interval = 100000;
563 	qopt->entries[2].gate_mask = 0x86;
564 	qopt->entries[2].interval = 100000;
565 	qopt->entries[3].gate_mask = 0x87;
566 	qopt->entries[3].interval = 100000;
567 	qopt->num_entries = 4;
568 	delay_base_time(adapter, qopt, 7);
569 	if (!enable_check_taprio(adapter, qopt, 100))
570 		goto failed;
571 	qopt->base_time = ktime_set(0, 0);
572 	qopt->cycle_time = 1700000;
573 	qopt->entries[0].gate_mask = 0x88;
574 	qopt->entries[0].interval = 200000;
575 	qopt->entries[1].gate_mask = 0x89;
576 	qopt->entries[1].interval = 300000;
577 	qopt->entries[2].gate_mask = 0x8A;
578 	qopt->entries[2].interval = 600000;
579 	qopt->entries[3].gate_mask = 0x8B;
580 	qopt->entries[3].interval = 100000;
581 	qopt->entries[4].gate_mask = 0x8C;
582 	qopt->entries[4].interval = 500000;
583 	qopt->num_entries = 5;
584 	delay_base_time(adapter, qopt, 6);
585 	if (!enable_check_taprio(adapter, qopt, 100))
586 		goto failed;
587 
588 	if (!disable_taprio(adapter))
589 		goto failed;
590 
591 	kfree(qopt);
592 
593 	return true;
594 
595 failed:
596 	disable_taprio(adapter);
597 	kfree(qopt);
598 
599 	return false;
600 }
601 
tsnep_test_taprio_extension(struct tsnep_adapter * adapter)602 static bool tsnep_test_taprio_extension(struct tsnep_adapter *adapter)
603 {
604 	struct tc_taprio_qopt_offload *qopt;
605 	int i;
606 
607 	qopt = kzalloc(struct_size(qopt, entries, 255), GFP_KERNEL);
608 	if (!qopt)
609 		return false;
610 	for (i = 0; i < 255; i++)
611 		qopt->entries[i].command = TC_TAPRIO_CMD_SET_GATES;
612 
613 	qopt->cmd = TAPRIO_CMD_REPLACE;
614 	qopt->base_time = ktime_set(0, 0);
615 	qopt->cycle_time = 100000;
616 	qopt->cycle_time_extension = 50000;
617 	qopt->entries[0].gate_mask = 0x90;
618 	qopt->entries[0].interval = 20000;
619 	qopt->entries[1].gate_mask = 0x91;
620 	qopt->entries[1].interval = 80000;
621 	qopt->num_entries = 2;
622 	if (!enable_check_taprio(adapter, qopt, 100))
623 		goto failed;
624 
625 	/* change to different phase */
626 	qopt->base_time = ktime_set(0, 50000);
627 	qopt->entries[0].gate_mask = 0x92;
628 	qopt->entries[0].interval = 33000;
629 	qopt->entries[1].gate_mask = 0x93;
630 	qopt->entries[1].interval = 67000;
631 	qopt->num_entries = 2;
632 	delay_base_time(adapter, qopt, 2);
633 	if (!enable_check_taprio(adapter, qopt, 100))
634 		goto failed;
635 
636 	/* change to different phase and longer cycle time */
637 	qopt->base_time = ktime_set(0, 0);
638 	qopt->cycle_time = 1000000;
639 	qopt->cycle_time_extension = 700000;
640 	qopt->entries[0].gate_mask = 0x94;
641 	qopt->entries[0].interval = 400000;
642 	qopt->entries[1].gate_mask = 0x95;
643 	qopt->entries[1].interval = 600000;
644 	qopt->num_entries = 2;
645 	delay_base_time(adapter, qopt, 7);
646 	if (!enable_check_taprio(adapter, qopt, 100))
647 		goto failed;
648 	qopt->base_time = ktime_set(0, 700000);
649 	qopt->cycle_time = 2000000;
650 	qopt->cycle_time_extension = 1900000;
651 	qopt->entries[0].gate_mask = 0x96;
652 	qopt->entries[0].interval = 400000;
653 	qopt->entries[1].gate_mask = 0x97;
654 	qopt->entries[1].interval = 1600000;
655 	qopt->num_entries = 2;
656 	delay_base_time(adapter, qopt, 9);
657 	if (!enable_check_taprio(adapter, qopt, 100))
658 		goto failed;
659 
660 	/* change to different phase and shorter cycle time */
661 	qopt->base_time = ktime_set(0, 0);
662 	qopt->cycle_time = 1500000;
663 	qopt->cycle_time_extension = 700000;
664 	qopt->entries[0].gate_mask = 0x98;
665 	qopt->entries[0].interval = 400000;
666 	qopt->entries[1].gate_mask = 0x99;
667 	qopt->entries[1].interval = 600000;
668 	qopt->entries[2].gate_mask = 0x9A;
669 	qopt->entries[2].interval = 500000;
670 	qopt->num_entries = 3;
671 	delay_base_time(adapter, qopt, 3);
672 	if (!enable_check_taprio(adapter, qopt, 100))
673 		goto failed;
674 	qopt->base_time = ktime_set(0, 100000);
675 	qopt->cycle_time = 500000;
676 	qopt->cycle_time_extension = 300000;
677 	qopt->entries[0].gate_mask = 0x9B;
678 	qopt->entries[0].interval = 150000;
679 	qopt->entries[1].gate_mask = 0x9C;
680 	qopt->entries[1].interval = 350000;
681 	qopt->num_entries = 2;
682 	delay_base_time(adapter, qopt, 9);
683 	if (!enable_check_taprio(adapter, qopt, 100))
684 		goto failed;
685 
686 	/* change to different cycle time */
687 	qopt->base_time = ktime_set(0, 0);
688 	qopt->cycle_time = 1000000;
689 	qopt->cycle_time_extension = 700000;
690 	qopt->entries[0].gate_mask = 0xAD;
691 	qopt->entries[0].interval = 400000;
692 	qopt->entries[1].gate_mask = 0xAE;
693 	qopt->entries[1].interval = 300000;
694 	qopt->entries[2].gate_mask = 0xAF;
695 	qopt->entries[2].interval = 300000;
696 	qopt->num_entries = 3;
697 	if (!enable_check_taprio(adapter, qopt, 100))
698 		goto failed;
699 	qopt->base_time = ktime_set(0, 0);
700 	qopt->cycle_time = 400000;
701 	qopt->cycle_time_extension = 100000;
702 	qopt->entries[0].gate_mask = 0xA0;
703 	qopt->entries[0].interval = 200000;
704 	qopt->entries[1].gate_mask = 0xA1;
705 	qopt->entries[1].interval = 200000;
706 	qopt->num_entries = 2;
707 	delay_base_time(adapter, qopt, 19);
708 	if (!enable_check_taprio(adapter, qopt, 100))
709 		goto failed;
710 	qopt->base_time = ktime_set(0, 0);
711 	qopt->cycle_time = 500000;
712 	qopt->cycle_time_extension = 499999;
713 	qopt->entries[0].gate_mask = 0xB2;
714 	qopt->entries[0].interval = 100000;
715 	qopt->entries[1].gate_mask = 0xB3;
716 	qopt->entries[1].interval = 100000;
717 	qopt->entries[2].gate_mask = 0xB4;
718 	qopt->entries[2].interval = 100000;
719 	qopt->entries[3].gate_mask = 0xB5;
720 	qopt->entries[3].interval = 200000;
721 	qopt->num_entries = 4;
722 	delay_base_time(adapter, qopt, 19);
723 	if (!enable_check_taprio(adapter, qopt, 100))
724 		goto failed;
725 	qopt->base_time = ktime_set(0, 0);
726 	qopt->cycle_time = 6000000;
727 	qopt->cycle_time_extension = 5999999;
728 	qopt->entries[0].gate_mask = 0xC6;
729 	qopt->entries[0].interval = 1000000;
730 	qopt->entries[1].gate_mask = 0xC7;
731 	qopt->entries[1].interval = 1000000;
732 	qopt->entries[2].gate_mask = 0xC8;
733 	qopt->entries[2].interval = 1000000;
734 	qopt->entries[3].gate_mask = 0xC9;
735 	qopt->entries[3].interval = 1500000;
736 	qopt->entries[4].gate_mask = 0xCA;
737 	qopt->entries[4].interval = 1500000;
738 	qopt->num_entries = 5;
739 	delay_base_time(adapter, qopt, 1);
740 	if (!enable_check_taprio(adapter, qopt, 100))
741 		goto failed;
742 
743 	if (!disable_taprio(adapter))
744 		goto failed;
745 
746 	kfree(qopt);
747 
748 	return true;
749 
750 failed:
751 	disable_taprio(adapter);
752 	kfree(qopt);
753 
754 	return false;
755 }
756 
tsnep_ethtool_get_test_count(void)757 int tsnep_ethtool_get_test_count(void)
758 {
759 	return TSNEP_TEST_COUNT;
760 }
761 
tsnep_ethtool_get_test_strings(u8 * data)762 void tsnep_ethtool_get_test_strings(u8 *data)
763 {
764 	memcpy(data, tsnep_test_strings, sizeof(tsnep_test_strings));
765 }
766 
tsnep_ethtool_self_test(struct net_device * netdev,struct ethtool_test * eth_test,u64 * data)767 void tsnep_ethtool_self_test(struct net_device *netdev,
768 			     struct ethtool_test *eth_test, u64 *data)
769 {
770 	struct tsnep_adapter *adapter = netdev_priv(netdev);
771 
772 	eth_test->len = TSNEP_TEST_COUNT;
773 
774 	if (eth_test->flags != ETH_TEST_FL_OFFLINE) {
775 		/* no tests are done online */
776 		data[TSNEP_TEST_ENABLE] = 0;
777 		data[TSNEP_TEST_TAPRIO] = 0;
778 		data[TSNEP_TEST_TAPRIO_CHANGE] = 0;
779 		data[TSNEP_TEST_TAPRIO_EXTENSION] = 0;
780 
781 		return;
782 	}
783 
784 	if (tsnep_test_gc_enable(adapter)) {
785 		data[TSNEP_TEST_ENABLE] = 0;
786 	} else {
787 		eth_test->flags |= ETH_TEST_FL_FAILED;
788 		data[TSNEP_TEST_ENABLE] = 1;
789 	}
790 
791 	if (tsnep_test_taprio(adapter)) {
792 		data[TSNEP_TEST_TAPRIO] = 0;
793 	} else {
794 		eth_test->flags |= ETH_TEST_FL_FAILED;
795 		data[TSNEP_TEST_TAPRIO] = 1;
796 	}
797 
798 	if (tsnep_test_taprio_change(adapter)) {
799 		data[TSNEP_TEST_TAPRIO_CHANGE] = 0;
800 	} else {
801 		eth_test->flags |= ETH_TEST_FL_FAILED;
802 		data[TSNEP_TEST_TAPRIO_CHANGE] = 1;
803 	}
804 
805 	if (tsnep_test_taprio_extension(adapter)) {
806 		data[TSNEP_TEST_TAPRIO_EXTENSION] = 0;
807 	} else {
808 		eth_test->flags |= ETH_TEST_FL_FAILED;
809 		data[TSNEP_TEST_TAPRIO_EXTENSION] = 1;
810 	}
811 }
812