summaryrefslogtreecommitdiffstats
path: root/drivers/net/ethernet/engleder/tsnep_tc.c
blob: c4c6e1357317936182dc7f77ba39c1f4c001b441 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
// SPDX-License-Identifier: GPL-2.0
/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */

#include "tsnep.h"

#include <net/pkt_sched.h>

/* save one operation at the end for additional operation at list change */
#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)

static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
{
	int i;
	u64 cycle_time;

	if (!qopt->cycle_time)
		return -ERANGE;
	if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
		return -EINVAL;
	cycle_time = 0;
	for (i = 0; i < qopt->num_entries; i++) {
		if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
			return -EINVAL;
		if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
			return -EINVAL;
		if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
			return -EINVAL;
		cycle_time += qopt->entries[i].interval;
	}
	if (qopt->cycle_time != cycle_time)
		return -EINVAL;
	if (qopt->cycle_time_extension >= qopt->cycle_time)
		return -EINVAL;

	return 0;
}

static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
				      u32 properties, u32 interval, bool flush)
{
	void __iomem *addr = gcl->addr +
			     sizeof(struct tsnep_gcl_operation) * index;

	gcl->operation[index].properties = properties;
	gcl->operation[index].interval = interval;

	iowrite32(properties, addr);
	iowrite32(interval, addr + sizeof(u32));

	if (flush) {
		/* flush write with read access */
		ioread32(addr);
	}
}

static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
{
	u64 duration;
	int count;

	/* change needs to be triggered one or two operations before start of
	 * new gate control list
	 * - change is triggered at start of operation (minimum one operation)
	 * - operation with adjusted interval is inserted on demand to exactly
	 *   meet the start of the new gate control list (optional)
	 *
	 * additionally properties are read directly after start of previous
	 * operation
	 *
	 * therefore, three operations needs to be considered for the limit
	 */
	duration = 0;
	count = 3;
	while (count) {
		duration += gcl->operation[index].interval;

		index--;
		if (index < 0)
			index = gcl->count - 1;

		count--;
	}

	return duration;
}

static void tsnep_write_gcl(struct tsnep_gcl *gcl,
			    struct tc_taprio_qopt_offload *qopt)
{
	int i;
	u32 properties;
	u64 extend;
	u64 cut;

	gcl->base_time = ktime_to_ns(qopt->base_time);
	gcl->cycle_time = qopt->cycle_time;
	gcl->cycle_time_extension = qopt->cycle_time_extension;

	for (i = 0; i < qopt->num_entries; i++) {
		properties = qopt->entries[i].gate_mask;
		if (i == (qopt->num_entries - 1))
			properties |= TSNEP_GCL_LAST;

		tsnep_write_gcl_operation(gcl, i, properties,
					  qopt->entries[i].interval, true);
	}
	gcl->count = qopt->num_entries;

	/* calculate change limit; i.e., the time needed between enable and
	 * start of new gate control list
	 */

	/* case 1: extend cycle time for change
	 * - change duration of last operation
	 * - cycle time extension
	 */
	extend = tsnep_change_duration(gcl, gcl->count - 1);
	extend += gcl->cycle_time_extension;

	/* case 2: cut cycle time for change
	 * - maximum change duration
	 */
	cut = 0;
	for (i = 0; i < gcl->count; i++)
		cut = max(cut, tsnep_change_duration(gcl, i));

	/* use maximum, because the actual case (extend or cut) can be
	 * determined only after limit is known (chicken-and-egg problem)
	 */
	gcl->change_limit = max(extend, cut);
}

static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
{
	u64 start = gcl->base_time;
	u64 n;

	if (start <= limit) {
		n = div64_u64(limit - start, gcl->cycle_time);
		start += (n + 1) * gcl->cycle_time;
	}

	return start;
}

static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
{
	u64 start = gcl->base_time;
	u64 n;

	n = div64_u64(limit - start, gcl->cycle_time);
	start += n * gcl->cycle_time;
	if (start == limit)
		start -= gcl->cycle_time;

	return start;
}

static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
				bool insert)
{
	/* previous operation triggers change and properties are evaluated at
	 * start of operation
	 */
	if (index == 0)
		index = gcl->count - 1;
	else
		index = index - 1;
	change -= gcl->operation[index].interval;

	/* optionally change to new list with additional operation in between */
	if (insert) {
		void __iomem *addr = gcl->addr +
				     sizeof(struct tsnep_gcl_operation) * index;

		gcl->operation[index].properties |= TSNEP_GCL_INSERT;
		iowrite32(gcl->operation[index].properties, addr);
	}

	return change;
}

static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
{
	int i;
	u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
	void __iomem *addr;

	/* search for insert operation and reset properties */
	for (i = 0; i < gcl->count; i++) {
		if (gcl->operation[i].properties & ~mask) {
			addr = gcl->addr +
			       sizeof(struct tsnep_gcl_operation) * i;

			gcl->operation[i].properties &= mask;
			iowrite32(gcl->operation[i].properties, addr);

			break;
		}
	}
}

static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
				      u64 change, u32 interval)
{
	u32 properties;

	properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
	/* change to new list directly after inserted operation */
	properties |= TSNEP_GCL_CHANGE;

	/* last operation of list is reserved to insert operation */
	tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
				  interval, false);

	return tsnep_set_gcl_change(gcl, ref, change, true);
}

