1	// SPDX-License-Identifier: GPL-2.0
2
3	/* net/sched/sch_taprio.c Time Aware Priority Scheduler
4	*
5	* Authors: Vinicius Costa Gomes <vinicius.gomes@intel.com>
6	*
7	*/
8
9	#include <linux/ethtool.h>
10	#include <linux/ethtool_netlink.h>
11	#include <linux/types.h>
12	#include <linux/slab.h>
13	#include <linux/kernel.h>
14	#include <linux/string.h>
15	#include <linux/list.h>
16	#include <linux/errno.h>
17	#include <linux/skbuff.h>
18	#include <linux/math64.h>
19	#include <linux/module.h>
20	#include <linux/spinlock.h>
21	#include <linux/rcupdate.h>
22	#include <linux/time.h>
23	#include <net/gso.h>
24	#include <net/netlink.h>
25	#include <net/pkt_sched.h>
26	#include <net/pkt_cls.h>
27	#include <net/sch_generic.h>
28	#include <net/sock.h>
29	#include <net/tcp.h>
30
31	#define TAPRIO_STAT_NOT_SET (~0ULL)
32
33	#include "sch_mqprio_lib.h"
34
35	static LIST_HEAD(taprio_list);
36	static struct static_key_false taprio_have_broken_mqprio;
37	static struct static_key_false taprio_have_working_mqprio;
38
39	#define TAPRIO_ALL_GATES_OPEN -1
40
41	#define TXTIME_ASSIST_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST)
42	#define FULL_OFFLOAD_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)
43	#define TAPRIO_SUPPORTED_FLAGS \
44	(TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST | TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)
45	#define TAPRIO_FLAGS_INVALID U32_MAX
46	/* Minimum value for picos_per_byte to ensure non-zero duration
47	* for minimum-sized Ethernet frames (ETH_ZLEN = 60).
48	* 60 * 17 > PSEC_PER_NSEC (1000)
49	*/
50	#define TAPRIO_PICOS_PER_BYTE_MIN 17
51
52	struct sched_entry {
53	/* Durations between this GCL entry and the GCL entry where the
54	* respective traffic class gate closes
55	*/
56	u64 gate_duration[TC_MAX_QUEUE];
57	atomic_t budget[TC_MAX_QUEUE];
58	/* The qdisc makes some effort so that no packet leaves
59	* after this time
60	*/
61	ktime_t gate_close_time[TC_MAX_QUEUE];
62	struct list_head list;
63	/* Used to calculate when to advance the schedule */
64	ktime_t end_time;
65	ktime_t next_txtime;
66	int index;
67	u32 gate_mask;
68	u32 interval;
69	u8 command;
70	};
71
72	struct sched_gate_list {
73	/* Longest non-zero contiguous gate durations per traffic class,
74	* or 0 if a traffic class gate never opens during the schedule.
75	*/
76	u64 max_open_gate_duration[TC_MAX_QUEUE];
77	u32 max_frm_len[TC_MAX_QUEUE]; /* for the fast path */
78	u32 max_sdu[TC_MAX_QUEUE]; /* for dump */
79	struct rcu_head rcu;
80	struct list_head entries;
81	size_t num_entries;
82	ktime_t cycle_end_time;
83	s64 cycle_time;
84	s64 cycle_time_extension;
85	s64 base_time;
86	};
87
88	struct taprio_sched {
89	struct Qdisc **qdiscs;
90	struct Qdisc *root;
91	u32 flags;
92	enum tk_offsets tk_offset;
93	int clockid;
94	bool offloaded;
95	bool detected_mqprio;
96	bool broken_mqprio;
97	atomic64_t picos_per_byte; /* Using picoseconds because for 10Gbps+
98	* speeds it's sub-nanoseconds per byte
99	*/
100
101	/* Protects the update side of the RCU protected current_entry */
102	spinlock_t current_entry_lock;
103	struct sched_entry __rcu *current_entry;
104	struct sched_gate_list __rcu *oper_sched;
105	struct sched_gate_list __rcu *admin_sched;
106	struct hrtimer advance_timer;
107	struct list_head taprio_list;
108	int cur_txq[TC_MAX_QUEUE];
109	u32 max_sdu[TC_MAX_QUEUE]; /* save info from the user */
110	u32 fp[TC_QOPT_MAX_QUEUE]; /* only for dump and offloading */
111	u32 txtime_delay;
112	};
113
114	struct __tc_taprio_qopt_offload {
115	refcount_t users;
116	struct tc_taprio_qopt_offload offload;
117	};
118
119	static void taprio_calculate_gate_durations(struct taprio_sched *q,
120	struct sched_gate_list *sched)
121	{
122	struct net_device *dev = qdisc_dev(qdisc: q->root);
123	int num_tc = netdev_get_num_tc(dev);
124	struct sched_entry *entry, *cur;
125	int tc;
126
127	list_for_each_entry(entry, &sched->entries, list) {
128	u32 gates_still_open = entry->gate_mask;
129
130	/* For each traffic class, calculate each open gate duration,
131	* starting at this schedule entry and ending at the schedule
132	* entry containing a gate close event for that TC.
133	*/
134	cur = entry;
135
136	do {
137	if (!gates_still_open)
138	break;
139
140	for (tc = 0; tc < num_tc; tc++) {
141	if (!(gates_still_open & BIT(tc)))
142	continue;
143
144	if (cur->gate_mask & BIT(tc))
145	entry->gate_duration[tc] += cur->interval;
146	else
147	gates_still_open &= ~BIT(tc);
148	}
149
150	cur = list_next_entry_circular(cur, &sched->entries, list);
151	} while (cur != entry);
152
153	/* Keep track of the maximum gate duration for each traffic
154	* class, taking care to not confuse a traffic class which is
155	* temporarily closed with one that is always closed.
156	*/
157	for (tc = 0; tc < num_tc; tc++)
158	if (entry->gate_duration[tc] &&
159	sched->max_open_gate_duration[tc] < entry->gate_duration[tc])
160	sched->max_open_gate_duration[tc] = entry->gate_duration[tc];
161	}
162	}
163
164	static bool taprio_entry_allows_tx(ktime_t skb_end_time,
165	struct sched_entry *entry, int tc)
166	{
167	return ktime_before(cmp1: skb_end_time, cmp2: entry->gate_close_time[tc]);
168	}
169
170	static ktime_t sched_base_time(const struct sched_gate_list *sched)
171	{
172	if (!sched)
173	return KTIME_MAX;
174
175	return ns_to_ktime(ns: sched->base_time);
176	}
177
178	static ktime_t taprio_mono_to_any(const struct taprio_sched *q, ktime_t mono)
179	{
180	/* This pairs with WRITE_ONCE() in taprio_parse_clockid() */
181	enum tk_offsets tk_offset = READ_ONCE(q->tk_offset);
182
183	switch (tk_offset) {
184	case TK_OFFS_MAX:
185	return mono;
186	default:
187	return ktime_mono_to_any(tmono: mono, offs: tk_offset);
188	}
189	}
190
191	static ktime_t taprio_get_time(const struct taprio_sched *q)
192	{
193	return taprio_mono_to_any(q, mono: ktime_get());
194	}
195
196	static void taprio_free_sched_cb(struct rcu_head *head)
197	{
198	struct sched_gate_list *sched = container_of(head, struct sched_gate_list, rcu);
199	struct sched_entry *entry, *n;
200
201	list_for_each_entry_safe(entry, n, &sched->entries, list) {
202	list_del(entry: &entry->list);
203	kfree(objp: entry);
204	}
205
206	kfree(objp: sched);
207	}
208
209	static void switch_schedules(struct taprio_sched *q,
210	struct sched_gate_list **admin,
211	struct sched_gate_list **oper)
212	{
213	rcu_assign_pointer(q->oper_sched, *admin);
214	rcu_assign_pointer(q->admin_sched, NULL);
215
216	if (*oper)
217	call_rcu(head: &(*oper)->rcu, func: taprio_free_sched_cb);
218
219	*oper = *admin;
220	*admin = NULL;
221	}
222
223	/* Get how much time has been already elapsed in the current cycle. */
224	static s32 get_cycle_time_elapsed(struct sched_gate_list *sched, ktime_t time)
225	{
226	ktime_t time_since_sched_start;
227	s32 time_elapsed;
228
229	time_since_sched_start = ktime_sub(time, sched->base_time);
230	div_s64_rem(dividend: time_since_sched_start, divisor: sched->cycle_time, remainder: &time_elapsed);
231
232	return time_elapsed;
233	}
234
235	static ktime_t get_interval_end_time(struct sched_gate_list *sched,
236	struct sched_gate_list *admin,
237	struct sched_entry *entry,
238	ktime_t intv_start)
239	{
240	s32 cycle_elapsed = get_cycle_time_elapsed(sched, time: intv_start);
241	ktime_t intv_end, cycle_ext_end, cycle_end;
242
243	cycle_end = ktime_add_ns(intv_start, sched->cycle_time - cycle_elapsed);
244	intv_end = ktime_add_ns(intv_start, entry->interval);
245	cycle_ext_end = ktime_add(cycle_end, sched->cycle_time_extension);
246
247	if (ktime_before(cmp1: intv_end, cmp2: cycle_end))
248	return intv_end;
249	else if (admin && admin != sched &&
250	ktime_after(cmp1: admin->base_time, cmp2: cycle_end) &&
251	ktime_before(cmp1: admin->base_time, cmp2: cycle_ext_end))
252	return admin->base_time;
253	else
254	return cycle_end;
255	}
256
257	static int length_to_duration(struct taprio_sched *q, int len)
258	{
259	return div_u64(dividend: len * atomic64_read(v: &q->picos_per_byte), PSEC_PER_NSEC);
260	}
261
262	static int duration_to_length(struct taprio_sched *q, u64 duration)
263	{
264	return div_u64(dividend: duration * PSEC_PER_NSEC, divisor: atomic64_read(v: &q->picos_per_byte));
265	}
266
267	/* Sets sched->max_sdu[] and sched->max_frm_len[] to the minimum between the
268	* q->max_sdu[] requested by the user and the max_sdu dynamically determined by
269	* the maximum open gate durations at the given link speed.
270	*/
271	static void taprio_update_queue_max_sdu(struct taprio_sched *q,
272	struct sched_gate_list *sched,
273	struct qdisc_size_table *stab)
274	{
275	struct net_device *dev = qdisc_dev(qdisc: q->root);
276	int num_tc = netdev_get_num_tc(dev);
277	u32 max_sdu_from_user;
278	u32 max_sdu_dynamic;
279	u32 max_sdu;
280	int tc;
281
282	for (tc = 0; tc < num_tc; tc++) {
283	max_sdu_from_user = q->max_sdu[tc] ?: U32_MAX;
284
285	/* TC gate never closes => keep the queueMaxSDU
286	* selected by the user
287	*/
288	if (sched->max_open_gate_duration[tc] == sched->cycle_time) {
289	max_sdu_dynamic = U32_MAX;
290	} else {
291	u32 max_frm_len;
292
293	max_frm_len = duration_to_length(q, duration: sched->max_open_gate_duration[tc]);
294	/* Compensate for L1 overhead from size table,
295	* but don't let the frame size go negative
296	*/
297	if (stab) {
298	max_frm_len -= stab->szopts.overhead;
299	max_frm_len = max_t(int, max_frm_len,
300	dev->hard_header_len + 1);
301	}
302	max_sdu_dynamic = max_frm_len - dev->hard_header_len;
303	if (max_sdu_dynamic > dev->max_mtu)
304	max_sdu_dynamic = U32_MAX;
305	}
306
307	max_sdu = min(max_sdu_dynamic, max_sdu_from_user);
308
309	if (max_sdu != U32_MAX) {
310	sched->max_frm_len[tc] = max_sdu + dev->hard_header_len;
311	sched->max_sdu[tc] = max_sdu;
312	} else {
313	sched->max_frm_len[tc] = U32_MAX; /* never oversized */
314	sched->max_sdu[tc] = 0;
315	}
316	}
317	}
318
319	/* Returns the entry corresponding to next available interval. If
320	* validate_interval is set, it only validates whether the timestamp occurs
321	* when the gate corresponding to the skb's traffic class is open.
322	*/
323	static struct sched_entry *find_entry_to_transmit(struct sk_buff *skb,
324	struct Qdisc *sch,
325	struct sched_gate_list *sched,
326	struct sched_gate_list *admin,
327	ktime_t time,
328	ktime_t *interval_start,
329	ktime_t *interval_end,
330	bool validate_interval)
331	{
332	ktime_t curr_intv_start, curr_intv_end, cycle_end, packet_transmit_time;
333	ktime_t earliest_txtime = KTIME_MAX, txtime, cycle, transmit_end_time;
334	struct sched_entry *entry = NULL, *entry_found = NULL;
335	struct taprio_sched *q = qdisc_priv(sch);
336	struct net_device *dev = qdisc_dev(qdisc: sch);
337	bool entry_available = false;
338	s32 cycle_elapsed;
339	int tc, n;
340
341	tc = netdev_get_prio_tc_map(dev, prio: skb->priority);
342	packet_transmit_time = length_to_duration(q, len: qdisc_pkt_len(skb));
343
344	*interval_start = 0;
345	*interval_end = 0;
346
347	if (!sched)
348	return NULL;
349
350	cycle = sched->cycle_time;
351	cycle_elapsed = get_cycle_time_elapsed(sched, time);
352	curr_intv_end = ktime_sub_ns(time, cycle_elapsed);
353	cycle_end = ktime_add_ns(curr_intv_end, cycle);
354
355	list_for_each_entry(entry, &sched->entries, list) {
356	curr_intv_start = curr_intv_end;
357	curr_intv_end = get_interval_end_time(sched, admin, entry,
358	intv_start: curr_intv_start);
359
360	if (ktime_after(cmp1: curr_intv_start, cmp2: cycle_end))
361	break;
362
363	if (!(entry->gate_mask & BIT(tc)) ||
364	packet_transmit_time > entry->interval)
365	continue;
366
367	txtime = entry->next_txtime;
368
369	if (ktime_before(cmp1: txtime, cmp2: time) || validate_interval) {
370	transmit_end_time = ktime_add_ns(time, packet_transmit_time);
371	if ((ktime_before(cmp1: curr_intv_start, cmp2: time) &&
372	ktime_before(cmp1: transmit_end_time, cmp2: curr_intv_end)) ||
373	(ktime_after(cmp1: curr_intv_start, cmp2: time) && !validate_interval)) {
374	entry_found = entry;
375	*interval_start = curr_intv_start;
376	*interval_end = curr_intv_end;
377	break;
378	} else if (!entry_available && !validate_interval) {
379	/* Here, we are just trying to find out the
380	* first available interval in the next cycle.
381	*/
382	entry_available = true;
383	entry_found = entry;
384	*interval_start = ktime_add_ns(curr_intv_start, cycle);
385	*interval_end = ktime_add_ns(curr_intv_end, cycle);
386	}
387	} else if (ktime_before(cmp1: txtime, cmp2: earliest_txtime) &&
388	!entry_available) {
389	earliest_txtime = txtime;
390	entry_found = entry;
391	n = div_s64(ktime_sub(txtime, curr_intv_start), divisor: cycle);
392	*interval_start = ktime_add(curr_intv_start, n * cycle);
393	*interval_end = ktime_add(curr_intv_end, n * cycle);
394	}
395	}
396
397	return entry_found;
398	}
399
400	static bool is_valid_interval(struct sk_buff *skb, struct Qdisc *sch)
401	{
402	struct taprio_sched *q = qdisc_priv(sch);
403	struct sched_gate_list *sched, *admin;
404	ktime_t interval_start, interval_end;
405	struct sched_entry *entry;
406
407	rcu_read_lock();
408	sched = rcu_dereference(q->oper_sched);
409	admin = rcu_dereference(q->admin_sched);
410
411	entry = find_entry_to_transmit(skb, sch, sched, admin, time: skb->tstamp,
412	interval_start: &interval_start, interval_end: &interval_end, validate_interval: true);
413	rcu_read_unlock();
414
415	return entry;
416	}
417
418	/* This returns the tstamp value set by TCP in terms of the set clock. */
419	static ktime_t get_tcp_tstamp(struct taprio_sched *q, struct sk_buff *skb)
420	{
421	unsigned int offset = skb_network_offset(skb);
422	const struct ipv6hdr *ipv6h;
423	const struct iphdr *iph;
424	struct ipv6hdr _ipv6h;
425
426	ipv6h = skb_header_pointer(skb, offset, len: sizeof(_ipv6h), buffer: &_ipv6h);
427	if (!ipv6h)
428	return 0;
429
430	if (ipv6h->version == 4) {
431	iph = (struct iphdr *)ipv6h;
432	offset += iph->ihl * 4;
433
434	/* special-case 6in4 tunnelling, as that is a common way to get
435	* v6 connectivity in the home
436	*/
437	if (iph->protocol == IPPROTO_IPV6) {
438	ipv6h = skb_header_pointer(skb, offset,
439	len: sizeof(_ipv6h), buffer: &_ipv6h);
440
441	if (!ipv6h || ipv6h->nexthdr != IPPROTO_TCP)
442	return 0;
443	} else if (iph->protocol != IPPROTO_TCP) {
444	return 0;
445	}
446	} else if (ipv6h->version == 6 && ipv6h->nexthdr != IPPROTO_TCP) {
447	return 0;
448	}
449
450	return taprio_mono_to_any(q, mono: skb->skb_mstamp_ns);
451	}
452
453	/* There are a few scenarios where we will have to modify the txtime from
454	* what is read from next_txtime in sched_entry. They are:
455	* 1. If txtime is in the past,
456	* a. The gate for the traffic class is currently open and packet can be
457	* transmitted before it closes, schedule the packet right away.
458	* b. If the gate corresponding to the traffic class is going to open later
459	* in the cycle, set the txtime of packet to the interval start.
460	* 2. If txtime is in the future, there are packets corresponding to the
461	* current traffic class waiting to be transmitted. So, the following
462	* possibilities exist:
463	* a. We can transmit the packet before the window containing the txtime
464	* closes.
465	* b. The window might close before the transmission can be completed
466	* successfully. So, schedule the packet in the next open window.
467	*/
468	static long get_packet_txtime(struct sk_buff *skb, struct Qdisc *sch)
469	{
470	ktime_t transmit_end_time, interval_end, interval_start, tcp_tstamp;
471	struct taprio_sched *q = qdisc_priv(sch);
472	struct sched_gate_list *sched, *admin;
473	ktime_t minimum_time, now, txtime;
474	int len, packet_transmit_time;
475	struct sched_entry *entry;
476	bool sched_changed;
477
478	now = taprio_get_time(q);
479	minimum_time = ktime_add_ns(now, q->txtime_delay);
480
481	tcp_tstamp = get_tcp_tstamp(q, skb);
482	minimum_time = max_t(ktime_t, minimum_time, tcp_tstamp);
483
484	rcu_read_lock();
485	admin = rcu_dereference(q->admin_sched);
486	sched = rcu_dereference(q->oper_sched);
487	if (admin && ktime_after(cmp1: minimum_time, cmp2: admin->base_time))
488	switch_schedules(q, admin: &admin, oper: &sched);
489
490	/* Until the schedule starts, all the queues are open */
491	if (!sched || ktime_before(cmp1: minimum_time, cmp2: sched->base_time)) {
492	txtime = minimum_time;
493	goto done;
494	}
495
496	len = qdisc_pkt_len(skb);
497	packet_transmit_time = length_to_duration(q, len);
498
499	do {
500	sched_changed = false;
501
502	entry = find_entry_to_transmit(skb, sch, sched, admin,
503	time: minimum_time,
504	interval_start: &interval_start, interval_end: &interval_end,
505	validate_interval: false);
506	if (!entry) {
507	txtime = 0;
508	goto done;
509	}
510
511	txtime = entry->next_txtime;
512	txtime = max_t(ktime_t, txtime, minimum_time);
513	txtime = max_t(ktime_t, txtime, interval_start);
514
515	if (admin && admin != sched &&
516	ktime_after(cmp1: txtime, cmp2: admin->base_time)) {
517	sched = admin;
518	sched_changed = true;
519	continue;
520	}
521
522	transmit_end_time = ktime_add(txtime, packet_transmit_time);
523	minimum_time = transmit_end_time;
524
525	/* Update the txtime of current entry to the next time it's
526	* interval starts.
527	*/
528	if (ktime_after(cmp1: transmit_end_time, cmp2: interval_end))
529	entry->next_txtime = ktime_add(interval_start, sched->cycle_time);
530	} while (sched_changed || ktime_after(cmp1: transmit_end_time, cmp2: interval_end));
531
532	entry->next_txtime = transmit_end_time;
533
534	done:
535	rcu_read_unlock();
536	return txtime;
537	}
538
539	/* Devices with full offload are expected to honor this in hardware */
540	static bool taprio_skb_exceeds_queue_max_sdu(struct Qdisc *sch,
541	struct sk_buff *skb)
542	{
543	struct taprio_sched *q = qdisc_priv(sch);
544	struct net_device *dev = qdisc_dev(qdisc: sch);
545	struct sched_gate_list *sched;
546	int prio = skb->priority;
547	bool exceeds = false;
548	u8 tc;
549
550	tc = netdev_get_prio_tc_map(dev, prio);
551
552	rcu_read_lock();
553	sched = rcu_dereference(q->oper_sched);
554	if (sched && skb->len > sched->max_frm_len[tc])
555	exceeds = true;
556	rcu_read_unlock();
557
558	return exceeds;
559	}
560
561	static int taprio_enqueue_one(struct sk_buff *skb, struct Qdisc *sch,
562	struct Qdisc *child, struct sk_buff **to_free)
563	{
564	struct taprio_sched *q = qdisc_priv(sch);
565
566	/* sk_flags are only safe to use on full sockets. */
567	if (skb->sk && sk_fullsock(sk: skb->sk) && sock_flag(sk: skb->sk, flag: SOCK_TXTIME)) {
568	if (!is_valid_interval(skb, sch))
569	return qdisc_drop(skb, sch, to_free);
570	} else if (TXTIME_ASSIST_IS_ENABLED(q->flags)) {
571	skb->tstamp = get_packet_txtime(skb, sch);
572	if (!skb->tstamp)
573	return qdisc_drop(skb, sch, to_free);
574	}
575
576	qdisc_qstats_backlog_inc(sch, skb);
577	sch->q.qlen++;
578
579	return qdisc_enqueue(skb, sch: child, to_free);
580	}
581
582	static int taprio_enqueue_segmented(struct sk_buff *skb, struct Qdisc *sch,
583	struct Qdisc *child,
584	struct sk_buff **to_free)
585	{
586	unsigned int slen = 0, numsegs = 0, len = qdisc_pkt_len(skb);
587	netdev_features_t features = netif_skb_features(skb);
588	struct sk_buff *segs, *nskb;
589	int ret;
590
591	segs = skb_gso_segment(skb, features: features & ~NETIF_F_GSO_MASK);
592	if (IS_ERR_OR_NULL(ptr: segs))
593	return qdisc_drop(skb, sch, to_free);
594
595	skb_list_walk_safe(segs, segs, nskb) {
596	skb_mark_not_on_list(skb: segs);
597	qdisc_skb_cb(skb: segs)->pkt_len = segs->len;
598	qdisc_skb_cb(skb: segs)->pkt_segs = 1;
599	slen += segs->len;
600
601	/* FIXME: we should be segmenting to a smaller size
602	* rather than dropping these
603	*/
604	if (taprio_skb_exceeds_queue_max_sdu(sch, skb: segs))
605	ret = qdisc_drop(skb: segs, sch, to_free);
606	else
607	ret = taprio_enqueue_one(skb: segs, sch, child, to_free);
608
609	if (ret != NET_XMIT_SUCCESS) {
610	if (net_xmit_drop_count(ret))
611	qdisc_qstats_drop(sch);
612	} else {
613	numsegs++;
614	}
615	}
616
617	if (numsegs > 1)
618	qdisc_tree_reduce_backlog(qdisc: sch, n: 1 - numsegs, len: len - slen);
619	consume_skb(skb);
620
621	return numsegs > 0 ? NET_XMIT_SUCCESS : NET_XMIT_DROP;
622	}
623
624	/* Will not be called in the full offload case, since the TX queues are
625	* attached to the Qdisc created using qdisc_create_dflt()
626	*/
627	static int taprio_enqueue(struct sk_buff *skb, struct Qdisc *sch,
628	struct sk_buff **to_free)
629	{
630	struct taprio_sched *q = qdisc_priv(sch);
631	struct Qdisc *child;
632	int queue;
633
634	queue = skb_get_queue_mapping(skb);
635
636	child = q->qdiscs[queue];
637	if (unlikely(!child))
638	return qdisc_drop(skb, sch, to_free);
639
640	if (taprio_skb_exceeds_queue_max_sdu(sch, skb)) {
641	/* Large packets might not be transmitted when the transmission
642	* duration exceeds any configured interval. Therefore, segment
643	* the skb into smaller chunks. Drivers with full offload are
644	* expected to handle this in hardware.
645	*/
646	if (skb_is_gso(skb))
647	return taprio_enqueue_segmented(skb, sch, child,
648	to_free);
649
650	return qdisc_drop(skb, sch, to_free);
651	}
652
653	return taprio_enqueue_one(skb, sch, child, to_free);
654	}
655
656	static struct sk_buff *taprio_peek(struct Qdisc *sch)
657	{
658	WARN_ONCE(1, "taprio only supports operating as root qdisc, peek() not implemented");
659	return NULL;
660	}
661
662	static void taprio_set_budgets(struct taprio_sched *q,
663	struct sched_gate_list *sched,
664	struct sched_entry *entry)
665	{
666	struct net_device *dev = qdisc_dev(qdisc: q->root);
667	int num_tc = netdev_get_num_tc(dev);
668	int tc, budget;
669
670	for (tc = 0; tc < num_tc; tc++) {
671	/* Traffic classes which never close have infinite budget */
672	if (entry->gate_duration[tc] == sched->cycle_time)
673	budget = INT_MAX;
674	else
675	budget = div64_u64(dividend: (u64)entry->gate_duration[tc] * PSEC_PER_NSEC,
676	divisor: atomic64_read(v: &q->picos_per_byte));
677
678	atomic_set(v: &entry->budget[tc], i: budget);
679	}
680	}
681
682	/* When an skb is sent, it consumes from the budget of all traffic classes */
683	static int taprio_update_budgets(struct sched_entry *entry, size_t len,
684	int tc_consumed, int num_tc)
685	{
686	int tc, budget, new_budget = 0;
687
688	for (tc = 0; tc < num_tc; tc++) {
689	budget = atomic_read(v: &entry->budget[tc]);
690	/* Don't consume from infinite budget */
691	if (budget == INT_MAX) {
692	if (tc == tc_consumed)
693	new_budget = budget;
694	continue;
695	}
696
697	if (tc == tc_consumed)
698	new_budget = atomic_sub_return(i: len, v: &entry->budget[tc]);
699	else
700	atomic_sub(i: len, v: &entry->budget[tc]);
701	}
702
703	return new_budget;
704	}
705
706	static struct sk_buff *taprio_dequeue_from_txq(struct Qdisc *sch, int txq,
707	struct sched_entry *entry,
708	u32 gate_mask)
709	{
710	struct taprio_sched *q = qdisc_priv(sch);
711	struct net_device *dev = qdisc_dev(qdisc: sch);
712	struct Qdisc *child = q->qdiscs[txq];
713	int num_tc = netdev_get_num_tc(dev);
714	struct sk_buff *skb;
715	ktime_t guard;
716	int prio;
717	int len;
718	u8 tc;
719
720	if (unlikely(!child))
721	return NULL;
722
723	if (TXTIME_ASSIST_IS_ENABLED(q->flags))
724	goto skip_peek_checks;
725
726	skb = child->ops->peek(child);
727	if (!skb)
728	return NULL;
729
730	prio = skb->priority;
731	tc = netdev_get_prio_tc_map(dev, prio);
732
733	if (!(gate_mask & BIT(tc)))
734	return NULL;
735
736	len = qdisc_pkt_len(skb);
737	guard = ktime_add_ns(taprio_get_time(q), length_to_duration(q, len));
738
739	/* In the case that there's no gate entry, there's no
740	* guard band ...
741	*/
742	if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
743	!taprio_entry_allows_tx(skb_end_time: guard, entry, tc))
744	return NULL;
745
746	/* ... and no budget. */
747	if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
748	taprio_update_budgets(entry, len, tc_consumed: tc, num_tc) < 0)
749	return NULL;
750
751	skip_peek_checks:
752	skb = child->ops->dequeue(child);
753	if (unlikely(!skb))
754	return NULL;
755
756	qdisc_bstats_update(sch, skb);
757	qdisc_qstats_backlog_dec(sch, skb);
758	sch->q.qlen--;
759
760	return skb;
761	}
762
763	static void taprio_next_tc_txq(struct net_device *dev, int tc, int *txq)
764	{
765	int offset = dev->tc_to_txq[tc].offset;
766	int count = dev->tc_to_txq[tc].count;
767
768	(*txq)++;
769	if (*txq == offset + count)
770	*txq = offset;
771	}
772
773	/* Prioritize higher traffic classes, and select among TXQs belonging to the
774	* same TC using round robin
775	*/
776	static struct sk_buff *taprio_dequeue_tc_priority(struct Qdisc *sch,
777	struct sched_entry *entry,
778	u32 gate_mask)
779	{
780	struct taprio_sched *q = qdisc_priv(sch);
781	struct net_device *dev = qdisc_dev(qdisc: sch);
782	int num_tc = netdev_get_num_tc(dev);
783	struct sk_buff *skb;
784	int tc;
785
786	for (tc = num_tc - 1; tc >= 0; tc--) {
787	int first_txq = q->cur_txq[tc];
788
789	if (!(gate_mask & BIT(tc)))
790	continue;
791
792	do {
793	skb = taprio_dequeue_from_txq(sch, txq: q->cur_txq[tc],
794	entry, gate_mask);
795
796	taprio_next_tc_txq(dev, tc, txq: &q->cur_txq[tc]);
797
798	if (q->cur_txq[tc] >= dev->num_tx_queues)
799	q->cur_txq[tc] = first_txq;
800
801	if (skb)
802	return skb;
803	} while (q->cur_txq[tc] != first_txq);
804	}
805
806	return NULL;
807	}
808
809	/* Broken way of prioritizing smaller TXQ indices and ignoring the traffic
810	* class other than to determine whether the gate is open or not
811	*/
812	static struct sk_buff *taprio_dequeue_txq_priority(struct Qdisc *sch,
813	struct sched_entry *entry,
814	u32 gate_mask)
815	{
816	struct net_device *dev = qdisc_dev(qdisc: sch);
817	struct sk_buff *skb;
818	int i;
819
820	for (i = 0; i < dev->num_tx_queues; i++) {
821	skb = taprio_dequeue_from_txq(sch, txq: i, entry, gate_mask);
822	if (skb)
823	return skb;
824	}
825
826	return NULL;
827	}
828
829	/* Will not be called in the full offload case, since the TX queues are
830	* attached to the Qdisc created using qdisc_create_dflt()
831	*/
832	static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
833	{
834	struct taprio_sched *q = qdisc_priv(sch);
835	struct sk_buff *skb = NULL;
836	struct sched_entry *entry;
837	u32 gate_mask;
838
839	rcu_read_lock();
840	entry = rcu_dereference(q->current_entry);
841	/* if there's no entry, it means that the schedule didn't
842	* start yet, so force all gates to be open, this is in
843	* accordance to IEEE 802.1Qbv-2015 Section 8.6.9.4.5
844	* "AdminGateStates"
845	*/
846	gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN;
847	if (!gate_mask)
848	goto done;
849
850	if (static_branch_unlikely(&taprio_have_broken_mqprio) &&
851	!static_branch_likely(&taprio_have_working_mqprio)) {
852	/* Single NIC kind which is broken */
853	skb = taprio_dequeue_txq_priority(sch, entry, gate_mask);
854	} else if (static_branch_likely(&taprio_have_working_mqprio) &&
855	!static_branch_unlikely(&taprio_have_broken_mqprio)) {
856	/* Single NIC kind which prioritizes properly */
857	skb = taprio_dequeue_tc_priority(sch, entry, gate_mask);
858	} else {
859	/* Mixed NIC kinds present in system, need dynamic testing */
860	if (q->broken_mqprio)
861	skb = taprio_dequeue_txq_priority(sch, entry, gate_mask);
862	else
863	skb = taprio_dequeue_tc_priority(sch, entry, gate_mask);
864	}
865
866	done:
867	rcu_read_unlock();
868
869	return skb;
870	}
871
872	static bool should_restart_cycle(const struct sched_gate_list *oper,
873	const struct sched_entry *entry)
874	{
875	if (list_is_last(list: &entry->list, head: &oper->entries))
876	return true;
877
878	if (ktime_compare(cmp1: entry->end_time, cmp2: oper->cycle_end_time) == 0)
879	return true;
880
881	return false;
882	}
883
884	static bool should_change_schedules(const struct sched_gate_list *admin,
885	const struct sched_gate_list *oper,
886	ktime_t end_time)
887	{
888	ktime_t next_base_time, extension_time;
889
890	if (!admin)
891	return false;
892
893	next_base_time = sched_base_time(sched: admin);
894
895	/* This is the simple case, the end_time would fall after
896	* the next schedule base_time.
897	*/
898	if (ktime_compare(cmp1: next_base_time, cmp2: end_time) <= 0)
899	return true;
900
901	/* This is the cycle_time_extension case, if the end_time
902	* plus the amount that can be extended would fall after the
903	* next schedule base_time, we can extend the current schedule
904	* for that amount.
905	*/
906	extension_time = ktime_add_ns(end_time, oper->cycle_time_extension);
907
908	/* FIXME: the IEEE 802.1Q-2018 Specification isn't clear about
909	* how precisely the extension should be made. So after
910	* conformance testing, this logic may change.
911	*/
912	if (ktime_compare(cmp1: next_base_time, cmp2: extension_time) <= 0)
913	return true;
914
915	return false;
916	}
917
918	static enum hrtimer_restart advance_sched(struct hrtimer *timer)
919	{
920	struct taprio_sched *q = container_of(timer, struct taprio_sched,
921	advance_timer);
922	struct net_device *dev = qdisc_dev(qdisc: q->root);
923	struct sched_gate_list *oper, *admin;
924	int num_tc = netdev_get_num_tc(dev);
925	struct sched_entry *entry, *next;
926	struct Qdisc *sch = q->root;
927	ktime_t end_time;
928	int tc;
929
930	spin_lock(lock: &q->current_entry_lock);
931	entry = rcu_dereference_protected(q->current_entry,
932	lockdep_is_held(&q->current_entry_lock));
933	oper = rcu_dereference_protected(q->oper_sched,
934	lockdep_is_held(&q->current_entry_lock));
935	admin = rcu_dereference_protected(q->admin_sched,
936	lockdep_is_held(&q->current_entry_lock));
937
938	if (!oper)
939	switch_schedules(q, admin: &admin, oper: &oper);
940
941	/* This can happen in two cases: 1. this is the very first run
942	* of this function (i.e. we weren't running any schedule
943	* previously); 2. The previous schedule just ended. The first
944	* entry of all schedules are pre-calculated during the
945	* schedule initialization.
946	*/
947	if (unlikely(!entry || entry->end_time == oper->base_time)) {
948	next = list_first_entry(&oper->entries, struct sched_entry,
949	list);
950	end_time = next->end_time;
951	goto first_run;
952	}
953
954	if (should_restart_cycle(oper, entry)) {
955	next = list_first_entry(&oper->entries, struct sched_entry,
956	list);
957	oper->cycle_end_time = ktime_add_ns(oper->cycle_end_time,
958	oper->cycle_time);
959	} else {
960	next = list_next_entry(entry, list);
961	}
962
963	end_time = ktime_add_ns(entry->end_time, next->interval);
964	end_time = min_t(ktime_t, end_time, oper->cycle_end_time);
965
966	for (tc = 0; tc < num_tc; tc++) {
967	if (next->gate_duration[tc] == oper->cycle_time)
968	next->gate_close_time[tc] = KTIME_MAX;
969	else
970	next->gate_close_time[tc] = ktime_add_ns(entry->end_time,
971	next->gate_duration[tc]);
972	}
973
974	if (should_change_schedules(admin, oper, end_time)) {
975	/* Set things so the next time this runs, the new
976	* schedule runs.
977	*/
978	end_time = sched_base_time(sched: admin);
979	switch_schedules(q, admin: &admin, oper: &oper);
980	}
981
982	next->end_time = end_time;
983	taprio_set_budgets(q, sched: oper, entry: next);
984
985	first_run:
986	rcu_assign_pointer(q->current_entry, next);
987	spin_unlock(lock: &q->current_entry_lock);
988
989	hrtimer_set_expires(timer: &q->advance_timer, time: end_time);
990
991	rcu_read_lock();
992	__netif_schedule(q: sch);
993	rcu_read_unlock();
994
995	return HRTIMER_RESTART;
996	}
997
998	static const struct nla_policy entry_policy[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = {
999	[TCA_TAPRIO_SCHED_ENTRY_INDEX] = { .type = NLA_U32 },
1000	[TCA_TAPRIO_SCHED_ENTRY_CMD] = { .type = NLA_U8 },
1001	[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK] = { .type = NLA_U32 },
1002	[TCA_TAPRIO_SCHED_ENTRY_INTERVAL] = { .type = NLA_U32 },
1003	};
1004
1005	static const struct nla_policy taprio_tc_policy[TCA_TAPRIO_TC_ENTRY_MAX + 1] = {
1006	[TCA_TAPRIO_TC_ENTRY_INDEX] = NLA_POLICY_MAX(NLA_U32,
1007	TC_QOPT_MAX_QUEUE - 1),
1008	[TCA_TAPRIO_TC_ENTRY_MAX_SDU] = { .type = NLA_U32 },
1009	[TCA_TAPRIO_TC_ENTRY_FP] = NLA_POLICY_RANGE(NLA_U32,
1010	TC_FP_EXPRESS,
1011	TC_FP_PREEMPTIBLE),
1012	};
1013
1014	static const struct netlink_range_validation_signed taprio_cycle_time_range = {
1015	.min = 0,
1016	.max = INT_MAX,
1017	};
1018
1019	static const struct nla_policy taprio_policy[TCA_TAPRIO_ATTR_MAX + 1] = {
1020	[TCA_TAPRIO_ATTR_PRIOMAP] = {
1021	.len = sizeof(struct tc_mqprio_qopt)
1022	},
1023	[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST] = { .type = NLA_NESTED },
1024	[TCA_TAPRIO_ATTR_SCHED_BASE_TIME] = { .type = NLA_S64 },
1025	[TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY] = { .type = NLA_NESTED },
1026	[TCA_TAPRIO_ATTR_SCHED_CLOCKID] = { .type = NLA_S32 },
1027	[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME] =
1028	NLA_POLICY_FULL_RANGE_SIGNED(NLA_S64, &taprio_cycle_time_range),
1029	[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION] = { .type = NLA_S64 },
1030	[TCA_TAPRIO_ATTR_FLAGS] =
1031	NLA_POLICY_MASK(NLA_U32, TAPRIO_SUPPORTED_FLAGS),
1032	[TCA_TAPRIO_ATTR_TXTIME_DELAY] = { .type = NLA_U32 },
1033	[TCA_TAPRIO_ATTR_TC_ENTRY] = { .type = NLA_NESTED },
1034	};
1035
1036	static int fill_sched_entry(struct taprio_sched *q, struct nlattr **tb,
1037	struct sched_entry *entry,
1038	struct netlink_ext_ack *extack)
1039	{
1040	int min_duration = length_to_duration(q, ETH_ZLEN);
1041	u32 interval = 0;
1042
1043	if (tb[TCA_TAPRIO_SCHED_ENTRY_CMD])
1044	entry->command = nla_get_u8(
1045	nla: tb[TCA_TAPRIO_SCHED_ENTRY_CMD]);
1046
1047	if (tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK])
1048	entry->gate_mask = nla_get_u32(
1049	nla: tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK]);
1050
1051	if (tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL])
1052	interval = nla_get_u32(
1053	nla: tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL]);
1054
1055	/* The interval should allow at least the minimum ethernet
1056	* frame to go out.
1057	*/
1058	if (interval < min_duration) {
1059	NL_SET_ERR_MSG(extack, "Invalid interval for schedule entry");
1060	return -EINVAL;
1061	}
1062
1063	entry->interval = interval;
1064
1065	return 0;
1066	}
1067
1068	static int parse_sched_entry(struct taprio_sched *q, struct nlattr *n,
1069	struct sched_entry *entry, int index,
1070	struct netlink_ext_ack *extack)
1071	{
1072	struct nlattr *tb[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = { };
1073	int err;
1074
1075	err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_SCHED_ENTRY_MAX, nla: n,
1076	policy: entry_policy, NULL);
1077	if (err < 0) {
1078	NL_SET_ERR_MSG(extack, "Could not parse nested entry");
1079	return -EINVAL;
1080	}
1081
1082	entry->index = index;
1083
1084	return fill_sched_entry(q, tb, entry, extack);
1085	}
1086
1087	static int parse_sched_list(struct taprio_sched *q, struct nlattr *list,
1088	struct sched_gate_list *sched,
1089	struct netlink_ext_ack *extack)
1090	{
1091	struct nlattr *n;
1092	int err, rem;
1093	int i = 0;
1094
1095	if (!list)
1096	return -EINVAL;
1097
1098	nla_for_each_nested(n, list, rem) {
1099	struct sched_entry *entry;
1100
1101	if (nla_type(nla: n) != TCA_TAPRIO_SCHED_ENTRY) {
1102	NL_SET_ERR_MSG(extack, "Attribute is not of type 'entry'");
1103	continue;
1104	}
1105
1106	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
1107	if (!entry) {
1108	NL_SET_ERR_MSG(extack, "Not enough memory for entry");
1109	return -ENOMEM;
1110	}
1111
1112	err = parse_sched_entry(q, n, entry, index: i, extack);
1113	if (err < 0) {
1114	kfree(objp: entry);
1115	return err;
1116	}
1117
1118	list_add_tail(new: &entry->list, head: &sched->entries);
1119	i++;
1120	}
1121
1122	sched->num_entries = i;
1123
1124	return i;
1125	}
1126
1127	static int parse_taprio_schedule(struct taprio_sched *q, struct nlattr **tb,
1128	struct sched_gate_list *new,
1129	struct netlink_ext_ack *extack)
1130	{
1131	int err = 0;
1132
1133	if (tb[TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY]) {
1134	NL_SET_ERR_MSG(extack, "Adding a single entry is not supported");
1135	return -ENOTSUPP;
1136	}
1137
1138	if (tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME])
1139	new->base_time = nla_get_s64(nla: tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME]);
1140
1141	if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION])
1142	new->cycle_time_extension = nla_get_s64(nla: tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION]);
1143
1144	if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME])
1145	new->cycle_time = nla_get_s64(nla: tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME]);
1146
1147	if (tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST])
1148	err = parse_sched_list(q, list: tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST],
1149	sched: new, extack);
1150	if (err < 0)
1151	return err;
1152
1153	if (!new->cycle_time) {
1154	struct sched_entry *entry;
1155	ktime_t cycle = 0;
1156
1157	list_for_each_entry(entry, &new->entries, list)
1158	cycle = ktime_add_ns(cycle, entry->interval);
1159
1160	if (cycle < 0 || cycle > INT_MAX) {
1161	NL_SET_ERR_MSG(extack, "'cycle_time' is too big");
1162	return -EINVAL;
1163	}
1164
1165	new->cycle_time = cycle;
1166	}
1167
1168	if (new->cycle_time < new->num_entries * length_to_duration(q, ETH_ZLEN)) {
1169	NL_SET_ERR_MSG(extack, "'cycle_time' is too small");
1170	return -EINVAL;
1171	}
1172
1173	taprio_calculate_gate_durations(q, sched: new);
1174
1175	return 0;
1176	}
1177
1178	static int taprio_parse_mqprio_opt(struct net_device *dev,
1179	struct tc_mqprio_qopt *qopt,
1180	struct netlink_ext_ack *extack,
1181	u32 taprio_flags)
1182	{
1183	bool allow_overlapping_txqs = TXTIME_ASSIST_IS_ENABLED(taprio_flags);
1184
1185	if (!qopt) {
1186	if (!dev->num_tc) {
1187	NL_SET_ERR_MSG(extack, "'mqprio' configuration is necessary");
1188	return -EINVAL;
1189	}
1190	return 0;
1191	}
1192
1193	/* taprio imposes that traffic classes map 1:n to tx queues */
1194	if (qopt->num_tc > dev->num_tx_queues) {
1195	NL_SET_ERR_MSG(extack, "Number of traffic classes is greater than number of HW queues");
1196	return -EINVAL;
1197	}
1198
1199	/* For some reason, in txtime-assist mode, we allow TXQ ranges for
1200	* different TCs to overlap, and just validate the TXQ ranges.
1201	*/
1202	return mqprio_validate_qopt(dev, qopt, validate_queue_counts: true, allow_overlapping_txqs,
1203	extack);
1204	}
1205
1206	static int taprio_get_start_time(struct Qdisc *sch,
1207	struct sched_gate_list *sched,
1208	ktime_t *start)
1209	{
1210	struct taprio_sched *q = qdisc_priv(sch);
1211	ktime_t now, base, cycle;
1212	s64 n;
1213
1214	base = sched_base_time(sched);
1215	now = taprio_get_time(q);
1216
1217	if (ktime_after(cmp1: base, cmp2: now)) {
1218	*start = base;
1219	return 0;
1220	}
1221
1222	cycle = sched->cycle_time;
1223
1224	/* The qdisc is expected to have at least one sched_entry. Moreover,
1225	* any entry must have 'interval' > 0. Thus if the cycle time is zero,
1226	* something went really wrong. In that case, we should warn about this
1227	* inconsistent state and return error.
1228	*/
1229	if (WARN_ON(!cycle))
1230	return -EFAULT;
1231
1232	/* Schedule the start time for the beginning of the next
1233	* cycle.
1234	*/
1235	n = div64_s64(ktime_sub_ns(now, base), divisor: cycle);
1236	*start = ktime_add_ns(base, (n + 1) * cycle);
1237	return 0;
1238	}
1239
1240	static void setup_first_end_time(struct taprio_sched *q,
1241	struct sched_gate_list *sched, ktime_t base)
1242	{
1243	struct net_device *dev = qdisc_dev(qdisc: q->root);
1244	int num_tc = netdev_get_num_tc(dev);
1245	struct sched_entry *first;
1246	ktime_t cycle;
1247	int tc;
1248
1249	first = list_first_entry(&sched->entries,
1250	struct sched_entry, list);
1251
1252	cycle = sched->cycle_time;
1253
1254	/* FIXME: find a better place to do this */
1255	sched->cycle_end_time = ktime_add_ns(base, cycle);
1256
1257	first->end_time = ktime_add_ns(base, first->interval);
1258	taprio_set_budgets(q, sched, entry: first);
1259
1260	for (tc = 0; tc < num_tc; tc++) {
1261	if (first->gate_duration[tc] == sched->cycle_time)
1262	first->gate_close_time[tc] = KTIME_MAX;
1263	else
1264	first->gate_close_time[tc] = ktime_add_ns(base, first->gate_duration[tc]);
1265	}
1266
1267	rcu_assign_pointer(q->current_entry, NULL);
1268	}
1269
1270	static void taprio_start_sched(struct Qdisc *sch,
1271	ktime_t start, struct sched_gate_list *new)
1272	{
1273	struct taprio_sched *q = qdisc_priv(sch);
1274	ktime_t expires;
1275
1276	if (FULL_OFFLOAD_IS_ENABLED(q->flags))
1277	return;
1278
1279	expires = hrtimer_get_expires(timer: &q->advance_timer);
1280	if (expires == 0)
1281	expires = KTIME_MAX;
1282
1283	/* If the new schedule starts before the next expiration, we
1284	* reprogram it to the earliest one, so we change the admin
1285	* schedule to the operational one at the right time.
1286	*/
1287	start = min_t(ktime_t, start, expires);
1288
1289	hrtimer_start(timer: &q->advance_timer, tim: start, mode: HRTIMER_MODE_ABS);
1290	}
1291
1292	static void taprio_set_picos_per_byte(struct net_device *dev,
1293	struct taprio_sched *q,
1294	struct netlink_ext_ack *extack)
1295	{
1296	struct ethtool_link_ksettings ecmd;
1297	int speed = SPEED_10;
1298	int picos_per_byte;
1299	int err;
1300
1301	err = __ethtool_get_link_ksettings(dev, link_ksettings: &ecmd);
1302	if (err < 0)
1303	goto skip;
1304
1305	if (ecmd.base.speed && ecmd.base.speed != SPEED_UNKNOWN)
1306	speed = ecmd.base.speed;
1307
1308	skip:
1309	picos_per_byte = (USEC_PER_SEC * 8) / speed;
1310	if (picos_per_byte < TAPRIO_PICOS_PER_BYTE_MIN) {
1311	if (!extack)
1312	pr_warn("Link speed %d is too high. Schedule may be inaccurate.\n",
1313	speed);
1314	NL_SET_ERR_MSG_FMT_MOD(extack,
1315	"Link speed %d is too high. Schedule may be inaccurate.",
1316	speed);
1317	picos_per_byte = TAPRIO_PICOS_PER_BYTE_MIN;
1318	}
1319
1320	atomic64_set(v: &q->picos_per_byte, i: picos_per_byte);
1321	netdev_dbg(dev, "taprio: set %s's picos_per_byte to: %lld, linkspeed: %d\n",
1322	dev->name, (long long)atomic64_read(&q->picos_per_byte),
1323	ecmd.base.speed);
1324	}
1325
1326	static int taprio_dev_notifier(struct notifier_block *nb, unsigned long event,
1327	void *ptr)
1328	{
1329	struct net_device *dev = netdev_notifier_info_to_dev(info: ptr);
1330	struct sched_gate_list *oper, *admin;
1331	struct qdisc_size_table *stab;
1332	struct taprio_sched *q;
1333
1334	ASSERT_RTNL();
1335
1336	if (event != NETDEV_UP && event != NETDEV_CHANGE)
1337	return NOTIFY_DONE;
1338
1339	list_for_each_entry(q, &taprio_list, taprio_list) {
1340	if (dev != qdisc_dev(qdisc: q->root))
1341	continue;
1342
1343	taprio_set_picos_per_byte(dev, q, NULL);
1344
1345	stab = rtnl_dereference(q->root->stab);
1346
1347	rcu_read_lock();
1348	oper = rcu_dereference(q->oper_sched);
1349	if (oper)
1350	taprio_update_queue_max_sdu(q, sched: oper, stab);
1351
1352	admin = rcu_dereference(q->admin_sched);
1353	if (admin)
1354	taprio_update_queue_max_sdu(q, sched: admin, stab);
1355	rcu_read_unlock();
1356
1357	break;
1358	}
1359
1360	return NOTIFY_DONE;
1361	}
1362
1363	static void setup_txtime(struct taprio_sched *q,
1364	struct sched_gate_list *sched, ktime_t base)
1365	{
1366	struct sched_entry *entry;
1367	u64 interval = 0;
1368
1369	list_for_each_entry(entry, &sched->entries, list) {
1370	entry->next_txtime = ktime_add_ns(base, interval);
1371	interval += entry->interval;
1372	}
1373	}
1374
1375	static struct tc_taprio_qopt_offload *taprio_offload_alloc(int num_entries)
1376	{
1377	struct __tc_taprio_qopt_offload *__offload;
1378
1379	__offload = kzalloc(struct_size(__offload, offload.entries, num_entries),
1380	GFP_KERNEL);
1381	if (!__offload)
1382	return NULL;
1383
1384	refcount_set(r: &__offload->users, n: 1);
1385
1386	return &__offload->offload;
1387	}
1388
1389	struct tc_taprio_qopt_offload *taprio_offload_get(struct tc_taprio_qopt_offload
1390	*offload)
1391	{
1392	struct __tc_taprio_qopt_offload *__offload;
1393
1394	__offload = container_of(offload, struct __tc_taprio_qopt_offload,
1395	offload);
1396
1397	refcount_inc(r: &__offload->users);
1398
1399	return offload;
1400	}
1401	EXPORT_SYMBOL_GPL(taprio_offload_get);
1402
1403	void taprio_offload_free(struct tc_taprio_qopt_offload *offload)
1404	{
1405	struct __tc_taprio_qopt_offload *__offload;
1406
1407	__offload = container_of(offload, struct __tc_taprio_qopt_offload,
1408	offload);
1409
1410	if (!refcount_dec_and_test(r: &__offload->users))
1411	return;
1412
1413	kfree(objp: __offload);
1414	}
1415	EXPORT_SYMBOL_GPL(taprio_offload_free);
1416
1417	/* The function will only serve to keep the pointers to the "oper" and "admin"
1418	* schedules valid in relation to their base times, so when calling dump() the
1419	* users looks at the right schedules.
1420	* When using full offload, the admin configuration is promoted to oper at the
1421	* base_time in the PHC time domain. But because the system time is not
1422	* necessarily in sync with that, we can't just trigger a hrtimer to call
1423	* switch_schedules at the right hardware time.
1424	* At the moment we call this by hand right away from taprio, but in the future
1425	* it will be useful to create a mechanism for drivers to notify taprio of the
1426	* offload state (PENDING, ACTIVE, INACTIVE) so it can be visible in dump().
1427	* This is left as TODO.
1428	*/
1429	static void taprio_offload_config_changed(struct taprio_sched *q)
1430	{
1431	struct sched_gate_list *oper, *admin;
1432
1433	oper = rtnl_dereference(q->oper_sched);
1434	admin = rtnl_dereference(q->admin_sched);
1435
1436	switch_schedules(q, admin: &admin, oper: &oper);
1437	}
1438
1439	static u32 tc_map_to_queue_mask(struct net_device *dev, u32 tc_mask)
1440	{
1441	u32 i, queue_mask = 0;
1442
1443	for (i = 0; i < dev->num_tc; i++) {
1444	u32 offset, count;
1445
1446	if (!(tc_mask & BIT(i)))
1447	continue;
1448
1449	offset = dev->tc_to_txq[i].offset;
1450	count = dev->tc_to_txq[i].count;
1451
1452	queue_mask |= GENMASK(offset + count - 1, offset);
1453	}
1454
1455	return queue_mask;
1456	}
1457
1458	static void taprio_sched_to_offload(struct net_device *dev,
1459	struct sched_gate_list *sched,
1460	struct tc_taprio_qopt_offload *offload,
1461	const struct tc_taprio_caps *caps)
1462	{
1463	struct sched_entry *entry;
1464	int i = 0;
1465
1466	offload->base_time = sched->base_time;
1467	offload->cycle_time = sched->cycle_time;
1468	offload->cycle_time_extension = sched->cycle_time_extension;
1469
1470	list_for_each_entry(entry, &sched->entries, list) {
1471	struct tc_taprio_sched_entry *e = &offload->entries[i];
1472
1473	e->command = entry->command;
1474	e->interval = entry->interval;
1475	if (caps->gate_mask_per_txq)
1476	e->gate_mask = tc_map_to_queue_mask(dev,
1477	tc_mask: entry->gate_mask);
1478	else
1479	e->gate_mask = entry->gate_mask;
1480
1481	i++;
1482	}
1483
1484	offload->num_entries = i;
1485	}
1486
1487	static void taprio_detect_broken_mqprio(struct taprio_sched *q)
1488	{
1489	struct net_device *dev = qdisc_dev(qdisc: q->root);
1490	struct tc_taprio_caps caps;
1491
1492	qdisc_offload_query_caps(dev, type: TC_SETUP_QDISC_TAPRIO,
1493	caps: &caps, caps_len: sizeof(caps));
1494
1495	q->broken_mqprio = caps.broken_mqprio;
1496	if (q->broken_mqprio)
1497	static_branch_inc(&taprio_have_broken_mqprio);
1498	else
1499	static_branch_inc(&taprio_have_working_mqprio);
1500
1501	q->detected_mqprio = true;
1502	}
1503
1504	static void taprio_cleanup_broken_mqprio(struct taprio_sched *q)
1505	{
1506	if (!q->detected_mqprio)
1507	return;
1508
1509	if (q->broken_mqprio)
1510	static_branch_dec(&taprio_have_broken_mqprio);
1511	else
1512	static_branch_dec(&taprio_have_working_mqprio);
1513	}
1514
1515	static int taprio_enable_offload(struct net_device *dev,
1516	struct taprio_sched *q,
1517	struct sched_gate_list *sched,
1518	struct netlink_ext_ack *extack)
1519	{
1520	const struct net_device_ops *ops = dev->netdev_ops;
1521	struct tc_taprio_qopt_offload *offload;
1522	struct tc_taprio_caps caps;
1523	int tc, err = 0;
1524
1525	if (!ops->ndo_setup_tc) {
1526	NL_SET_ERR_MSG(extack,
1527	"Device does not support taprio offload");
1528	return -EOPNOTSUPP;
1529	}
1530
1531	qdisc_offload_query_caps(dev, type: TC_SETUP_QDISC_TAPRIO,
1532	caps: &caps, caps_len: sizeof(caps));
1533
1534	if (!caps.supports_queue_max_sdu) {
1535	for (tc = 0; tc < TC_MAX_QUEUE; tc++) {
1536	if (q->max_sdu[tc]) {
1537	NL_SET_ERR_MSG_MOD(extack,
1538	"Device does not handle queueMaxSDU");
1539	return -EOPNOTSUPP;
1540	}
1541	}
1542	}
1543
1544	offload = taprio_offload_alloc(num_entries: sched->num_entries);
1545	if (!offload) {
1546	NL_SET_ERR_MSG(extack,
1547	"Not enough memory for enabling offload mode");
1548	return -ENOMEM;
1549	}
1550	offload->cmd = TAPRIO_CMD_REPLACE;
1551	offload->extack = extack;
1552	mqprio_qopt_reconstruct(dev, qopt: &offload->mqprio.qopt);
1553	offload->mqprio.extack = extack;
1554	taprio_sched_to_offload(dev, sched, offload, caps: &caps);
1555	mqprio_fp_to_offload(fp: q->fp, mqprio: &offload->mqprio);
1556
1557	for (tc = 0; tc < TC_MAX_QUEUE; tc++)
1558	offload->max_sdu[tc] = q->max_sdu[tc];
1559
1560	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload);
1561	if (err < 0) {
1562	NL_SET_ERR_MSG_WEAK(extack,
1563	"Device failed to setup taprio offload");
1564	goto done;
1565	}
1566
1567	q->offloaded = true;
1568
1569	done:
1570	/* The offload structure may linger around via a reference taken by the
1571	* device driver, so clear up the netlink extack pointer so that the
1572	* driver isn't tempted to dereference data which stopped being valid
1573	*/
1574	offload->extack = NULL;
1575	offload->mqprio.extack = NULL;
1576	taprio_offload_free(offload);
1577
1578	return err;
1579	}
1580
1581	static int taprio_disable_offload(struct net_device *dev,
1582	struct taprio_sched *q,
1583	struct netlink_ext_ack *extack)
1584	{
1585	const struct net_device_ops *ops = dev->netdev_ops;
1586	struct tc_taprio_qopt_offload *offload;
1587	int err;
1588
1589	if (!q->offloaded)
1590	return 0;
1591
1592	offload = taprio_offload_alloc(num_entries: 0);
1593	if (!offload) {
1594	NL_SET_ERR_MSG(extack,
1595	"Not enough memory to disable offload mode");
1596	return -ENOMEM;
1597	}
1598	offload->cmd = TAPRIO_CMD_DESTROY;
1599
1600	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload);
1601	if (err < 0) {
1602	NL_SET_ERR_MSG(extack,
1603	"Device failed to disable offload");
1604	goto out;
1605	}
1606
1607	q->offloaded = false;
1608
1609	out:
1610	taprio_offload_free(offload);
1611
1612	return err;
1613	}
1614
1615	/* If full offload is enabled, the only possible clockid is the net device's
1616	* PHC. For that reason, specifying a clockid through netlink is incorrect.
1617	* For txtime-assist, it is implicitly assumed that the device's PHC is kept
1618	* in sync with the specified clockid via a user space daemon such as phc2sys.
1619	* For both software taprio and txtime-assist, the clockid is used for the
1620	* hrtimer that advances the schedule and hence mandatory.
1621	*/
1622	static int taprio_parse_clockid(struct Qdisc *sch, struct nlattr **tb,
1623	struct netlink_ext_ack *extack)
1624	{
1625	struct taprio_sched *q = qdisc_priv(sch);
1626	struct net_device *dev = qdisc_dev(qdisc: sch);
1627	int err = -EINVAL;
1628
1629	if (FULL_OFFLOAD_IS_ENABLED(q->flags)) {
1630	const struct ethtool_ops *ops = dev->ethtool_ops;
1631	struct kernel_ethtool_ts_info info = {
1632	.cmd = ETHTOOL_GET_TS_INFO,
1633	.phc_index = -1,
1634	};
1635
1636	if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) {
1637	NL_SET_ERR_MSG(extack,
1638	"The 'clockid' cannot be specified for full offload");
1639	goto out;
1640	}
1641
1642	if (ops && ops->get_ts_info)
1643	err = ops->get_ts_info(dev, &info);
1644
1645	if (err || info.phc_index < 0) {
1646	NL_SET_ERR_MSG(extack,
1647	"Device does not have a PTP clock");
1648	err = -ENOTSUPP;
1649	goto out;
1650	}
1651	} else if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) {
1652	int clockid = nla_get_s32(nla: tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]);
1653	enum tk_offsets tk_offset;
1654
1655	/* We only support static clockids and we don't allow
1656	* for it to be modified after the first init.
1657	*/
1658	if (clockid < 0 ||
1659	(q->clockid != -1 && q->clockid != clockid)) {
1660	NL_SET_ERR_MSG(extack,
1661	"Changing the 'clockid' of a running schedule is not supported");
1662	err = -ENOTSUPP;
1663	goto out;
1664	}
1665
1666	switch (clockid) {
1667	case CLOCK_REALTIME:
1668	tk_offset = TK_OFFS_REAL;
1669	break;
1670	case CLOCK_MONOTONIC:
1671	tk_offset = TK_OFFS_MAX;
1672	break;
1673	case CLOCK_BOOTTIME:
1674	tk_offset = TK_OFFS_BOOT;
1675	break;
1676	case CLOCK_TAI:
1677	tk_offset = TK_OFFS_TAI;
1678	break;
1679	default:
1680	NL_SET_ERR_MSG(extack, "Invalid 'clockid'");
1681	err = -EINVAL;
1682	goto out;
1683	}
1684	/* This pairs with READ_ONCE() in taprio_mono_to_any */
1685	WRITE_ONCE(q->tk_offset, tk_offset);
1686
1687	q->clockid = clockid;
1688	} else {
1689	NL_SET_ERR_MSG(extack, "Specifying a 'clockid' is mandatory");
1690	goto out;
1691	}
1692
1693	/* Everything went ok, return success. */
1694	err = 0;
1695
1696	out:
1697	return err;
1698	}
1699
1700	static int taprio_parse_tc_entry(struct Qdisc *sch,
1701	struct nlattr *opt,
1702	u32 max_sdu[TC_QOPT_MAX_QUEUE],
1703	u32 fp[TC_QOPT_MAX_QUEUE],
1704	unsigned long *seen_tcs,
1705	struct netlink_ext_ack *extack)
1706	{
1707	struct nlattr *tb[TCA_TAPRIO_TC_ENTRY_MAX + 1] = { };
1708	struct net_device *dev = qdisc_dev(qdisc: sch);
1709	int err, tc;
1710	u32 val;
1711
1712	err = nla_parse_nested(tb, maxtype: TCA_TAPRIO_TC_ENTRY_MAX, nla: opt,
1713	policy: taprio_tc_policy, extack);
1714	if (err < 0)
1715	return err;
1716
1717	if (NL_REQ_ATTR_CHECK(extack, opt, tb, TCA_TAPRIO_TC_ENTRY_INDEX)) {
1718	NL_SET_ERR_MSG_MOD(extack, "TC entry index missing");
1719	return -EINVAL;
1720	}
1721
1722	tc = nla_get_u32(nla: tb[TCA_TAPRIO_TC_ENTRY_INDEX]);
1723	if (*seen_tcs & BIT(tc)) {
1724	NL_SET_ERR_MSG_ATTR(extack, tb[TCA_TAPRIO_TC_ENTRY_INDEX],
1725	"Duplicate tc entry");
1726	return -EINVAL;
1727	}
1728
1729	*seen_tcs |= BIT(tc);
1730
1731	if (tb[TCA_TAPRIO_TC_ENTRY_MAX_SDU]) {
1732	val = nla_get_u32(nla: tb[TCA_TAPRIO_TC_ENTRY_MAX_SDU]);
1733	if (val > dev->max_mtu) {
1734	NL_SET_ERR_MSG_MOD(extack, "TC max SDU exceeds device max MTU");
1735	return -ERANGE;
1736	}
1737
1738	max_sdu[tc] = val;
1739	}
1740
1741	if (tb[TCA_TAPRIO_TC_ENTRY_FP])
1742	fp[tc] = nla_get_u32(nla: tb[TCA_TAPRIO_TC_ENTRY_FP]);
1743
1744	return 0;
1745	}
1746
1747	static int taprio_parse_tc_entries(struct Qdisc *sch,
1748	struct nlattr *opt,
1749	struct netlink_ext_ack *extack)
1750	{
1751	struct taprio_sched *q = qdisc_priv(sch);
1752	struct net_device *dev = qdisc_dev(qdisc: sch);
1753	u32 max_sdu[TC_QOPT_MAX_QUEUE];
1754	bool have_preemption = false;
1755	unsigned long seen_tcs = 0;
1756	u32 fp[TC_QOPT_MAX_QUEUE];
1757	struct nlattr *n;
1758	int tc, rem;
1759	int err = 0;
1760
1761	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) {
1762	max_sdu[tc] = q->max_sdu[tc];
1763	fp[tc] = q->fp[tc];
1764	}
1765
1766	nla_for_each_nested_type(n, TCA_TAPRIO_ATTR_TC_ENTRY, opt, rem) {
1767	err = taprio_parse_tc_entry(sch, opt: n, max_sdu, fp, seen_tcs: &seen_tcs,
1768	extack);
1769	if (err)
1770	return err;
1771	}
1772
1773	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) {
1774	q->max_sdu[tc] = max_sdu[tc];
1775	q->fp[tc] = fp[tc];
1776	if (fp[tc] != TC_FP_EXPRESS)
1777	have_preemption = true;
1778	}
1779
1780	if (have_preemption) {
1781	if (!FULL_OFFLOAD_IS_ENABLED(q->flags)) {
1782	NL_SET_ERR_MSG(extack,
1783	"Preemption only supported with full offload");
1784	return -EOPNOTSUPP;
1785	}
1786
1787	if (!ethtool_dev_mm_supported(dev)) {
1788	NL_SET_ERR_MSG(extack,
1789	"Device does not support preemption");
1790	return -EOPNOTSUPP;
1791	}
1792	}
1793
1794	return err;
1795	}
1796
1797	static int taprio_mqprio_cmp(const struct net_device *dev,
1798	const struct tc_mqprio_qopt *mqprio)
1799	{
1800	int i;
1801
1802	if (!mqprio || mqprio->num_tc != dev->num_tc)
1803	return -1;
1804
1805	for (i = 0; i < mqprio->num_tc; i++)
1806	if (dev->tc_to_txq[i].count != mqprio->count[i] ||
1807	dev->tc_to_txq[i].offset != mqprio->offset[i])
1808	return -1;
1809
1810	for (i = 0; i <= TC_BITMASK; i++)
1811	if (dev->prio_tc_map[i] != mqprio->prio_tc_map[i])
1812	return -1;
1813
1814	return 0;
1815	}
1816
1817	static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
1818	struct netlink_ext_ack *extack)
1819	{
1820	struct qdisc_size_table *stab = rtnl_dereference(sch->stab);
1821	struct nlattr *tb[TCA_TAPRIO_ATTR_MAX + 1] = { };
1822	struct sched_gate_list *oper, *admin, *new_admin;
1823	struct taprio_sched *q = qdisc_priv(sch);
1824	struct net_device *dev = qdisc_dev(qdisc: sch);
1825	struct tc_mqprio_qopt *mqprio = NULL;
1826	unsigned long flags;
1827	u32 taprio_flags;
1828	ktime_t start;
1829	int i, err;
1830
1831	err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_ATTR_MAX, nla: opt,
1832	policy: taprio_policy, extack);
1833	if (err < 0)
1834	return err;
1835
1836	if (tb[TCA_TAPRIO_ATTR_PRIOMAP])
1837	mqprio = nla_data(nla: tb[TCA_TAPRIO_ATTR_PRIOMAP]);
1838
1839	/* The semantics of the 'flags' argument in relation to 'change()'
1840	* requests, are interpreted following two rules (which are applied in
1841	* this order): (1) an omitted 'flags' argument is interpreted as
1842	* zero; (2) the 'flags' of a "running" taprio instance cannot be
1843	* changed.
1844	*/
1845	taprio_flags = nla_get_u32_default(nla: tb[TCA_TAPRIO_ATTR_FLAGS], defvalue: 0);
1846
1847	/* txtime-assist and full offload are mutually exclusive */
1848	if ((taprio_flags & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST) &&
1849	(taprio_flags & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)) {
1850	NL_SET_ERR_MSG_ATTR(extack, tb[TCA_TAPRIO_ATTR_FLAGS],
1851	"TXTIME_ASSIST and FULL_OFFLOAD are mutually exclusive");
1852	return -EINVAL;
1853	}
1854
1855	if (q->flags != TAPRIO_FLAGS_INVALID && q->flags != taprio_flags) {
1856	NL_SET_ERR_MSG_MOD(extack,
1857	"Changing 'flags' of a running schedule is not supported");
1858	return -EOPNOTSUPP;
1859	}
1860	q->flags = taprio_flags;
1861
1862	/* Needed for length_to_duration() during netlink attribute parsing */
1863	taprio_set_picos_per_byte(dev, q, extack);
1864
1865	err = taprio_parse_mqprio_opt(dev, qopt: mqprio, extack, taprio_flags: q->flags);
1866	if (err < 0)
1867	return err;
1868
1869	err = taprio_parse_tc_entries(sch, opt, extack);
1870	if (err)
1871	return err;
1872
1873	new_admin = kzalloc(sizeof(*new_admin), GFP_KERNEL);
1874	if (!new_admin) {
1875	NL_SET_ERR_MSG(extack, "Not enough memory for a new schedule");
1876	return -ENOMEM;
1877	}
1878	INIT_LIST_HEAD(list: &new_admin->entries);
1879
1880	oper = rtnl_dereference(q->oper_sched);
1881	admin = rtnl_dereference(q->admin_sched);
1882
1883	/* no changes - no new mqprio settings */
1884	if (!taprio_mqprio_cmp(dev, mqprio))
1885	mqprio = NULL;
1886
1887	if (mqprio && (oper || admin)) {
1888	NL_SET_ERR_MSG(extack, "Changing the traffic mapping of a running schedule is not supported");
1889	err = -ENOTSUPP;
1890	goto free_sched;
1891	}
1892
1893	if (mqprio) {
1894	err = netdev_set_num_tc(dev, num_tc: mqprio->num_tc);
1895	if (err)
1896	goto free_sched;
1897	for (i = 0; i < mqprio->num_tc; i++) {
1898	netdev_set_tc_queue(dev, tc: i,
1899	count: mqprio->count[i],
1900	offset: mqprio->offset[i]);
1901	q->cur_txq[i] = mqprio->offset[i];
1902	}
1903
1904	/* Always use supplied priority mappings */
1905	for (i = 0; i <= TC_BITMASK; i++)
1906	netdev_set_prio_tc_map(dev, prio: i,
1907	tc: mqprio->prio_tc_map[i]);
1908	}
1909
1910	err = parse_taprio_schedule(q, tb, new: new_admin, extack);
1911	if (err < 0)
1912	goto free_sched;
1913
1914	if (new_admin->num_entries == 0) {
1915	NL_SET_ERR_MSG(extack, "There should be at least one entry in the schedule");
1916	err = -EINVAL;
1917	goto free_sched;
1918	}
1919
1920	err = taprio_parse_clockid(sch, tb, extack);
1921	if (err < 0)
1922	goto free_sched;
1923
1924	taprio_update_queue_max_sdu(q, sched: new_admin, stab);
1925
1926	if (FULL_OFFLOAD_IS_ENABLED(q->flags))
1927	err = taprio_enable_offload(dev, q, sched: new_admin, extack);
1928	else
1929	err = taprio_disable_offload(dev, q, extack);
1930	if (err)
1931	goto free_sched;
1932
1933	/* Protects against enqueue()/dequeue() */
1934	spin_lock_bh(lock: qdisc_lock(qdisc: sch));
1935
1936	if (tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]) {
1937	if (!TXTIME_ASSIST_IS_ENABLED(q->flags)) {
1938	NL_SET_ERR_MSG_MOD(extack, "txtime-delay can only be set when txtime-assist mode is enabled");
1939	err = -EINVAL;
1940	goto unlock;
1941	}
1942
1943	q->txtime_delay = nla_get_u32(nla: tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]);
1944	}
1945
1946	if (!TXTIME_ASSIST_IS_ENABLED(q->flags) &&
1947	!FULL_OFFLOAD_IS_ENABLED(q->flags) &&
1948	!hrtimer_active(timer: &q->advance_timer)) {
1949	hrtimer_setup(timer: &q->advance_timer, function: advance_sched, clock_id: q->clockid, mode: HRTIMER_MODE_ABS);
1950	}
1951
1952	err = taprio_get_start_time(sch, sched: new_admin, start: &start);
1953	if (err < 0) {
1954	NL_SET_ERR_MSG(extack, "Internal error: failed get start time");
1955	goto unlock;
1956	}
1957
1958	setup_txtime(q, sched: new_admin, base: start);
1959
1960	if (TXTIME_ASSIST_IS_ENABLED(q->flags)) {
1961	if (!oper) {
1962	rcu_assign_pointer(q->oper_sched, new_admin);
1963	err = 0;
1964	new_admin = NULL;
1965	goto unlock;
1966	}
1967
1968	/* Not going to race against advance_sched(), but still */
1969	admin = rcu_replace_pointer(q->admin_sched, new_admin,
1970	lockdep_rtnl_is_held());
1971	if (admin)
1972	call_rcu(head: &admin->rcu, func: taprio_free_sched_cb);
1973	} else {
1974	setup_first_end_time(q, sched: new_admin, base: start);
1975
1976	/* Protects against advance_sched() */
1977	spin_lock_irqsave(&q->current_entry_lock, flags);
1978
1979	taprio_start_sched(sch, start, new: new_admin);
1980
1981	admin = rcu_replace_pointer(q->admin_sched, new_admin,
1982	lockdep_rtnl_is_held());
1983	if (admin)
1984	call_rcu(head: &admin->rcu, func: taprio_free_sched_cb);
1985
1986	spin_unlock_irqrestore(lock: &q->current_entry_lock, flags);
1987
1988	if (FULL_OFFLOAD_IS_ENABLED(q->flags))
1989	taprio_offload_config_changed(q);
1990	}
1991
1992	new_admin = NULL;
1993	err = 0;
1994
1995	if (!stab)
1996	NL_SET_ERR_MSG_MOD(extack,
1997	"Size table not specified, frame length estimations may be inaccurate");
1998
1999	unlock:
2000	spin_unlock_bh(lock: qdisc_lock(qdisc: sch));
2001
2002	free_sched:
2003	if (new_admin)
2004	call_rcu(head: &new_admin->rcu, func: taprio_free_sched_cb);
2005
2006	return err;
2007	}
2008
2009	static void taprio_reset(struct Qdisc *sch)
2010	{
2011	struct taprio_sched *q = qdisc_priv(sch);
2012	struct net_device *dev = qdisc_dev(qdisc: sch);
2013	int i;
2014
2015	hrtimer_cancel(timer: &q->advance_timer);
2016
2017	if (q->qdiscs) {
2018	for (i = 0; i < dev->num_tx_queues; i++)
2019	if (q->qdiscs[i])
2020	qdisc_reset(qdisc: q->qdiscs[i]);
2021	}
2022	}
2023
2024	static void taprio_destroy(struct Qdisc *sch)
2025	{
2026	struct taprio_sched *q = qdisc_priv(sch);
2027	struct net_device *dev = qdisc_dev(qdisc: sch);
2028	struct sched_gate_list *oper, *admin;
2029	unsigned int i;
2030
2031	list_del(entry: &q->taprio_list);
2032
2033	/* Note that taprio_reset() might not be called if an error
2034	* happens in qdisc_create(), after taprio_init() has been called.
2035	*/
2036	hrtimer_cancel(timer: &q->advance_timer);
2037	qdisc_synchronize(q: sch);
2038
2039	taprio_disable_offload(dev, q, NULL);
2040
2041	if (q->qdiscs) {
2042	for (i = 0; i < dev->num_tx_queues; i++)
2043	qdisc_put(qdisc: q->qdiscs[i]);
2044
2045	kfree(objp: q->qdiscs);
2046	}
2047	q->qdiscs = NULL;
2048
2049	netdev_reset_tc(dev);
2050
2051	oper = rtnl_dereference(q->oper_sched);
2052	admin = rtnl_dereference(q->admin_sched);
2053
2054	if (oper)
2055	call_rcu(head: &oper->rcu, func: taprio_free_sched_cb);
2056
2057	if (admin)
2058	call_rcu(head: &admin->rcu, func: taprio_free_sched_cb);
2059
2060	taprio_cleanup_broken_mqprio(q);
2061	}
2062
2063	static int taprio_init(struct Qdisc *sch, struct nlattr *opt,
2064	struct netlink_ext_ack *extack)
2065	{
2066	struct taprio_sched *q = qdisc_priv(sch);
2067	struct net_device *dev = qdisc_dev(qdisc: sch);
2068	int i, tc;
2069
2070	spin_lock_init(&q->current_entry_lock);
2071
2072	hrtimer_setup(timer: &q->advance_timer, function: advance_sched, CLOCK_TAI, mode: HRTIMER_MODE_ABS);
2073
2074	q->root = sch;
2075
2076	/* We only support static clockids. Use an invalid value as default
2077	* and get the valid one on taprio_change().
2078	*/
2079	q->clockid = -1;
2080	q->flags = TAPRIO_FLAGS_INVALID;
2081
2082	list_add(new: &q->taprio_list, head: &taprio_list);
2083
2084	if (sch->parent != TC_H_ROOT) {
2085	NL_SET_ERR_MSG_MOD(extack, "Can only be attached as root qdisc");
2086	return -EOPNOTSUPP;
2087	}
2088
2089	if (!netif_is_multiqueue(dev)) {
2090	NL_SET_ERR_MSG_MOD(extack, "Multi-queue device is required");
2091	return -EOPNOTSUPP;
2092	}
2093
2094	q->qdiscs = kcalloc(dev->num_tx_queues, sizeof(q->qdiscs[0]),
2095	GFP_KERNEL);
2096	if (!q->qdiscs)
2097	return -ENOMEM;
2098
2099	if (!opt)
2100	return -EINVAL;
2101
2102	for (i = 0; i < dev->num_tx_queues; i++) {
2103	struct netdev_queue *dev_queue;
2104	struct Qdisc *qdisc;
2105
2106	dev_queue = netdev_get_tx_queue(dev, index: i);
2107	qdisc = qdisc_create_dflt(dev_queue,
2108	ops: &pfifo_qdisc_ops,
2109	TC_H_MAKE(TC_H_MAJ(sch->handle),
2110	TC_H_MIN(i + 1)),
2111	extack);
2112	if (!qdisc)
2113	return -ENOMEM;
2114
2115	if (i < dev->real_num_tx_queues)
2116	qdisc_hash_add(q: qdisc, invisible: false);
2117
2118	q->qdiscs[i] = qdisc;
2119	}
2120
2121	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++)
2122	q->fp[tc] = TC_FP_EXPRESS;
2123
2124	taprio_detect_broken_mqprio(q);
2125
2126	return taprio_change(sch, opt, extack);
2127	}
2128
2129	static void taprio_attach(struct Qdisc *sch)
2130	{
2131	struct taprio_sched *q = qdisc_priv(sch);
2132	struct net_device *dev = qdisc_dev(qdisc: sch);
2133	unsigned int ntx;
2134
2135	/* Attach underlying qdisc */
2136	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
2137	struct netdev_queue *dev_queue = netdev_get_tx_queue(dev, index: ntx);
2138	struct Qdisc *old, *dev_queue_qdisc;
2139
2140	if (FULL_OFFLOAD_IS_ENABLED(q->flags)) {
2141	struct Qdisc *qdisc = q->qdiscs[ntx];
2142
2143	/* In offload mode, the root taprio qdisc is bypassed
2144	* and the netdev TX queues see the children directly
2145	*/
2146	qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
2147	dev_queue_qdisc = qdisc;
2148	} else {
2149	/* In software mode, attach the root taprio qdisc
2150	* to all netdev TX queues, so that dev_qdisc_enqueue()
2151	* goes through taprio_enqueue().
2152	*/
2153	dev_queue_qdisc = sch;
2154	}
2155	old = dev_graft_qdisc(dev_queue, qdisc: dev_queue_qdisc);
2156	/* The qdisc's refcount requires to be elevated once
2157	* for each netdev TX queue it is grafted onto
2158	*/
2159	qdisc_refcount_inc(qdisc: dev_queue_qdisc);
2160	if (old)
2161	qdisc_put(qdisc: old);
2162	}
2163	}
2164
2165	static struct netdev_queue *taprio_queue_get(struct Qdisc *sch,
2166	unsigned long cl)
2167	{
2168	struct net_device *dev = qdisc_dev(qdisc: sch);
2169	unsigned long ntx = cl - 1;
2170
2171	if (ntx >= dev->num_tx_queues)
2172	return NULL;
2173
2174	return netdev_get_tx_queue(dev, index: ntx);
2175	}
2176
2177	static int taprio_graft(struct Qdisc *sch, unsigned long cl,
2178	struct Qdisc *new, struct Qdisc **old,
2179	struct netlink_ext_ack *extack)
2180	{
2181	struct taprio_sched *q = qdisc_priv(sch);
2182	struct net_device *dev = qdisc_dev(qdisc: sch);
2183	struct netdev_queue *dev_queue = taprio_queue_get(sch, cl);
2184
2185	if (!dev_queue)
2186	return -EINVAL;
2187
2188	if (dev->flags & IFF_UP)
2189	dev_deactivate(dev);
2190
2191	/* In offload mode, the child Qdisc is directly attached to the netdev
2192	* TX queue, and thus, we need to keep its refcount elevated in order
2193	* to counteract qdisc_graft()'s call to qdisc_put() once per TX queue.
2194	* However, save the reference to the new qdisc in the private array in
2195	* both software and offload cases, to have an up-to-date reference to
2196	* our children.
2197	*/
2198	*old = q->qdiscs[cl - 1];
2199	if (FULL_OFFLOAD_IS_ENABLED(q->flags)) {
2200	WARN_ON_ONCE(dev_graft_qdisc(dev_queue, new) != *old);
2201	if (new)
2202	qdisc_refcount_inc(qdisc: new);
2203	if (*old)
2204	qdisc_put(qdisc: *old);
2205	}
2206
2207	q->qdiscs[cl - 1] = new;
2208	if (new)
2209	new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
2210
2211	if (dev->flags & IFF_UP)
2212	dev_activate(dev);
2213
2214	return 0;
2215	}
2216
2217	static int dump_entry(struct sk_buff *msg,
2218	const struct sched_entry *entry)
2219	{
2220	struct nlattr *item;
2221
2222	item = nla_nest_start_noflag(skb: msg, attrtype: TCA_TAPRIO_SCHED_ENTRY);
2223	if (!item)
2224	return -ENOSPC;
2225
2226	if (nla_put_u32(skb: msg, attrtype: TCA_TAPRIO_SCHED_ENTRY_INDEX, value: entry->index))
2227	goto nla_put_failure;
2228
2229	if (nla_put_u8(skb: msg, attrtype: TCA_TAPRIO_SCHED_ENTRY_CMD, value: entry->command))
2230	goto nla_put_failure;
2231
2232	if (nla_put_u32(skb: msg, attrtype: TCA_TAPRIO_SCHED_ENTRY_GATE_MASK,
2233	value: entry->gate_mask))
2234	goto nla_put_failure;
2235
2236	if (nla_put_u32(skb: msg, attrtype: TCA_TAPRIO_SCHED_ENTRY_INTERVAL,
2237	value: entry->interval))
2238	goto nla_put_failure;
2239
2240	return nla_nest_end(skb: msg, start: item);
2241
2242	nla_put_failure:
2243	nla_nest_cancel(skb: msg, start: item);
2244	return -1;
2245	}
2246
2247	static int dump_schedule(struct sk_buff *msg,
2248	const struct sched_gate_list *root)
2249	{
2250	struct nlattr *entry_list;
2251	struct sched_entry *entry;
2252
2253	if (nla_put_s64(skb: msg, attrtype: TCA_TAPRIO_ATTR_SCHED_BASE_TIME,
2254	value: root->base_time, padattr: TCA_TAPRIO_PAD))
2255	return -1;
2256
2257	if (nla_put_s64(skb: msg, attrtype: TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME,
2258	value: root->cycle_time, padattr: TCA_TAPRIO_PAD))
2259	return -1;
2260
2261	if (nla_put_s64(skb: msg, attrtype: TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION,
2262	value: root->cycle_time_extension, padattr: TCA_TAPRIO_PAD))
2263	return -1;
2264
2265	entry_list = nla_nest_start_noflag(skb: msg,
2266	attrtype: TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST);
2267	if (!entry_list)
2268	goto error_nest;
2269
2270	list_for_each_entry(entry, &root->entries, list) {
2271	if (dump_entry(msg, entry) < 0)
2272	goto error_nest;
2273	}
2274
2275	nla_nest_end(skb: msg, start: entry_list);
2276	return 0;
2277
2278	error_nest:
2279	nla_nest_cancel(skb: msg, start: entry_list);
2280	return -1;
2281	}
2282
2283	static int taprio_dump_tc_entries(struct sk_buff *skb,
2284	struct taprio_sched *q,
2285	struct sched_gate_list *sched)
2286	{
2287	struct nlattr *n;
2288	int tc;
2289
2290	for (tc = 0; tc < TC_MAX_QUEUE; tc++) {
2291	n = nla_nest_start(skb, attrtype: TCA_TAPRIO_ATTR_TC_ENTRY);
2292	if (!n)
2293	return -EMSGSIZE;
2294
2295	if (nla_put_u32(skb, attrtype: TCA_TAPRIO_TC_ENTRY_INDEX, value: tc))
2296	goto nla_put_failure;
2297
2298	if (nla_put_u32(skb, attrtype: TCA_TAPRIO_TC_ENTRY_MAX_SDU,
2299	value: sched->max_sdu[tc]))
2300	goto nla_put_failure;
2301
2302	if (nla_put_u32(skb, attrtype: TCA_TAPRIO_TC_ENTRY_FP, value: q->fp[tc]))
2303	goto nla_put_failure;
2304
2305	nla_nest_end(skb, start: n);
2306	}
2307
2308	return 0;
2309
2310	nla_put_failure:
2311	nla_nest_cancel(skb, start: n);
2312	return -EMSGSIZE;
2313	}
2314
2315	static int taprio_put_stat(struct sk_buff *skb, u64 val, u16 attrtype)
2316	{
2317	if (val == TAPRIO_STAT_NOT_SET)
2318	return 0;
2319	if (nla_put_u64_64bit(skb, attrtype, value: val, padattr: TCA_TAPRIO_OFFLOAD_STATS_PAD))
2320	return -EMSGSIZE;
2321	return 0;
2322	}
2323
2324	static int taprio_dump_xstats(struct Qdisc *sch, struct gnet_dump *d,
2325	struct tc_taprio_qopt_offload *offload,
2326	struct tc_taprio_qopt_stats *stats)
2327	{
2328	struct net_device *dev = qdisc_dev(qdisc: sch);
2329	const struct net_device_ops *ops;
2330	struct sk_buff *skb = d->skb;
2331	struct nlattr *xstats;
2332	int err;
2333
2334	ops = qdisc_dev(qdisc: sch)->netdev_ops;
2335
2336	/* FIXME I could use qdisc_offload_dump_helper(), but that messes
2337	* with sch->flags depending on whether the device reports taprio
2338	* stats, and I'm not sure whether that's a good idea, considering
2339	* that stats are optional to the offload itself
2340	*/
2341	if (!ops->ndo_setup_tc)
2342	return 0;
2343
2344	memset(stats, 0xff, sizeof(*stats));
2345
2346	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload);
2347	if (err == -EOPNOTSUPP)
2348	return 0;
2349	if (err)
2350	return err;
2351
2352	xstats = nla_nest_start(skb, attrtype: TCA_STATS_APP);
2353	if (!xstats)
2354	goto err;
2355
2356	if (taprio_put_stat(skb, val: stats->window_drops,
2357	attrtype: TCA_TAPRIO_OFFLOAD_STATS_WINDOW_DROPS) ||
2358	taprio_put_stat(skb, val: stats->tx_overruns,
2359	attrtype: TCA_TAPRIO_OFFLOAD_STATS_TX_OVERRUNS))
2360	goto err_cancel;
2361
2362	nla_nest_end(skb, start: xstats);
2363
2364	return 0;
2365
2366	err_cancel:
2367	nla_nest_cancel(skb, start: xstats);
2368	err:
2369	return -EMSGSIZE;
2370	}
2371
2372	static int taprio_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
2373	{
2374	struct tc_taprio_qopt_offload offload = {
2375	.cmd = TAPRIO_CMD_STATS,
2376	};
2377
2378	return taprio_dump_xstats(sch, d, offload: &offload, stats: &offload.stats);
2379	}
2380
2381	static int taprio_dump(struct Qdisc *sch, struct sk_buff *skb)
2382	{
2383	struct taprio_sched *q = qdisc_priv(sch);
2384	struct net_device *dev = qdisc_dev(qdisc: sch);
2385	struct sched_gate_list *oper, *admin;
2386	struct tc_mqprio_qopt opt = { 0 };
2387	struct nlattr *nest, *sched_nest;
2388
2389	mqprio_qopt_reconstruct(dev, qopt: &opt);
2390
2391	nest = nla_nest_start_noflag(skb, attrtype: TCA_OPTIONS);
2392	if (!nest)
2393	goto start_error;
2394
2395	if (nla_put(skb, attrtype: TCA_TAPRIO_ATTR_PRIOMAP, attrlen: sizeof(opt), data: &opt))
2396	goto options_error;
2397
2398	if (!FULL_OFFLOAD_IS_ENABLED(q->flags) &&
2399	nla_put_s32(skb, attrtype: TCA_TAPRIO_ATTR_SCHED_CLOCKID, value: q->clockid))
2400	goto options_error;
2401
2402	if (q->flags && nla_put_u32(skb, attrtype: TCA_TAPRIO_ATTR_FLAGS, value: q->flags))
2403	goto options_error;
2404
2405	if (q->txtime_delay &&
2406	nla_put_u32(skb, attrtype: TCA_TAPRIO_ATTR_TXTIME_DELAY, value: q->txtime_delay))
2407	goto options_error;
2408
2409	rcu_read_lock();
2410
2411	oper = rtnl_dereference(q->oper_sched);
2412	admin = rtnl_dereference(q->admin_sched);
2413
2414	if (oper && taprio_dump_tc_entries(skb, q, sched: oper))
2415	goto options_error_rcu;
2416
2417	if (oper && dump_schedule(msg: skb, root: oper))
2418	goto options_error_rcu;
2419
2420	if (!admin)
2421	goto done;
2422
2423	sched_nest = nla_nest_start_noflag(skb, attrtype: TCA_TAPRIO_ATTR_ADMIN_SCHED);
2424	if (!sched_nest)
2425	goto options_error_rcu;
2426
2427	if (dump_schedule(msg: skb, root: admin))
2428	goto admin_error;
2429
2430	nla_nest_end(skb, start: sched_nest);
2431
2432	done:
2433	rcu_read_unlock();
2434	return nla_nest_end(skb, start: nest);
2435
2436	admin_error:
2437	nla_nest_cancel(skb, start: sched_nest);
2438
2439	options_error_rcu:
2440	rcu_read_unlock();
2441
2442	options_error:
2443	nla_nest_cancel(skb, start: nest);
2444
2445	start_error:
2446	return -ENOSPC;
2447	}
2448
2449	static struct Qdisc *taprio_leaf(struct Qdisc *sch, unsigned long cl)
2450	{
2451	struct taprio_sched *q = qdisc_priv(sch);
2452	struct net_device *dev = qdisc_dev(qdisc: sch);
2453	unsigned int ntx = cl - 1;
2454
2455	if (ntx >= dev->num_tx_queues)
2456	return NULL;
2457
2458	return q->qdiscs[ntx];
2459	}
2460
2461	static unsigned long taprio_find(struct Qdisc *sch, u32 classid)
2462	{
2463	unsigned int ntx = TC_H_MIN(classid);
2464
2465	if (!taprio_queue_get(sch, cl: ntx))
2466	return 0;
2467	return ntx;
2468	}
2469
2470	static int taprio_dump_class(struct Qdisc *sch, unsigned long cl,
2471	struct sk_buff *skb, struct tcmsg *tcm)
2472	{
2473	struct Qdisc *child = taprio_leaf(sch, cl);
2474
2475	tcm->tcm_parent = TC_H_ROOT;
2476	tcm->tcm_handle |= TC_H_MIN(cl);
2477	tcm->tcm_info = child->handle;
2478
2479	return 0;
2480	}
2481
2482	static int taprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
2483	struct gnet_dump *d)
2484	__releases(d->lock)
2485	__acquires(d->lock)
2486	{
2487	struct Qdisc *child = taprio_leaf(sch, cl);
2488	struct tc_taprio_qopt_offload offload = {
2489	.cmd = TAPRIO_CMD_QUEUE_STATS,
2490	.queue_stats = {
2491	.queue = cl - 1,
2492	},
2493	};
2494
2495	if (gnet_stats_copy_basic(d, NULL, b: &child->bstats, running: true) < 0 ||
2496	qdisc_qstats_copy(d, sch: child) < 0)
2497	return -1;
2498
2499	return taprio_dump_xstats(sch, d, offload: &offload, stats: &offload.queue_stats.stats);
2500	}
2501
2502	static void taprio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
2503	{
2504	struct net_device *dev = qdisc_dev(qdisc: sch);
2505	unsigned long ntx;
2506
2507	if (arg->stop)
2508	return;
2509
2510	arg->count = arg->skip;
2511	for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) {
2512	if (!tc_qdisc_stats_dump(sch, cl: ntx + 1, arg))
2513	break;
2514	}
2515	}
2516
2517	static struct netdev_queue *taprio_select_queue(struct Qdisc *sch,
2518	struct tcmsg *tcm)
2519	{
2520	return taprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
2521	}
2522
2523	static const struct Qdisc_class_ops taprio_class_ops = {
2524	.graft = taprio_graft,
2525	.leaf = taprio_leaf,
2526	.find = taprio_find,
2527	.walk = taprio_walk,
2528	.dump = taprio_dump_class,
2529	.dump_stats = taprio_dump_class_stats,
2530	.select_queue = taprio_select_queue,
2531	};
2532
2533	static struct Qdisc_ops taprio_qdisc_ops __read_mostly = {
2534	.cl_ops = &taprio_class_ops,
2535	.id = "taprio",
2536	.priv_size = sizeof(struct taprio_sched),
2537	.init = taprio_init,
2538	.change = taprio_change,
2539	.destroy = taprio_destroy,
2540	.reset = taprio_reset,
2541	.attach = taprio_attach,
2542	.peek = taprio_peek,
2543	.dequeue = taprio_dequeue,
2544	.enqueue = taprio_enqueue,
2545	.dump = taprio_dump,
2546	.dump_stats = taprio_dump_stats,
2547	.owner = THIS_MODULE,
2548	};
2549	MODULE_ALIAS_NET_SCH("taprio");
2550
2551	static struct notifier_block taprio_device_notifier = {
2552	.notifier_call = taprio_dev_notifier,
2553	};
2554
2555	static int __init taprio_module_init(void)
2556	{
2557	int err = register_netdevice_notifier(nb: &taprio_device_notifier);
2558
2559	if (err)
2560	return err;
2561
2562	return register_qdisc(qops: &taprio_qdisc_ops);
2563	}
2564
2565	static void __exit taprio_module_exit(void)
2566	{
2567	unregister_qdisc(qops: &taprio_qdisc_ops);
2568	unregister_netdevice_notifier(nb: &taprio_device_notifier);
2569	}
2570
2571	module_init(taprio_module_init);
2572	module_exit(taprio_module_exit);
2573	MODULE_LICENSE("GPL");
2574	MODULE_DESCRIPTION("Time Aware Priority qdisc");
2575