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 | /* save one operation at the end for additional operation at list change */ |
9 | #define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1) |
10 | |
11 | static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt) |
12 | { |
13 | int i; |
14 | u64 cycle_time; |
15 | |
16 | if (!qopt->cycle_time) |
17 | return -ERANGE; |
18 | if (qopt->num_entries > TSNEP_MAX_GCL_NUM) |
19 | return -EINVAL; |
20 | cycle_time = 0; |
21 | for (i = 0; i < qopt->num_entries; i++) { |
22 | if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES) |
23 | return -EINVAL; |
24 | if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK) |
25 | return -EINVAL; |
26 | if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL) |
27 | return -EINVAL; |
28 | cycle_time += qopt->entries[i].interval; |
29 | } |
30 | if (qopt->cycle_time != cycle_time) |
31 | return -EINVAL; |
32 | if (qopt->cycle_time_extension >= qopt->cycle_time) |
33 | return -EINVAL; |
34 | |
35 | return 0; |
36 | } |
37 | |
38 | static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index, |
39 | u32 properties, u32 interval, bool flush) |
40 | { |
41 | void __iomem *addr = gcl->addr + |
42 | sizeof(struct tsnep_gcl_operation) * index; |
43 | |
44 | gcl->operation[index].properties = properties; |
45 | gcl->operation[index].interval = interval; |
46 | |
47 | iowrite32(properties, addr); |
48 | iowrite32(interval, addr + sizeof(u32)); |
49 | |
50 | if (flush) { |
51 | /* flush write with read access */ |
52 | ioread32(addr); |
53 | } |
54 | } |
55 | |
56 | static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index) |
57 | { |
58 | u64 duration; |
59 | int count; |
60 | |
61 | /* change needs to be triggered one or two operations before start of |
62 | * new gate control list |
63 | * - change is triggered at start of operation (minimum one operation) |
64 | * - operation with adjusted interval is inserted on demand to exactly |
65 | * meet the start of the new gate control list (optional) |
66 | * |
67 | * additionally properties are read directly after start of previous |
68 | * operation |
69 | * |
70 | * therefore, three operations needs to be considered for the limit |
71 | */ |
72 | duration = 0; |
73 | count = 3; |
74 | while (count) { |
75 | duration += gcl->operation[index].interval; |
76 | |
77 | index--; |
78 | if (index < 0) |
79 | index = gcl->count - 1; |
80 | |
81 | count--; |
82 | } |
83 | |
84 | return duration; |
85 | } |
86 | |
87 | static void tsnep_write_gcl(struct tsnep_gcl *gcl, |
88 | struct tc_taprio_qopt_offload *qopt) |
89 | { |
90 | int i; |
91 | u32 properties; |
92 | u64 extend; |
93 | u64 cut; |
94 | |
95 | gcl->base_time = ktime_to_ns(kt: qopt->base_time); |
96 | gcl->cycle_time = qopt->cycle_time; |
97 | gcl->cycle_time_extension = qopt->cycle_time_extension; |
98 | |
99 | for (i = 0; i < qopt->num_entries; i++) { |
100 | properties = qopt->entries[i].gate_mask; |
101 | if (i == (qopt->num_entries - 1)) |
102 | properties |= TSNEP_GCL_LAST; |
103 | |
104 | tsnep_write_gcl_operation(gcl, index: i, properties, |
105 | interval: qopt->entries[i].interval, flush: true); |
106 | } |
107 | gcl->count = qopt->num_entries; |
108 | |
109 | /* calculate change limit; i.e., the time needed between enable and |
110 | * start of new gate control list |
111 | */ |
112 | |
113 | /* case 1: extend cycle time for change |
114 | * - change duration of last operation |
115 | * - cycle time extension |
116 | */ |
117 | extend = tsnep_change_duration(gcl, index: gcl->count - 1); |
118 | extend += gcl->cycle_time_extension; |
119 | |
120 | /* case 2: cut cycle time for change |
121 | * - maximum change duration |
122 | */ |
123 | cut = 0; |
124 | for (i = 0; i < gcl->count; i++) |
125 | cut = max(cut, tsnep_change_duration(gcl, i)); |
126 | |
127 | /* use maximum, because the actual case (extend or cut) can be |
128 | * determined only after limit is known (chicken-and-egg problem) |
129 | */ |
130 | gcl->change_limit = max(extend, cut); |
131 | } |
132 | |
133 | static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit) |
134 | { |
135 | u64 start = gcl->base_time; |
136 | u64 n; |
137 | |
138 | if (start <= limit) { |
139 | n = div64_u64(dividend: limit - start, divisor: gcl->cycle_time); |
140 | start += (n + 1) * gcl->cycle_time; |
141 | } |
142 | |
143 | return start; |
144 | } |
145 | |
146 | static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit) |
147 | { |
148 | u64 start = gcl->base_time; |
149 | u64 n; |
150 | |
151 | n = div64_u64(dividend: limit - start, divisor: gcl->cycle_time); |
152 | start += n * gcl->cycle_time; |
153 | if (start == limit) |
154 | start -= gcl->cycle_time; |
155 | |
156 | return start; |
157 | } |
158 | |
159 | static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change, |
160 | bool insert) |
161 | { |
162 | /* previous operation triggers change and properties are evaluated at |
163 | * start of operation |
164 | */ |
165 | if (index == 0) |
166 | index = gcl->count - 1; |
167 | else |
168 | index = index - 1; |
169 | change -= gcl->operation[index].interval; |
170 | |
171 | /* optionally change to new list with additional operation in between */ |
172 | if (insert) { |
173 | void __iomem *addr = gcl->addr + |
174 | sizeof(struct tsnep_gcl_operation) * index; |
175 | |
176 | gcl->operation[index].properties |= TSNEP_GCL_INSERT; |
177 | iowrite32(gcl->operation[index].properties, addr); |
178 | } |
179 | |
180 | return change; |
181 | } |
182 | |
183 | static void tsnep_clean_gcl(struct tsnep_gcl *gcl) |
184 | { |
185 | int i; |
186 | u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK; |
187 | void __iomem *addr; |
188 | |
189 | /* search for insert operation and reset properties */ |
190 | for (i = 0; i < gcl->count; i++) { |
191 | if (gcl->operation[i].properties & ~mask) { |
192 | addr = gcl->addr + |
193 | sizeof(struct tsnep_gcl_operation) * i; |
194 | |
195 | gcl->operation[i].properties &= mask; |
196 | iowrite32(gcl->operation[i].properties, addr); |
197 | |
198 | break; |
199 | } |
200 | } |
201 | } |
202 | |
203 | static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref, |
204 | u64 change, u32 interval) |
205 | { |
206 | u32 properties; |
207 | |
208 | properties = gcl->operation[ref].properties & TSNEP_GCL_MASK; |
209 | /* change to new list directly after inserted operation */ |
210 | properties |= TSNEP_GCL_CHANGE; |
211 | |
212 | /* last operation of list is reserved to insert operation */ |
213 | tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties, |
214 | interval, flush: false); |
215 | |
216 | return tsnep_set_gcl_change(gcl, index: ref, change, insert: true); |
217 | } |
218 | |
219 | static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension) |
220 | { |
221 | int ref = gcl->count - 1; |
222 | u32 interval = gcl->operation[ref].interval + extension; |
223 | |
224 | start -= gcl->operation[ref].interval; |
225 | |
226 | return tsnep_insert_gcl_operation(gcl, ref, change: start, interval); |
227 | } |
228 | |
229 | static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time) |
230 | { |
231 | u64 sum = 0; |
232 | int i; |
233 | |
234 | /* find operation which shall be cutted */ |
235 | for (i = 0; i < gcl->count; i++) { |
236 | u64 sum_tmp = sum + gcl->operation[i].interval; |
237 | u64 interval; |
238 | |
239 | /* sum up operations as long as cycle time is not exceeded */ |
240 | if (sum_tmp > cycle_time) |
241 | break; |
242 | |
243 | /* remaining interval must be big enough for hardware */ |
244 | interval = cycle_time - sum_tmp; |
245 | if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL) |
246 | break; |
247 | |
248 | sum = sum_tmp; |
249 | } |
250 | if (sum == cycle_time) { |
251 | /* no need to cut operation itself or whole cycle |
252 | * => change exactly at operation |
253 | */ |
254 | return tsnep_set_gcl_change(gcl, index: i, change: start + sum, insert: false); |
255 | } |
256 | return tsnep_insert_gcl_operation(gcl, ref: i, change: start + sum, |
257 | interval: cycle_time - sum); |
258 | } |
259 | |
260 | static int tsnep_enable_gcl(struct tsnep_adapter *adapter, |
261 | struct tsnep_gcl *gcl, struct tsnep_gcl *curr) |
262 | { |
263 | u64 system_time; |
264 | u64 timeout; |
265 | u64 limit; |
266 | |
267 | /* estimate timeout limit after timeout enable, actually timeout limit |
268 | * in hardware will be earlier than estimate so we are on the safe side |
269 | */ |
270 | tsnep_get_system_time(adapter, time: &system_time); |
271 | timeout = system_time + TSNEP_GC_TIMEOUT; |
272 | |
273 | if (curr) |
274 | limit = timeout + curr->change_limit; |
275 | else |
276 | limit = timeout; |
277 | |
278 | gcl->start_time = tsnep_gcl_start_after(gcl, limit); |
279 | |
280 | /* gate control time register is only 32bit => time shall be in the near |
281 | * future (no driver support for far future implemented) |
282 | */ |
283 | if ((gcl->start_time - system_time) >= U32_MAX) |
284 | return -EAGAIN; |
285 | |
286 | if (curr) { |
287 | /* change gate control list */ |
288 | u64 last; |
289 | u64 change; |
290 | |
291 | last = tsnep_gcl_start_before(gcl: curr, limit: gcl->start_time); |
292 | if ((last + curr->cycle_time) == gcl->start_time) |
293 | change = tsnep_cut_gcl(gcl: curr, start: last, |
294 | cycle_time: gcl->start_time - last); |
295 | else if (((gcl->start_time - last) <= |
296 | curr->cycle_time_extension) || |
297 | ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL)) |
298 | change = tsnep_extend_gcl(gcl: curr, start: last, |
299 | extension: gcl->start_time - last); |
300 | else |
301 | change = tsnep_cut_gcl(gcl: curr, start: last, |
302 | cycle_time: gcl->start_time - last); |
303 | |
304 | WARN_ON(change <= timeout); |
305 | gcl->change = true; |
306 | iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE); |
307 | } else { |
308 | /* start gate control list */ |
309 | WARN_ON(gcl->start_time <= timeout); |
310 | gcl->change = false; |
311 | iowrite32(gcl->start_time & 0xFFFFFFFF, |
312 | adapter->addr + TSNEP_GC_TIME); |
313 | } |
314 | |
315 | return 0; |
316 | } |
317 | |
318 | static int tsnep_taprio(struct tsnep_adapter *adapter, |
319 | struct tc_taprio_qopt_offload *qopt) |
320 | { |
321 | struct tsnep_gcl *gcl; |
322 | struct tsnep_gcl *curr; |
323 | int retval; |
324 | |
325 | if (!adapter->gate_control) |
326 | return -EOPNOTSUPP; |
327 | |
328 | if (qopt->cmd == TAPRIO_CMD_DESTROY) { |
329 | /* disable gate control if active */ |
330 | mutex_lock(&adapter->gate_control_lock); |
331 | |
332 | if (adapter->gate_control_active) { |
333 | iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC); |
334 | adapter->gate_control_active = false; |
335 | } |
336 | |
337 | mutex_unlock(lock: &adapter->gate_control_lock); |
338 | |
339 | return 0; |
340 | } else if (qopt->cmd != TAPRIO_CMD_REPLACE) { |
341 | return -EOPNOTSUPP; |
342 | } |
343 | |
344 | retval = tsnep_validate_gcl(qopt); |
345 | if (retval) |
346 | return retval; |
347 | |
348 | mutex_lock(&adapter->gate_control_lock); |
349 | |
350 | gcl = &adapter->gcl[adapter->next_gcl]; |
351 | tsnep_write_gcl(gcl, qopt); |
352 | |
353 | /* select current gate control list if active */ |
354 | if (adapter->gate_control_active) { |
355 | if (adapter->next_gcl == 0) |
356 | curr = &adapter->gcl[1]; |
357 | else |
358 | curr = &adapter->gcl[0]; |
359 | } else { |
360 | curr = NULL; |
361 | } |
362 | |
363 | for (;;) { |
364 | /* start timeout which discards late enable, this helps ensuring |
365 | * that start/change time are in the future at enable |
366 | */ |
367 | iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC); |
368 | |
369 | retval = tsnep_enable_gcl(adapter, gcl, curr); |
370 | if (retval) { |
371 | mutex_unlock(lock: &adapter->gate_control_lock); |
372 | |
373 | return retval; |
374 | } |
375 | |
376 | /* enable gate control list */ |
377 | if (adapter->next_gcl == 0) |
378 | iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC); |
379 | else |
380 | iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC); |
381 | |
382 | /* done if timeout did not happen */ |
383 | if (!(ioread32(adapter->addr + TSNEP_GC) & |
384 | TSNEP_GC_TIMEOUT_SIGNAL)) |
385 | break; |
386 | |
387 | /* timeout is acknowledged with any enable */ |
388 | iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC); |
389 | |
390 | if (curr) |
391 | tsnep_clean_gcl(gcl: curr); |
392 | |
393 | /* retry because of timeout */ |
394 | } |
395 | |
396 | adapter->gate_control_active = true; |
397 | |
398 | if (adapter->next_gcl == 0) |
399 | adapter->next_gcl = 1; |
400 | else |
401 | adapter->next_gcl = 0; |
402 | |
403 | mutex_unlock(lock: &adapter->gate_control_lock); |
404 | |
405 | return 0; |
406 | } |
407 | |
408 | static int tsnep_tc_query_caps(struct tsnep_adapter *adapter, |
409 | struct tc_query_caps_base *base) |
410 | { |
411 | switch (base->type) { |
412 | case TC_SETUP_QDISC_TAPRIO: { |
413 | struct tc_taprio_caps *caps = base->caps; |
414 | |
415 | if (!adapter->gate_control) |
416 | return -EOPNOTSUPP; |
417 | |
418 | caps->gate_mask_per_txq = true; |
419 | |
420 | return 0; |
421 | } |
422 | default: |
423 | return -EOPNOTSUPP; |
424 | } |
425 | } |
426 | |
427 | int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type, |
428 | void *type_data) |
429 | { |
430 | struct tsnep_adapter *adapter = netdev_priv(dev: netdev); |
431 | |
432 | switch (type) { |
433 | case TC_QUERY_CAPS: |
434 | return tsnep_tc_query_caps(adapter, base: type_data); |
435 | case TC_SETUP_QDISC_TAPRIO: |
436 | return tsnep_taprio(adapter, qopt: type_data); |
437 | default: |
438 | return -EOPNOTSUPP; |
439 | } |
440 | } |
441 | |
442 | int tsnep_tc_init(struct tsnep_adapter *adapter) |
443 | { |
444 | if (!adapter->gate_control) |
445 | return 0; |
446 | |
447 | /* open all gates */ |
448 | iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC); |
449 | iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC); |
450 | |
451 | adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A; |
452 | adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B; |
453 | |
454 | return 0; |
455 | } |
456 | |
457 | void tsnep_tc_cleanup(struct tsnep_adapter *adapter) |
458 | { |
459 | if (!adapter->gate_control) |
460 | return; |
461 | |
462 | if (adapter->gate_control_active) { |
463 | iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC); |
464 | adapter->gate_control_active = false; |
465 | } |
466 | } |
467 | |