static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
{
	int ref = gcl->count - 1;
	u32 interval = gcl->operation[ref].interval + extension;

	start -= gcl->operation[ref].interval;

	return tsnep_insert_gcl_operation(gcl, ref, start, interval);
}

static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
{
	u64 sum = 0;
	int i;

	/* find operation which shall be cutted */
	for (i = 0; i < gcl->count; i++) {
		u64 sum_tmp = sum + gcl->operation[i].interval;
		u64 interval;

		/* sum up operations as long as cycle time is not exceeded */
		if (sum_tmp > cycle_time)
			break;

		/* remaining interval must be big enough for hardware */
		interval = cycle_time - sum_tmp;
		if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
			break;

		sum = sum_tmp;
	}
	if (sum == cycle_time) {
		/* no need to cut operation itself or whole cycle
		 * => change exactly at operation
		 */
		return tsnep_set_gcl_change(gcl, i, start + sum, false);
	}
	return tsnep_insert_gcl_operation(gcl, i, start + sum,
					  cycle_time - sum);
}

static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
			    struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
{
	u64 system_time;
	u64 timeout;
	u64 limit;

	/* estimate timeout limit after timeout enable, actually timeout limit
	 * in hardware will be earlier than estimate so we are on the safe side
	 */
	tsnep_get_system_time(adapter, &system_time);
	timeout = system_time + TSNEP_GC_TIMEOUT;

	if (curr)
		limit = timeout + curr->change_limit;
	else
		limit = timeout;

	gcl->start_time = tsnep_gcl_start_after(gcl, limit);

	/* gate control time register is only 32bit => time shall be in the near
	 * future (no driver support for far future implemented)
	 */
	if ((gcl->start_time - system_time) >= U32_MAX)
		return -EAGAIN;

	if (curr) {
		/* change gate control list */
		u64 last;
		u64 change;

		last = tsnep_gcl_start_before(curr, gcl->start_time);
		if ((last + curr->cycle_time) == gcl->start_time)
			change = tsnep_cut_gcl(curr, last,
					       gcl->start_time - last);
		else if (((gcl->start_time - last) <=
			  curr->cycle_time_extension) ||
			 ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
			change = tsnep_extend_gcl(curr, last,
						  gcl->start_time - last);
		else
			change = tsnep_cut_gcl(curr, last,
					       gcl->start_time - last);

		WARN_ON(change <= timeout);
		gcl->change = true;
		iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
	} else {
		/* start gate control list */
		WARN_ON(gcl->start_time <= timeout);
		gcl->change = false;
		iowrite32(gcl->start_time & 0xFFFFFFFF,
			  adapter->addr + TSNEP_GC_TIME);
	}

	return 0;
}

static int tsnep_taprio(struct tsnep_adapter *adapter,
			struct tc_taprio_qopt_offload *qopt)
{
	struct tsnep_gcl *gcl;
	struct tsnep_gcl *curr;
	int retval;

	if (!adapter->gate_control)
		return -EOPNOTSUPP;

	if (!qopt->enable) {
		/* disable gate control if active */
		mutex_lock(&adapter->gate_control_lock);

		if (adapter->gate_control_active) {
			iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
			adapter->gate_control_active = false;
		}

		mutex_unlock(&adapter->gate_control_lock);

		return 0;
	}

	retval = tsnep_validate_gcl(qopt);
	if (retval)
		return retval;

	mutex_lock(&adapter->gate_control_lock);

	gcl = &adapter->gcl[adapter->next_gcl];
	tsnep_write_gcl(gcl, qopt);

	/* select current gate control list if active */
	if (adapter->gate_control_active) {
		if (adapter->next_gcl == 0)
			curr = &adapter->gcl[1];
		else
			curr = &adapter->gcl[0];
	} else {
		curr = NULL;
	}

	for (;;) {
		/* start timeout which discards late enable, this helps ensuring
		 * that start/change time are in the future at enable
		 */
		iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);

		retval = tsnep_enable_gcl(adapter, gcl, curr);
		if (retval) {
			mutex_unlock(&adapter->gate_control_lock);

			return retval;
		}

		/* enable gate control list */
		if (adapter->next_gcl == 0)
			iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
		else
			iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);

		/* done if timeout did not happen */
		if (!(ioread32(adapter->addr + TSNEP_GC) &
		      TSNEP_GC_TIMEOUT_SIGNAL))
			break;

		/* timeout is acknowledged with any enable */
		iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);

		if (curr)
			tsnep_clean_gcl(curr);

		/* retry because of timeout */
	}

	adapter->gate_control_active = true;

	if (adapter->next_gcl == 0)
		adapter->next_gcl = 1;
	else
		adapter->next_gcl = 0;

	mutex_unlock(&adapter->gate_control_lock);

	return 0;
}

int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
		   void *type_data)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);

	switch (type) {
	case TC_SETUP_QDISC_TAPRIO:
		return tsnep_taprio(adapter, type_data);
	default:
		return -EOPNOTSUPP;
	}
}

int tsnep_tc_init(struct tsnep_adapter *adapter)
{
	if (!adapter->gate_control)
		return 0;

	/* open all gates */
	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
	iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);

	adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
	adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;

	return 0;
}

void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
{
	if (!adapter->gate_control)
		return;

	if (adapter->gate_control_active) {
		iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
		adapter->gate_control_active = false;
	}
}