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