sch_taprio.c 50 KB

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