xref: /linux/net/sched/sch_taprio.c (revision 8795a739e5c72abeec51caf36b6df2b37e5720c5)
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/types.h>
10 #include <linux/slab.h>
11 #include <linux/kernel.h>
12 #include <linux/string.h>
13 #include <linux/list.h>
14 #include <linux/errno.h>
15 #include <linux/skbuff.h>
16 #include <linux/math64.h>
17 #include <linux/module.h>
18 #include <linux/spinlock.h>
19 #include <linux/rcupdate.h>
20 #include <net/netlink.h>
21 #include <net/pkt_sched.h>
22 #include <net/pkt_cls.h>
23 #include <net/sch_generic.h>
24 #include <net/sock.h>
25 #include <net/tcp.h>
26 
27 static LIST_HEAD(taprio_list);
28 static DEFINE_SPINLOCK(taprio_list_lock);
29 
30 #define TAPRIO_ALL_GATES_OPEN -1
31 
32 #define TXTIME_ASSIST_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST)
33 #define FULL_OFFLOAD_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)
34 
35 struct sched_entry {
36 	struct list_head list;
37 
38 	/* The instant that this entry "closes" and the next one
39 	 * should open, the qdisc will make some effort so that no
40 	 * packet leaves after this time.
41 	 */
42 	ktime_t close_time;
43 	ktime_t next_txtime;
44 	atomic_t budget;
45 	int index;
46 	u32 gate_mask;
47 	u32 interval;
48 	u8 command;
49 };
50 
51 struct sched_gate_list {
52 	struct rcu_head rcu;
53 	struct list_head entries;
54 	size_t num_entries;
55 	ktime_t cycle_close_time;
56 	s64 cycle_time;
57 	s64 cycle_time_extension;
58 	s64 base_time;
59 };
60 
61 struct taprio_sched {
62 	struct Qdisc **qdiscs;
63 	struct Qdisc *root;
64 	u32 flags;
65 	enum tk_offsets tk_offset;
66 	int clockid;
67 	atomic64_t picos_per_byte; /* Using picoseconds because for 10Gbps+
68 				    * speeds it's sub-nanoseconds per byte
69 				    */
70 
71 	/* Protects the update side of the RCU protected current_entry */
72 	spinlock_t current_entry_lock;
73 	struct sched_entry __rcu *current_entry;
74 	struct sched_gate_list __rcu *oper_sched;
75 	struct sched_gate_list __rcu *admin_sched;
76 	struct hrtimer advance_timer;
77 	struct list_head taprio_list;
78 	struct sk_buff *(*dequeue)(struct Qdisc *sch);
79 	struct sk_buff *(*peek)(struct Qdisc *sch);
80 	u32 txtime_delay;
81 };
82 
83 struct __tc_taprio_qopt_offload {
84 	refcount_t users;
85 	struct tc_taprio_qopt_offload offload;
86 };
87 
88 static ktime_t sched_base_time(const struct sched_gate_list *sched)
89 {
90 	if (!sched)
91 		return KTIME_MAX;
92 
93 	return ns_to_ktime(sched->base_time);
94 }
95 
96 static ktime_t taprio_get_time(struct taprio_sched *q)
97 {
98 	ktime_t mono = ktime_get();
99 
100 	switch (q->tk_offset) {
101 	case TK_OFFS_MAX:
102 		return mono;
103 	default:
104 		return ktime_mono_to_any(mono, q->tk_offset);
105 	}
106 
107 	return KTIME_MAX;
108 }
109 
110 static void taprio_free_sched_cb(struct rcu_head *head)
111 {
112 	struct sched_gate_list *sched = container_of(head, struct sched_gate_list, rcu);
113 	struct sched_entry *entry, *n;
114 
115 	if (!sched)
116 		return;
117 
118 	list_for_each_entry_safe(entry, n, &sched->entries, list) {
119 		list_del(&entry->list);
120 		kfree(entry);
121 	}
122 
123 	kfree(sched);
124 }
125 
126 static void switch_schedules(struct taprio_sched *q,
127 			     struct sched_gate_list **admin,
128 			     struct sched_gate_list **oper)
129 {
130 	rcu_assign_pointer(q->oper_sched, *admin);
131 	rcu_assign_pointer(q->admin_sched, NULL);
132 
133 	if (*oper)
134 		call_rcu(&(*oper)->rcu, taprio_free_sched_cb);
135 
136 	*oper = *admin;
137 	*admin = NULL;
138 }
139 
140 /* Get how much time has been already elapsed in the current cycle. */
141 static s32 get_cycle_time_elapsed(struct sched_gate_list *sched, ktime_t time)
142 {
143 	ktime_t time_since_sched_start;
144 	s32 time_elapsed;
145 
146 	time_since_sched_start = ktime_sub(time, sched->base_time);
147 	div_s64_rem(time_since_sched_start, sched->cycle_time, &time_elapsed);
148 
149 	return time_elapsed;
150 }
151 
152 static ktime_t get_interval_end_time(struct sched_gate_list *sched,
153 				     struct sched_gate_list *admin,
154 				     struct sched_entry *entry,
155 				     ktime_t intv_start)
156 {
157 	s32 cycle_elapsed = get_cycle_time_elapsed(sched, intv_start);
158 	ktime_t intv_end, cycle_ext_end, cycle_end;
159 
160 	cycle_end = ktime_add_ns(intv_start, sched->cycle_time - cycle_elapsed);
161 	intv_end = ktime_add_ns(intv_start, entry->interval);
162 	cycle_ext_end = ktime_add(cycle_end, sched->cycle_time_extension);
163 
164 	if (ktime_before(intv_end, cycle_end))
165 		return intv_end;
166 	else if (admin && admin != sched &&
167 		 ktime_after(admin->base_time, cycle_end) &&
168 		 ktime_before(admin->base_time, cycle_ext_end))
169 		return admin->base_time;
170 	else
171 		return cycle_end;
172 }
173 
174 static int length_to_duration(struct taprio_sched *q, int len)
175 {
176 	return div_u64(len * atomic64_read(&q->picos_per_byte), 1000);
177 }
178 
179 /* Returns the entry corresponding to next available interval. If
180  * validate_interval is set, it only validates whether the timestamp occurs
181  * when the gate corresponding to the skb's traffic class is open.
182  */
183 static struct sched_entry *find_entry_to_transmit(struct sk_buff *skb,
184 						  struct Qdisc *sch,
185 						  struct sched_gate_list *sched,
186 						  struct sched_gate_list *admin,
187 						  ktime_t time,
188 						  ktime_t *interval_start,
189 						  ktime_t *interval_end,
190 						  bool validate_interval)
191 {
192 	ktime_t curr_intv_start, curr_intv_end, cycle_end, packet_transmit_time;
193 	ktime_t earliest_txtime = KTIME_MAX, txtime, cycle, transmit_end_time;
194 	struct sched_entry *entry = NULL, *entry_found = NULL;
195 	struct taprio_sched *q = qdisc_priv(sch);
196 	struct net_device *dev = qdisc_dev(sch);
197 	bool entry_available = false;
198 	s32 cycle_elapsed;
199 	int tc, n;
200 
201 	tc = netdev_get_prio_tc_map(dev, skb->priority);
202 	packet_transmit_time = length_to_duration(q, qdisc_pkt_len(skb));
203 
204 	*interval_start = 0;
205 	*interval_end = 0;
206 
207 	if (!sched)
208 		return NULL;
209 
210 	cycle = sched->cycle_time;
211 	cycle_elapsed = get_cycle_time_elapsed(sched, time);
212 	curr_intv_end = ktime_sub_ns(time, cycle_elapsed);
213 	cycle_end = ktime_add_ns(curr_intv_end, cycle);
214 
215 	list_for_each_entry(entry, &sched->entries, list) {
216 		curr_intv_start = curr_intv_end;
217 		curr_intv_end = get_interval_end_time(sched, admin, entry,
218 						      curr_intv_start);
219 
220 		if (ktime_after(curr_intv_start, cycle_end))
221 			break;
222 
223 		if (!(entry->gate_mask & BIT(tc)) ||
224 		    packet_transmit_time > entry->interval)
225 			continue;
226 
227 		txtime = entry->next_txtime;
228 
229 		if (ktime_before(txtime, time) || validate_interval) {
230 			transmit_end_time = ktime_add_ns(time, packet_transmit_time);
231 			if ((ktime_before(curr_intv_start, time) &&
232 			     ktime_before(transmit_end_time, curr_intv_end)) ||
233 			    (ktime_after(curr_intv_start, time) && !validate_interval)) {
234 				entry_found = entry;
235 				*interval_start = curr_intv_start;
236 				*interval_end = curr_intv_end;
237 				break;
238 			} else if (!entry_available && !validate_interval) {
239 				/* Here, we are just trying to find out the
240 				 * first available interval in the next cycle.
241 				 */
242 				entry_available = 1;
243 				entry_found = entry;
244 				*interval_start = ktime_add_ns(curr_intv_start, cycle);
245 				*interval_end = ktime_add_ns(curr_intv_end, cycle);
246 			}
247 		} else if (ktime_before(txtime, earliest_txtime) &&
248 			   !entry_available) {
249 			earliest_txtime = txtime;
250 			entry_found = entry;
251 			n = div_s64(ktime_sub(txtime, curr_intv_start), cycle);
252 			*interval_start = ktime_add(curr_intv_start, n * cycle);
253 			*interval_end = ktime_add(curr_intv_end, n * cycle);
254 		}
255 	}
256 
257 	return entry_found;
258 }
259 
260 static bool is_valid_interval(struct sk_buff *skb, struct Qdisc *sch)
261 {
262 	struct taprio_sched *q = qdisc_priv(sch);
263 	struct sched_gate_list *sched, *admin;
264 	ktime_t interval_start, interval_end;
265 	struct sched_entry *entry;
266 
267 	rcu_read_lock();
268 	sched = rcu_dereference(q->oper_sched);
269 	admin = rcu_dereference(q->admin_sched);
270 
271 	entry = find_entry_to_transmit(skb, sch, sched, admin, skb->tstamp,
272 				       &interval_start, &interval_end, true);
273 	rcu_read_unlock();
274 
275 	return entry;
276 }
277 
278 static bool taprio_flags_valid(u32 flags)
279 {
280 	/* Make sure no other flag bits are set. */
281 	if (flags & ~(TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST |
282 		      TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD))
283 		return false;
284 	/* txtime-assist and full offload are mutually exclusive */
285 	if ((flags & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST) &&
286 	    (flags & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD))
287 		return false;
288 	return true;
289 }
290 
291 /* This returns the tstamp value set by TCP in terms of the set clock. */
292 static ktime_t get_tcp_tstamp(struct taprio_sched *q, struct sk_buff *skb)
293 {
294 	unsigned int offset = skb_network_offset(skb);
295 	const struct ipv6hdr *ipv6h;
296 	const struct iphdr *iph;
297 	struct ipv6hdr _ipv6h;
298 
299 	ipv6h = skb_header_pointer(skb, offset, sizeof(_ipv6h), &_ipv6h);
300 	if (!ipv6h)
301 		return 0;
302 
303 	if (ipv6h->version == 4) {
304 		iph = (struct iphdr *)ipv6h;
305 		offset += iph->ihl * 4;
306 
307 		/* special-case 6in4 tunnelling, as that is a common way to get
308 		 * v6 connectivity in the home
309 		 */
310 		if (iph->protocol == IPPROTO_IPV6) {
311 			ipv6h = skb_header_pointer(skb, offset,
312 						   sizeof(_ipv6h), &_ipv6h);
313 
314 			if (!ipv6h || ipv6h->nexthdr != IPPROTO_TCP)
315 				return 0;
316 		} else if (iph->protocol != IPPROTO_TCP) {
317 			return 0;
318 		}
319 	} else if (ipv6h->version == 6 && ipv6h->nexthdr != IPPROTO_TCP) {
320 		return 0;
321 	}
322 
323 	return ktime_mono_to_any(skb->skb_mstamp_ns, q->tk_offset);
324 }
325 
326 /* There are a few scenarios where we will have to modify the txtime from
327  * what is read from next_txtime in sched_entry. They are:
328  * 1. If txtime is in the past,
329  *    a. The gate for the traffic class is currently open and packet can be
330  *       transmitted before it closes, schedule the packet right away.
331  *    b. If the gate corresponding to the traffic class is going to open later
332  *       in the cycle, set the txtime of packet to the interval start.
333  * 2. If txtime is in the future, there are packets corresponding to the
334  *    current traffic class waiting to be transmitted. So, the following
335  *    possibilities exist:
336  *    a. We can transmit the packet before the window containing the txtime
337  *       closes.
338  *    b. The window might close before the transmission can be completed
339  *       successfully. So, schedule the packet in the next open window.
340  */
341 static long get_packet_txtime(struct sk_buff *skb, struct Qdisc *sch)
342 {
343 	ktime_t transmit_end_time, interval_end, interval_start, tcp_tstamp;
344 	struct taprio_sched *q = qdisc_priv(sch);
345 	struct sched_gate_list *sched, *admin;
346 	ktime_t minimum_time, now, txtime;
347 	int len, packet_transmit_time;
348 	struct sched_entry *entry;
349 	bool sched_changed;
350 
351 	now = taprio_get_time(q);
352 	minimum_time = ktime_add_ns(now, q->txtime_delay);
353 
354 	tcp_tstamp = get_tcp_tstamp(q, skb);
355 	minimum_time = max_t(ktime_t, minimum_time, tcp_tstamp);
356 
357 	rcu_read_lock();
358 	admin = rcu_dereference(q->admin_sched);
359 	sched = rcu_dereference(q->oper_sched);
360 	if (admin && ktime_after(minimum_time, admin->base_time))
361 		switch_schedules(q, &admin, &sched);
362 
363 	/* Until the schedule starts, all the queues are open */
364 	if (!sched || ktime_before(minimum_time, sched->base_time)) {
365 		txtime = minimum_time;
366 		goto done;
367 	}
368 
369 	len = qdisc_pkt_len(skb);
370 	packet_transmit_time = length_to_duration(q, len);
371 
372 	do {
373 		sched_changed = 0;
374 
375 		entry = find_entry_to_transmit(skb, sch, sched, admin,
376 					       minimum_time,
377 					       &interval_start, &interval_end,
378 					       false);
379 		if (!entry) {
380 			txtime = 0;
381 			goto done;
382 		}
383 
384 		txtime = entry->next_txtime;
385 		txtime = max_t(ktime_t, txtime, minimum_time);
386 		txtime = max_t(ktime_t, txtime, interval_start);
387 
388 		if (admin && admin != sched &&
389 		    ktime_after(txtime, admin->base_time)) {
390 			sched = admin;
391 			sched_changed = 1;
392 			continue;
393 		}
394 
395 		transmit_end_time = ktime_add(txtime, packet_transmit_time);
396 		minimum_time = transmit_end_time;
397 
398 		/* Update the txtime of current entry to the next time it's
399 		 * interval starts.
400 		 */
401 		if (ktime_after(transmit_end_time, interval_end))
402 			entry->next_txtime = ktime_add(interval_start, sched->cycle_time);
403 	} while (sched_changed || ktime_after(transmit_end_time, interval_end));
404 
405 	entry->next_txtime = transmit_end_time;
406 
407 done:
408 	rcu_read_unlock();
409 	return txtime;
410 }
411 
412 static int taprio_enqueue(struct sk_buff *skb, struct Qdisc *sch,
413 			  struct sk_buff **to_free)
414 {
415 	struct taprio_sched *q = qdisc_priv(sch);
416 	struct Qdisc *child;
417 	int queue;
418 
419 	queue = skb_get_queue_mapping(skb);
420 
421 	child = q->qdiscs[queue];
422 	if (unlikely(!child))
423 		return qdisc_drop(skb, sch, to_free);
424 
425 	if (skb->sk && sock_flag(skb->sk, SOCK_TXTIME)) {
426 		if (!is_valid_interval(skb, sch))
427 			return qdisc_drop(skb, sch, to_free);
428 	} else if (TXTIME_ASSIST_IS_ENABLED(q->flags)) {
429 		skb->tstamp = get_packet_txtime(skb, sch);
430 		if (!skb->tstamp)
431 			return qdisc_drop(skb, sch, to_free);
432 	}
433 
434 	qdisc_qstats_backlog_inc(sch, skb);
435 	sch->q.qlen++;
436 
437 	return qdisc_enqueue(skb, child, to_free);
438 }
439 
440 static struct sk_buff *taprio_peek_soft(struct Qdisc *sch)
441 {
442 	struct taprio_sched *q = qdisc_priv(sch);
443 	struct net_device *dev = qdisc_dev(sch);
444 	struct sched_entry *entry;
445 	struct sk_buff *skb;
446 	u32 gate_mask;
447 	int i;
448 
449 	rcu_read_lock();
450 	entry = rcu_dereference(q->current_entry);
451 	gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN;
452 	rcu_read_unlock();
453 
454 	if (!gate_mask)
455 		return NULL;
456 
457 	for (i = 0; i < dev->num_tx_queues; i++) {
458 		struct Qdisc *child = q->qdiscs[i];
459 		int prio;
460 		u8 tc;
461 
462 		if (unlikely(!child))
463 			continue;
464 
465 		skb = child->ops->peek(child);
466 		if (!skb)
467 			continue;
468 
469 		if (TXTIME_ASSIST_IS_ENABLED(q->flags))
470 			return skb;
471 
472 		prio = skb->priority;
473 		tc = netdev_get_prio_tc_map(dev, prio);
474 
475 		if (!(gate_mask & BIT(tc)))
476 			continue;
477 
478 		return skb;
479 	}
480 
481 	return NULL;
482 }
483 
484 static struct sk_buff *taprio_peek_offload(struct Qdisc *sch)
485 {
486 	struct taprio_sched *q = qdisc_priv(sch);
487 	struct net_device *dev = qdisc_dev(sch);
488 	struct sk_buff *skb;
489 	int i;
490 
491 	for (i = 0; i < dev->num_tx_queues; i++) {
492 		struct Qdisc *child = q->qdiscs[i];
493 
494 		if (unlikely(!child))
495 			continue;
496 
497 		skb = child->ops->peek(child);
498 		if (!skb)
499 			continue;
500 
501 		return skb;
502 	}
503 
504 	return NULL;
505 }
506 
507 static struct sk_buff *taprio_peek(struct Qdisc *sch)
508 {
509 	struct taprio_sched *q = qdisc_priv(sch);
510 
511 	return q->peek(sch);
512 }
513 
514 static void taprio_set_budget(struct taprio_sched *q, struct sched_entry *entry)
515 {
516 	atomic_set(&entry->budget,
517 		   div64_u64((u64)entry->interval * 1000,
518 			     atomic64_read(&q->picos_per_byte)));
519 }
520 
521 static struct sk_buff *taprio_dequeue_soft(struct Qdisc *sch)
522 {
523 	struct taprio_sched *q = qdisc_priv(sch);
524 	struct net_device *dev = qdisc_dev(sch);
525 	struct sk_buff *skb = NULL;
526 	struct sched_entry *entry;
527 	u32 gate_mask;
528 	int i;
529 
530 	rcu_read_lock();
531 	entry = rcu_dereference(q->current_entry);
532 	/* if there's no entry, it means that the schedule didn't
533 	 * start yet, so force all gates to be open, this is in
534 	 * accordance to IEEE 802.1Qbv-2015 Section 8.6.9.4.5
535 	 * "AdminGateSates"
536 	 */
537 	gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN;
538 
539 	if (!gate_mask)
540 		goto done;
541 
542 	for (i = 0; i < dev->num_tx_queues; i++) {
543 		struct Qdisc *child = q->qdiscs[i];
544 		ktime_t guard;
545 		int prio;
546 		int len;
547 		u8 tc;
548 
549 		if (unlikely(!child))
550 			continue;
551 
552 		if (TXTIME_ASSIST_IS_ENABLED(q->flags)) {
553 			skb = child->ops->dequeue(child);
554 			if (!skb)
555 				continue;
556 			goto skb_found;
557 		}
558 
559 		skb = child->ops->peek(child);
560 		if (!skb)
561 			continue;
562 
563 		prio = skb->priority;
564 		tc = netdev_get_prio_tc_map(dev, prio);
565 
566 		if (!(gate_mask & BIT(tc)))
567 			continue;
568 
569 		len = qdisc_pkt_len(skb);
570 		guard = ktime_add_ns(taprio_get_time(q),
571 				     length_to_duration(q, len));
572 
573 		/* In the case that there's no gate entry, there's no
574 		 * guard band ...
575 		 */
576 		if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
577 		    ktime_after(guard, entry->close_time))
578 			continue;
579 
580 		/* ... and no budget. */
581 		if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
582 		    atomic_sub_return(len, &entry->budget) < 0)
583 			continue;
584 
585 		skb = child->ops->dequeue(child);
586 		if (unlikely(!skb))
587 			goto done;
588 
589 skb_found:
590 		qdisc_bstats_update(sch, skb);
591 		qdisc_qstats_backlog_dec(sch, skb);
592 		sch->q.qlen--;
593 
594 		goto done;
595 	}
596 
597 done:
598 	rcu_read_unlock();
599 
600 	return skb;
601 }
602 
603 static struct sk_buff *taprio_dequeue_offload(struct Qdisc *sch)
604 {
605 	struct taprio_sched *q = qdisc_priv(sch);
606 	struct net_device *dev = qdisc_dev(sch);
607 	struct sk_buff *skb;
608 	int i;
609 
610 	for (i = 0; i < dev->num_tx_queues; i++) {
611 		struct Qdisc *child = q->qdiscs[i];
612 
613 		if (unlikely(!child))
614 			continue;
615 
616 		skb = child->ops->dequeue(child);
617 		if (unlikely(!skb))
618 			continue;
619 
620 		qdisc_bstats_update(sch, skb);
621 		qdisc_qstats_backlog_dec(sch, skb);
622 		sch->q.qlen--;
623 
624 		return skb;
625 	}
626 
627 	return NULL;
628 }
629 
630 static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
631 {
632 	struct taprio_sched *q = qdisc_priv(sch);
633 
634 	return q->dequeue(sch);
635 }
636 
637 static bool should_restart_cycle(const struct sched_gate_list *oper,
638 				 const struct sched_entry *entry)
639 {
640 	if (list_is_last(&entry->list, &oper->entries))
641 		return true;
642 
643 	if (ktime_compare(entry->close_time, oper->cycle_close_time) == 0)
644 		return true;
645 
646 	return false;
647 }
648 
649 static bool should_change_schedules(const struct sched_gate_list *admin,
650 				    const struct sched_gate_list *oper,
651 				    ktime_t close_time)
652 {
653 	ktime_t next_base_time, extension_time;
654 
655 	if (!admin)
656 		return false;
657 
658 	next_base_time = sched_base_time(admin);
659 
660 	/* This is the simple case, the close_time would fall after
661 	 * the next schedule base_time.
662 	 */
663 	if (ktime_compare(next_base_time, close_time) <= 0)
664 		return true;
665 
666 	/* This is the cycle_time_extension case, if the close_time
667 	 * plus the amount that can be extended would fall after the
668 	 * next schedule base_time, we can extend the current schedule
669 	 * for that amount.
670 	 */
671 	extension_time = ktime_add_ns(close_time, oper->cycle_time_extension);
672 
673 	/* FIXME: the IEEE 802.1Q-2018 Specification isn't clear about
674 	 * how precisely the extension should be made. So after
675 	 * conformance testing, this logic may change.
676 	 */
677 	if (ktime_compare(next_base_time, extension_time) <= 0)
678 		return true;
679 
680 	return false;
681 }
682 
683 static enum hrtimer_restart advance_sched(struct hrtimer *timer)
684 {
685 	struct taprio_sched *q = container_of(timer, struct taprio_sched,
686 					      advance_timer);
687 	struct sched_gate_list *oper, *admin;
688 	struct sched_entry *entry, *next;
689 	struct Qdisc *sch = q->root;
690 	ktime_t close_time;
691 
692 	spin_lock(&q->current_entry_lock);
693 	entry = rcu_dereference_protected(q->current_entry,
694 					  lockdep_is_held(&q->current_entry_lock));
695 	oper = rcu_dereference_protected(q->oper_sched,
696 					 lockdep_is_held(&q->current_entry_lock));
697 	admin = rcu_dereference_protected(q->admin_sched,
698 					  lockdep_is_held(&q->current_entry_lock));
699 
700 	if (!oper)
701 		switch_schedules(q, &admin, &oper);
702 
703 	/* This can happen in two cases: 1. this is the very first run
704 	 * of this function (i.e. we weren't running any schedule
705 	 * previously); 2. The previous schedule just ended. The first
706 	 * entry of all schedules are pre-calculated during the
707 	 * schedule initialization.
708 	 */
709 	if (unlikely(!entry || entry->close_time == oper->base_time)) {
710 		next = list_first_entry(&oper->entries, struct sched_entry,
711 					list);
712 		close_time = next->close_time;
713 		goto first_run;
714 	}
715 
716 	if (should_restart_cycle(oper, entry)) {
717 		next = list_first_entry(&oper->entries, struct sched_entry,
718 					list);
719 		oper->cycle_close_time = ktime_add_ns(oper->cycle_close_time,
720 						      oper->cycle_time);
721 	} else {
722 		next = list_next_entry(entry, list);
723 	}
724 
725 	close_time = ktime_add_ns(entry->close_time, next->interval);
726 	close_time = min_t(ktime_t, close_time, oper->cycle_close_time);
727 
728 	if (should_change_schedules(admin, oper, close_time)) {
729 		/* Set things so the next time this runs, the new
730 		 * schedule runs.
731 		 */
732 		close_time = sched_base_time(admin);
733 		switch_schedules(q, &admin, &oper);
734 	}
735 
736 	next->close_time = close_time;
737 	taprio_set_budget(q, next);
738 
739 first_run:
740 	rcu_assign_pointer(q->current_entry, next);
741 	spin_unlock(&q->current_entry_lock);
742 
743 	hrtimer_set_expires(&q->advance_timer, close_time);
744 
745 	rcu_read_lock();
746 	__netif_schedule(sch);
747 	rcu_read_unlock();
748 
749 	return HRTIMER_RESTART;
750 }
751 
752 static const struct nla_policy entry_policy[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = {
753 	[TCA_TAPRIO_SCHED_ENTRY_INDEX]	   = { .type = NLA_U32 },
754 	[TCA_TAPRIO_SCHED_ENTRY_CMD]	   = { .type = NLA_U8 },
755 	[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK] = { .type = NLA_U32 },
756 	[TCA_TAPRIO_SCHED_ENTRY_INTERVAL]  = { .type = NLA_U32 },
757 };
758 
759 static const struct nla_policy taprio_policy[TCA_TAPRIO_ATTR_MAX + 1] = {
760 	[TCA_TAPRIO_ATTR_PRIOMAP]	       = {
761 		.len = sizeof(struct tc_mqprio_qopt)
762 	},
763 	[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST]           = { .type = NLA_NESTED },
764 	[TCA_TAPRIO_ATTR_SCHED_BASE_TIME]            = { .type = NLA_S64 },
765 	[TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY]         = { .type = NLA_NESTED },
766 	[TCA_TAPRIO_ATTR_SCHED_CLOCKID]              = { .type = NLA_S32 },
767 	[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME]           = { .type = NLA_S64 },
768 	[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION] = { .type = NLA_S64 },
769 };
770 
771 static int fill_sched_entry(struct nlattr **tb, struct sched_entry *entry,
772 			    struct netlink_ext_ack *extack)
773 {
774 	u32 interval = 0;
775 
776 	if (tb[TCA_TAPRIO_SCHED_ENTRY_CMD])
777 		entry->command = nla_get_u8(
778 			tb[TCA_TAPRIO_SCHED_ENTRY_CMD]);
779 
780 	if (tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK])
781 		entry->gate_mask = nla_get_u32(
782 			tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK]);
783 
784 	if (tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL])
785 		interval = nla_get_u32(
786 			tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL]);
787 
788 	if (interval == 0) {
789 		NL_SET_ERR_MSG(extack, "Invalid interval for schedule entry");
790 		return -EINVAL;
791 	}
792 
793 	entry->interval = interval;
794 
795 	return 0;
796 }
797 
798 static int parse_sched_entry(struct nlattr *n, struct sched_entry *entry,
799 			     int index, struct netlink_ext_ack *extack)
800 {
801 	struct nlattr *tb[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = { };
802 	int err;
803 
804 	err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_SCHED_ENTRY_MAX, n,
805 					  entry_policy, NULL);
806 	if (err < 0) {
807 		NL_SET_ERR_MSG(extack, "Could not parse nested entry");
808 		return -EINVAL;
809 	}
810 
811 	entry->index = index;
812 
813 	return fill_sched_entry(tb, entry, extack);
814 }
815 
816 static int parse_sched_list(struct nlattr *list,
817 			    struct sched_gate_list *sched,
818 			    struct netlink_ext_ack *extack)
819 {
820 	struct nlattr *n;
821 	int err, rem;
822 	int i = 0;
823 
824 	if (!list)
825 		return -EINVAL;
826 
827 	nla_for_each_nested(n, list, rem) {
828 		struct sched_entry *entry;
829 
830 		if (nla_type(n) != TCA_TAPRIO_SCHED_ENTRY) {
831 			NL_SET_ERR_MSG(extack, "Attribute is not of type 'entry'");
832 			continue;
833 		}
834 
835 		entry = kzalloc(sizeof(*entry), GFP_KERNEL);
836 		if (!entry) {
837 			NL_SET_ERR_MSG(extack, "Not enough memory for entry");
838 			return -ENOMEM;
839 		}
840 
841 		err = parse_sched_entry(n, entry, i, extack);
842 		if (err < 0) {
843 			kfree(entry);
844 			return err;
845 		}
846 
847 		list_add_tail(&entry->list, &sched->entries);
848 		i++;
849 	}
850 
851 	sched->num_entries = i;
852 
853 	return i;
854 }
855 
856 static int parse_taprio_schedule(struct nlattr **tb,
857 				 struct sched_gate_list *new,
858 				 struct netlink_ext_ack *extack)
859 {
860 	int err = 0;
861 
862 	if (tb[TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY]) {
863 		NL_SET_ERR_MSG(extack, "Adding a single entry is not supported");
864 		return -ENOTSUPP;
865 	}
866 
867 	if (tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME])
868 		new->base_time = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME]);
869 
870 	if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION])
871 		new->cycle_time_extension = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION]);
872 
873 	if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME])
874 		new->cycle_time = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME]);
875 
876 	if (tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST])
877 		err = parse_sched_list(
878 			tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST], new, extack);
879 	if (err < 0)
880 		return err;
881 
882 	if (!new->cycle_time) {
883 		struct sched_entry *entry;
884 		ktime_t cycle = 0;
885 
886 		list_for_each_entry(entry, &new->entries, list)
887 			cycle = ktime_add_ns(cycle, entry->interval);
888 		new->cycle_time = cycle;
889 	}
890 
891 	return 0;
892 }
893 
894 static int taprio_parse_mqprio_opt(struct net_device *dev,
895 				   struct tc_mqprio_qopt *qopt,
896 				   struct netlink_ext_ack *extack,
897 				   u32 taprio_flags)
898 {
899 	int i, j;
900 
901 	if (!qopt && !dev->num_tc) {
902 		NL_SET_ERR_MSG(extack, "'mqprio' configuration is necessary");
903 		return -EINVAL;
904 	}
905 
906 	/* If num_tc is already set, it means that the user already
907 	 * configured the mqprio part
908 	 */
909 	if (dev->num_tc)
910 		return 0;
911 
912 	/* Verify num_tc is not out of max range */
913 	if (qopt->num_tc > TC_MAX_QUEUE) {
914 		NL_SET_ERR_MSG(extack, "Number of traffic classes is outside valid range");
915 		return -EINVAL;
916 	}
917 
918 	/* taprio imposes that traffic classes map 1:n to tx queues */
919 	if (qopt->num_tc > dev->num_tx_queues) {
920 		NL_SET_ERR_MSG(extack, "Number of traffic classes is greater than number of HW queues");
921 		return -EINVAL;
922 	}
923 
924 	/* Verify priority mapping uses valid tcs */
925 	for (i = 0; i < TC_BITMASK + 1; i++) {
926 		if (qopt->prio_tc_map[i] >= qopt->num_tc) {
927 			NL_SET_ERR_MSG(extack, "Invalid traffic class in priority to traffic class mapping");
928 			return -EINVAL;
929 		}
930 	}
931 
932 	for (i = 0; i < qopt->num_tc; i++) {
933 		unsigned int last = qopt->offset[i] + qopt->count[i];
934 
935 		/* Verify the queue count is in tx range being equal to the
936 		 * real_num_tx_queues indicates the last queue is in use.
937 		 */
938 		if (qopt->offset[i] >= dev->num_tx_queues ||
939 		    !qopt->count[i] ||
940 		    last > dev->real_num_tx_queues) {
941 			NL_SET_ERR_MSG(extack, "Invalid queue in traffic class to queue mapping");
942 			return -EINVAL;
943 		}
944 
945 		if (TXTIME_ASSIST_IS_ENABLED(taprio_flags))
946 			continue;
947 
948 		/* Verify that the offset and counts do not overlap */
949 		for (j = i + 1; j < qopt->num_tc; j++) {
950 			if (last > qopt->offset[j]) {
951 				NL_SET_ERR_MSG(extack, "Detected overlap in the traffic class to queue mapping");
952 				return -EINVAL;
953 			}
954 		}
955 	}
956 
957 	return 0;
958 }
959 
960 static int taprio_get_start_time(struct Qdisc *sch,
961 				 struct sched_gate_list *sched,
962 				 ktime_t *start)
963 {
964 	struct taprio_sched *q = qdisc_priv(sch);
965 	ktime_t now, base, cycle;
966 	s64 n;
967 
968 	base = sched_base_time(sched);
969 	now = taprio_get_time(q);
970 
971 	if (ktime_after(base, now)) {
972 		*start = base;
973 		return 0;
974 	}
975 
976 	cycle = sched->cycle_time;
977 
978 	/* The qdisc is expected to have at least one sched_entry.  Moreover,
979 	 * any entry must have 'interval' > 0. Thus if the cycle time is zero,
980 	 * something went really wrong. In that case, we should warn about this
981 	 * inconsistent state and return error.
982 	 */
983 	if (WARN_ON(!cycle))
984 		return -EFAULT;
985 
986 	/* Schedule the start time for the beginning of the next
987 	 * cycle.
988 	 */
989 	n = div64_s64(ktime_sub_ns(now, base), cycle);
990 	*start = ktime_add_ns(base, (n + 1) * cycle);
991 	return 0;
992 }
993 
994 static void setup_first_close_time(struct taprio_sched *q,
995 				   struct sched_gate_list *sched, ktime_t base)
996 {
997 	struct sched_entry *first;
998 	ktime_t cycle;
999 
1000 	first = list_first_entry(&sched->entries,
1001 				 struct sched_entry, list);
1002 
1003 	cycle = sched->cycle_time;
1004 
1005 	/* FIXME: find a better place to do this */
1006 	sched->cycle_close_time = ktime_add_ns(base, cycle);
1007 
1008 	first->close_time = ktime_add_ns(base, first->interval);
1009 	taprio_set_budget(q, first);
1010 	rcu_assign_pointer(q->current_entry, NULL);
1011 }
1012 
1013 static void taprio_start_sched(struct Qdisc *sch,
1014 			       ktime_t start, struct sched_gate_list *new)
1015 {
1016 	struct taprio_sched *q = qdisc_priv(sch);
1017 	ktime_t expires;
1018 
1019 	if (FULL_OFFLOAD_IS_ENABLED(q->flags))
1020 		return;
1021 
1022 	expires = hrtimer_get_expires(&q->advance_timer);
1023 	if (expires == 0)
1024 		expires = KTIME_MAX;
1025 
1026 	/* If the new schedule starts before the next expiration, we
1027 	 * reprogram it to the earliest one, so we change the admin
1028 	 * schedule to the operational one at the right time.
1029 	 */
1030 	start = min_t(ktime_t, start, expires);
1031 
1032 	hrtimer_start(&q->advance_timer, start, HRTIMER_MODE_ABS);
1033 }
1034 
1035 static void taprio_set_picos_per_byte(struct net_device *dev,
1036 				      struct taprio_sched *q)
1037 {
1038 	struct ethtool_link_ksettings ecmd;
1039 	int speed = SPEED_10;
1040 	int picos_per_byte;
1041 	int err;
1042 
1043 	err = __ethtool_get_link_ksettings(dev, &ecmd);
1044 	if (err < 0)
1045 		goto skip;
1046 
1047 	if (ecmd.base.speed && ecmd.base.speed != SPEED_UNKNOWN)
1048 		speed = ecmd.base.speed;
1049 
1050 skip:
1051 	picos_per_byte = (USEC_PER_SEC * 8) / speed;
1052 
1053 	atomic64_set(&q->picos_per_byte, picos_per_byte);
1054 	netdev_dbg(dev, "taprio: set %s's picos_per_byte to: %lld, linkspeed: %d\n",
1055 		   dev->name, (long long)atomic64_read(&q->picos_per_byte),
1056 		   ecmd.base.speed);
1057 }
1058 
1059 static int taprio_dev_notifier(struct notifier_block *nb, unsigned long event,
1060 			       void *ptr)
1061 {
1062 	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1063 	struct net_device *qdev;
1064 	struct taprio_sched *q;
1065 	bool found = false;
1066 
1067 	ASSERT_RTNL();
1068 
1069 	if (event != NETDEV_UP && event != NETDEV_CHANGE)
1070 		return NOTIFY_DONE;
1071 
1072 	spin_lock(&taprio_list_lock);
1073 	list_for_each_entry(q, &taprio_list, taprio_list) {
1074 		qdev = qdisc_dev(q->root);
1075 		if (qdev == dev) {
1076 			found = true;
1077 			break;
1078 		}
1079 	}
1080 	spin_unlock(&taprio_list_lock);
1081 
1082 	if (found)
1083 		taprio_set_picos_per_byte(dev, q);
1084 
1085 	return NOTIFY_DONE;
1086 }
1087 
1088 static void setup_txtime(struct taprio_sched *q,
1089 			 struct sched_gate_list *sched, ktime_t base)
1090 {
1091 	struct sched_entry *entry;
1092 	u32 interval = 0;
1093 
1094 	list_for_each_entry(entry, &sched->entries, list) {
1095 		entry->next_txtime = ktime_add_ns(base, interval);
1096 		interval += entry->interval;
1097 	}
1098 }
1099 
1100 static struct tc_taprio_qopt_offload *taprio_offload_alloc(int num_entries)
1101 {
1102 	size_t size = sizeof(struct tc_taprio_sched_entry) * num_entries +
1103 		      sizeof(struct __tc_taprio_qopt_offload);
1104 	struct __tc_taprio_qopt_offload *__offload;
1105 
1106 	__offload = kzalloc(size, GFP_KERNEL);
1107 	if (!__offload)
1108 		return NULL;
1109 
1110 	refcount_set(&__offload->users, 1);
1111 
1112 	return &__offload->offload;
1113 }
1114 
1115 struct tc_taprio_qopt_offload *taprio_offload_get(struct tc_taprio_qopt_offload
1116 						  *offload)
1117 {
1118 	struct __tc_taprio_qopt_offload *__offload;
1119 
1120 	__offload = container_of(offload, struct __tc_taprio_qopt_offload,
1121 				 offload);
1122 
1123 	refcount_inc(&__offload->users);
1124 
1125 	return offload;
1126 }
1127 EXPORT_SYMBOL_GPL(taprio_offload_get);
1128 
1129 void taprio_offload_free(struct tc_taprio_qopt_offload *offload)
1130 {
1131 	struct __tc_taprio_qopt_offload *__offload;
1132 
1133 	__offload = container_of(offload, struct __tc_taprio_qopt_offload,
1134 				 offload);
1135 
1136 	if (!refcount_dec_and_test(&__offload->users))
1137 		return;
1138 
1139 	kfree(__offload);
1140 }
1141 EXPORT_SYMBOL_GPL(taprio_offload_free);
1142 
1143 /* The function will only serve to keep the pointers to the "oper" and "admin"
1144  * schedules valid in relation to their base times, so when calling dump() the
1145  * users looks at the right schedules.
1146  * When using full offload, the admin configuration is promoted to oper at the
1147  * base_time in the PHC time domain.  But because the system time is not
1148  * necessarily in sync with that, we can't just trigger a hrtimer to call
1149  * switch_schedules at the right hardware time.
1150  * At the moment we call this by hand right away from taprio, but in the future
1151  * it will be useful to create a mechanism for drivers to notify taprio of the
1152  * offload state (PENDING, ACTIVE, INACTIVE) so it can be visible in dump().
1153  * This is left as TODO.
1154  */
1155 void taprio_offload_config_changed(struct taprio_sched *q)
1156 {
1157 	struct sched_gate_list *oper, *admin;
1158 
1159 	spin_lock(&q->current_entry_lock);
1160 
1161 	oper = rcu_dereference_protected(q->oper_sched,
1162 					 lockdep_is_held(&q->current_entry_lock));
1163 	admin = rcu_dereference_protected(q->admin_sched,
1164 					  lockdep_is_held(&q->current_entry_lock));
1165 
1166 	switch_schedules(q, &admin, &oper);
1167 
1168 	spin_unlock(&q->current_entry_lock);
1169 }
1170 
1171 static void taprio_sched_to_offload(struct taprio_sched *q,
1172 				    struct sched_gate_list *sched,
1173 				    const struct tc_mqprio_qopt *mqprio,
1174 				    struct tc_taprio_qopt_offload *offload)
1175 {
1176 	struct sched_entry *entry;
1177 	int i = 0;
1178 
1179 	offload->base_time = sched->base_time;
1180 	offload->cycle_time = sched->cycle_time;
1181 	offload->cycle_time_extension = sched->cycle_time_extension;
1182 
1183 	list_for_each_entry(entry, &sched->entries, list) {
1184 		struct tc_taprio_sched_entry *e = &offload->entries[i];
1185 
1186 		e->command = entry->command;
1187 		e->interval = entry->interval;
1188 		e->gate_mask = entry->gate_mask;
1189 		i++;
1190 	}
1191 
1192 	offload->num_entries = i;
1193 }
1194 
1195 static int taprio_enable_offload(struct net_device *dev,
1196 				 struct tc_mqprio_qopt *mqprio,
1197 				 struct taprio_sched *q,
1198 				 struct sched_gate_list *sched,
1199 				 struct netlink_ext_ack *extack)
1200 {
1201 	const struct net_device_ops *ops = dev->netdev_ops;
1202 	struct tc_taprio_qopt_offload *offload;
1203 	int err = 0;
1204 
1205 	if (!ops->ndo_setup_tc) {
1206 		NL_SET_ERR_MSG(extack,
1207 			       "Device does not support taprio offload");
1208 		return -EOPNOTSUPP;
1209 	}
1210 
1211 	offload = taprio_offload_alloc(sched->num_entries);
1212 	if (!offload) {
1213 		NL_SET_ERR_MSG(extack,
1214 			       "Not enough memory for enabling offload mode");
1215 		return -ENOMEM;
1216 	}
1217 	offload->enable = 1;
1218 	taprio_sched_to_offload(q, sched, mqprio, offload);
1219 
1220 	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload);
1221 	if (err < 0) {
1222 		NL_SET_ERR_MSG(extack,
1223 			       "Device failed to setup taprio offload");
1224 		goto done;
1225 	}
1226 
1227 	taprio_offload_config_changed(q);
1228 
1229 done:
1230 	taprio_offload_free(offload);
1231 
1232 	return err;
1233 }
1234 
1235 static int taprio_disable_offload(struct net_device *dev,
1236 				  struct taprio_sched *q,
1237 				  struct netlink_ext_ack *extack)
1238 {
1239 	const struct net_device_ops *ops = dev->netdev_ops;
1240 	struct tc_taprio_qopt_offload *offload;
1241 	int err;
1242 
1243 	if (!FULL_OFFLOAD_IS_ENABLED(q->flags))
1244 		return 0;
1245 
1246 	if (!ops->ndo_setup_tc)
1247 		return -EOPNOTSUPP;
1248 
1249 	offload = taprio_offload_alloc(0);
1250 	if (!offload) {
1251 		NL_SET_ERR_MSG(extack,
1252 			       "Not enough memory to disable offload mode");
1253 		return -ENOMEM;
1254 	}
1255 	offload->enable = 0;
1256 
1257 	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload);
1258 	if (err < 0) {
1259 		NL_SET_ERR_MSG(extack,
1260 			       "Device failed to disable offload");
1261 		goto out;
1262 	}
1263 
1264 out:
1265 	taprio_offload_free(offload);
1266 
1267 	return err;
1268 }
1269 
1270 /* If full offload is enabled, the only possible clockid is the net device's
1271  * PHC. For that reason, specifying a clockid through netlink is incorrect.
1272  * For txtime-assist, it is implicitly assumed that the device's PHC is kept
1273  * in sync with the specified clockid via a user space daemon such as phc2sys.
1274  * For both software taprio and txtime-assist, the clockid is used for the
1275  * hrtimer that advances the schedule and hence mandatory.
1276  */
1277 static int taprio_parse_clockid(struct Qdisc *sch, struct nlattr **tb,
1278 				struct netlink_ext_ack *extack)
1279 {
1280 	struct taprio_sched *q = qdisc_priv(sch);
1281 	struct net_device *dev = qdisc_dev(sch);
1282 	int err = -EINVAL;
1283 
1284 	if (FULL_OFFLOAD_IS_ENABLED(q->flags)) {
1285 		const struct ethtool_ops *ops = dev->ethtool_ops;
1286 		struct ethtool_ts_info info = {
1287 			.cmd = ETHTOOL_GET_TS_INFO,
1288 			.phc_index = -1,
1289 		};
1290 
1291 		if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) {
1292 			NL_SET_ERR_MSG(extack,
1293 				       "The 'clockid' cannot be specified for full offload");
1294 			goto out;
1295 		}
1296 
1297 		if (ops && ops->get_ts_info)
1298 			err = ops->get_ts_info(dev, &info);
1299 
1300 		if (err || info.phc_index < 0) {
1301 			NL_SET_ERR_MSG(extack,
1302 				       "Device does not have a PTP clock");
1303 			err = -ENOTSUPP;
1304 			goto out;
1305 		}
1306 	} else if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) {
1307 		int clockid = nla_get_s32(tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]);
1308 
1309 		/* We only support static clockids and we don't allow
1310 		 * for it to be modified after the first init.
1311 		 */
1312 		if (clockid < 0 ||
1313 		    (q->clockid != -1 && q->clockid != clockid)) {
1314 			NL_SET_ERR_MSG(extack,
1315 				       "Changing the 'clockid' of a running schedule is not supported");
1316 			err = -ENOTSUPP;
1317 			goto out;
1318 		}
1319 
1320 		switch (clockid) {
1321 		case CLOCK_REALTIME:
1322 			q->tk_offset = TK_OFFS_REAL;
1323 			break;
1324 		case CLOCK_MONOTONIC:
1325 			q->tk_offset = TK_OFFS_MAX;
1326 			break;
1327 		case CLOCK_BOOTTIME:
1328 			q->tk_offset = TK_OFFS_BOOT;
1329 			break;
1330 		case CLOCK_TAI:
1331 			q->tk_offset = TK_OFFS_TAI;
1332 			break;
1333 		default:
1334 			NL_SET_ERR_MSG(extack, "Invalid 'clockid'");
1335 			err = -EINVAL;
1336 			goto out;
1337 		}
1338 
1339 		q->clockid = clockid;
1340 	} else {
1341 		NL_SET_ERR_MSG(extack, "Specifying a 'clockid' is mandatory");
1342 		goto out;
1343 	}
1344 out:
1345 	return err;
1346 }
1347 
1348 static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
1349 			 struct netlink_ext_ack *extack)
1350 {
1351 	struct nlattr *tb[TCA_TAPRIO_ATTR_MAX + 1] = { };
1352 	struct sched_gate_list *oper, *admin, *new_admin;
1353 	struct taprio_sched *q = qdisc_priv(sch);
1354 	struct net_device *dev = qdisc_dev(sch);
1355 	struct tc_mqprio_qopt *mqprio = NULL;
1356 	u32 taprio_flags = 0;
1357 	unsigned long flags;
1358 	ktime_t start;
1359 	int i, err;
1360 
1361 	err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_ATTR_MAX, opt,
1362 					  taprio_policy, extack);
1363 	if (err < 0)
1364 		return err;
1365 
1366 	if (tb[TCA_TAPRIO_ATTR_PRIOMAP])
1367 		mqprio = nla_data(tb[TCA_TAPRIO_ATTR_PRIOMAP]);
1368 
1369 	if (tb[TCA_TAPRIO_ATTR_FLAGS]) {
1370 		taprio_flags = nla_get_u32(tb[TCA_TAPRIO_ATTR_FLAGS]);
1371 
1372 		if (q->flags != 0 && q->flags != taprio_flags) {
1373 			NL_SET_ERR_MSG_MOD(extack, "Changing 'flags' of a running schedule is not supported");
1374 			return -EOPNOTSUPP;
1375 		} else if (!taprio_flags_valid(taprio_flags)) {
1376 			NL_SET_ERR_MSG_MOD(extack, "Specified 'flags' are not valid");
1377 			return -EINVAL;
1378 		}
1379 
1380 		q->flags = taprio_flags;
1381 	}
1382 
1383 	err = taprio_parse_mqprio_opt(dev, mqprio, extack, taprio_flags);
1384 	if (err < 0)
1385 		return err;
1386 
1387 	new_admin = kzalloc(sizeof(*new_admin), GFP_KERNEL);
1388 	if (!new_admin) {
1389 		NL_SET_ERR_MSG(extack, "Not enough memory for a new schedule");
1390 		return -ENOMEM;
1391 	}
1392 	INIT_LIST_HEAD(&new_admin->entries);
1393 
1394 	rcu_read_lock();
1395 	oper = rcu_dereference(q->oper_sched);
1396 	admin = rcu_dereference(q->admin_sched);
1397 	rcu_read_unlock();
1398 
1399 	if (mqprio && (oper || admin)) {
1400 		NL_SET_ERR_MSG(extack, "Changing the traffic mapping of a running schedule is not supported");
1401 		err = -ENOTSUPP;
1402 		goto free_sched;
1403 	}
1404 
1405 	err = parse_taprio_schedule(tb, new_admin, extack);
1406 	if (err < 0)
1407 		goto free_sched;
1408 
1409 	if (new_admin->num_entries == 0) {
1410 		NL_SET_ERR_MSG(extack, "There should be at least one entry in the schedule");
1411 		err = -EINVAL;
1412 		goto free_sched;
1413 	}
1414 
1415 	err = taprio_parse_clockid(sch, tb, extack);
1416 	if (err < 0)
1417 		goto free_sched;
1418 
1419 	taprio_set_picos_per_byte(dev, q);
1420 
1421 	if (FULL_OFFLOAD_IS_ENABLED(taprio_flags))
1422 		err = taprio_enable_offload(dev, mqprio, q, new_admin, extack);
1423 	else
1424 		err = taprio_disable_offload(dev, q, extack);
1425 	if (err)
1426 		goto free_sched;
1427 
1428 	/* Protects against enqueue()/dequeue() */
1429 	spin_lock_bh(qdisc_lock(sch));
1430 
1431 	if (tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]) {
1432 		if (!TXTIME_ASSIST_IS_ENABLED(q->flags)) {
1433 			NL_SET_ERR_MSG_MOD(extack, "txtime-delay can only be set when txtime-assist mode is enabled");
1434 			err = -EINVAL;
1435 			goto unlock;
1436 		}
1437 
1438 		q->txtime_delay = nla_get_u32(tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]);
1439 	}
1440 
1441 	if (!TXTIME_ASSIST_IS_ENABLED(taprio_flags) &&
1442 	    !FULL_OFFLOAD_IS_ENABLED(taprio_flags) &&
1443 	    !hrtimer_active(&q->advance_timer)) {
1444 		hrtimer_init(&q->advance_timer, q->clockid, HRTIMER_MODE_ABS);
1445 		q->advance_timer.function = advance_sched;
1446 	}
1447 
1448 	if (mqprio) {
1449 		netdev_set_num_tc(dev, mqprio->num_tc);
1450 		for (i = 0; i < mqprio->num_tc; i++)
1451 			netdev_set_tc_queue(dev, i,
1452 					    mqprio->count[i],
1453 					    mqprio->offset[i]);
1454 
1455 		/* Always use supplied priority mappings */
1456 		for (i = 0; i < TC_BITMASK + 1; i++)
1457 			netdev_set_prio_tc_map(dev, i,
1458 					       mqprio->prio_tc_map[i]);
1459 	}
1460 
1461 	if (FULL_OFFLOAD_IS_ENABLED(taprio_flags)) {
1462 		q->dequeue = taprio_dequeue_offload;
1463 		q->peek = taprio_peek_offload;
1464 	} else {
1465 		/* Be sure to always keep the function pointers
1466 		 * in a consistent state.
1467 		 */
1468 		q->dequeue = taprio_dequeue_soft;
1469 		q->peek = taprio_peek_soft;
1470 	}
1471 
1472 	err = taprio_get_start_time(sch, new_admin, &start);
1473 	if (err < 0) {
1474 		NL_SET_ERR_MSG(extack, "Internal error: failed get start time");
1475 		goto unlock;
1476 	}
1477 
1478 	if (TXTIME_ASSIST_IS_ENABLED(taprio_flags)) {
1479 		setup_txtime(q, new_admin, start);
1480 
1481 		if (!oper) {
1482 			rcu_assign_pointer(q->oper_sched, new_admin);
1483 			err = 0;
1484 			new_admin = NULL;
1485 			goto unlock;
1486 		}
1487 
1488 		rcu_assign_pointer(q->admin_sched, new_admin);
1489 		if (admin)
1490 			call_rcu(&admin->rcu, taprio_free_sched_cb);
1491 	} else {
1492 		setup_first_close_time(q, new_admin, start);
1493 
1494 		/* Protects against advance_sched() */
1495 		spin_lock_irqsave(&q->current_entry_lock, flags);
1496 
1497 		taprio_start_sched(sch, start, new_admin);
1498 
1499 		rcu_assign_pointer(q->admin_sched, new_admin);
1500 		if (admin)
1501 			call_rcu(&admin->rcu, taprio_free_sched_cb);
1502 
1503 		spin_unlock_irqrestore(&q->current_entry_lock, flags);
1504 	}
1505 
1506 	new_admin = NULL;
1507 	err = 0;
1508 
1509 unlock:
1510 	spin_unlock_bh(qdisc_lock(sch));
1511 
1512 free_sched:
1513 	if (new_admin)
1514 		call_rcu(&new_admin->rcu, taprio_free_sched_cb);
1515 
1516 	return err;
1517 }
1518 
1519 static void taprio_destroy(struct Qdisc *sch)
1520 {
1521 	struct taprio_sched *q = qdisc_priv(sch);
1522 	struct net_device *dev = qdisc_dev(sch);
1523 	unsigned int i;
1524 
1525 	spin_lock(&taprio_list_lock);
1526 	list_del(&q->taprio_list);
1527 	spin_unlock(&taprio_list_lock);
1528 
1529 	hrtimer_cancel(&q->advance_timer);
1530 
1531 	taprio_disable_offload(dev, q, NULL);
1532 
1533 	if (q->qdiscs) {
1534 		for (i = 0; i < dev->num_tx_queues && q->qdiscs[i]; i++)
1535 			qdisc_put(q->qdiscs[i]);
1536 
1537 		kfree(q->qdiscs);
1538 	}
1539 	q->qdiscs = NULL;
1540 
1541 	netdev_set_num_tc(dev, 0);
1542 
1543 	if (q->oper_sched)
1544 		call_rcu(&q->oper_sched->rcu, taprio_free_sched_cb);
1545 
1546 	if (q->admin_sched)
1547 		call_rcu(&q->admin_sched->rcu, taprio_free_sched_cb);
1548 }
1549 
1550 static int taprio_init(struct Qdisc *sch, struct nlattr *opt,
1551 		       struct netlink_ext_ack *extack)
1552 {
1553 	struct taprio_sched *q = qdisc_priv(sch);
1554 	struct net_device *dev = qdisc_dev(sch);
1555 	int i;
1556 
1557 	spin_lock_init(&q->current_entry_lock);
1558 
1559 	hrtimer_init(&q->advance_timer, CLOCK_TAI, HRTIMER_MODE_ABS);
1560 	q->advance_timer.function = advance_sched;
1561 
1562 	q->dequeue = taprio_dequeue_soft;
1563 	q->peek = taprio_peek_soft;
1564 
1565 	q->root = sch;
1566 
1567 	/* We only support static clockids. Use an invalid value as default
1568 	 * and get the valid one on taprio_change().
1569 	 */
1570 	q->clockid = -1;
1571 
1572 	spin_lock(&taprio_list_lock);
1573 	list_add(&q->taprio_list, &taprio_list);
1574 	spin_unlock(&taprio_list_lock);
1575 
1576 	if (sch->parent != TC_H_ROOT)
1577 		return -EOPNOTSUPP;
1578 
1579 	if (!netif_is_multiqueue(dev))
1580 		return -EOPNOTSUPP;
1581 
1582 	/* pre-allocate qdisc, attachment can't fail */
1583 	q->qdiscs = kcalloc(dev->num_tx_queues,
1584 			    sizeof(q->qdiscs[0]),
1585 			    GFP_KERNEL);
1586 
1587 	if (!q->qdiscs)
1588 		return -ENOMEM;
1589 
1590 	if (!opt)
1591 		return -EINVAL;
1592 
1593 	for (i = 0; i < dev->num_tx_queues; i++) {
1594 		struct netdev_queue *dev_queue;
1595 		struct Qdisc *qdisc;
1596 
1597 		dev_queue = netdev_get_tx_queue(dev, i);
1598 		qdisc = qdisc_create_dflt(dev_queue,
1599 					  &pfifo_qdisc_ops,
1600 					  TC_H_MAKE(TC_H_MAJ(sch->handle),
1601 						    TC_H_MIN(i + 1)),
1602 					  extack);
1603 		if (!qdisc)
1604 			return -ENOMEM;
1605 
1606 		if (i < dev->real_num_tx_queues)
1607 			qdisc_hash_add(qdisc, false);
1608 
1609 		q->qdiscs[i] = qdisc;
1610 	}
1611 
1612 	return taprio_change(sch, opt, extack);
1613 }
1614 
1615 static struct netdev_queue *taprio_queue_get(struct Qdisc *sch,
1616 					     unsigned long cl)
1617 {
1618 	struct net_device *dev = qdisc_dev(sch);
1619 	unsigned long ntx = cl - 1;
1620 
1621 	if (ntx >= dev->num_tx_queues)
1622 		return NULL;
1623 
1624 	return netdev_get_tx_queue(dev, ntx);
1625 }
1626 
1627 static int taprio_graft(struct Qdisc *sch, unsigned long cl,
1628 			struct Qdisc *new, struct Qdisc **old,
1629 			struct netlink_ext_ack *extack)
1630 {
1631 	struct taprio_sched *q = qdisc_priv(sch);
1632 	struct net_device *dev = qdisc_dev(sch);
1633 	struct netdev_queue *dev_queue = taprio_queue_get(sch, cl);
1634 
1635 	if (!dev_queue)
1636 		return -EINVAL;
1637 
1638 	if (dev->flags & IFF_UP)
1639 		dev_deactivate(dev);
1640 
1641 	*old = q->qdiscs[cl - 1];
1642 	q->qdiscs[cl - 1] = new;
1643 
1644 	if (new)
1645 		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
1646 
1647 	if (dev->flags & IFF_UP)
1648 		dev_activate(dev);
1649 
1650 	return 0;
1651 }
1652 
1653 static int dump_entry(struct sk_buff *msg,
1654 		      const struct sched_entry *entry)
1655 {
1656 	struct nlattr *item;
1657 
1658 	item = nla_nest_start_noflag(msg, TCA_TAPRIO_SCHED_ENTRY);
1659 	if (!item)
1660 		return -ENOSPC;
1661 
1662 	if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_INDEX, entry->index))
1663 		goto nla_put_failure;
1664 
1665 	if (nla_put_u8(msg, TCA_TAPRIO_SCHED_ENTRY_CMD, entry->command))
1666 		goto nla_put_failure;
1667 
1668 	if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_GATE_MASK,
1669 			entry->gate_mask))
1670 		goto nla_put_failure;
1671 
1672 	if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_INTERVAL,
1673 			entry->interval))
1674 		goto nla_put_failure;
1675 
1676 	return nla_nest_end(msg, item);
1677 
1678 nla_put_failure:
1679 	nla_nest_cancel(msg, item);
1680 	return -1;
1681 }
1682 
1683 static int dump_schedule(struct sk_buff *msg,
1684 			 const struct sched_gate_list *root)
1685 {
1686 	struct nlattr *entry_list;
1687 	struct sched_entry *entry;
1688 
1689 	if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_BASE_TIME,
1690 			root->base_time, TCA_TAPRIO_PAD))
1691 		return -1;
1692 
1693 	if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME,
1694 			root->cycle_time, TCA_TAPRIO_PAD))
1695 		return -1;
1696 
1697 	if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION,
1698 			root->cycle_time_extension, TCA_TAPRIO_PAD))
1699 		return -1;
1700 
1701 	entry_list = nla_nest_start_noflag(msg,
1702 					   TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST);
1703 	if (!entry_list)
1704 		goto error_nest;
1705 
1706 	list_for_each_entry(entry, &root->entries, list) {
1707 		if (dump_entry(msg, entry) < 0)
1708 			goto error_nest;
1709 	}
1710 
1711 	nla_nest_end(msg, entry_list);
1712 	return 0;
1713 
1714 error_nest:
1715 	nla_nest_cancel(msg, entry_list);
1716 	return -1;
1717 }
1718 
1719 static int taprio_dump(struct Qdisc *sch, struct sk_buff *skb)
1720 {
1721 	struct taprio_sched *q = qdisc_priv(sch);
1722 	struct net_device *dev = qdisc_dev(sch);
1723 	struct sched_gate_list *oper, *admin;
1724 	struct tc_mqprio_qopt opt = { 0 };
1725 	struct nlattr *nest, *sched_nest;
1726 	unsigned int i;
1727 
1728 	rcu_read_lock();
1729 	oper = rcu_dereference(q->oper_sched);
1730 	admin = rcu_dereference(q->admin_sched);
1731 
1732 	opt.num_tc = netdev_get_num_tc(dev);
1733 	memcpy(opt.prio_tc_map, dev->prio_tc_map, sizeof(opt.prio_tc_map));
1734 
1735 	for (i = 0; i < netdev_get_num_tc(dev); i++) {
1736 		opt.count[i] = dev->tc_to_txq[i].count;
1737 		opt.offset[i] = dev->tc_to_txq[i].offset;
1738 	}
1739 
1740 	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
1741 	if (!nest)
1742 		goto start_error;
1743 
1744 	if (nla_put(skb, TCA_TAPRIO_ATTR_PRIOMAP, sizeof(opt), &opt))
1745 		goto options_error;
1746 
1747 	if (!FULL_OFFLOAD_IS_ENABLED(q->flags) &&
1748 	    nla_put_s32(skb, TCA_TAPRIO_ATTR_SCHED_CLOCKID, q->clockid))
1749 		goto options_error;
1750 
1751 	if (q->flags && nla_put_u32(skb, TCA_TAPRIO_ATTR_FLAGS, q->flags))
1752 		goto options_error;
1753 
1754 	if (q->txtime_delay &&
1755 	    nla_put_u32(skb, TCA_TAPRIO_ATTR_TXTIME_DELAY, q->txtime_delay))
1756 		goto options_error;
1757 
1758 	if (oper && dump_schedule(skb, oper))
1759 		goto options_error;
1760 
1761 	if (!admin)
1762 		goto done;
1763 
1764 	sched_nest = nla_nest_start_noflag(skb, TCA_TAPRIO_ATTR_ADMIN_SCHED);
1765 	if (!sched_nest)
1766 		goto options_error;
1767 
1768 	if (dump_schedule(skb, admin))
1769 		goto admin_error;
1770 
1771 	nla_nest_end(skb, sched_nest);
1772 
1773 done:
1774 	rcu_read_unlock();
1775 
1776 	return nla_nest_end(skb, nest);
1777 
1778 admin_error:
1779 	nla_nest_cancel(skb, sched_nest);
1780 
1781 options_error:
1782 	nla_nest_cancel(skb, nest);
1783 
1784 start_error:
1785 	rcu_read_unlock();
1786 	return -ENOSPC;
1787 }
1788 
1789 static struct Qdisc *taprio_leaf(struct Qdisc *sch, unsigned long cl)
1790 {
1791 	struct netdev_queue *dev_queue = taprio_queue_get(sch, cl);
1792 
1793 	if (!dev_queue)
1794 		return NULL;
1795 
1796 	return dev_queue->qdisc_sleeping;
1797 }
1798 
1799 static unsigned long taprio_find(struct Qdisc *sch, u32 classid)
1800 {
1801 	unsigned int ntx = TC_H_MIN(classid);
1802 
1803 	if (!taprio_queue_get(sch, ntx))
1804 		return 0;
1805 	return ntx;
1806 }
1807 
1808 static int taprio_dump_class(struct Qdisc *sch, unsigned long cl,
1809 			     struct sk_buff *skb, struct tcmsg *tcm)
1810 {
1811 	struct netdev_queue *dev_queue = taprio_queue_get(sch, cl);
1812 
1813 	tcm->tcm_parent = TC_H_ROOT;
1814 	tcm->tcm_handle |= TC_H_MIN(cl);
1815 	tcm->tcm_info = dev_queue->qdisc_sleeping->handle;
1816 
1817 	return 0;
1818 }
1819 
1820 static int taprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
1821 				   struct gnet_dump *d)
1822 	__releases(d->lock)
1823 	__acquires(d->lock)
1824 {
1825 	struct netdev_queue *dev_queue = taprio_queue_get(sch, cl);
1826 
1827 	sch = dev_queue->qdisc_sleeping;
1828 	if (gnet_stats_copy_basic(&sch->running, d, NULL, &sch->bstats) < 0 ||
1829 	    qdisc_qstats_copy(d, sch) < 0)
1830 		return -1;
1831 	return 0;
1832 }
1833 
1834 static void taprio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
1835 {
1836 	struct net_device *dev = qdisc_dev(sch);
1837 	unsigned long ntx;
1838 
1839 	if (arg->stop)
1840 		return;
1841 
1842 	arg->count = arg->skip;
1843 	for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) {
1844 		if (arg->fn(sch, ntx + 1, arg) < 0) {
1845 			arg->stop = 1;
1846 			break;
1847 		}
1848 		arg->count++;
1849 	}
1850 }
1851 
1852 static struct netdev_queue *taprio_select_queue(struct Qdisc *sch,
1853 						struct tcmsg *tcm)
1854 {
1855 	return taprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
1856 }
1857 
1858 static const struct Qdisc_class_ops taprio_class_ops = {
1859 	.graft		= taprio_graft,
1860 	.leaf		= taprio_leaf,
1861 	.find		= taprio_find,
1862 	.walk		= taprio_walk,
1863 	.dump		= taprio_dump_class,
1864 	.dump_stats	= taprio_dump_class_stats,
1865 	.select_queue	= taprio_select_queue,
1866 };
1867 
1868 static struct Qdisc_ops taprio_qdisc_ops __read_mostly = {
1869 	.cl_ops		= &taprio_class_ops,
1870 	.id		= "taprio",
1871 	.priv_size	= sizeof(struct taprio_sched),
1872 	.init		= taprio_init,
1873 	.change		= taprio_change,
1874 	.destroy	= taprio_destroy,
1875 	.peek		= taprio_peek,
1876 	.dequeue	= taprio_dequeue,
1877 	.enqueue	= taprio_enqueue,
1878 	.dump		= taprio_dump,
1879 	.owner		= THIS_MODULE,
1880 };
1881 
1882 static struct notifier_block taprio_device_notifier = {
1883 	.notifier_call = taprio_dev_notifier,
1884 };
1885 
1886 static int __init taprio_module_init(void)
1887 {
1888 	int err = register_netdevice_notifier(&taprio_device_notifier);
1889 
1890 	if (err)
1891 		return err;
1892 
1893 	return register_qdisc(&taprio_qdisc_ops);
1894 }
1895 
1896 static void __exit taprio_module_exit(void)
1897 {
1898 	unregister_qdisc(&taprio_qdisc_ops);
1899 	unregister_netdevice_notifier(&taprio_device_notifier);
1900 }
1901 
1902 module_init(taprio_module_init);
1903 module_exit(taprio_module_exit);
1904 MODULE_LICENSE("GPL");
1905