15a781ccbSVinicius Costa Gomes // SPDX-License-Identifier: GPL-2.0 25a781ccbSVinicius Costa Gomes 35a781ccbSVinicius Costa Gomes /* net/sched/sch_taprio.c Time Aware Priority Scheduler 45a781ccbSVinicius Costa Gomes * 55a781ccbSVinicius Costa Gomes * Authors: Vinicius Costa Gomes <vinicius.gomes@intel.com> 65a781ccbSVinicius Costa Gomes * 75a781ccbSVinicius Costa Gomes */ 85a781ccbSVinicius Costa Gomes 9cc69837fSJakub Kicinski #include <linux/ethtool.h> 105a781ccbSVinicius Costa Gomes #include <linux/types.h> 115a781ccbSVinicius Costa Gomes #include <linux/slab.h> 125a781ccbSVinicius Costa Gomes #include <linux/kernel.h> 135a781ccbSVinicius Costa Gomes #include <linux/string.h> 145a781ccbSVinicius Costa Gomes #include <linux/list.h> 155a781ccbSVinicius Costa Gomes #include <linux/errno.h> 165a781ccbSVinicius Costa Gomes #include <linux/skbuff.h> 1723bddf69SJakub Kicinski #include <linux/math64.h> 185a781ccbSVinicius Costa Gomes #include <linux/module.h> 195a781ccbSVinicius Costa Gomes #include <linux/spinlock.h> 20a3d43c0dSVinicius Costa Gomes #include <linux/rcupdate.h> 21837ced3aSVladimir Oltean #include <linux/time.h> 225a781ccbSVinicius Costa Gomes #include <net/netlink.h> 235a781ccbSVinicius Costa Gomes #include <net/pkt_sched.h> 245a781ccbSVinicius Costa Gomes #include <net/pkt_cls.h> 255a781ccbSVinicius Costa Gomes #include <net/sch_generic.h> 264cfd5779SVedang Patel #include <net/sock.h> 2754002066SVedang Patel #include <net/tcp.h> 285a781ccbSVinicius Costa Gomes 297b9eba7bSLeandro Dorileo static LIST_HEAD(taprio_list); 307b9eba7bSLeandro Dorileo 315a781ccbSVinicius Costa Gomes #define TAPRIO_ALL_GATES_OPEN -1 325a781ccbSVinicius Costa Gomes 334cfd5779SVedang Patel #define TXTIME_ASSIST_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST) 349c66d156SVinicius Costa Gomes #define FULL_OFFLOAD_IS_ENABLED(flags) ((flags) & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD) 35a9d62274SVinicius Costa Gomes #define TAPRIO_FLAGS_INVALID U32_MAX 364cfd5779SVedang Patel 375a781ccbSVinicius Costa Gomes struct sched_entry { 385a781ccbSVinicius Costa Gomes struct list_head list; 395a781ccbSVinicius Costa Gomes 405a781ccbSVinicius Costa Gomes /* The instant that this entry "closes" and the next one 415a781ccbSVinicius Costa Gomes * should open, the qdisc will make some effort so that no 425a781ccbSVinicius Costa Gomes * packet leaves after this time. 435a781ccbSVinicius Costa Gomes */ 445a781ccbSVinicius Costa Gomes ktime_t close_time; 454cfd5779SVedang Patel ktime_t next_txtime; 465a781ccbSVinicius Costa Gomes atomic_t budget; 475a781ccbSVinicius Costa Gomes int index; 485a781ccbSVinicius Costa Gomes u32 gate_mask; 495a781ccbSVinicius Costa Gomes u32 interval; 505a781ccbSVinicius Costa Gomes u8 command; 515a781ccbSVinicius Costa Gomes }; 525a781ccbSVinicius Costa Gomes 53a3d43c0dSVinicius Costa Gomes struct sched_gate_list { 54a3d43c0dSVinicius Costa Gomes struct rcu_head rcu; 55a3d43c0dSVinicius Costa Gomes struct list_head entries; 56a3d43c0dSVinicius Costa Gomes size_t num_entries; 576ca6a665SVinicius Costa Gomes ktime_t cycle_close_time; 586ca6a665SVinicius Costa Gomes s64 cycle_time; 59c25031e9SVinicius Costa Gomes s64 cycle_time_extension; 60a3d43c0dSVinicius Costa Gomes s64 base_time; 61a3d43c0dSVinicius Costa Gomes }; 62a3d43c0dSVinicius Costa Gomes 635a781ccbSVinicius Costa Gomes struct taprio_sched { 645a781ccbSVinicius Costa Gomes struct Qdisc **qdiscs; 655a781ccbSVinicius Costa Gomes struct Qdisc *root; 664cfd5779SVedang Patel u32 flags; 677ede7b03SVedang Patel enum tk_offsets tk_offset; 685a781ccbSVinicius Costa Gomes int clockid; 69db46e3a8SVladimir Oltean bool offloaded; 707b9eba7bSLeandro Dorileo atomic64_t picos_per_byte; /* Using picoseconds because for 10Gbps+ 715a781ccbSVinicius Costa Gomes * speeds it's sub-nanoseconds per byte 725a781ccbSVinicius Costa Gomes */ 735a781ccbSVinicius Costa Gomes 745a781ccbSVinicius Costa Gomes /* Protects the update side of the RCU protected current_entry */ 755a781ccbSVinicius Costa Gomes spinlock_t current_entry_lock; 765a781ccbSVinicius Costa Gomes struct sched_entry __rcu *current_entry; 77a3d43c0dSVinicius Costa Gomes struct sched_gate_list __rcu *oper_sched; 78a3d43c0dSVinicius Costa Gomes struct sched_gate_list __rcu *admin_sched; 795a781ccbSVinicius Costa Gomes struct hrtimer advance_timer; 807b9eba7bSLeandro Dorileo struct list_head taprio_list; 81a5b64700SVedang Patel u32 txtime_delay; 825a781ccbSVinicius Costa Gomes }; 835a781ccbSVinicius Costa Gomes 849c66d156SVinicius Costa Gomes struct __tc_taprio_qopt_offload { 859c66d156SVinicius Costa Gomes refcount_t users; 869c66d156SVinicius Costa Gomes struct tc_taprio_qopt_offload offload; 879c66d156SVinicius Costa Gomes }; 889c66d156SVinicius Costa Gomes 89a3d43c0dSVinicius Costa Gomes static ktime_t sched_base_time(const struct sched_gate_list *sched) 90a3d43c0dSVinicius Costa Gomes { 91a3d43c0dSVinicius Costa Gomes if (!sched) 92a3d43c0dSVinicius Costa Gomes return KTIME_MAX; 93a3d43c0dSVinicius Costa Gomes 94a3d43c0dSVinicius Costa Gomes return ns_to_ktime(sched->base_time); 95a3d43c0dSVinicius Costa Gomes } 96a3d43c0dSVinicius Costa Gomes 976dc25401SEric Dumazet static ktime_t taprio_mono_to_any(const struct taprio_sched *q, ktime_t mono) 987ede7b03SVedang Patel { 996dc25401SEric Dumazet /* This pairs with WRITE_ONCE() in taprio_parse_clockid() */ 1006dc25401SEric Dumazet enum tk_offsets tk_offset = READ_ONCE(q->tk_offset); 1017ede7b03SVedang Patel 1026dc25401SEric Dumazet switch (tk_offset) { 1037ede7b03SVedang Patel case TK_OFFS_MAX: 1047ede7b03SVedang Patel return mono; 1057ede7b03SVedang Patel default: 1066dc25401SEric Dumazet return ktime_mono_to_any(mono, tk_offset); 1076dc25401SEric Dumazet } 1087ede7b03SVedang Patel } 1097ede7b03SVedang Patel 1106dc25401SEric Dumazet static ktime_t taprio_get_time(const struct taprio_sched *q) 1116dc25401SEric Dumazet { 1126dc25401SEric Dumazet return taprio_mono_to_any(q, ktime_get()); 1137ede7b03SVedang Patel } 1147ede7b03SVedang Patel 115a3d43c0dSVinicius Costa Gomes static void taprio_free_sched_cb(struct rcu_head *head) 116a3d43c0dSVinicius Costa Gomes { 117a3d43c0dSVinicius Costa Gomes struct sched_gate_list *sched = container_of(head, struct sched_gate_list, rcu); 118a3d43c0dSVinicius Costa Gomes struct sched_entry *entry, *n; 119a3d43c0dSVinicius Costa Gomes 120a3d43c0dSVinicius Costa Gomes list_for_each_entry_safe(entry, n, &sched->entries, list) { 121a3d43c0dSVinicius Costa Gomes list_del(&entry->list); 122a3d43c0dSVinicius Costa Gomes kfree(entry); 123a3d43c0dSVinicius Costa Gomes } 124a3d43c0dSVinicius Costa Gomes 125a3d43c0dSVinicius Costa Gomes kfree(sched); 126a3d43c0dSVinicius Costa Gomes } 127a3d43c0dSVinicius Costa Gomes 128a3d43c0dSVinicius Costa Gomes static void switch_schedules(struct taprio_sched *q, 129a3d43c0dSVinicius Costa Gomes struct sched_gate_list **admin, 130a3d43c0dSVinicius Costa Gomes struct sched_gate_list **oper) 131a3d43c0dSVinicius Costa Gomes { 132a3d43c0dSVinicius Costa Gomes rcu_assign_pointer(q->oper_sched, *admin); 133a3d43c0dSVinicius Costa Gomes rcu_assign_pointer(q->admin_sched, NULL); 134a3d43c0dSVinicius Costa Gomes 135a3d43c0dSVinicius Costa Gomes if (*oper) 136a3d43c0dSVinicius Costa Gomes call_rcu(&(*oper)->rcu, taprio_free_sched_cb); 137a3d43c0dSVinicius Costa Gomes 138a3d43c0dSVinicius Costa Gomes *oper = *admin; 139a3d43c0dSVinicius Costa Gomes *admin = NULL; 140a3d43c0dSVinicius Costa Gomes } 141a3d43c0dSVinicius Costa Gomes 1424cfd5779SVedang Patel /* Get how much time has been already elapsed in the current cycle. */ 1434cfd5779SVedang Patel static s32 get_cycle_time_elapsed(struct sched_gate_list *sched, ktime_t time) 1444cfd5779SVedang Patel { 1454cfd5779SVedang Patel ktime_t time_since_sched_start; 1464cfd5779SVedang Patel s32 time_elapsed; 1474cfd5779SVedang Patel 1484cfd5779SVedang Patel time_since_sched_start = ktime_sub(time, sched->base_time); 1494cfd5779SVedang Patel div_s64_rem(time_since_sched_start, sched->cycle_time, &time_elapsed); 1504cfd5779SVedang Patel 1514cfd5779SVedang Patel return time_elapsed; 1524cfd5779SVedang Patel } 1534cfd5779SVedang Patel 1544cfd5779SVedang Patel static ktime_t get_interval_end_time(struct sched_gate_list *sched, 1554cfd5779SVedang Patel struct sched_gate_list *admin, 1564cfd5779SVedang Patel struct sched_entry *entry, 1574cfd5779SVedang Patel ktime_t intv_start) 1584cfd5779SVedang Patel { 1594cfd5779SVedang Patel s32 cycle_elapsed = get_cycle_time_elapsed(sched, intv_start); 1604cfd5779SVedang Patel ktime_t intv_end, cycle_ext_end, cycle_end; 1614cfd5779SVedang Patel 1624cfd5779SVedang Patel cycle_end = ktime_add_ns(intv_start, sched->cycle_time - cycle_elapsed); 1634cfd5779SVedang Patel intv_end = ktime_add_ns(intv_start, entry->interval); 1644cfd5779SVedang Patel cycle_ext_end = ktime_add(cycle_end, sched->cycle_time_extension); 1654cfd5779SVedang Patel 1664cfd5779SVedang Patel if (ktime_before(intv_end, cycle_end)) 1674cfd5779SVedang Patel return intv_end; 1684cfd5779SVedang Patel else if (admin && admin != sched && 1694cfd5779SVedang Patel ktime_after(admin->base_time, cycle_end) && 1704cfd5779SVedang Patel ktime_before(admin->base_time, cycle_ext_end)) 1714cfd5779SVedang Patel return admin->base_time; 1724cfd5779SVedang Patel else 1734cfd5779SVedang Patel return cycle_end; 1744cfd5779SVedang Patel } 1754cfd5779SVedang Patel 1764cfd5779SVedang Patel static int length_to_duration(struct taprio_sched *q, int len) 1774cfd5779SVedang Patel { 178837ced3aSVladimir Oltean return div_u64(len * atomic64_read(&q->picos_per_byte), PSEC_PER_NSEC); 1794cfd5779SVedang Patel } 1804cfd5779SVedang Patel 1814cfd5779SVedang Patel /* Returns the entry corresponding to next available interval. If 1824cfd5779SVedang Patel * validate_interval is set, it only validates whether the timestamp occurs 1834cfd5779SVedang Patel * when the gate corresponding to the skb's traffic class is open. 1844cfd5779SVedang Patel */ 1854cfd5779SVedang Patel static struct sched_entry *find_entry_to_transmit(struct sk_buff *skb, 1864cfd5779SVedang Patel struct Qdisc *sch, 1874cfd5779SVedang Patel struct sched_gate_list *sched, 1884cfd5779SVedang Patel struct sched_gate_list *admin, 1894cfd5779SVedang Patel ktime_t time, 1904cfd5779SVedang Patel ktime_t *interval_start, 1914cfd5779SVedang Patel ktime_t *interval_end, 1924cfd5779SVedang Patel bool validate_interval) 1934cfd5779SVedang Patel { 1944cfd5779SVedang Patel ktime_t curr_intv_start, curr_intv_end, cycle_end, packet_transmit_time; 1954cfd5779SVedang Patel ktime_t earliest_txtime = KTIME_MAX, txtime, cycle, transmit_end_time; 1964cfd5779SVedang Patel struct sched_entry *entry = NULL, *entry_found = NULL; 1974cfd5779SVedang Patel struct taprio_sched *q = qdisc_priv(sch); 1984cfd5779SVedang Patel struct net_device *dev = qdisc_dev(sch); 1994cfd5779SVedang Patel bool entry_available = false; 2004cfd5779SVedang Patel s32 cycle_elapsed; 2014cfd5779SVedang Patel int tc, n; 2024cfd5779SVedang Patel 2034cfd5779SVedang Patel tc = netdev_get_prio_tc_map(dev, skb->priority); 2044cfd5779SVedang Patel packet_transmit_time = length_to_duration(q, qdisc_pkt_len(skb)); 2054cfd5779SVedang Patel 2064cfd5779SVedang Patel *interval_start = 0; 2074cfd5779SVedang Patel *interval_end = 0; 2084cfd5779SVedang Patel 2094cfd5779SVedang Patel if (!sched) 2104cfd5779SVedang Patel return NULL; 2114cfd5779SVedang Patel 2124cfd5779SVedang Patel cycle = sched->cycle_time; 2134cfd5779SVedang Patel cycle_elapsed = get_cycle_time_elapsed(sched, time); 2144cfd5779SVedang Patel curr_intv_end = ktime_sub_ns(time, cycle_elapsed); 2154cfd5779SVedang Patel cycle_end = ktime_add_ns(curr_intv_end, cycle); 2164cfd5779SVedang Patel 2174cfd5779SVedang Patel list_for_each_entry(entry, &sched->entries, list) { 2184cfd5779SVedang Patel curr_intv_start = curr_intv_end; 2194cfd5779SVedang Patel curr_intv_end = get_interval_end_time(sched, admin, entry, 2204cfd5779SVedang Patel curr_intv_start); 2214cfd5779SVedang Patel 2224cfd5779SVedang Patel if (ktime_after(curr_intv_start, cycle_end)) 2234cfd5779SVedang Patel break; 2244cfd5779SVedang Patel 2254cfd5779SVedang Patel if (!(entry->gate_mask & BIT(tc)) || 2264cfd5779SVedang Patel packet_transmit_time > entry->interval) 2274cfd5779SVedang Patel continue; 2284cfd5779SVedang Patel 2294cfd5779SVedang Patel txtime = entry->next_txtime; 2304cfd5779SVedang Patel 2314cfd5779SVedang Patel if (ktime_before(txtime, time) || validate_interval) { 2324cfd5779SVedang Patel transmit_end_time = ktime_add_ns(time, packet_transmit_time); 2334cfd5779SVedang Patel if ((ktime_before(curr_intv_start, time) && 2344cfd5779SVedang Patel ktime_before(transmit_end_time, curr_intv_end)) || 2354cfd5779SVedang Patel (ktime_after(curr_intv_start, time) && !validate_interval)) { 2364cfd5779SVedang Patel entry_found = entry; 2374cfd5779SVedang Patel *interval_start = curr_intv_start; 2384cfd5779SVedang Patel *interval_end = curr_intv_end; 2394cfd5779SVedang Patel break; 2404cfd5779SVedang Patel } else if (!entry_available && !validate_interval) { 2414cfd5779SVedang Patel /* Here, we are just trying to find out the 2424cfd5779SVedang Patel * first available interval in the next cycle. 2434cfd5779SVedang Patel */ 2440deee7aaSJiapeng Zhong entry_available = true; 2454cfd5779SVedang Patel entry_found = entry; 2464cfd5779SVedang Patel *interval_start = ktime_add_ns(curr_intv_start, cycle); 2474cfd5779SVedang Patel *interval_end = ktime_add_ns(curr_intv_end, cycle); 2484cfd5779SVedang Patel } 2494cfd5779SVedang Patel } else if (ktime_before(txtime, earliest_txtime) && 2504cfd5779SVedang Patel !entry_available) { 2514cfd5779SVedang Patel earliest_txtime = txtime; 2524cfd5779SVedang Patel entry_found = entry; 2534cfd5779SVedang Patel n = div_s64(ktime_sub(txtime, curr_intv_start), cycle); 2544cfd5779SVedang Patel *interval_start = ktime_add(curr_intv_start, n * cycle); 2554cfd5779SVedang Patel *interval_end = ktime_add(curr_intv_end, n * cycle); 2564cfd5779SVedang Patel } 2574cfd5779SVedang Patel } 2584cfd5779SVedang Patel 2594cfd5779SVedang Patel return entry_found; 2604cfd5779SVedang Patel } 2614cfd5779SVedang Patel 2624cfd5779SVedang Patel static bool is_valid_interval(struct sk_buff *skb, struct Qdisc *sch) 2634cfd5779SVedang Patel { 2644cfd5779SVedang Patel struct taprio_sched *q = qdisc_priv(sch); 2654cfd5779SVedang Patel struct sched_gate_list *sched, *admin; 2664cfd5779SVedang Patel ktime_t interval_start, interval_end; 2674cfd5779SVedang Patel struct sched_entry *entry; 2684cfd5779SVedang Patel 2694cfd5779SVedang Patel rcu_read_lock(); 2704cfd5779SVedang Patel sched = rcu_dereference(q->oper_sched); 2714cfd5779SVedang Patel admin = rcu_dereference(q->admin_sched); 2724cfd5779SVedang Patel 2734cfd5779SVedang Patel entry = find_entry_to_transmit(skb, sch, sched, admin, skb->tstamp, 2744cfd5779SVedang Patel &interval_start, &interval_end, true); 2754cfd5779SVedang Patel rcu_read_unlock(); 2764cfd5779SVedang Patel 2774cfd5779SVedang Patel return entry; 2784cfd5779SVedang Patel } 2794cfd5779SVedang Patel 2809c66d156SVinicius Costa Gomes static bool taprio_flags_valid(u32 flags) 2819c66d156SVinicius Costa Gomes { 2829c66d156SVinicius Costa Gomes /* Make sure no other flag bits are set. */ 2839c66d156SVinicius Costa Gomes if (flags & ~(TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST | 2849c66d156SVinicius Costa Gomes TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)) 2859c66d156SVinicius Costa Gomes return false; 2869c66d156SVinicius Costa Gomes /* txtime-assist and full offload are mutually exclusive */ 2879c66d156SVinicius Costa Gomes if ((flags & TCA_TAPRIO_ATTR_FLAG_TXTIME_ASSIST) && 2889c66d156SVinicius Costa Gomes (flags & TCA_TAPRIO_ATTR_FLAG_FULL_OFFLOAD)) 2899c66d156SVinicius Costa Gomes return false; 2909c66d156SVinicius Costa Gomes return true; 2919c66d156SVinicius Costa Gomes } 2929c66d156SVinicius Costa Gomes 29354002066SVedang Patel /* This returns the tstamp value set by TCP in terms of the set clock. */ 29454002066SVedang Patel static ktime_t get_tcp_tstamp(struct taprio_sched *q, struct sk_buff *skb) 29554002066SVedang Patel { 29654002066SVedang Patel unsigned int offset = skb_network_offset(skb); 29754002066SVedang Patel const struct ipv6hdr *ipv6h; 29854002066SVedang Patel const struct iphdr *iph; 29954002066SVedang Patel struct ipv6hdr _ipv6h; 30054002066SVedang Patel 30154002066SVedang Patel ipv6h = skb_header_pointer(skb, offset, sizeof(_ipv6h), &_ipv6h); 30254002066SVedang Patel if (!ipv6h) 30354002066SVedang Patel return 0; 30454002066SVedang Patel 30554002066SVedang Patel if (ipv6h->version == 4) { 30654002066SVedang Patel iph = (struct iphdr *)ipv6h; 30754002066SVedang Patel offset += iph->ihl * 4; 30854002066SVedang Patel 30954002066SVedang Patel /* special-case 6in4 tunnelling, as that is a common way to get 31054002066SVedang Patel * v6 connectivity in the home 31154002066SVedang Patel */ 31254002066SVedang Patel if (iph->protocol == IPPROTO_IPV6) { 31354002066SVedang Patel ipv6h = skb_header_pointer(skb, offset, 31454002066SVedang Patel sizeof(_ipv6h), &_ipv6h); 31554002066SVedang Patel 31654002066SVedang Patel if (!ipv6h || ipv6h->nexthdr != IPPROTO_TCP) 31754002066SVedang Patel return 0; 31854002066SVedang Patel } else if (iph->protocol != IPPROTO_TCP) { 31954002066SVedang Patel return 0; 32054002066SVedang Patel } 32154002066SVedang Patel } else if (ipv6h->version == 6 && ipv6h->nexthdr != IPPROTO_TCP) { 32254002066SVedang Patel return 0; 32354002066SVedang Patel } 32454002066SVedang Patel 3256dc25401SEric Dumazet return taprio_mono_to_any(q, skb->skb_mstamp_ns); 32654002066SVedang Patel } 32754002066SVedang Patel 3284cfd5779SVedang Patel /* There are a few scenarios where we will have to modify the txtime from 3294cfd5779SVedang Patel * what is read from next_txtime in sched_entry. They are: 3304cfd5779SVedang Patel * 1. If txtime is in the past, 3314cfd5779SVedang Patel * a. The gate for the traffic class is currently open and packet can be 3324cfd5779SVedang Patel * transmitted before it closes, schedule the packet right away. 3334cfd5779SVedang Patel * b. If the gate corresponding to the traffic class is going to open later 3344cfd5779SVedang Patel * in the cycle, set the txtime of packet to the interval start. 3354cfd5779SVedang Patel * 2. If txtime is in the future, there are packets corresponding to the 3364cfd5779SVedang Patel * current traffic class waiting to be transmitted. So, the following 3374cfd5779SVedang Patel * possibilities exist: 3384cfd5779SVedang Patel * a. We can transmit the packet before the window containing the txtime 3394cfd5779SVedang Patel * closes. 3404cfd5779SVedang Patel * b. The window might close before the transmission can be completed 3414cfd5779SVedang Patel * successfully. So, schedule the packet in the next open window. 3424cfd5779SVedang Patel */ 3434cfd5779SVedang Patel static long get_packet_txtime(struct sk_buff *skb, struct Qdisc *sch) 3444cfd5779SVedang Patel { 34554002066SVedang Patel ktime_t transmit_end_time, interval_end, interval_start, tcp_tstamp; 3464cfd5779SVedang Patel struct taprio_sched *q = qdisc_priv(sch); 3474cfd5779SVedang Patel struct sched_gate_list *sched, *admin; 3484cfd5779SVedang Patel ktime_t minimum_time, now, txtime; 3494cfd5779SVedang Patel int len, packet_transmit_time; 3504cfd5779SVedang Patel struct sched_entry *entry; 3514cfd5779SVedang Patel bool sched_changed; 3524cfd5779SVedang Patel 3537ede7b03SVedang Patel now = taprio_get_time(q); 3544cfd5779SVedang Patel minimum_time = ktime_add_ns(now, q->txtime_delay); 3554cfd5779SVedang Patel 35654002066SVedang Patel tcp_tstamp = get_tcp_tstamp(q, skb); 35754002066SVedang Patel minimum_time = max_t(ktime_t, minimum_time, tcp_tstamp); 35854002066SVedang Patel 3594cfd5779SVedang Patel rcu_read_lock(); 3604cfd5779SVedang Patel admin = rcu_dereference(q->admin_sched); 3614cfd5779SVedang Patel sched = rcu_dereference(q->oper_sched); 3624cfd5779SVedang Patel if (admin && ktime_after(minimum_time, admin->base_time)) 3634cfd5779SVedang Patel switch_schedules(q, &admin, &sched); 3644cfd5779SVedang Patel 3654cfd5779SVedang Patel /* Until the schedule starts, all the queues are open */ 3664cfd5779SVedang Patel if (!sched || ktime_before(minimum_time, sched->base_time)) { 3674cfd5779SVedang Patel txtime = minimum_time; 3684cfd5779SVedang Patel goto done; 3694cfd5779SVedang Patel } 3704cfd5779SVedang Patel 3714cfd5779SVedang Patel len = qdisc_pkt_len(skb); 3724cfd5779SVedang Patel packet_transmit_time = length_to_duration(q, len); 3734cfd5779SVedang Patel 3744cfd5779SVedang Patel do { 3750deee7aaSJiapeng Zhong sched_changed = false; 3764cfd5779SVedang Patel 3774cfd5779SVedang Patel entry = find_entry_to_transmit(skb, sch, sched, admin, 3784cfd5779SVedang Patel minimum_time, 3794cfd5779SVedang Patel &interval_start, &interval_end, 3804cfd5779SVedang Patel false); 3814cfd5779SVedang Patel if (!entry) { 3824cfd5779SVedang Patel txtime = 0; 3834cfd5779SVedang Patel goto done; 3844cfd5779SVedang Patel } 3854cfd5779SVedang Patel 3864cfd5779SVedang Patel txtime = entry->next_txtime; 3874cfd5779SVedang Patel txtime = max_t(ktime_t, txtime, minimum_time); 3884cfd5779SVedang Patel txtime = max_t(ktime_t, txtime, interval_start); 3894cfd5779SVedang Patel 3904cfd5779SVedang Patel if (admin && admin != sched && 3914cfd5779SVedang Patel ktime_after(txtime, admin->base_time)) { 3924cfd5779SVedang Patel sched = admin; 3930deee7aaSJiapeng Zhong sched_changed = true; 3944cfd5779SVedang Patel continue; 3954cfd5779SVedang Patel } 3964cfd5779SVedang Patel 3974cfd5779SVedang Patel transmit_end_time = ktime_add(txtime, packet_transmit_time); 3984cfd5779SVedang Patel minimum_time = transmit_end_time; 3994cfd5779SVedang Patel 4004cfd5779SVedang Patel /* Update the txtime of current entry to the next time it's 4014cfd5779SVedang Patel * interval starts. 4024cfd5779SVedang Patel */ 4034cfd5779SVedang Patel if (ktime_after(transmit_end_time, interval_end)) 4044cfd5779SVedang Patel entry->next_txtime = ktime_add(interval_start, sched->cycle_time); 4054cfd5779SVedang Patel } while (sched_changed || ktime_after(transmit_end_time, interval_end)); 4064cfd5779SVedang Patel 4074cfd5779SVedang Patel entry->next_txtime = transmit_end_time; 4084cfd5779SVedang Patel 4094cfd5779SVedang Patel done: 4104cfd5779SVedang Patel rcu_read_unlock(); 4114cfd5779SVedang Patel return txtime; 4124cfd5779SVedang Patel } 4134cfd5779SVedang Patel 414497cc002SKurt Kanzenbach static int taprio_enqueue_one(struct sk_buff *skb, struct Qdisc *sch, 415497cc002SKurt Kanzenbach struct Qdisc *child, struct sk_buff **to_free) 4165a781ccbSVinicius Costa Gomes { 4175a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 4185a781ccbSVinicius Costa Gomes 419e8a64bbaSBenedikt Spranger /* sk_flags are only safe to use on full sockets. */ 420e8a64bbaSBenedikt Spranger if (skb->sk && sk_fullsock(skb->sk) && sock_flag(skb->sk, SOCK_TXTIME)) { 4214cfd5779SVedang Patel if (!is_valid_interval(skb, sch)) 4224cfd5779SVedang Patel return qdisc_drop(skb, sch, to_free); 4234cfd5779SVedang Patel } else if (TXTIME_ASSIST_IS_ENABLED(q->flags)) { 4244cfd5779SVedang Patel skb->tstamp = get_packet_txtime(skb, sch); 4254cfd5779SVedang Patel if (!skb->tstamp) 4264cfd5779SVedang Patel return qdisc_drop(skb, sch, to_free); 4274cfd5779SVedang Patel } 4284cfd5779SVedang Patel 4295a781ccbSVinicius Costa Gomes qdisc_qstats_backlog_inc(sch, skb); 4305a781ccbSVinicius Costa Gomes sch->q.qlen++; 4315a781ccbSVinicius Costa Gomes 432ac5c66f2SPetr Machata return qdisc_enqueue(skb, child, to_free); 4335a781ccbSVinicius Costa Gomes } 4345a781ccbSVinicius Costa Gomes 4352c08a4f8SVladimir Oltean /* Will not be called in the full offload case, since the TX queues are 4362c08a4f8SVladimir Oltean * attached to the Qdisc created using qdisc_create_dflt() 4372c08a4f8SVladimir Oltean */ 438497cc002SKurt Kanzenbach static int taprio_enqueue(struct sk_buff *skb, struct Qdisc *sch, 439497cc002SKurt Kanzenbach struct sk_buff **to_free) 440497cc002SKurt Kanzenbach { 441497cc002SKurt Kanzenbach struct taprio_sched *q = qdisc_priv(sch); 442497cc002SKurt Kanzenbach struct Qdisc *child; 443497cc002SKurt Kanzenbach int queue; 444497cc002SKurt Kanzenbach 445497cc002SKurt Kanzenbach queue = skb_get_queue_mapping(skb); 446497cc002SKurt Kanzenbach 447497cc002SKurt Kanzenbach child = q->qdiscs[queue]; 448497cc002SKurt Kanzenbach if (unlikely(!child)) 449497cc002SKurt Kanzenbach return qdisc_drop(skb, sch, to_free); 450497cc002SKurt Kanzenbach 451497cc002SKurt Kanzenbach /* Large packets might not be transmitted when the transmission duration 452497cc002SKurt Kanzenbach * exceeds any configured interval. Therefore, segment the skb into 453fa65eddeSVladimir Oltean * smaller chunks. Drivers with full offload are expected to handle 454fa65eddeSVladimir Oltean * this in hardware. 455497cc002SKurt Kanzenbach */ 456fa65eddeSVladimir Oltean if (skb_is_gso(skb)) { 457497cc002SKurt Kanzenbach unsigned int slen = 0, numsegs = 0, len = qdisc_pkt_len(skb); 458497cc002SKurt Kanzenbach netdev_features_t features = netif_skb_features(skb); 459497cc002SKurt Kanzenbach struct sk_buff *segs, *nskb; 460497cc002SKurt Kanzenbach int ret; 461497cc002SKurt Kanzenbach 462497cc002SKurt Kanzenbach segs = skb_gso_segment(skb, features & ~NETIF_F_GSO_MASK); 463497cc002SKurt Kanzenbach if (IS_ERR_OR_NULL(segs)) 464497cc002SKurt Kanzenbach return qdisc_drop(skb, sch, to_free); 465497cc002SKurt Kanzenbach 466497cc002SKurt Kanzenbach skb_list_walk_safe(segs, segs, nskb) { 467497cc002SKurt Kanzenbach skb_mark_not_on_list(segs); 468497cc002SKurt Kanzenbach qdisc_skb_cb(segs)->pkt_len = segs->len; 469497cc002SKurt Kanzenbach slen += segs->len; 470497cc002SKurt Kanzenbach 471497cc002SKurt Kanzenbach ret = taprio_enqueue_one(segs, sch, child, to_free); 472497cc002SKurt Kanzenbach if (ret != NET_XMIT_SUCCESS) { 473497cc002SKurt Kanzenbach if (net_xmit_drop_count(ret)) 474497cc002SKurt Kanzenbach qdisc_qstats_drop(sch); 475497cc002SKurt Kanzenbach } else { 476497cc002SKurt Kanzenbach numsegs++; 477497cc002SKurt Kanzenbach } 478497cc002SKurt Kanzenbach } 479497cc002SKurt Kanzenbach 480497cc002SKurt Kanzenbach if (numsegs > 1) 481497cc002SKurt Kanzenbach qdisc_tree_reduce_backlog(sch, 1 - numsegs, len - slen); 482497cc002SKurt Kanzenbach consume_skb(skb); 483497cc002SKurt Kanzenbach 484497cc002SKurt Kanzenbach return numsegs > 0 ? NET_XMIT_SUCCESS : NET_XMIT_DROP; 485497cc002SKurt Kanzenbach } 486497cc002SKurt Kanzenbach 487497cc002SKurt Kanzenbach return taprio_enqueue_one(skb, sch, child, to_free); 488497cc002SKurt Kanzenbach } 489497cc002SKurt Kanzenbach 4902c08a4f8SVladimir Oltean /* Will not be called in the full offload case, since the TX queues are 4912c08a4f8SVladimir Oltean * attached to the Qdisc created using qdisc_create_dflt() 4922c08a4f8SVladimir Oltean */ 49325becba6SVladimir Oltean static struct sk_buff *taprio_peek(struct Qdisc *sch) 4945a781ccbSVinicius Costa Gomes { 4955a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 4965a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 4975a781ccbSVinicius Costa Gomes struct sched_entry *entry; 4985a781ccbSVinicius Costa Gomes struct sk_buff *skb; 4995a781ccbSVinicius Costa Gomes u32 gate_mask; 5005a781ccbSVinicius Costa Gomes int i; 5015a781ccbSVinicius Costa Gomes 5025a781ccbSVinicius Costa Gomes rcu_read_lock(); 5035a781ccbSVinicius Costa Gomes entry = rcu_dereference(q->current_entry); 5042684d1b7SAndre Guedes gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN; 5055a781ccbSVinicius Costa Gomes rcu_read_unlock(); 5065a781ccbSVinicius Costa Gomes 5075a781ccbSVinicius Costa Gomes if (!gate_mask) 5085a781ccbSVinicius Costa Gomes return NULL; 5095a781ccbSVinicius Costa Gomes 5105a781ccbSVinicius Costa Gomes for (i = 0; i < dev->num_tx_queues; i++) { 5115a781ccbSVinicius Costa Gomes struct Qdisc *child = q->qdiscs[i]; 5125a781ccbSVinicius Costa Gomes int prio; 5135a781ccbSVinicius Costa Gomes u8 tc; 5145a781ccbSVinicius Costa Gomes 5155a781ccbSVinicius Costa Gomes if (unlikely(!child)) 5165a781ccbSVinicius Costa Gomes continue; 5175a781ccbSVinicius Costa Gomes 5185a781ccbSVinicius Costa Gomes skb = child->ops->peek(child); 5195a781ccbSVinicius Costa Gomes if (!skb) 5205a781ccbSVinicius Costa Gomes continue; 5215a781ccbSVinicius Costa Gomes 5224cfd5779SVedang Patel if (TXTIME_ASSIST_IS_ENABLED(q->flags)) 5234cfd5779SVedang Patel return skb; 5244cfd5779SVedang Patel 5255a781ccbSVinicius Costa Gomes prio = skb->priority; 5265a781ccbSVinicius Costa Gomes tc = netdev_get_prio_tc_map(dev, prio); 5275a781ccbSVinicius Costa Gomes 5285a781ccbSVinicius Costa Gomes if (!(gate_mask & BIT(tc))) 5292684d1b7SAndre Guedes continue; 5305a781ccbSVinicius Costa Gomes 5315a781ccbSVinicius Costa Gomes return skb; 5325a781ccbSVinicius Costa Gomes } 5335a781ccbSVinicius Costa Gomes 5345a781ccbSVinicius Costa Gomes return NULL; 5355a781ccbSVinicius Costa Gomes } 5365a781ccbSVinicius Costa Gomes 53723bddf69SJakub Kicinski static void taprio_set_budget(struct taprio_sched *q, struct sched_entry *entry) 53823bddf69SJakub Kicinski { 53923bddf69SJakub Kicinski atomic_set(&entry->budget, 540837ced3aSVladimir Oltean div64_u64((u64)entry->interval * PSEC_PER_NSEC, 54123bddf69SJakub Kicinski atomic64_read(&q->picos_per_byte))); 5425a781ccbSVinicius Costa Gomes } 5435a781ccbSVinicius Costa Gomes 5442c08a4f8SVladimir Oltean /* Will not be called in the full offload case, since the TX queues are 5452c08a4f8SVladimir Oltean * attached to the Qdisc created using qdisc_create_dflt() 5462c08a4f8SVladimir Oltean */ 54725becba6SVladimir Oltean static struct sk_buff *taprio_dequeue(struct Qdisc *sch) 5485a781ccbSVinicius Costa Gomes { 5495a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 5505a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 5518c79f0eaSVinicius Costa Gomes struct sk_buff *skb = NULL; 5525a781ccbSVinicius Costa Gomes struct sched_entry *entry; 5535a781ccbSVinicius Costa Gomes u32 gate_mask; 5545a781ccbSVinicius Costa Gomes int i; 5555a781ccbSVinicius Costa Gomes 5565a781ccbSVinicius Costa Gomes rcu_read_lock(); 5575a781ccbSVinicius Costa Gomes entry = rcu_dereference(q->current_entry); 5585a781ccbSVinicius Costa Gomes /* if there's no entry, it means that the schedule didn't 5595a781ccbSVinicius Costa Gomes * start yet, so force all gates to be open, this is in 5605a781ccbSVinicius Costa Gomes * accordance to IEEE 802.1Qbv-2015 Section 8.6.9.4.5 561633fa666SJesper Dangaard Brouer * "AdminGateStates" 5625a781ccbSVinicius Costa Gomes */ 5635a781ccbSVinicius Costa Gomes gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN; 5645a781ccbSVinicius Costa Gomes 5655a781ccbSVinicius Costa Gomes if (!gate_mask) 5668c79f0eaSVinicius Costa Gomes goto done; 5675a781ccbSVinicius Costa Gomes 5685a781ccbSVinicius Costa Gomes for (i = 0; i < dev->num_tx_queues; i++) { 5695a781ccbSVinicius Costa Gomes struct Qdisc *child = q->qdiscs[i]; 5705a781ccbSVinicius Costa Gomes ktime_t guard; 5715a781ccbSVinicius Costa Gomes int prio; 5725a781ccbSVinicius Costa Gomes int len; 5735a781ccbSVinicius Costa Gomes u8 tc; 5745a781ccbSVinicius Costa Gomes 5755a781ccbSVinicius Costa Gomes if (unlikely(!child)) 5765a781ccbSVinicius Costa Gomes continue; 5775a781ccbSVinicius Costa Gomes 5784cfd5779SVedang Patel if (TXTIME_ASSIST_IS_ENABLED(q->flags)) { 5794cfd5779SVedang Patel skb = child->ops->dequeue(child); 5804cfd5779SVedang Patel if (!skb) 5814cfd5779SVedang Patel continue; 5824cfd5779SVedang Patel goto skb_found; 5834cfd5779SVedang Patel } 5844cfd5779SVedang Patel 5855a781ccbSVinicius Costa Gomes skb = child->ops->peek(child); 5865a781ccbSVinicius Costa Gomes if (!skb) 5875a781ccbSVinicius Costa Gomes continue; 5885a781ccbSVinicius Costa Gomes 5895a781ccbSVinicius Costa Gomes prio = skb->priority; 5905a781ccbSVinicius Costa Gomes tc = netdev_get_prio_tc_map(dev, prio); 5915a781ccbSVinicius Costa Gomes 592b09fe70eSVinicius Costa Gomes if (!(gate_mask & BIT(tc))) { 593b09fe70eSVinicius Costa Gomes skb = NULL; 5945a781ccbSVinicius Costa Gomes continue; 595b09fe70eSVinicius Costa Gomes } 5965a781ccbSVinicius Costa Gomes 5975a781ccbSVinicius Costa Gomes len = qdisc_pkt_len(skb); 5987ede7b03SVedang Patel guard = ktime_add_ns(taprio_get_time(q), 5995a781ccbSVinicius Costa Gomes length_to_duration(q, len)); 6005a781ccbSVinicius Costa Gomes 6015a781ccbSVinicius Costa Gomes /* In the case that there's no gate entry, there's no 6025a781ccbSVinicius Costa Gomes * guard band ... 6035a781ccbSVinicius Costa Gomes */ 6045a781ccbSVinicius Costa Gomes if (gate_mask != TAPRIO_ALL_GATES_OPEN && 605b09fe70eSVinicius Costa Gomes ktime_after(guard, entry->close_time)) { 606b09fe70eSVinicius Costa Gomes skb = NULL; 6076e734c82SAndre Guedes continue; 608b09fe70eSVinicius Costa Gomes } 6095a781ccbSVinicius Costa Gomes 6105a781ccbSVinicius Costa Gomes /* ... and no budget. */ 6115a781ccbSVinicius Costa Gomes if (gate_mask != TAPRIO_ALL_GATES_OPEN && 612b09fe70eSVinicius Costa Gomes atomic_sub_return(len, &entry->budget) < 0) { 613b09fe70eSVinicius Costa Gomes skb = NULL; 6146e734c82SAndre Guedes continue; 615b09fe70eSVinicius Costa Gomes } 6165a781ccbSVinicius Costa Gomes 6175a781ccbSVinicius Costa Gomes skb = child->ops->dequeue(child); 6185a781ccbSVinicius Costa Gomes if (unlikely(!skb)) 6198c79f0eaSVinicius Costa Gomes goto done; 6205a781ccbSVinicius Costa Gomes 6214cfd5779SVedang Patel skb_found: 6225a781ccbSVinicius Costa Gomes qdisc_bstats_update(sch, skb); 6235a781ccbSVinicius Costa Gomes qdisc_qstats_backlog_dec(sch, skb); 6245a781ccbSVinicius Costa Gomes sch->q.qlen--; 6255a781ccbSVinicius Costa Gomes 6268c79f0eaSVinicius Costa Gomes goto done; 6275a781ccbSVinicius Costa Gomes } 6285a781ccbSVinicius Costa Gomes 6298c79f0eaSVinicius Costa Gomes done: 6308c79f0eaSVinicius Costa Gomes rcu_read_unlock(); 6318c79f0eaSVinicius Costa Gomes 6328c79f0eaSVinicius Costa Gomes return skb; 6335a781ccbSVinicius Costa Gomes } 6345a781ccbSVinicius Costa Gomes 6356ca6a665SVinicius Costa Gomes static bool should_restart_cycle(const struct sched_gate_list *oper, 6366ca6a665SVinicius Costa Gomes const struct sched_entry *entry) 6376ca6a665SVinicius Costa Gomes { 6386ca6a665SVinicius Costa Gomes if (list_is_last(&entry->list, &oper->entries)) 6396ca6a665SVinicius Costa Gomes return true; 6406ca6a665SVinicius Costa Gomes 6416ca6a665SVinicius Costa Gomes if (ktime_compare(entry->close_time, oper->cycle_close_time) == 0) 6426ca6a665SVinicius Costa Gomes return true; 6436ca6a665SVinicius Costa Gomes 6446ca6a665SVinicius Costa Gomes return false; 6456ca6a665SVinicius Costa Gomes } 6466ca6a665SVinicius Costa Gomes 647a3d43c0dSVinicius Costa Gomes static bool should_change_schedules(const struct sched_gate_list *admin, 648a3d43c0dSVinicius Costa Gomes const struct sched_gate_list *oper, 649a3d43c0dSVinicius Costa Gomes ktime_t close_time) 650a3d43c0dSVinicius Costa Gomes { 651c25031e9SVinicius Costa Gomes ktime_t next_base_time, extension_time; 652a3d43c0dSVinicius Costa Gomes 653a3d43c0dSVinicius Costa Gomes if (!admin) 654a3d43c0dSVinicius Costa Gomes return false; 655a3d43c0dSVinicius Costa Gomes 656a3d43c0dSVinicius Costa Gomes next_base_time = sched_base_time(admin); 657a3d43c0dSVinicius Costa Gomes 658a3d43c0dSVinicius Costa Gomes /* This is the simple case, the close_time would fall after 659a3d43c0dSVinicius Costa Gomes * the next schedule base_time. 660a3d43c0dSVinicius Costa Gomes */ 661a3d43c0dSVinicius Costa Gomes if (ktime_compare(next_base_time, close_time) <= 0) 662a3d43c0dSVinicius Costa Gomes return true; 663a3d43c0dSVinicius Costa Gomes 664c25031e9SVinicius Costa Gomes /* This is the cycle_time_extension case, if the close_time 665c25031e9SVinicius Costa Gomes * plus the amount that can be extended would fall after the 666c25031e9SVinicius Costa Gomes * next schedule base_time, we can extend the current schedule 667c25031e9SVinicius Costa Gomes * for that amount. 668c25031e9SVinicius Costa Gomes */ 669c25031e9SVinicius Costa Gomes extension_time = ktime_add_ns(close_time, oper->cycle_time_extension); 670c25031e9SVinicius Costa Gomes 671c25031e9SVinicius Costa Gomes /* FIXME: the IEEE 802.1Q-2018 Specification isn't clear about 672c25031e9SVinicius Costa Gomes * how precisely the extension should be made. So after 673c25031e9SVinicius Costa Gomes * conformance testing, this logic may change. 674c25031e9SVinicius Costa Gomes */ 675c25031e9SVinicius Costa Gomes if (ktime_compare(next_base_time, extension_time) <= 0) 676c25031e9SVinicius Costa Gomes return true; 677c25031e9SVinicius Costa Gomes 678a3d43c0dSVinicius Costa Gomes return false; 679a3d43c0dSVinicius Costa Gomes } 680a3d43c0dSVinicius Costa Gomes 6815a781ccbSVinicius Costa Gomes static enum hrtimer_restart advance_sched(struct hrtimer *timer) 6825a781ccbSVinicius Costa Gomes { 6835a781ccbSVinicius Costa Gomes struct taprio_sched *q = container_of(timer, struct taprio_sched, 6845a781ccbSVinicius Costa Gomes advance_timer); 685a3d43c0dSVinicius Costa Gomes struct sched_gate_list *oper, *admin; 6865a781ccbSVinicius Costa Gomes struct sched_entry *entry, *next; 6875a781ccbSVinicius Costa Gomes struct Qdisc *sch = q->root; 6885a781ccbSVinicius Costa Gomes ktime_t close_time; 6895a781ccbSVinicius Costa Gomes 6905a781ccbSVinicius Costa Gomes spin_lock(&q->current_entry_lock); 6915a781ccbSVinicius Costa Gomes entry = rcu_dereference_protected(q->current_entry, 6925a781ccbSVinicius Costa Gomes lockdep_is_held(&q->current_entry_lock)); 693a3d43c0dSVinicius Costa Gomes oper = rcu_dereference_protected(q->oper_sched, 694a3d43c0dSVinicius Costa Gomes lockdep_is_held(&q->current_entry_lock)); 695a3d43c0dSVinicius Costa Gomes admin = rcu_dereference_protected(q->admin_sched, 696a3d43c0dSVinicius Costa Gomes lockdep_is_held(&q->current_entry_lock)); 6975a781ccbSVinicius Costa Gomes 698a3d43c0dSVinicius Costa Gomes if (!oper) 699a3d43c0dSVinicius Costa Gomes switch_schedules(q, &admin, &oper); 700a3d43c0dSVinicius Costa Gomes 701a3d43c0dSVinicius Costa Gomes /* This can happen in two cases: 1. this is the very first run 702a3d43c0dSVinicius Costa Gomes * of this function (i.e. we weren't running any schedule 703a3d43c0dSVinicius Costa Gomes * previously); 2. The previous schedule just ended. The first 704a3d43c0dSVinicius Costa Gomes * entry of all schedules are pre-calculated during the 705a3d43c0dSVinicius Costa Gomes * schedule initialization. 7065a781ccbSVinicius Costa Gomes */ 707a3d43c0dSVinicius Costa Gomes if (unlikely(!entry || entry->close_time == oper->base_time)) { 708a3d43c0dSVinicius Costa Gomes next = list_first_entry(&oper->entries, struct sched_entry, 7095a781ccbSVinicius Costa Gomes list); 7105a781ccbSVinicius Costa Gomes close_time = next->close_time; 7115a781ccbSVinicius Costa Gomes goto first_run; 7125a781ccbSVinicius Costa Gomes } 7135a781ccbSVinicius Costa Gomes 7146ca6a665SVinicius Costa Gomes if (should_restart_cycle(oper, entry)) { 715a3d43c0dSVinicius Costa Gomes next = list_first_entry(&oper->entries, struct sched_entry, 7165a781ccbSVinicius Costa Gomes list); 7176ca6a665SVinicius Costa Gomes oper->cycle_close_time = ktime_add_ns(oper->cycle_close_time, 7186ca6a665SVinicius Costa Gomes oper->cycle_time); 7196ca6a665SVinicius Costa Gomes } else { 7205a781ccbSVinicius Costa Gomes next = list_next_entry(entry, list); 7216ca6a665SVinicius Costa Gomes } 7225a781ccbSVinicius Costa Gomes 7235a781ccbSVinicius Costa Gomes close_time = ktime_add_ns(entry->close_time, next->interval); 7246ca6a665SVinicius Costa Gomes close_time = min_t(ktime_t, close_time, oper->cycle_close_time); 7255a781ccbSVinicius Costa Gomes 726a3d43c0dSVinicius Costa Gomes if (should_change_schedules(admin, oper, close_time)) { 727a3d43c0dSVinicius Costa Gomes /* Set things so the next time this runs, the new 728a3d43c0dSVinicius Costa Gomes * schedule runs. 729a3d43c0dSVinicius Costa Gomes */ 730a3d43c0dSVinicius Costa Gomes close_time = sched_base_time(admin); 731a3d43c0dSVinicius Costa Gomes switch_schedules(q, &admin, &oper); 732a3d43c0dSVinicius Costa Gomes } 733a3d43c0dSVinicius Costa Gomes 7345a781ccbSVinicius Costa Gomes next->close_time = close_time; 73523bddf69SJakub Kicinski taprio_set_budget(q, next); 7365a781ccbSVinicius Costa Gomes 7375a781ccbSVinicius Costa Gomes first_run: 7385a781ccbSVinicius Costa Gomes rcu_assign_pointer(q->current_entry, next); 7395a781ccbSVinicius Costa Gomes spin_unlock(&q->current_entry_lock); 7405a781ccbSVinicius Costa Gomes 7415a781ccbSVinicius Costa Gomes hrtimer_set_expires(&q->advance_timer, close_time); 7425a781ccbSVinicius Costa Gomes 7435a781ccbSVinicius Costa Gomes rcu_read_lock(); 7445a781ccbSVinicius Costa Gomes __netif_schedule(sch); 7455a781ccbSVinicius Costa Gomes rcu_read_unlock(); 7465a781ccbSVinicius Costa Gomes 7475a781ccbSVinicius Costa Gomes return HRTIMER_RESTART; 7485a781ccbSVinicius Costa Gomes } 7495a781ccbSVinicius Costa Gomes 7505a781ccbSVinicius Costa Gomes static const struct nla_policy entry_policy[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = { 7515a781ccbSVinicius Costa Gomes [TCA_TAPRIO_SCHED_ENTRY_INDEX] = { .type = NLA_U32 }, 7525a781ccbSVinicius Costa Gomes [TCA_TAPRIO_SCHED_ENTRY_CMD] = { .type = NLA_U8 }, 7535a781ccbSVinicius Costa Gomes [TCA_TAPRIO_SCHED_ENTRY_GATE_MASK] = { .type = NLA_U32 }, 7545a781ccbSVinicius Costa Gomes [TCA_TAPRIO_SCHED_ENTRY_INTERVAL] = { .type = NLA_U32 }, 7555a781ccbSVinicius Costa Gomes }; 7565a781ccbSVinicius Costa Gomes 7575a781ccbSVinicius Costa Gomes static const struct nla_policy taprio_policy[TCA_TAPRIO_ATTR_MAX + 1] = { 7585a781ccbSVinicius Costa Gomes [TCA_TAPRIO_ATTR_PRIOMAP] = { 7595a781ccbSVinicius Costa Gomes .len = sizeof(struct tc_mqprio_qopt) 7605a781ccbSVinicius Costa Gomes }, 7615a781ccbSVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST] = { .type = NLA_NESTED }, 7625a781ccbSVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_BASE_TIME] = { .type = NLA_S64 }, 7635a781ccbSVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY] = { .type = NLA_NESTED }, 7645a781ccbSVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_CLOCKID] = { .type = NLA_S32 }, 7656ca6a665SVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME] = { .type = NLA_S64 }, 766c25031e9SVinicius Costa Gomes [TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION] = { .type = NLA_S64 }, 76749c684d7SVinicius Costa Gomes [TCA_TAPRIO_ATTR_FLAGS] = { .type = NLA_U32 }, 768e13aaa06SJakub Kicinski [TCA_TAPRIO_ATTR_TXTIME_DELAY] = { .type = NLA_U32 }, 7695a781ccbSVinicius Costa Gomes }; 7705a781ccbSVinicius Costa Gomes 771b5b73b26SVinicius Costa Gomes static int fill_sched_entry(struct taprio_sched *q, struct nlattr **tb, 772b5b73b26SVinicius Costa Gomes struct sched_entry *entry, 7735a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 7745a781ccbSVinicius Costa Gomes { 775b5b73b26SVinicius Costa Gomes int min_duration = length_to_duration(q, ETH_ZLEN); 7765a781ccbSVinicius Costa Gomes u32 interval = 0; 7775a781ccbSVinicius Costa Gomes 7785a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_SCHED_ENTRY_CMD]) 7795a781ccbSVinicius Costa Gomes entry->command = nla_get_u8( 7805a781ccbSVinicius Costa Gomes tb[TCA_TAPRIO_SCHED_ENTRY_CMD]); 7815a781ccbSVinicius Costa Gomes 7825a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK]) 7835a781ccbSVinicius Costa Gomes entry->gate_mask = nla_get_u32( 7845a781ccbSVinicius Costa Gomes tb[TCA_TAPRIO_SCHED_ENTRY_GATE_MASK]); 7855a781ccbSVinicius Costa Gomes 7865a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL]) 7875a781ccbSVinicius Costa Gomes interval = nla_get_u32( 7885a781ccbSVinicius Costa Gomes tb[TCA_TAPRIO_SCHED_ENTRY_INTERVAL]); 7895a781ccbSVinicius Costa Gomes 790b5b73b26SVinicius Costa Gomes /* The interval should allow at least the minimum ethernet 791b5b73b26SVinicius Costa Gomes * frame to go out. 792b5b73b26SVinicius Costa Gomes */ 793b5b73b26SVinicius Costa Gomes if (interval < min_duration) { 7945a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Invalid interval for schedule entry"); 7955a781ccbSVinicius Costa Gomes return -EINVAL; 7965a781ccbSVinicius Costa Gomes } 7975a781ccbSVinicius Costa Gomes 7985a781ccbSVinicius Costa Gomes entry->interval = interval; 7995a781ccbSVinicius Costa Gomes 8005a781ccbSVinicius Costa Gomes return 0; 8015a781ccbSVinicius Costa Gomes } 8025a781ccbSVinicius Costa Gomes 803b5b73b26SVinicius Costa Gomes static int parse_sched_entry(struct taprio_sched *q, struct nlattr *n, 804b5b73b26SVinicius Costa Gomes struct sched_entry *entry, int index, 805b5b73b26SVinicius Costa Gomes struct netlink_ext_ack *extack) 8065a781ccbSVinicius Costa Gomes { 8075a781ccbSVinicius Costa Gomes struct nlattr *tb[TCA_TAPRIO_SCHED_ENTRY_MAX + 1] = { }; 8085a781ccbSVinicius Costa Gomes int err; 8095a781ccbSVinicius Costa Gomes 8108cb08174SJohannes Berg err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_SCHED_ENTRY_MAX, n, 8115a781ccbSVinicius Costa Gomes entry_policy, NULL); 8125a781ccbSVinicius Costa Gomes if (err < 0) { 8135a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Could not parse nested entry"); 8145a781ccbSVinicius Costa Gomes return -EINVAL; 8155a781ccbSVinicius Costa Gomes } 8165a781ccbSVinicius Costa Gomes 8175a781ccbSVinicius Costa Gomes entry->index = index; 8185a781ccbSVinicius Costa Gomes 819b5b73b26SVinicius Costa Gomes return fill_sched_entry(q, tb, entry, extack); 8205a781ccbSVinicius Costa Gomes } 8215a781ccbSVinicius Costa Gomes 822b5b73b26SVinicius Costa Gomes static int parse_sched_list(struct taprio_sched *q, struct nlattr *list, 823a3d43c0dSVinicius Costa Gomes struct sched_gate_list *sched, 8245a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 8255a781ccbSVinicius Costa Gomes { 8265a781ccbSVinicius Costa Gomes struct nlattr *n; 8275a781ccbSVinicius Costa Gomes int err, rem; 8285a781ccbSVinicius Costa Gomes int i = 0; 8295a781ccbSVinicius Costa Gomes 8305a781ccbSVinicius Costa Gomes if (!list) 8315a781ccbSVinicius Costa Gomes return -EINVAL; 8325a781ccbSVinicius Costa Gomes 8335a781ccbSVinicius Costa Gomes nla_for_each_nested(n, list, rem) { 8345a781ccbSVinicius Costa Gomes struct sched_entry *entry; 8355a781ccbSVinicius Costa Gomes 8365a781ccbSVinicius Costa Gomes if (nla_type(n) != TCA_TAPRIO_SCHED_ENTRY) { 8375a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Attribute is not of type 'entry'"); 8385a781ccbSVinicius Costa Gomes continue; 8395a781ccbSVinicius Costa Gomes } 8405a781ccbSVinicius Costa Gomes 8415a781ccbSVinicius Costa Gomes entry = kzalloc(sizeof(*entry), GFP_KERNEL); 8425a781ccbSVinicius Costa Gomes if (!entry) { 8435a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Not enough memory for entry"); 8445a781ccbSVinicius Costa Gomes return -ENOMEM; 8455a781ccbSVinicius Costa Gomes } 8465a781ccbSVinicius Costa Gomes 847b5b73b26SVinicius Costa Gomes err = parse_sched_entry(q, n, entry, i, extack); 8485a781ccbSVinicius Costa Gomes if (err < 0) { 8495a781ccbSVinicius Costa Gomes kfree(entry); 8505a781ccbSVinicius Costa Gomes return err; 8515a781ccbSVinicius Costa Gomes } 8525a781ccbSVinicius Costa Gomes 853a3d43c0dSVinicius Costa Gomes list_add_tail(&entry->list, &sched->entries); 8545a781ccbSVinicius Costa Gomes i++; 8555a781ccbSVinicius Costa Gomes } 8565a781ccbSVinicius Costa Gomes 857a3d43c0dSVinicius Costa Gomes sched->num_entries = i; 8585a781ccbSVinicius Costa Gomes 8595a781ccbSVinicius Costa Gomes return i; 8605a781ccbSVinicius Costa Gomes } 8615a781ccbSVinicius Costa Gomes 862b5b73b26SVinicius Costa Gomes static int parse_taprio_schedule(struct taprio_sched *q, struct nlattr **tb, 863a3d43c0dSVinicius Costa Gomes struct sched_gate_list *new, 8645a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 8655a781ccbSVinicius Costa Gomes { 8665a781ccbSVinicius Costa Gomes int err = 0; 8675a781ccbSVinicius Costa Gomes 868a3d43c0dSVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_SINGLE_ENTRY]) { 869a3d43c0dSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Adding a single entry is not supported"); 870a3d43c0dSVinicius Costa Gomes return -ENOTSUPP; 871a3d43c0dSVinicius Costa Gomes } 8725a781ccbSVinicius Costa Gomes 8735a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME]) 874a3d43c0dSVinicius Costa Gomes new->base_time = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_BASE_TIME]); 8755a781ccbSVinicius Costa Gomes 876c25031e9SVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION]) 877c25031e9SVinicius Costa Gomes new->cycle_time_extension = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION]); 878c25031e9SVinicius Costa Gomes 8796ca6a665SVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME]) 8806ca6a665SVinicius Costa Gomes new->cycle_time = nla_get_s64(tb[TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME]); 8816ca6a665SVinicius Costa Gomes 8825a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST]) 883b5b73b26SVinicius Costa Gomes err = parse_sched_list(q, tb[TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST], 884b5b73b26SVinicius Costa Gomes new, extack); 885a3d43c0dSVinicius Costa Gomes if (err < 0) 8865a781ccbSVinicius Costa Gomes return err; 887a3d43c0dSVinicius Costa Gomes 888037be037SVedang Patel if (!new->cycle_time) { 889037be037SVedang Patel struct sched_entry *entry; 890037be037SVedang Patel ktime_t cycle = 0; 891037be037SVedang Patel 892037be037SVedang Patel list_for_each_entry(entry, &new->entries, list) 893037be037SVedang Patel cycle = ktime_add_ns(cycle, entry->interval); 894ed8157f1SDu Cheng 895ed8157f1SDu Cheng if (!cycle) { 896ed8157f1SDu Cheng NL_SET_ERR_MSG(extack, "'cycle_time' can never be 0"); 897ed8157f1SDu Cheng return -EINVAL; 898ed8157f1SDu Cheng } 899ed8157f1SDu Cheng 900037be037SVedang Patel new->cycle_time = cycle; 901037be037SVedang Patel } 902037be037SVedang Patel 903a3d43c0dSVinicius Costa Gomes return 0; 9045a781ccbSVinicius Costa Gomes } 9055a781ccbSVinicius Costa Gomes 9065a781ccbSVinicius Costa Gomes static int taprio_parse_mqprio_opt(struct net_device *dev, 9075a781ccbSVinicius Costa Gomes struct tc_mqprio_qopt *qopt, 9084cfd5779SVedang Patel struct netlink_ext_ack *extack, 9094cfd5779SVedang Patel u32 taprio_flags) 9105a781ccbSVinicius Costa Gomes { 9115a781ccbSVinicius Costa Gomes int i, j; 9125a781ccbSVinicius Costa Gomes 913a3d43c0dSVinicius Costa Gomes if (!qopt && !dev->num_tc) { 9145a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "'mqprio' configuration is necessary"); 9155a781ccbSVinicius Costa Gomes return -EINVAL; 9165a781ccbSVinicius Costa Gomes } 9175a781ccbSVinicius Costa Gomes 918a3d43c0dSVinicius Costa Gomes /* If num_tc is already set, it means that the user already 919a3d43c0dSVinicius Costa Gomes * configured the mqprio part 920a3d43c0dSVinicius Costa Gomes */ 921a3d43c0dSVinicius Costa Gomes if (dev->num_tc) 922a3d43c0dSVinicius Costa Gomes return 0; 923a3d43c0dSVinicius Costa Gomes 9245a781ccbSVinicius Costa Gomes /* Verify num_tc is not out of max range */ 9255a781ccbSVinicius Costa Gomes if (qopt->num_tc > TC_MAX_QUEUE) { 9265a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Number of traffic classes is outside valid range"); 9275a781ccbSVinicius Costa Gomes return -EINVAL; 9285a781ccbSVinicius Costa Gomes } 9295a781ccbSVinicius Costa Gomes 9305a781ccbSVinicius Costa Gomes /* taprio imposes that traffic classes map 1:n to tx queues */ 9315a781ccbSVinicius Costa Gomes if (qopt->num_tc > dev->num_tx_queues) { 9325a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Number of traffic classes is greater than number of HW queues"); 9335a781ccbSVinicius Costa Gomes return -EINVAL; 9345a781ccbSVinicius Costa Gomes } 9355a781ccbSVinicius Costa Gomes 9365a781ccbSVinicius Costa Gomes /* Verify priority mapping uses valid tcs */ 937b5a0faa3SIvan Khoronzhuk for (i = 0; i <= TC_BITMASK; i++) { 9385a781ccbSVinicius Costa Gomes if (qopt->prio_tc_map[i] >= qopt->num_tc) { 9395a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Invalid traffic class in priority to traffic class mapping"); 9405a781ccbSVinicius Costa Gomes return -EINVAL; 9415a781ccbSVinicius Costa Gomes } 9425a781ccbSVinicius Costa Gomes } 9435a781ccbSVinicius Costa Gomes 9445a781ccbSVinicius Costa Gomes for (i = 0; i < qopt->num_tc; i++) { 9455a781ccbSVinicius Costa Gomes unsigned int last = qopt->offset[i] + qopt->count[i]; 9465a781ccbSVinicius Costa Gomes 9475a781ccbSVinicius Costa Gomes /* Verify the queue count is in tx range being equal to the 9485a781ccbSVinicius Costa Gomes * real_num_tx_queues indicates the last queue is in use. 9495a781ccbSVinicius Costa Gomes */ 9505a781ccbSVinicius Costa Gomes if (qopt->offset[i] >= dev->num_tx_queues || 9515a781ccbSVinicius Costa Gomes !qopt->count[i] || 9525a781ccbSVinicius Costa Gomes last > dev->real_num_tx_queues) { 9535a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Invalid queue in traffic class to queue mapping"); 9545a781ccbSVinicius Costa Gomes return -EINVAL; 9555a781ccbSVinicius Costa Gomes } 9565a781ccbSVinicius Costa Gomes 9574cfd5779SVedang Patel if (TXTIME_ASSIST_IS_ENABLED(taprio_flags)) 9584cfd5779SVedang Patel continue; 9594cfd5779SVedang Patel 9605a781ccbSVinicius Costa Gomes /* Verify that the offset and counts do not overlap */ 9615a781ccbSVinicius Costa Gomes for (j = i + 1; j < qopt->num_tc; j++) { 9625a781ccbSVinicius Costa Gomes if (last > qopt->offset[j]) { 9635a781ccbSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Detected overlap in the traffic class to queue mapping"); 9645a781ccbSVinicius Costa Gomes return -EINVAL; 9655a781ccbSVinicius Costa Gomes } 9665a781ccbSVinicius Costa Gomes } 9675a781ccbSVinicius Costa Gomes } 9685a781ccbSVinicius Costa Gomes 9695a781ccbSVinicius Costa Gomes return 0; 9705a781ccbSVinicius Costa Gomes } 9715a781ccbSVinicius Costa Gomes 972a3d43c0dSVinicius Costa Gomes static int taprio_get_start_time(struct Qdisc *sch, 973a3d43c0dSVinicius Costa Gomes struct sched_gate_list *sched, 974a3d43c0dSVinicius Costa Gomes ktime_t *start) 9755a781ccbSVinicius Costa Gomes { 9765a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 9775a781ccbSVinicius Costa Gomes ktime_t now, base, cycle; 9785a781ccbSVinicius Costa Gomes s64 n; 9795a781ccbSVinicius Costa Gomes 980a3d43c0dSVinicius Costa Gomes base = sched_base_time(sched); 9817ede7b03SVedang Patel now = taprio_get_time(q); 9828599099fSAndre Guedes 9838599099fSAndre Guedes if (ktime_after(base, now)) { 9848599099fSAndre Guedes *start = base; 9858599099fSAndre Guedes return 0; 9868599099fSAndre Guedes } 9875a781ccbSVinicius Costa Gomes 988037be037SVedang Patel cycle = sched->cycle_time; 9895a781ccbSVinicius Costa Gomes 9908599099fSAndre Guedes /* The qdisc is expected to have at least one sched_entry. Moreover, 9918599099fSAndre Guedes * any entry must have 'interval' > 0. Thus if the cycle time is zero, 9928599099fSAndre Guedes * something went really wrong. In that case, we should warn about this 9938599099fSAndre Guedes * inconsistent state and return error. 9948599099fSAndre Guedes */ 9958599099fSAndre Guedes if (WARN_ON(!cycle)) 9968599099fSAndre Guedes return -EFAULT; 9975a781ccbSVinicius Costa Gomes 9985a781ccbSVinicius Costa Gomes /* Schedule the start time for the beginning of the next 9995a781ccbSVinicius Costa Gomes * cycle. 10005a781ccbSVinicius Costa Gomes */ 10015a781ccbSVinicius Costa Gomes n = div64_s64(ktime_sub_ns(now, base), cycle); 10028599099fSAndre Guedes *start = ktime_add_ns(base, (n + 1) * cycle); 10038599099fSAndre Guedes return 0; 10045a781ccbSVinicius Costa Gomes } 10055a781ccbSVinicius Costa Gomes 1006a3d43c0dSVinicius Costa Gomes static void setup_first_close_time(struct taprio_sched *q, 1007a3d43c0dSVinicius Costa Gomes struct sched_gate_list *sched, ktime_t base) 10085a781ccbSVinicius Costa Gomes { 10095a781ccbSVinicius Costa Gomes struct sched_entry *first; 10106ca6a665SVinicius Costa Gomes ktime_t cycle; 10115a781ccbSVinicius Costa Gomes 1012a3d43c0dSVinicius Costa Gomes first = list_first_entry(&sched->entries, 1013a3d43c0dSVinicius Costa Gomes struct sched_entry, list); 10145a781ccbSVinicius Costa Gomes 1015037be037SVedang Patel cycle = sched->cycle_time; 10166ca6a665SVinicius Costa Gomes 10176ca6a665SVinicius Costa Gomes /* FIXME: find a better place to do this */ 10186ca6a665SVinicius Costa Gomes sched->cycle_close_time = ktime_add_ns(base, cycle); 10196ca6a665SVinicius Costa Gomes 1020a3d43c0dSVinicius Costa Gomes first->close_time = ktime_add_ns(base, first->interval); 102123bddf69SJakub Kicinski taprio_set_budget(q, first); 10225a781ccbSVinicius Costa Gomes rcu_assign_pointer(q->current_entry, NULL); 1023a3d43c0dSVinicius Costa Gomes } 10245a781ccbSVinicius Costa Gomes 1025a3d43c0dSVinicius Costa Gomes static void taprio_start_sched(struct Qdisc *sch, 1026a3d43c0dSVinicius Costa Gomes ktime_t start, struct sched_gate_list *new) 1027a3d43c0dSVinicius Costa Gomes { 1028a3d43c0dSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 1029a3d43c0dSVinicius Costa Gomes ktime_t expires; 1030a3d43c0dSVinicius Costa Gomes 10319c66d156SVinicius Costa Gomes if (FULL_OFFLOAD_IS_ENABLED(q->flags)) 10329c66d156SVinicius Costa Gomes return; 10339c66d156SVinicius Costa Gomes 1034a3d43c0dSVinicius Costa Gomes expires = hrtimer_get_expires(&q->advance_timer); 1035a3d43c0dSVinicius Costa Gomes if (expires == 0) 1036a3d43c0dSVinicius Costa Gomes expires = KTIME_MAX; 1037a3d43c0dSVinicius Costa Gomes 1038a3d43c0dSVinicius Costa Gomes /* If the new schedule starts before the next expiration, we 1039a3d43c0dSVinicius Costa Gomes * reprogram it to the earliest one, so we change the admin 1040a3d43c0dSVinicius Costa Gomes * schedule to the operational one at the right time. 1041a3d43c0dSVinicius Costa Gomes */ 1042a3d43c0dSVinicius Costa Gomes start = min_t(ktime_t, start, expires); 10435a781ccbSVinicius Costa Gomes 10445a781ccbSVinicius Costa Gomes hrtimer_start(&q->advance_timer, start, HRTIMER_MODE_ABS); 10455a781ccbSVinicius Costa Gomes } 10465a781ccbSVinicius Costa Gomes 10477b9eba7bSLeandro Dorileo static void taprio_set_picos_per_byte(struct net_device *dev, 10487b9eba7bSLeandro Dorileo struct taprio_sched *q) 10497b9eba7bSLeandro Dorileo { 10507b9eba7bSLeandro Dorileo struct ethtool_link_ksettings ecmd; 1051f04b514cSVladimir Oltean int speed = SPEED_10; 1052f04b514cSVladimir Oltean int picos_per_byte; 1053f04b514cSVladimir Oltean int err; 10547b9eba7bSLeandro Dorileo 1055f04b514cSVladimir Oltean err = __ethtool_get_link_ksettings(dev, &ecmd); 1056f04b514cSVladimir Oltean if (err < 0) 1057f04b514cSVladimir Oltean goto skip; 1058f04b514cSVladimir Oltean 10599a9251a3SVladimir Oltean if (ecmd.base.speed && ecmd.base.speed != SPEED_UNKNOWN) 1060f04b514cSVladimir Oltean speed = ecmd.base.speed; 1061f04b514cSVladimir Oltean 1062f04b514cSVladimir Oltean skip: 106368ce6688SVladimir Oltean picos_per_byte = (USEC_PER_SEC * 8) / speed; 10647b9eba7bSLeandro Dorileo 10657b9eba7bSLeandro Dorileo atomic64_set(&q->picos_per_byte, picos_per_byte); 10667b9eba7bSLeandro Dorileo netdev_dbg(dev, "taprio: set %s's picos_per_byte to: %lld, linkspeed: %d\n", 10677b9eba7bSLeandro Dorileo dev->name, (long long)atomic64_read(&q->picos_per_byte), 10687b9eba7bSLeandro Dorileo ecmd.base.speed); 10697b9eba7bSLeandro Dorileo } 10707b9eba7bSLeandro Dorileo 10717b9eba7bSLeandro Dorileo static int taprio_dev_notifier(struct notifier_block *nb, unsigned long event, 10727b9eba7bSLeandro Dorileo void *ptr) 10737b9eba7bSLeandro Dorileo { 10747b9eba7bSLeandro Dorileo struct net_device *dev = netdev_notifier_info_to_dev(ptr); 10757b9eba7bSLeandro Dorileo struct taprio_sched *q; 10767b9eba7bSLeandro Dorileo 10777b9eba7bSLeandro Dorileo ASSERT_RTNL(); 10787b9eba7bSLeandro Dorileo 10797b9eba7bSLeandro Dorileo if (event != NETDEV_UP && event != NETDEV_CHANGE) 10807b9eba7bSLeandro Dorileo return NOTIFY_DONE; 10817b9eba7bSLeandro Dorileo 10827b9eba7bSLeandro Dorileo list_for_each_entry(q, &taprio_list, taprio_list) { 1083*fc4f2fd0SVladimir Oltean if (dev != qdisc_dev(q->root)) 1084*fc4f2fd0SVladimir Oltean continue; 1085*fc4f2fd0SVladimir Oltean 1086*fc4f2fd0SVladimir Oltean taprio_set_picos_per_byte(dev, q); 10877b9eba7bSLeandro Dorileo break; 10887b9eba7bSLeandro Dorileo } 10897b9eba7bSLeandro Dorileo 10907b9eba7bSLeandro Dorileo return NOTIFY_DONE; 10917b9eba7bSLeandro Dorileo } 10927b9eba7bSLeandro Dorileo 10934cfd5779SVedang Patel static void setup_txtime(struct taprio_sched *q, 10944cfd5779SVedang Patel struct sched_gate_list *sched, ktime_t base) 10954cfd5779SVedang Patel { 10964cfd5779SVedang Patel struct sched_entry *entry; 10974cfd5779SVedang Patel u32 interval = 0; 10984cfd5779SVedang Patel 10994cfd5779SVedang Patel list_for_each_entry(entry, &sched->entries, list) { 11004cfd5779SVedang Patel entry->next_txtime = ktime_add_ns(base, interval); 11014cfd5779SVedang Patel interval += entry->interval; 11024cfd5779SVedang Patel } 11034cfd5779SVedang Patel } 11044cfd5779SVedang Patel 11059c66d156SVinicius Costa Gomes static struct tc_taprio_qopt_offload *taprio_offload_alloc(int num_entries) 11069c66d156SVinicius Costa Gomes { 11079c66d156SVinicius Costa Gomes struct __tc_taprio_qopt_offload *__offload; 11089c66d156SVinicius Costa Gomes 110911a33de2SGustavo A. R. Silva __offload = kzalloc(struct_size(__offload, offload.entries, num_entries), 111011a33de2SGustavo A. R. Silva GFP_KERNEL); 11119c66d156SVinicius Costa Gomes if (!__offload) 11129c66d156SVinicius Costa Gomes return NULL; 11139c66d156SVinicius Costa Gomes 11149c66d156SVinicius Costa Gomes refcount_set(&__offload->users, 1); 11159c66d156SVinicius Costa Gomes 11169c66d156SVinicius Costa Gomes return &__offload->offload; 11179c66d156SVinicius Costa Gomes } 11189c66d156SVinicius Costa Gomes 11199c66d156SVinicius Costa Gomes struct tc_taprio_qopt_offload *taprio_offload_get(struct tc_taprio_qopt_offload 11209c66d156SVinicius Costa Gomes *offload) 11219c66d156SVinicius Costa Gomes { 11229c66d156SVinicius Costa Gomes struct __tc_taprio_qopt_offload *__offload; 11239c66d156SVinicius Costa Gomes 11249c66d156SVinicius Costa Gomes __offload = container_of(offload, struct __tc_taprio_qopt_offload, 11259c66d156SVinicius Costa Gomes offload); 11269c66d156SVinicius Costa Gomes 11279c66d156SVinicius Costa Gomes refcount_inc(&__offload->users); 11289c66d156SVinicius Costa Gomes 11299c66d156SVinicius Costa Gomes return offload; 11309c66d156SVinicius Costa Gomes } 11319c66d156SVinicius Costa Gomes EXPORT_SYMBOL_GPL(taprio_offload_get); 11329c66d156SVinicius Costa Gomes 11339c66d156SVinicius Costa Gomes void taprio_offload_free(struct tc_taprio_qopt_offload *offload) 11349c66d156SVinicius Costa Gomes { 11359c66d156SVinicius Costa Gomes struct __tc_taprio_qopt_offload *__offload; 11369c66d156SVinicius Costa Gomes 11379c66d156SVinicius Costa Gomes __offload = container_of(offload, struct __tc_taprio_qopt_offload, 11389c66d156SVinicius Costa Gomes offload); 11399c66d156SVinicius Costa Gomes 11409c66d156SVinicius Costa Gomes if (!refcount_dec_and_test(&__offload->users)) 11419c66d156SVinicius Costa Gomes return; 11429c66d156SVinicius Costa Gomes 11439c66d156SVinicius Costa Gomes kfree(__offload); 11449c66d156SVinicius Costa Gomes } 11459c66d156SVinicius Costa Gomes EXPORT_SYMBOL_GPL(taprio_offload_free); 11469c66d156SVinicius Costa Gomes 11479c66d156SVinicius Costa Gomes /* The function will only serve to keep the pointers to the "oper" and "admin" 11489c66d156SVinicius Costa Gomes * schedules valid in relation to their base times, so when calling dump() the 11499c66d156SVinicius Costa Gomes * users looks at the right schedules. 11509c66d156SVinicius Costa Gomes * When using full offload, the admin configuration is promoted to oper at the 11519c66d156SVinicius Costa Gomes * base_time in the PHC time domain. But because the system time is not 11529c66d156SVinicius Costa Gomes * necessarily in sync with that, we can't just trigger a hrtimer to call 11539c66d156SVinicius Costa Gomes * switch_schedules at the right hardware time. 11549c66d156SVinicius Costa Gomes * At the moment we call this by hand right away from taprio, but in the future 11559c66d156SVinicius Costa Gomes * it will be useful to create a mechanism for drivers to notify taprio of the 11569c66d156SVinicius Costa Gomes * offload state (PENDING, ACTIVE, INACTIVE) so it can be visible in dump(). 11579c66d156SVinicius Costa Gomes * This is left as TODO. 11589c66d156SVinicius Costa Gomes */ 1159d665c128SYi Wang static void taprio_offload_config_changed(struct taprio_sched *q) 11609c66d156SVinicius Costa Gomes { 11619c66d156SVinicius Costa Gomes struct sched_gate_list *oper, *admin; 11629c66d156SVinicius Costa Gomes 1163c8cbe123SVladimir Oltean oper = rtnl_dereference(q->oper_sched); 1164c8cbe123SVladimir Oltean admin = rtnl_dereference(q->admin_sched); 11659c66d156SVinicius Costa Gomes 11669c66d156SVinicius Costa Gomes switch_schedules(q, &admin, &oper); 11679c66d156SVinicius Costa Gomes } 11689c66d156SVinicius Costa Gomes 116909e31cf0SVinicius Costa Gomes static u32 tc_map_to_queue_mask(struct net_device *dev, u32 tc_mask) 117009e31cf0SVinicius Costa Gomes { 117109e31cf0SVinicius Costa Gomes u32 i, queue_mask = 0; 117209e31cf0SVinicius Costa Gomes 117309e31cf0SVinicius Costa Gomes for (i = 0; i < dev->num_tc; i++) { 117409e31cf0SVinicius Costa Gomes u32 offset, count; 117509e31cf0SVinicius Costa Gomes 117609e31cf0SVinicius Costa Gomes if (!(tc_mask & BIT(i))) 117709e31cf0SVinicius Costa Gomes continue; 117809e31cf0SVinicius Costa Gomes 117909e31cf0SVinicius Costa Gomes offset = dev->tc_to_txq[i].offset; 118009e31cf0SVinicius Costa Gomes count = dev->tc_to_txq[i].count; 118109e31cf0SVinicius Costa Gomes 118209e31cf0SVinicius Costa Gomes queue_mask |= GENMASK(offset + count - 1, offset); 118309e31cf0SVinicius Costa Gomes } 118409e31cf0SVinicius Costa Gomes 118509e31cf0SVinicius Costa Gomes return queue_mask; 118609e31cf0SVinicius Costa Gomes } 118709e31cf0SVinicius Costa Gomes 118809e31cf0SVinicius Costa Gomes static void taprio_sched_to_offload(struct net_device *dev, 11899c66d156SVinicius Costa Gomes struct sched_gate_list *sched, 11909c66d156SVinicius Costa Gomes struct tc_taprio_qopt_offload *offload) 11919c66d156SVinicius Costa Gomes { 11929c66d156SVinicius Costa Gomes struct sched_entry *entry; 11939c66d156SVinicius Costa Gomes int i = 0; 11949c66d156SVinicius Costa Gomes 11959c66d156SVinicius Costa Gomes offload->base_time = sched->base_time; 11969c66d156SVinicius Costa Gomes offload->cycle_time = sched->cycle_time; 11979c66d156SVinicius Costa Gomes offload->cycle_time_extension = sched->cycle_time_extension; 11989c66d156SVinicius Costa Gomes 11999c66d156SVinicius Costa Gomes list_for_each_entry(entry, &sched->entries, list) { 12009c66d156SVinicius Costa Gomes struct tc_taprio_sched_entry *e = &offload->entries[i]; 12019c66d156SVinicius Costa Gomes 12029c66d156SVinicius Costa Gomes e->command = entry->command; 12039c66d156SVinicius Costa Gomes e->interval = entry->interval; 120409e31cf0SVinicius Costa Gomes e->gate_mask = tc_map_to_queue_mask(dev, entry->gate_mask); 120509e31cf0SVinicius Costa Gomes 12069c66d156SVinicius Costa Gomes i++; 12079c66d156SVinicius Costa Gomes } 12089c66d156SVinicius Costa Gomes 12099c66d156SVinicius Costa Gomes offload->num_entries = i; 12109c66d156SVinicius Costa Gomes } 12119c66d156SVinicius Costa Gomes 12129c66d156SVinicius Costa Gomes static int taprio_enable_offload(struct net_device *dev, 12139c66d156SVinicius Costa Gomes struct taprio_sched *q, 12149c66d156SVinicius Costa Gomes struct sched_gate_list *sched, 12159c66d156SVinicius Costa Gomes struct netlink_ext_ack *extack) 12169c66d156SVinicius Costa Gomes { 12179c66d156SVinicius Costa Gomes const struct net_device_ops *ops = dev->netdev_ops; 12189c66d156SVinicius Costa Gomes struct tc_taprio_qopt_offload *offload; 12199c66d156SVinicius Costa Gomes int err = 0; 12209c66d156SVinicius Costa Gomes 12219c66d156SVinicius Costa Gomes if (!ops->ndo_setup_tc) { 12229c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 12239c66d156SVinicius Costa Gomes "Device does not support taprio offload"); 12249c66d156SVinicius Costa Gomes return -EOPNOTSUPP; 12259c66d156SVinicius Costa Gomes } 12269c66d156SVinicius Costa Gomes 12279c66d156SVinicius Costa Gomes offload = taprio_offload_alloc(sched->num_entries); 12289c66d156SVinicius Costa Gomes if (!offload) { 12299c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 12309c66d156SVinicius Costa Gomes "Not enough memory for enabling offload mode"); 12319c66d156SVinicius Costa Gomes return -ENOMEM; 12329c66d156SVinicius Costa Gomes } 12339c66d156SVinicius Costa Gomes offload->enable = 1; 123409e31cf0SVinicius Costa Gomes taprio_sched_to_offload(dev, sched, offload); 12359c66d156SVinicius Costa Gomes 12369c66d156SVinicius Costa Gomes err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload); 12379c66d156SVinicius Costa Gomes if (err < 0) { 12389c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 12399c66d156SVinicius Costa Gomes "Device failed to setup taprio offload"); 12409c66d156SVinicius Costa Gomes goto done; 12419c66d156SVinicius Costa Gomes } 12429c66d156SVinicius Costa Gomes 1243db46e3a8SVladimir Oltean q->offloaded = true; 1244db46e3a8SVladimir Oltean 12459c66d156SVinicius Costa Gomes done: 12469c66d156SVinicius Costa Gomes taprio_offload_free(offload); 12479c66d156SVinicius Costa Gomes 12489c66d156SVinicius Costa Gomes return err; 12499c66d156SVinicius Costa Gomes } 12509c66d156SVinicius Costa Gomes 12519c66d156SVinicius Costa Gomes static int taprio_disable_offload(struct net_device *dev, 12529c66d156SVinicius Costa Gomes struct taprio_sched *q, 12539c66d156SVinicius Costa Gomes struct netlink_ext_ack *extack) 12549c66d156SVinicius Costa Gomes { 12559c66d156SVinicius Costa Gomes const struct net_device_ops *ops = dev->netdev_ops; 12569c66d156SVinicius Costa Gomes struct tc_taprio_qopt_offload *offload; 12579c66d156SVinicius Costa Gomes int err; 12589c66d156SVinicius Costa Gomes 1259db46e3a8SVladimir Oltean if (!q->offloaded) 12609c66d156SVinicius Costa Gomes return 0; 12619c66d156SVinicius Costa Gomes 12629c66d156SVinicius Costa Gomes offload = taprio_offload_alloc(0); 12639c66d156SVinicius Costa Gomes if (!offload) { 12649c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 12659c66d156SVinicius Costa Gomes "Not enough memory to disable offload mode"); 12669c66d156SVinicius Costa Gomes return -ENOMEM; 12679c66d156SVinicius Costa Gomes } 12689c66d156SVinicius Costa Gomes offload->enable = 0; 12699c66d156SVinicius Costa Gomes 12709c66d156SVinicius Costa Gomes err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, offload); 12719c66d156SVinicius Costa Gomes if (err < 0) { 12729c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 12739c66d156SVinicius Costa Gomes "Device failed to disable offload"); 12749c66d156SVinicius Costa Gomes goto out; 12759c66d156SVinicius Costa Gomes } 12769c66d156SVinicius Costa Gomes 1277db46e3a8SVladimir Oltean q->offloaded = false; 1278db46e3a8SVladimir Oltean 12799c66d156SVinicius Costa Gomes out: 12809c66d156SVinicius Costa Gomes taprio_offload_free(offload); 12819c66d156SVinicius Costa Gomes 12829c66d156SVinicius Costa Gomes return err; 12839c66d156SVinicius Costa Gomes } 12849c66d156SVinicius Costa Gomes 12859c66d156SVinicius Costa Gomes /* If full offload is enabled, the only possible clockid is the net device's 12869c66d156SVinicius Costa Gomes * PHC. For that reason, specifying a clockid through netlink is incorrect. 12879c66d156SVinicius Costa Gomes * For txtime-assist, it is implicitly assumed that the device's PHC is kept 12889c66d156SVinicius Costa Gomes * in sync with the specified clockid via a user space daemon such as phc2sys. 12899c66d156SVinicius Costa Gomes * For both software taprio and txtime-assist, the clockid is used for the 12909c66d156SVinicius Costa Gomes * hrtimer that advances the schedule and hence mandatory. 12919c66d156SVinicius Costa Gomes */ 12929c66d156SVinicius Costa Gomes static int taprio_parse_clockid(struct Qdisc *sch, struct nlattr **tb, 12939c66d156SVinicius Costa Gomes struct netlink_ext_ack *extack) 12949c66d156SVinicius Costa Gomes { 12959c66d156SVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 12969c66d156SVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 12979c66d156SVinicius Costa Gomes int err = -EINVAL; 12989c66d156SVinicius Costa Gomes 12999c66d156SVinicius Costa Gomes if (FULL_OFFLOAD_IS_ENABLED(q->flags)) { 13009c66d156SVinicius Costa Gomes const struct ethtool_ops *ops = dev->ethtool_ops; 13019c66d156SVinicius Costa Gomes struct ethtool_ts_info info = { 13029c66d156SVinicius Costa Gomes .cmd = ETHTOOL_GET_TS_INFO, 13039c66d156SVinicius Costa Gomes .phc_index = -1, 13049c66d156SVinicius Costa Gomes }; 13059c66d156SVinicius Costa Gomes 13069c66d156SVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) { 13079c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 13089c66d156SVinicius Costa Gomes "The 'clockid' cannot be specified for full offload"); 13099c66d156SVinicius Costa Gomes goto out; 13109c66d156SVinicius Costa Gomes } 13119c66d156SVinicius Costa Gomes 13129c66d156SVinicius Costa Gomes if (ops && ops->get_ts_info) 13139c66d156SVinicius Costa Gomes err = ops->get_ts_info(dev, &info); 13149c66d156SVinicius Costa Gomes 13159c66d156SVinicius Costa Gomes if (err || info.phc_index < 0) { 13169c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 13179c66d156SVinicius Costa Gomes "Device does not have a PTP clock"); 13189c66d156SVinicius Costa Gomes err = -ENOTSUPP; 13199c66d156SVinicius Costa Gomes goto out; 13209c66d156SVinicius Costa Gomes } 13219c66d156SVinicius Costa Gomes } else if (tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]) { 13229c66d156SVinicius Costa Gomes int clockid = nla_get_s32(tb[TCA_TAPRIO_ATTR_SCHED_CLOCKID]); 13236dc25401SEric Dumazet enum tk_offsets tk_offset; 13249c66d156SVinicius Costa Gomes 13259c66d156SVinicius Costa Gomes /* We only support static clockids and we don't allow 13269c66d156SVinicius Costa Gomes * for it to be modified after the first init. 13279c66d156SVinicius Costa Gomes */ 13289c66d156SVinicius Costa Gomes if (clockid < 0 || 13299c66d156SVinicius Costa Gomes (q->clockid != -1 && q->clockid != clockid)) { 13309c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 13319c66d156SVinicius Costa Gomes "Changing the 'clockid' of a running schedule is not supported"); 13329c66d156SVinicius Costa Gomes err = -ENOTSUPP; 13339c66d156SVinicius Costa Gomes goto out; 13349c66d156SVinicius Costa Gomes } 13359c66d156SVinicius Costa Gomes 13369c66d156SVinicius Costa Gomes switch (clockid) { 13379c66d156SVinicius Costa Gomes case CLOCK_REALTIME: 13386dc25401SEric Dumazet tk_offset = TK_OFFS_REAL; 13399c66d156SVinicius Costa Gomes break; 13409c66d156SVinicius Costa Gomes case CLOCK_MONOTONIC: 13416dc25401SEric Dumazet tk_offset = TK_OFFS_MAX; 13429c66d156SVinicius Costa Gomes break; 13439c66d156SVinicius Costa Gomes case CLOCK_BOOTTIME: 13446dc25401SEric Dumazet tk_offset = TK_OFFS_BOOT; 13459c66d156SVinicius Costa Gomes break; 13469c66d156SVinicius Costa Gomes case CLOCK_TAI: 13476dc25401SEric Dumazet tk_offset = TK_OFFS_TAI; 13489c66d156SVinicius Costa Gomes break; 13499c66d156SVinicius Costa Gomes default: 13509c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Invalid 'clockid'"); 13519c66d156SVinicius Costa Gomes err = -EINVAL; 13529c66d156SVinicius Costa Gomes goto out; 13539c66d156SVinicius Costa Gomes } 13546dc25401SEric Dumazet /* This pairs with READ_ONCE() in taprio_mono_to_any */ 13556dc25401SEric Dumazet WRITE_ONCE(q->tk_offset, tk_offset); 13569c66d156SVinicius Costa Gomes 13579c66d156SVinicius Costa Gomes q->clockid = clockid; 13589c66d156SVinicius Costa Gomes } else { 13599c66d156SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Specifying a 'clockid' is mandatory"); 13609c66d156SVinicius Costa Gomes goto out; 13619c66d156SVinicius Costa Gomes } 1362a954380aSVinicius Costa Gomes 1363a954380aSVinicius Costa Gomes /* Everything went ok, return success. */ 1364a954380aSVinicius Costa Gomes err = 0; 1365a954380aSVinicius Costa Gomes 13669c66d156SVinicius Costa Gomes out: 13679c66d156SVinicius Costa Gomes return err; 13689c66d156SVinicius Costa Gomes } 13699c66d156SVinicius Costa Gomes 1370b5a0faa3SIvan Khoronzhuk static int taprio_mqprio_cmp(const struct net_device *dev, 1371b5a0faa3SIvan Khoronzhuk const struct tc_mqprio_qopt *mqprio) 1372b5a0faa3SIvan Khoronzhuk { 1373b5a0faa3SIvan Khoronzhuk int i; 1374b5a0faa3SIvan Khoronzhuk 1375b5a0faa3SIvan Khoronzhuk if (!mqprio || mqprio->num_tc != dev->num_tc) 1376b5a0faa3SIvan Khoronzhuk return -1; 1377b5a0faa3SIvan Khoronzhuk 1378b5a0faa3SIvan Khoronzhuk for (i = 0; i < mqprio->num_tc; i++) 1379b5a0faa3SIvan Khoronzhuk if (dev->tc_to_txq[i].count != mqprio->count[i] || 1380b5a0faa3SIvan Khoronzhuk dev->tc_to_txq[i].offset != mqprio->offset[i]) 1381b5a0faa3SIvan Khoronzhuk return -1; 1382b5a0faa3SIvan Khoronzhuk 1383b5a0faa3SIvan Khoronzhuk for (i = 0; i <= TC_BITMASK; i++) 1384b5a0faa3SIvan Khoronzhuk if (dev->prio_tc_map[i] != mqprio->prio_tc_map[i]) 1385b5a0faa3SIvan Khoronzhuk return -1; 1386b5a0faa3SIvan Khoronzhuk 1387b5a0faa3SIvan Khoronzhuk return 0; 1388b5a0faa3SIvan Khoronzhuk } 1389b5a0faa3SIvan Khoronzhuk 1390a9d62274SVinicius Costa Gomes /* The semantics of the 'flags' argument in relation to 'change()' 1391a9d62274SVinicius Costa Gomes * requests, are interpreted following two rules (which are applied in 1392a9d62274SVinicius Costa Gomes * this order): (1) an omitted 'flags' argument is interpreted as 1393a9d62274SVinicius Costa Gomes * zero; (2) the 'flags' of a "running" taprio instance cannot be 1394a9d62274SVinicius Costa Gomes * changed. 1395a9d62274SVinicius Costa Gomes */ 1396a9d62274SVinicius Costa Gomes static int taprio_new_flags(const struct nlattr *attr, u32 old, 1397a9d62274SVinicius Costa Gomes struct netlink_ext_ack *extack) 1398a9d62274SVinicius Costa Gomes { 1399a9d62274SVinicius Costa Gomes u32 new = 0; 1400a9d62274SVinicius Costa Gomes 1401a9d62274SVinicius Costa Gomes if (attr) 1402a9d62274SVinicius Costa Gomes new = nla_get_u32(attr); 1403a9d62274SVinicius Costa Gomes 1404a9d62274SVinicius Costa Gomes if (old != TAPRIO_FLAGS_INVALID && old != new) { 1405a9d62274SVinicius Costa Gomes NL_SET_ERR_MSG_MOD(extack, "Changing 'flags' of a running schedule is not supported"); 1406a9d62274SVinicius Costa Gomes return -EOPNOTSUPP; 1407a9d62274SVinicius Costa Gomes } 1408a9d62274SVinicius Costa Gomes 1409a9d62274SVinicius Costa Gomes if (!taprio_flags_valid(new)) { 1410a9d62274SVinicius Costa Gomes NL_SET_ERR_MSG_MOD(extack, "Specified 'flags' are not valid"); 1411a9d62274SVinicius Costa Gomes return -EINVAL; 1412a9d62274SVinicius Costa Gomes } 1413a9d62274SVinicius Costa Gomes 1414a9d62274SVinicius Costa Gomes return new; 1415a9d62274SVinicius Costa Gomes } 1416a9d62274SVinicius Costa Gomes 14175a781ccbSVinicius Costa Gomes static int taprio_change(struct Qdisc *sch, struct nlattr *opt, 14185a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 14195a781ccbSVinicius Costa Gomes { 14205a781ccbSVinicius Costa Gomes struct nlattr *tb[TCA_TAPRIO_ATTR_MAX + 1] = { }; 1421a3d43c0dSVinicius Costa Gomes struct sched_gate_list *oper, *admin, *new_admin; 14225a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 14235a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 14245a781ccbSVinicius Costa Gomes struct tc_mqprio_qopt *mqprio = NULL; 1425a3d43c0dSVinicius Costa Gomes unsigned long flags; 14265a781ccbSVinicius Costa Gomes ktime_t start; 14279c66d156SVinicius Costa Gomes int i, err; 14285a781ccbSVinicius Costa Gomes 14298cb08174SJohannes Berg err = nla_parse_nested_deprecated(tb, TCA_TAPRIO_ATTR_MAX, opt, 14305a781ccbSVinicius Costa Gomes taprio_policy, extack); 14315a781ccbSVinicius Costa Gomes if (err < 0) 14325a781ccbSVinicius Costa Gomes return err; 14335a781ccbSVinicius Costa Gomes 14345a781ccbSVinicius Costa Gomes if (tb[TCA_TAPRIO_ATTR_PRIOMAP]) 14355a781ccbSVinicius Costa Gomes mqprio = nla_data(tb[TCA_TAPRIO_ATTR_PRIOMAP]); 14365a781ccbSVinicius Costa Gomes 1437a9d62274SVinicius Costa Gomes err = taprio_new_flags(tb[TCA_TAPRIO_ATTR_FLAGS], 1438a9d62274SVinicius Costa Gomes q->flags, extack); 1439a9d62274SVinicius Costa Gomes if (err < 0) 1440a9d62274SVinicius Costa Gomes return err; 14414cfd5779SVedang Patel 1442a9d62274SVinicius Costa Gomes q->flags = err; 14434cfd5779SVedang Patel 1444a9d62274SVinicius Costa Gomes err = taprio_parse_mqprio_opt(dev, mqprio, extack, q->flags); 14455a781ccbSVinicius Costa Gomes if (err < 0) 14465a781ccbSVinicius Costa Gomes return err; 14475a781ccbSVinicius Costa Gomes 1448a3d43c0dSVinicius Costa Gomes new_admin = kzalloc(sizeof(*new_admin), GFP_KERNEL); 1449a3d43c0dSVinicius Costa Gomes if (!new_admin) { 1450a3d43c0dSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Not enough memory for a new schedule"); 1451a3d43c0dSVinicius Costa Gomes return -ENOMEM; 1452a3d43c0dSVinicius Costa Gomes } 1453a3d43c0dSVinicius Costa Gomes INIT_LIST_HEAD(&new_admin->entries); 14545a781ccbSVinicius Costa Gomes 145518cdd2f0SVladimir Oltean oper = rtnl_dereference(q->oper_sched); 145618cdd2f0SVladimir Oltean admin = rtnl_dereference(q->admin_sched); 14575a781ccbSVinicius Costa Gomes 1458b5a0faa3SIvan Khoronzhuk /* no changes - no new mqprio settings */ 1459b5a0faa3SIvan Khoronzhuk if (!taprio_mqprio_cmp(dev, mqprio)) 1460b5a0faa3SIvan Khoronzhuk mqprio = NULL; 1461b5a0faa3SIvan Khoronzhuk 1462a3d43c0dSVinicius Costa Gomes if (mqprio && (oper || admin)) { 1463a3d43c0dSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Changing the traffic mapping of a running schedule is not supported"); 1464a3d43c0dSVinicius Costa Gomes err = -ENOTSUPP; 1465a3d43c0dSVinicius Costa Gomes goto free_sched; 14665a781ccbSVinicius Costa Gomes } 14675a781ccbSVinicius Costa Gomes 1468b5b73b26SVinicius Costa Gomes err = parse_taprio_schedule(q, tb, new_admin, extack); 1469a3d43c0dSVinicius Costa Gomes if (err < 0) 1470a3d43c0dSVinicius Costa Gomes goto free_sched; 14715a781ccbSVinicius Costa Gomes 1472a3d43c0dSVinicius Costa Gomes if (new_admin->num_entries == 0) { 1473a3d43c0dSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "There should be at least one entry in the schedule"); 1474a3d43c0dSVinicius Costa Gomes err = -EINVAL; 1475a3d43c0dSVinicius Costa Gomes goto free_sched; 1476a3d43c0dSVinicius Costa Gomes } 14775a781ccbSVinicius Costa Gomes 14789c66d156SVinicius Costa Gomes err = taprio_parse_clockid(sch, tb, extack); 14799c66d156SVinicius Costa Gomes if (err < 0) 1480a3d43c0dSVinicius Costa Gomes goto free_sched; 1481a3d43c0dSVinicius Costa Gomes 1482a3d43c0dSVinicius Costa Gomes taprio_set_picos_per_byte(dev, q); 1483a3d43c0dSVinicius Costa Gomes 14845652e63dSVinicius Costa Gomes if (mqprio) { 1485efe487fcSHaimin Zhang err = netdev_set_num_tc(dev, mqprio->num_tc); 1486efe487fcSHaimin Zhang if (err) 1487efe487fcSHaimin Zhang goto free_sched; 14885652e63dSVinicius Costa Gomes for (i = 0; i < mqprio->num_tc; i++) 14895652e63dSVinicius Costa Gomes netdev_set_tc_queue(dev, i, 14905652e63dSVinicius Costa Gomes mqprio->count[i], 14915652e63dSVinicius Costa Gomes mqprio->offset[i]); 14925652e63dSVinicius Costa Gomes 14935652e63dSVinicius Costa Gomes /* Always use supplied priority mappings */ 14945652e63dSVinicius Costa Gomes for (i = 0; i <= TC_BITMASK; i++) 14955652e63dSVinicius Costa Gomes netdev_set_prio_tc_map(dev, i, 14965652e63dSVinicius Costa Gomes mqprio->prio_tc_map[i]); 14975652e63dSVinicius Costa Gomes } 14985652e63dSVinicius Costa Gomes 1499a9d62274SVinicius Costa Gomes if (FULL_OFFLOAD_IS_ENABLED(q->flags)) 150009e31cf0SVinicius Costa Gomes err = taprio_enable_offload(dev, q, new_admin, extack); 15019c66d156SVinicius Costa Gomes else 15029c66d156SVinicius Costa Gomes err = taprio_disable_offload(dev, q, extack); 15039c66d156SVinicius Costa Gomes if (err) 15049c66d156SVinicius Costa Gomes goto free_sched; 15059c66d156SVinicius Costa Gomes 1506a3d43c0dSVinicius Costa Gomes /* Protects against enqueue()/dequeue() */ 1507a3d43c0dSVinicius Costa Gomes spin_lock_bh(qdisc_lock(sch)); 1508a3d43c0dSVinicius Costa Gomes 15094cfd5779SVedang Patel if (tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]) { 15104cfd5779SVedang Patel if (!TXTIME_ASSIST_IS_ENABLED(q->flags)) { 15114cfd5779SVedang Patel NL_SET_ERR_MSG_MOD(extack, "txtime-delay can only be set when txtime-assist mode is enabled"); 15124cfd5779SVedang Patel err = -EINVAL; 15134cfd5779SVedang Patel goto unlock; 15144cfd5779SVedang Patel } 15154cfd5779SVedang Patel 1516a5b64700SVedang Patel q->txtime_delay = nla_get_u32(tb[TCA_TAPRIO_ATTR_TXTIME_DELAY]); 15174cfd5779SVedang Patel } 15184cfd5779SVedang Patel 1519a9d62274SVinicius Costa Gomes if (!TXTIME_ASSIST_IS_ENABLED(q->flags) && 1520a9d62274SVinicius Costa Gomes !FULL_OFFLOAD_IS_ENABLED(q->flags) && 15214cfd5779SVedang Patel !hrtimer_active(&q->advance_timer)) { 1522a3d43c0dSVinicius Costa Gomes hrtimer_init(&q->advance_timer, q->clockid, HRTIMER_MODE_ABS); 1523a3d43c0dSVinicius Costa Gomes q->advance_timer.function = advance_sched; 15245a781ccbSVinicius Costa Gomes } 15255a781ccbSVinicius Costa Gomes 1526a3d43c0dSVinicius Costa Gomes err = taprio_get_start_time(sch, new_admin, &start); 1527a3d43c0dSVinicius Costa Gomes if (err < 0) { 1528a3d43c0dSVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Internal error: failed get start time"); 1529a3d43c0dSVinicius Costa Gomes goto unlock; 1530a3d43c0dSVinicius Costa Gomes } 15315a781ccbSVinicius Costa Gomes 15324cfd5779SVedang Patel setup_txtime(q, new_admin, start); 15334cfd5779SVedang Patel 1534bfabd41dSVinicius Costa Gomes if (TXTIME_ASSIST_IS_ENABLED(q->flags)) { 15354cfd5779SVedang Patel if (!oper) { 15364cfd5779SVedang Patel rcu_assign_pointer(q->oper_sched, new_admin); 15374cfd5779SVedang Patel err = 0; 15384cfd5779SVedang Patel new_admin = NULL; 15394cfd5779SVedang Patel goto unlock; 15404cfd5779SVedang Patel } 15414cfd5779SVedang Patel 15424cfd5779SVedang Patel rcu_assign_pointer(q->admin_sched, new_admin); 15434cfd5779SVedang Patel if (admin) 15444cfd5779SVedang Patel call_rcu(&admin->rcu, taprio_free_sched_cb); 15454cfd5779SVedang Patel } else { 1546a3d43c0dSVinicius Costa Gomes setup_first_close_time(q, new_admin, start); 1547a3d43c0dSVinicius Costa Gomes 1548a3d43c0dSVinicius Costa Gomes /* Protects against advance_sched() */ 1549a3d43c0dSVinicius Costa Gomes spin_lock_irqsave(&q->current_entry_lock, flags); 1550a3d43c0dSVinicius Costa Gomes 1551a3d43c0dSVinicius Costa Gomes taprio_start_sched(sch, start, new_admin); 1552a3d43c0dSVinicius Costa Gomes 1553a3d43c0dSVinicius Costa Gomes rcu_assign_pointer(q->admin_sched, new_admin); 1554a3d43c0dSVinicius Costa Gomes if (admin) 1555a3d43c0dSVinicius Costa Gomes call_rcu(&admin->rcu, taprio_free_sched_cb); 1556a3d43c0dSVinicius Costa Gomes 1557a3d43c0dSVinicius Costa Gomes spin_unlock_irqrestore(&q->current_entry_lock, flags); 15580763b3e8SIvan Khoronzhuk 1559a9d62274SVinicius Costa Gomes if (FULL_OFFLOAD_IS_ENABLED(q->flags)) 15600763b3e8SIvan Khoronzhuk taprio_offload_config_changed(q); 15614cfd5779SVedang Patel } 1562a3d43c0dSVinicius Costa Gomes 15634cfd5779SVedang Patel new_admin = NULL; 1564a3d43c0dSVinicius Costa Gomes err = 0; 1565a3d43c0dSVinicius Costa Gomes 1566a3d43c0dSVinicius Costa Gomes unlock: 1567a3d43c0dSVinicius Costa Gomes spin_unlock_bh(qdisc_lock(sch)); 1568a3d43c0dSVinicius Costa Gomes 1569a3d43c0dSVinicius Costa Gomes free_sched: 157051650d33SIvan Khoronzhuk if (new_admin) 157151650d33SIvan Khoronzhuk call_rcu(&new_admin->rcu, taprio_free_sched_cb); 1572a3d43c0dSVinicius Costa Gomes 1573a3d43c0dSVinicius Costa Gomes return err; 15745a781ccbSVinicius Costa Gomes } 15755a781ccbSVinicius Costa Gomes 157644d4775cSDavide Caratti static void taprio_reset(struct Qdisc *sch) 157744d4775cSDavide Caratti { 157844d4775cSDavide Caratti struct taprio_sched *q = qdisc_priv(sch); 157944d4775cSDavide Caratti struct net_device *dev = qdisc_dev(sch); 158044d4775cSDavide Caratti int i; 158144d4775cSDavide Caratti 158244d4775cSDavide Caratti hrtimer_cancel(&q->advance_timer); 158344d4775cSDavide Caratti if (q->qdiscs) { 1584698285daSDavide Caratti for (i = 0; i < dev->num_tx_queues; i++) 1585698285daSDavide Caratti if (q->qdiscs[i]) 158644d4775cSDavide Caratti qdisc_reset(q->qdiscs[i]); 158744d4775cSDavide Caratti } 158844d4775cSDavide Caratti } 158944d4775cSDavide Caratti 15905a781ccbSVinicius Costa Gomes static void taprio_destroy(struct Qdisc *sch) 15915a781ccbSVinicius Costa Gomes { 15925a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 15935a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 15949af23657SVladimir Oltean struct sched_gate_list *oper, *admin; 15955a781ccbSVinicius Costa Gomes unsigned int i; 15965a781ccbSVinicius Costa Gomes 15977b9eba7bSLeandro Dorileo list_del(&q->taprio_list); 15987b9eba7bSLeandro Dorileo 1599a56d447fSEric Dumazet /* Note that taprio_reset() might not be called if an error 1600a56d447fSEric Dumazet * happens in qdisc_create(), after taprio_init() has been called. 1601a56d447fSEric Dumazet */ 1602a56d447fSEric Dumazet hrtimer_cancel(&q->advance_timer); 16035a781ccbSVinicius Costa Gomes 16049c66d156SVinicius Costa Gomes taprio_disable_offload(dev, q, NULL); 16059c66d156SVinicius Costa Gomes 16065a781ccbSVinicius Costa Gomes if (q->qdiscs) { 1607698285daSDavide Caratti for (i = 0; i < dev->num_tx_queues; i++) 16085a781ccbSVinicius Costa Gomes qdisc_put(q->qdiscs[i]); 16095a781ccbSVinicius Costa Gomes 16105a781ccbSVinicius Costa Gomes kfree(q->qdiscs); 16115a781ccbSVinicius Costa Gomes } 16125a781ccbSVinicius Costa Gomes q->qdiscs = NULL; 16135a781ccbSVinicius Costa Gomes 16147c16680aSVinicius Costa Gomes netdev_reset_tc(dev); 16155a781ccbSVinicius Costa Gomes 16169af23657SVladimir Oltean oper = rtnl_dereference(q->oper_sched); 16179af23657SVladimir Oltean admin = rtnl_dereference(q->admin_sched); 1618a3d43c0dSVinicius Costa Gomes 16199af23657SVladimir Oltean if (oper) 16209af23657SVladimir Oltean call_rcu(&oper->rcu, taprio_free_sched_cb); 16219af23657SVladimir Oltean 16229af23657SVladimir Oltean if (admin) 16239af23657SVladimir Oltean call_rcu(&admin->rcu, taprio_free_sched_cb); 16245a781ccbSVinicius Costa Gomes } 16255a781ccbSVinicius Costa Gomes 16265a781ccbSVinicius Costa Gomes static int taprio_init(struct Qdisc *sch, struct nlattr *opt, 16275a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 16285a781ccbSVinicius Costa Gomes { 16295a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 16305a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 1631a3d43c0dSVinicius Costa Gomes int i; 16325a781ccbSVinicius Costa Gomes 16335a781ccbSVinicius Costa Gomes spin_lock_init(&q->current_entry_lock); 16345a781ccbSVinicius Costa Gomes 16355a781ccbSVinicius Costa Gomes hrtimer_init(&q->advance_timer, CLOCK_TAI, HRTIMER_MODE_ABS); 1636a3d43c0dSVinicius Costa Gomes q->advance_timer.function = advance_sched; 16375a781ccbSVinicius Costa Gomes 16385a781ccbSVinicius Costa Gomes q->root = sch; 16395a781ccbSVinicius Costa Gomes 16405a781ccbSVinicius Costa Gomes /* We only support static clockids. Use an invalid value as default 16415a781ccbSVinicius Costa Gomes * and get the valid one on taprio_change(). 16425a781ccbSVinicius Costa Gomes */ 16435a781ccbSVinicius Costa Gomes q->clockid = -1; 1644a9d62274SVinicius Costa Gomes q->flags = TAPRIO_FLAGS_INVALID; 16455a781ccbSVinicius Costa Gomes 1646efb55222SVladimir Oltean list_add(&q->taprio_list, &taprio_list); 1647efb55222SVladimir Oltean 1648026de64dSVladimir Oltean if (sch->parent != TC_H_ROOT) { 1649026de64dSVladimir Oltean NL_SET_ERR_MSG_MOD(extack, "Can only be attached as root qdisc"); 16505a781ccbSVinicius Costa Gomes return -EOPNOTSUPP; 1651026de64dSVladimir Oltean } 16525a781ccbSVinicius Costa Gomes 1653026de64dSVladimir Oltean if (!netif_is_multiqueue(dev)) { 1654026de64dSVladimir Oltean NL_SET_ERR_MSG_MOD(extack, "Multi-queue device is required"); 16555a781ccbSVinicius Costa Gomes return -EOPNOTSUPP; 1656026de64dSVladimir Oltean } 16575a781ccbSVinicius Costa Gomes 16585a781ccbSVinicius Costa Gomes /* pre-allocate qdisc, attachment can't fail */ 16595a781ccbSVinicius Costa Gomes q->qdiscs = kcalloc(dev->num_tx_queues, 16605a781ccbSVinicius Costa Gomes sizeof(q->qdiscs[0]), 16615a781ccbSVinicius Costa Gomes GFP_KERNEL); 16625a781ccbSVinicius Costa Gomes 16635a781ccbSVinicius Costa Gomes if (!q->qdiscs) 16645a781ccbSVinicius Costa Gomes return -ENOMEM; 16655a781ccbSVinicius Costa Gomes 16665a781ccbSVinicius Costa Gomes if (!opt) 16675a781ccbSVinicius Costa Gomes return -EINVAL; 16685a781ccbSVinicius Costa Gomes 1669a3d43c0dSVinicius Costa Gomes for (i = 0; i < dev->num_tx_queues; i++) { 1670a3d43c0dSVinicius Costa Gomes struct netdev_queue *dev_queue; 1671a3d43c0dSVinicius Costa Gomes struct Qdisc *qdisc; 1672a3d43c0dSVinicius Costa Gomes 1673a3d43c0dSVinicius Costa Gomes dev_queue = netdev_get_tx_queue(dev, i); 1674a3d43c0dSVinicius Costa Gomes qdisc = qdisc_create_dflt(dev_queue, 1675a3d43c0dSVinicius Costa Gomes &pfifo_qdisc_ops, 1676a3d43c0dSVinicius Costa Gomes TC_H_MAKE(TC_H_MAJ(sch->handle), 1677a3d43c0dSVinicius Costa Gomes TC_H_MIN(i + 1)), 1678a3d43c0dSVinicius Costa Gomes extack); 1679a3d43c0dSVinicius Costa Gomes if (!qdisc) 1680a3d43c0dSVinicius Costa Gomes return -ENOMEM; 1681a3d43c0dSVinicius Costa Gomes 1682a3d43c0dSVinicius Costa Gomes if (i < dev->real_num_tx_queues) 1683a3d43c0dSVinicius Costa Gomes qdisc_hash_add(qdisc, false); 1684a3d43c0dSVinicius Costa Gomes 1685a3d43c0dSVinicius Costa Gomes q->qdiscs[i] = qdisc; 1686a3d43c0dSVinicius Costa Gomes } 1687a3d43c0dSVinicius Costa Gomes 16885a781ccbSVinicius Costa Gomes return taprio_change(sch, opt, extack); 16895a781ccbSVinicius Costa Gomes } 16905a781ccbSVinicius Costa Gomes 169113511704SYannick Vignon static void taprio_attach(struct Qdisc *sch) 169213511704SYannick Vignon { 169313511704SYannick Vignon struct taprio_sched *q = qdisc_priv(sch); 169413511704SYannick Vignon struct net_device *dev = qdisc_dev(sch); 169513511704SYannick Vignon unsigned int ntx; 169613511704SYannick Vignon 169713511704SYannick Vignon /* Attach underlying qdisc */ 169813511704SYannick Vignon for (ntx = 0; ntx < dev->num_tx_queues; ntx++) { 169913511704SYannick Vignon struct Qdisc *qdisc = q->qdiscs[ntx]; 170013511704SYannick Vignon struct Qdisc *old; 170113511704SYannick Vignon 170213511704SYannick Vignon if (FULL_OFFLOAD_IS_ENABLED(q->flags)) { 170313511704SYannick Vignon qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT; 170413511704SYannick Vignon old = dev_graft_qdisc(qdisc->dev_queue, qdisc); 170513511704SYannick Vignon } else { 170613511704SYannick Vignon old = dev_graft_qdisc(qdisc->dev_queue, sch); 170713511704SYannick Vignon qdisc_refcount_inc(sch); 170813511704SYannick Vignon } 170913511704SYannick Vignon if (old) 171013511704SYannick Vignon qdisc_put(old); 171113511704SYannick Vignon } 171213511704SYannick Vignon 171313511704SYannick Vignon /* access to the child qdiscs is not needed in offload mode */ 171413511704SYannick Vignon if (FULL_OFFLOAD_IS_ENABLED(q->flags)) { 171513511704SYannick Vignon kfree(q->qdiscs); 171613511704SYannick Vignon q->qdiscs = NULL; 171713511704SYannick Vignon } 171813511704SYannick Vignon } 171913511704SYannick Vignon 17205a781ccbSVinicius Costa Gomes static struct netdev_queue *taprio_queue_get(struct Qdisc *sch, 17215a781ccbSVinicius Costa Gomes unsigned long cl) 17225a781ccbSVinicius Costa Gomes { 17235a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 17245a781ccbSVinicius Costa Gomes unsigned long ntx = cl - 1; 17255a781ccbSVinicius Costa Gomes 17265a781ccbSVinicius Costa Gomes if (ntx >= dev->num_tx_queues) 17275a781ccbSVinicius Costa Gomes return NULL; 17285a781ccbSVinicius Costa Gomes 17295a781ccbSVinicius Costa Gomes return netdev_get_tx_queue(dev, ntx); 17305a781ccbSVinicius Costa Gomes } 17315a781ccbSVinicius Costa Gomes 17325a781ccbSVinicius Costa Gomes static int taprio_graft(struct Qdisc *sch, unsigned long cl, 17335a781ccbSVinicius Costa Gomes struct Qdisc *new, struct Qdisc **old, 17345a781ccbSVinicius Costa Gomes struct netlink_ext_ack *extack) 17355a781ccbSVinicius Costa Gomes { 17365a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 17375a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 17385a781ccbSVinicius Costa Gomes struct netdev_queue *dev_queue = taprio_queue_get(sch, cl); 17395a781ccbSVinicius Costa Gomes 17405a781ccbSVinicius Costa Gomes if (!dev_queue) 17415a781ccbSVinicius Costa Gomes return -EINVAL; 17425a781ccbSVinicius Costa Gomes 17435a781ccbSVinicius Costa Gomes if (dev->flags & IFF_UP) 17445a781ccbSVinicius Costa Gomes dev_deactivate(dev); 17455a781ccbSVinicius Costa Gomes 174613511704SYannick Vignon if (FULL_OFFLOAD_IS_ENABLED(q->flags)) { 174713511704SYannick Vignon *old = dev_graft_qdisc(dev_queue, new); 174813511704SYannick Vignon } else { 17495a781ccbSVinicius Costa Gomes *old = q->qdiscs[cl - 1]; 17505a781ccbSVinicius Costa Gomes q->qdiscs[cl - 1] = new; 175113511704SYannick Vignon } 17525a781ccbSVinicius Costa Gomes 17535a781ccbSVinicius Costa Gomes if (new) 17545a781ccbSVinicius Costa Gomes new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT; 17555a781ccbSVinicius Costa Gomes 17565a781ccbSVinicius Costa Gomes if (dev->flags & IFF_UP) 17575a781ccbSVinicius Costa Gomes dev_activate(dev); 17585a781ccbSVinicius Costa Gomes 17595a781ccbSVinicius Costa Gomes return 0; 17605a781ccbSVinicius Costa Gomes } 17615a781ccbSVinicius Costa Gomes 17625a781ccbSVinicius Costa Gomes static int dump_entry(struct sk_buff *msg, 17635a781ccbSVinicius Costa Gomes const struct sched_entry *entry) 17645a781ccbSVinicius Costa Gomes { 17655a781ccbSVinicius Costa Gomes struct nlattr *item; 17665a781ccbSVinicius Costa Gomes 1767ae0be8deSMichal Kubecek item = nla_nest_start_noflag(msg, TCA_TAPRIO_SCHED_ENTRY); 17685a781ccbSVinicius Costa Gomes if (!item) 17695a781ccbSVinicius Costa Gomes return -ENOSPC; 17705a781ccbSVinicius Costa Gomes 17715a781ccbSVinicius Costa Gomes if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_INDEX, entry->index)) 17725a781ccbSVinicius Costa Gomes goto nla_put_failure; 17735a781ccbSVinicius Costa Gomes 17745a781ccbSVinicius Costa Gomes if (nla_put_u8(msg, TCA_TAPRIO_SCHED_ENTRY_CMD, entry->command)) 17755a781ccbSVinicius Costa Gomes goto nla_put_failure; 17765a781ccbSVinicius Costa Gomes 17775a781ccbSVinicius Costa Gomes if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_GATE_MASK, 17785a781ccbSVinicius Costa Gomes entry->gate_mask)) 17795a781ccbSVinicius Costa Gomes goto nla_put_failure; 17805a781ccbSVinicius Costa Gomes 17815a781ccbSVinicius Costa Gomes if (nla_put_u32(msg, TCA_TAPRIO_SCHED_ENTRY_INTERVAL, 17825a781ccbSVinicius Costa Gomes entry->interval)) 17835a781ccbSVinicius Costa Gomes goto nla_put_failure; 17845a781ccbSVinicius Costa Gomes 17855a781ccbSVinicius Costa Gomes return nla_nest_end(msg, item); 17865a781ccbSVinicius Costa Gomes 17875a781ccbSVinicius Costa Gomes nla_put_failure: 17885a781ccbSVinicius Costa Gomes nla_nest_cancel(msg, item); 17895a781ccbSVinicius Costa Gomes return -1; 17905a781ccbSVinicius Costa Gomes } 17915a781ccbSVinicius Costa Gomes 1792a3d43c0dSVinicius Costa Gomes static int dump_schedule(struct sk_buff *msg, 1793a3d43c0dSVinicius Costa Gomes const struct sched_gate_list *root) 1794a3d43c0dSVinicius Costa Gomes { 1795a3d43c0dSVinicius Costa Gomes struct nlattr *entry_list; 1796a3d43c0dSVinicius Costa Gomes struct sched_entry *entry; 1797a3d43c0dSVinicius Costa Gomes 1798a3d43c0dSVinicius Costa Gomes if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_BASE_TIME, 1799a3d43c0dSVinicius Costa Gomes root->base_time, TCA_TAPRIO_PAD)) 1800a3d43c0dSVinicius Costa Gomes return -1; 1801a3d43c0dSVinicius Costa Gomes 18026ca6a665SVinicius Costa Gomes if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME, 18036ca6a665SVinicius Costa Gomes root->cycle_time, TCA_TAPRIO_PAD)) 18046ca6a665SVinicius Costa Gomes return -1; 18056ca6a665SVinicius Costa Gomes 1806c25031e9SVinicius Costa Gomes if (nla_put_s64(msg, TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION, 1807c25031e9SVinicius Costa Gomes root->cycle_time_extension, TCA_TAPRIO_PAD)) 1808c25031e9SVinicius Costa Gomes return -1; 1809c25031e9SVinicius Costa Gomes 1810a3d43c0dSVinicius Costa Gomes entry_list = nla_nest_start_noflag(msg, 1811a3d43c0dSVinicius Costa Gomes TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST); 1812a3d43c0dSVinicius Costa Gomes if (!entry_list) 1813a3d43c0dSVinicius Costa Gomes goto error_nest; 1814a3d43c0dSVinicius Costa Gomes 1815a3d43c0dSVinicius Costa Gomes list_for_each_entry(entry, &root->entries, list) { 1816a3d43c0dSVinicius Costa Gomes if (dump_entry(msg, entry) < 0) 1817a3d43c0dSVinicius Costa Gomes goto error_nest; 1818a3d43c0dSVinicius Costa Gomes } 1819a3d43c0dSVinicius Costa Gomes 1820a3d43c0dSVinicius Costa Gomes nla_nest_end(msg, entry_list); 1821a3d43c0dSVinicius Costa Gomes return 0; 1822a3d43c0dSVinicius Costa Gomes 1823a3d43c0dSVinicius Costa Gomes error_nest: 1824a3d43c0dSVinicius Costa Gomes nla_nest_cancel(msg, entry_list); 1825a3d43c0dSVinicius Costa Gomes return -1; 1826a3d43c0dSVinicius Costa Gomes } 1827a3d43c0dSVinicius Costa Gomes 18285a781ccbSVinicius Costa Gomes static int taprio_dump(struct Qdisc *sch, struct sk_buff *skb) 18295a781ccbSVinicius Costa Gomes { 18305a781ccbSVinicius Costa Gomes struct taprio_sched *q = qdisc_priv(sch); 18315a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 1832a3d43c0dSVinicius Costa Gomes struct sched_gate_list *oper, *admin; 18335a781ccbSVinicius Costa Gomes struct tc_mqprio_qopt opt = { 0 }; 1834a3d43c0dSVinicius Costa Gomes struct nlattr *nest, *sched_nest; 18355a781ccbSVinicius Costa Gomes unsigned int i; 18365a781ccbSVinicius Costa Gomes 183718cdd2f0SVladimir Oltean oper = rtnl_dereference(q->oper_sched); 183818cdd2f0SVladimir Oltean admin = rtnl_dereference(q->admin_sched); 1839a3d43c0dSVinicius Costa Gomes 18405a781ccbSVinicius Costa Gomes opt.num_tc = netdev_get_num_tc(dev); 18415a781ccbSVinicius Costa Gomes memcpy(opt.prio_tc_map, dev->prio_tc_map, sizeof(opt.prio_tc_map)); 18425a781ccbSVinicius Costa Gomes 18435a781ccbSVinicius Costa Gomes for (i = 0; i < netdev_get_num_tc(dev); i++) { 18445a781ccbSVinicius Costa Gomes opt.count[i] = dev->tc_to_txq[i].count; 18455a781ccbSVinicius Costa Gomes opt.offset[i] = dev->tc_to_txq[i].offset; 18465a781ccbSVinicius Costa Gomes } 18475a781ccbSVinicius Costa Gomes 1848ae0be8deSMichal Kubecek nest = nla_nest_start_noflag(skb, TCA_OPTIONS); 18495a781ccbSVinicius Costa Gomes if (!nest) 1850a3d43c0dSVinicius Costa Gomes goto start_error; 18515a781ccbSVinicius Costa Gomes 18525a781ccbSVinicius Costa Gomes if (nla_put(skb, TCA_TAPRIO_ATTR_PRIOMAP, sizeof(opt), &opt)) 18535a781ccbSVinicius Costa Gomes goto options_error; 18545a781ccbSVinicius Costa Gomes 18559c66d156SVinicius Costa Gomes if (!FULL_OFFLOAD_IS_ENABLED(q->flags) && 18569c66d156SVinicius Costa Gomes nla_put_s32(skb, TCA_TAPRIO_ATTR_SCHED_CLOCKID, q->clockid)) 18575a781ccbSVinicius Costa Gomes goto options_error; 18585a781ccbSVinicius Costa Gomes 18594cfd5779SVedang Patel if (q->flags && nla_put_u32(skb, TCA_TAPRIO_ATTR_FLAGS, q->flags)) 18604cfd5779SVedang Patel goto options_error; 18614cfd5779SVedang Patel 18624cfd5779SVedang Patel if (q->txtime_delay && 1863a5b64700SVedang Patel nla_put_u32(skb, TCA_TAPRIO_ATTR_TXTIME_DELAY, q->txtime_delay)) 18644cfd5779SVedang Patel goto options_error; 18654cfd5779SVedang Patel 1866a3d43c0dSVinicius Costa Gomes if (oper && dump_schedule(skb, oper)) 18675a781ccbSVinicius Costa Gomes goto options_error; 18685a781ccbSVinicius Costa Gomes 1869a3d43c0dSVinicius Costa Gomes if (!admin) 1870a3d43c0dSVinicius Costa Gomes goto done; 18715a781ccbSVinicius Costa Gomes 1872a3d43c0dSVinicius Costa Gomes sched_nest = nla_nest_start_noflag(skb, TCA_TAPRIO_ATTR_ADMIN_SCHED); 1873e4acf427SColin Ian King if (!sched_nest) 1874e4acf427SColin Ian King goto options_error; 1875a3d43c0dSVinicius Costa Gomes 1876a3d43c0dSVinicius Costa Gomes if (dump_schedule(skb, admin)) 1877a3d43c0dSVinicius Costa Gomes goto admin_error; 1878a3d43c0dSVinicius Costa Gomes 1879a3d43c0dSVinicius Costa Gomes nla_nest_end(skb, sched_nest); 1880a3d43c0dSVinicius Costa Gomes 1881a3d43c0dSVinicius Costa Gomes done: 18825a781ccbSVinicius Costa Gomes return nla_nest_end(skb, nest); 18835a781ccbSVinicius Costa Gomes 1884a3d43c0dSVinicius Costa Gomes admin_error: 1885a3d43c0dSVinicius Costa Gomes nla_nest_cancel(skb, sched_nest); 1886a3d43c0dSVinicius Costa Gomes 18875a781ccbSVinicius Costa Gomes options_error: 18885a781ccbSVinicius Costa Gomes nla_nest_cancel(skb, nest); 1889a3d43c0dSVinicius Costa Gomes 1890a3d43c0dSVinicius Costa Gomes start_error: 1891a3d43c0dSVinicius Costa Gomes return -ENOSPC; 18925a781ccbSVinicius Costa Gomes } 18935a781ccbSVinicius Costa Gomes 18945a781ccbSVinicius Costa Gomes static struct Qdisc *taprio_leaf(struct Qdisc *sch, unsigned long cl) 18955a781ccbSVinicius Costa Gomes { 18961461d212SVladimir Oltean struct taprio_sched *q = qdisc_priv(sch); 18971461d212SVladimir Oltean struct net_device *dev = qdisc_dev(sch); 18981461d212SVladimir Oltean unsigned int ntx = cl - 1; 18995a781ccbSVinicius Costa Gomes 19001461d212SVladimir Oltean if (ntx >= dev->num_tx_queues) 19015a781ccbSVinicius Costa Gomes return NULL; 19025a781ccbSVinicius Costa Gomes 19031461d212SVladimir Oltean return q->qdiscs[ntx]; 19045a781ccbSVinicius Costa Gomes } 19055a781ccbSVinicius Costa Gomes 19065a781ccbSVinicius Costa Gomes static unsigned long taprio_find(struct Qdisc *sch, u32 classid) 19075a781ccbSVinicius Costa Gomes { 19085a781ccbSVinicius Costa Gomes unsigned int ntx = TC_H_MIN(classid); 19095a781ccbSVinicius Costa Gomes 19105a781ccbSVinicius Costa Gomes if (!taprio_queue_get(sch, ntx)) 19115a781ccbSVinicius Costa Gomes return 0; 19125a781ccbSVinicius Costa Gomes return ntx; 19135a781ccbSVinicius Costa Gomes } 19145a781ccbSVinicius Costa Gomes 19155a781ccbSVinicius Costa Gomes static int taprio_dump_class(struct Qdisc *sch, unsigned long cl, 19165a781ccbSVinicius Costa Gomes struct sk_buff *skb, struct tcmsg *tcm) 19175a781ccbSVinicius Costa Gomes { 19185a781ccbSVinicius Costa Gomes struct netdev_queue *dev_queue = taprio_queue_get(sch, cl); 19195a781ccbSVinicius Costa Gomes 19205a781ccbSVinicius Costa Gomes tcm->tcm_parent = TC_H_ROOT; 19215a781ccbSVinicius Costa Gomes tcm->tcm_handle |= TC_H_MIN(cl); 19225a781ccbSVinicius Costa Gomes tcm->tcm_info = dev_queue->qdisc_sleeping->handle; 19235a781ccbSVinicius Costa Gomes 19245a781ccbSVinicius Costa Gomes return 0; 19255a781ccbSVinicius Costa Gomes } 19265a781ccbSVinicius Costa Gomes 19275a781ccbSVinicius Costa Gomes static int taprio_dump_class_stats(struct Qdisc *sch, unsigned long cl, 19285a781ccbSVinicius Costa Gomes struct gnet_dump *d) 19295a781ccbSVinicius Costa Gomes __releases(d->lock) 19305a781ccbSVinicius Costa Gomes __acquires(d->lock) 19315a781ccbSVinicius Costa Gomes { 19325a781ccbSVinicius Costa Gomes struct netdev_queue *dev_queue = taprio_queue_get(sch, cl); 19335a781ccbSVinicius Costa Gomes 19345a781ccbSVinicius Costa Gomes sch = dev_queue->qdisc_sleeping; 193529cbcd85SAhmed S. Darwish if (gnet_stats_copy_basic(d, NULL, &sch->bstats, true) < 0 || 19365dd431b6SPaolo Abeni qdisc_qstats_copy(d, sch) < 0) 19375a781ccbSVinicius Costa Gomes return -1; 19385a781ccbSVinicius Costa Gomes return 0; 19395a781ccbSVinicius Costa Gomes } 19405a781ccbSVinicius Costa Gomes 19415a781ccbSVinicius Costa Gomes static void taprio_walk(struct Qdisc *sch, struct qdisc_walker *arg) 19425a781ccbSVinicius Costa Gomes { 19435a781ccbSVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 19445a781ccbSVinicius Costa Gomes unsigned long ntx; 19455a781ccbSVinicius Costa Gomes 19465a781ccbSVinicius Costa Gomes if (arg->stop) 19475a781ccbSVinicius Costa Gomes return; 19485a781ccbSVinicius Costa Gomes 19495a781ccbSVinicius Costa Gomes arg->count = arg->skip; 19505a781ccbSVinicius Costa Gomes for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) { 1951e046fa89SZhengchao Shao if (!tc_qdisc_stats_dump(sch, ntx + 1, arg)) 19525a781ccbSVinicius Costa Gomes break; 19535a781ccbSVinicius Costa Gomes } 19545a781ccbSVinicius Costa Gomes } 19555a781ccbSVinicius Costa Gomes 19565a781ccbSVinicius Costa Gomes static struct netdev_queue *taprio_select_queue(struct Qdisc *sch, 19575a781ccbSVinicius Costa Gomes struct tcmsg *tcm) 19585a781ccbSVinicius Costa Gomes { 19595a781ccbSVinicius Costa Gomes return taprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent)); 19605a781ccbSVinicius Costa Gomes } 19615a781ccbSVinicius Costa Gomes 19625a781ccbSVinicius Costa Gomes static const struct Qdisc_class_ops taprio_class_ops = { 19635a781ccbSVinicius Costa Gomes .graft = taprio_graft, 19645a781ccbSVinicius Costa Gomes .leaf = taprio_leaf, 19655a781ccbSVinicius Costa Gomes .find = taprio_find, 19665a781ccbSVinicius Costa Gomes .walk = taprio_walk, 19675a781ccbSVinicius Costa Gomes .dump = taprio_dump_class, 19685a781ccbSVinicius Costa Gomes .dump_stats = taprio_dump_class_stats, 19695a781ccbSVinicius Costa Gomes .select_queue = taprio_select_queue, 19705a781ccbSVinicius Costa Gomes }; 19715a781ccbSVinicius Costa Gomes 19725a781ccbSVinicius Costa Gomes static struct Qdisc_ops taprio_qdisc_ops __read_mostly = { 19735a781ccbSVinicius Costa Gomes .cl_ops = &taprio_class_ops, 19745a781ccbSVinicius Costa Gomes .id = "taprio", 19755a781ccbSVinicius Costa Gomes .priv_size = sizeof(struct taprio_sched), 19765a781ccbSVinicius Costa Gomes .init = taprio_init, 1977a3d43c0dSVinicius Costa Gomes .change = taprio_change, 19785a781ccbSVinicius Costa Gomes .destroy = taprio_destroy, 197944d4775cSDavide Caratti .reset = taprio_reset, 198013511704SYannick Vignon .attach = taprio_attach, 19815a781ccbSVinicius Costa Gomes .peek = taprio_peek, 19825a781ccbSVinicius Costa Gomes .dequeue = taprio_dequeue, 19835a781ccbSVinicius Costa Gomes .enqueue = taprio_enqueue, 19845a781ccbSVinicius Costa Gomes .dump = taprio_dump, 19855a781ccbSVinicius Costa Gomes .owner = THIS_MODULE, 19865a781ccbSVinicius Costa Gomes }; 19875a781ccbSVinicius Costa Gomes 19887b9eba7bSLeandro Dorileo static struct notifier_block taprio_device_notifier = { 19897b9eba7bSLeandro Dorileo .notifier_call = taprio_dev_notifier, 19907b9eba7bSLeandro Dorileo }; 19917b9eba7bSLeandro Dorileo 19925a781ccbSVinicius Costa Gomes static int __init taprio_module_init(void) 19935a781ccbSVinicius Costa Gomes { 19947b9eba7bSLeandro Dorileo int err = register_netdevice_notifier(&taprio_device_notifier); 19957b9eba7bSLeandro Dorileo 19967b9eba7bSLeandro Dorileo if (err) 19977b9eba7bSLeandro Dorileo return err; 19987b9eba7bSLeandro Dorileo 19995a781ccbSVinicius Costa Gomes return register_qdisc(&taprio_qdisc_ops); 20005a781ccbSVinicius Costa Gomes } 20015a781ccbSVinicius Costa Gomes 20025a781ccbSVinicius Costa Gomes static void __exit taprio_module_exit(void) 20035a781ccbSVinicius Costa Gomes { 20045a781ccbSVinicius Costa Gomes unregister_qdisc(&taprio_qdisc_ops); 20057b9eba7bSLeandro Dorileo unregister_netdevice_notifier(&taprio_device_notifier); 20065a781ccbSVinicius Costa Gomes } 20075a781ccbSVinicius Costa Gomes 20085a781ccbSVinicius Costa Gomes module_init(taprio_module_init); 20095a781ccbSVinicius Costa Gomes module_exit(taprio_module_exit); 20105a781ccbSVinicius Costa Gomes MODULE_LICENSE("GPL"); 2011