1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* net/sched/sch_teql.c "True" (or "trivial") link equalizer.
3 *
4 * Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
5 */
6
7 #include <linux/module.h>
8 #include <linux/types.h>
9 #include <linux/kernel.h>
10 #include <linux/slab.h>
11 #include <linux/string.h>
12 #include <linux/errno.h>
13 #include <linux/if_arp.h>
14 #include <linux/netdevice.h>
15 #include <linux/init.h>
16 #include <linux/skbuff.h>
17 #include <linux/moduleparam.h>
18 #include <net/dst.h>
19 #include <net/neighbour.h>
20 #include <net/pkt_sched.h>
21
22 /*
23 How to setup it.
24 ----------------
25
26 After loading this module you will find a new device teqlN
27 and new qdisc with the same name. To join a slave to the equalizer
28 you should just set this qdisc on a device f.e.
29
30 # tc qdisc add dev eth0 root teql0
31 # tc qdisc add dev eth1 root teql0
32
33 That's all. Full PnP 8)
34
35 Applicability.
36 --------------
37
38 1. Slave devices MUST be active devices, i.e., they must raise the tbusy
39 signal and generate EOI events. If you want to equalize virtual devices
40 like tunnels, use a normal eql device.
41 2. This device puts no limitations on physical slave characteristics
42 f.e. it will equalize 9600baud line and 100Mb ethernet perfectly :-)
43 Certainly, large difference in link speeds will make the resulting
44 eqalized link unusable, because of huge packet reordering.
45 I estimate an upper useful difference as ~10 times.
46 3. If the slave requires address resolution, only protocols using
47 neighbour cache (IPv4/IPv6) will work over the equalized link.
48 Other protocols are still allowed to use the slave device directly,
49 which will not break load balancing, though native slave
50 traffic will have the highest priority. */
51
52 struct teql_master {
53 struct Qdisc_ops qops;
54 struct net_device *dev;
55 struct Qdisc __rcu *slaves;
56 spinlock_t slaves_lock; /* serializes writes to ->slaves */
57 struct list_head master_list;
58 unsigned long tx_bytes;
59 unsigned long tx_packets;
60 unsigned long tx_errors;
61 unsigned long tx_dropped;
62 };
63
64 struct teql_sched_data {
65 struct Qdisc __rcu *next;
66 struct teql_master *m;
67 struct sk_buff_head q;
68 };
69
70 #define NEXT_SLAVE(q) (((struct teql_sched_data *)qdisc_priv(q))->next)
71
72 #define FMASK (IFF_BROADCAST | IFF_POINTOPOINT)
73
74 /* "teql*" qdisc routines */
75
76 static int
teql_enqueue(struct sk_buff * skb,struct Qdisc * sch,struct sk_buff ** to_free)77 teql_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
78 {
79 struct net_device *dev = qdisc_dev(sch);
80 struct teql_sched_data *q = qdisc_priv(sch);
81
82 if (q->q.qlen < READ_ONCE(dev->tx_queue_len)) {
83 __skb_queue_tail(&q->q, skb);
84 return NET_XMIT_SUCCESS;
85 }
86
87 return qdisc_drop(skb, sch, to_free);
88 }
89
90 static struct sk_buff *
teql_dequeue(struct Qdisc * sch)91 teql_dequeue(struct Qdisc *sch)
92 {
93 struct teql_sched_data *dat = qdisc_priv(sch);
94 struct netdev_queue *dat_queue;
95 struct sk_buff *skb;
96 struct Qdisc *q;
97
98 skb = __skb_dequeue(&dat->q);
99 dat_queue = netdev_get_tx_queue(dat->m->dev, 0);
100 q = rcu_dereference_bh(dat_queue->qdisc);
101
102 if (skb == NULL) {
103 struct net_device *m = qdisc_dev(q);
104 if (m) {
105 spin_lock_bh(&dat->m->slaves_lock);
106 rcu_assign_pointer(dat->m->slaves, sch);
107 spin_unlock_bh(&dat->m->slaves_lock);
108 netif_wake_queue(m);
109 }
110 } else {
111 qdisc_bstats_update(sch, skb);
112 }
113 WRITE_ONCE(sch->q.qlen, dat->q.qlen + READ_ONCE(q->q.qlen));
114 return skb;
115 }
116
117 static struct sk_buff *
teql_peek(struct Qdisc * sch)118 teql_peek(struct Qdisc *sch)
119 {
120 /* teql is meant to be used as root qdisc */
121 return NULL;
122 }
123
124 static void
teql_reset(struct Qdisc * sch)125 teql_reset(struct Qdisc *sch)
126 {
127 struct teql_sched_data *dat = qdisc_priv(sch);
128
129 skb_queue_purge(&dat->q);
130 }
131
132 static void
teql_destroy(struct Qdisc * sch)133 teql_destroy(struct Qdisc *sch)
134 {
135 struct Qdisc *q, *prev;
136 struct teql_sched_data *dat = qdisc_priv(sch);
137 struct teql_master *master = dat->m;
138 struct netdev_queue *txq = NULL;
139 bool reset_master_queue = false;
140
141 if (!master)
142 return;
143
144 spin_lock_bh(&master->slaves_lock);
145 prev = rcu_dereference_protected(master->slaves,
146 lockdep_is_held(&master->slaves_lock));
147 if (prev) {
148 do {
149 struct Qdisc *head, *next;
150
151 q = rcu_dereference_protected(NEXT_SLAVE(prev),
152 lockdep_is_held(&master->slaves_lock));
153 if (q != sch) {
154 prev = q;
155 continue;
156 }
157
158 next = rcu_dereference_protected(NEXT_SLAVE(q),
159 lockdep_is_held(&master->slaves_lock));
160 rcu_assign_pointer(NEXT_SLAVE(prev), next);
161
162 head = rcu_dereference_protected(master->slaves,
163 lockdep_is_held(&master->slaves_lock));
164 if (q == head) {
165 rcu_assign_pointer(master->slaves, next);
166 if (q == next) {
167 txq = netdev_get_tx_queue(master->dev, 0);
168 rcu_assign_pointer(master->slaves, NULL);
169 reset_master_queue = true;
170 }
171 }
172 skb_queue_purge(&dat->q);
173 break;
174 } while (prev != rcu_dereference_protected(master->slaves,
175 lockdep_is_held(&master->slaves_lock)));
176 }
177 spin_unlock_bh(&master->slaves_lock);
178
179 if (reset_master_queue)
180 dev_reset_queue(master->dev, txq, NULL);
181 }
182
teql_qdisc_init(struct Qdisc * sch,struct nlattr * opt,struct netlink_ext_ack * extack)183 static int teql_qdisc_init(struct Qdisc *sch, struct nlattr *opt,
184 struct netlink_ext_ack *extack)
185 {
186 struct net_device *dev = qdisc_dev(sch);
187 struct teql_master *m = (struct teql_master *)sch->ops;
188 struct teql_sched_data *q = qdisc_priv(sch);
189 struct Qdisc *first;
190
191 if (dev->hard_header_len > m->dev->hard_header_len)
192 return -EINVAL;
193
194 if (m->dev == dev)
195 return -ELOOP;
196
197 if (sch->parent != TC_H_ROOT) {
198 NL_SET_ERR_MSG_MOD(extack, "teql can only be used as root");
199 return -EOPNOTSUPP;
200 }
201
202 q->m = m;
203
204 skb_queue_head_init(&q->q);
205
206 spin_lock_bh(&m->slaves_lock);
207 first = rcu_dereference_protected(m->slaves, lockdep_is_held(&m->slaves_lock));
208 if (first) {
209 if (m->dev->flags & IFF_UP) {
210 if ((m->dev->flags & IFF_POINTOPOINT &&
211 !(dev->flags & IFF_POINTOPOINT)) ||
212 (m->dev->flags & IFF_BROADCAST &&
213 !(dev->flags & IFF_BROADCAST)) ||
214 (m->dev->flags & IFF_MULTICAST &&
215 !(dev->flags & IFF_MULTICAST)) ||
216 dev->mtu < m->dev->mtu) {
217 spin_unlock_bh(&m->slaves_lock);
218 return -EINVAL;
219 }
220 } else {
221 if (!(dev->flags&IFF_POINTOPOINT))
222 m->dev->flags &= ~IFF_POINTOPOINT;
223 if (!(dev->flags&IFF_BROADCAST))
224 m->dev->flags &= ~IFF_BROADCAST;
225 if (!(dev->flags&IFF_MULTICAST))
226 m->dev->flags &= ~IFF_MULTICAST;
227 if (dev->mtu < m->dev->mtu)
228 m->dev->mtu = dev->mtu;
229 }
230 rcu_assign_pointer(q->next,
231 rcu_dereference_protected(NEXT_SLAVE(first),
232 lockdep_is_held(&m->slaves_lock)));
233 rcu_assign_pointer(NEXT_SLAVE(first), sch);
234 } else {
235 rcu_assign_pointer(q->next, sch);
236 rcu_assign_pointer(m->slaves, sch);
237 m->dev->mtu = dev->mtu;
238 m->dev->flags = (m->dev->flags&~FMASK)|(dev->flags&FMASK);
239 }
240 spin_unlock_bh(&m->slaves_lock);
241 return 0;
242 }
243
244
245 static int
__teql_resolve(struct sk_buff * skb,struct sk_buff * skb_res,struct net_device * dev,struct netdev_queue * txq,struct dst_entry * dst)246 __teql_resolve(struct sk_buff *skb, struct sk_buff *skb_res,
247 struct net_device *dev, struct netdev_queue *txq,
248 struct dst_entry *dst)
249 {
250 struct neighbour *n;
251 int err = 0;
252
253 n = dst_neigh_lookup_skb(dst, skb);
254 if (!n)
255 return -ENOENT;
256
257 if (dst->dev != dev) {
258 struct neighbour *mn;
259
260 mn = __neigh_lookup_errno(n->tbl, n->primary_key, dev);
261 neigh_release(n);
262 if (IS_ERR(mn))
263 return PTR_ERR(mn);
264 n = mn;
265 }
266
267 if (neigh_event_send(n, skb_res) == 0) {
268 char haddr[MAX_ADDR_LEN];
269
270 neigh_ha_snapshot(haddr, n, dev);
271 if (dev_hard_header(skb, dev, ntohs(skb_protocol(skb, false)),
272 haddr, NULL, skb->len) < 0)
273 err = -EINVAL;
274 } else {
275 err = (skb_res == NULL) ? -EAGAIN : 1;
276 }
277 neigh_release(n);
278 return err;
279 }
280
teql_resolve(struct sk_buff * skb,struct sk_buff * skb_res,struct net_device * dev,struct netdev_queue * txq)281 static inline int teql_resolve(struct sk_buff *skb,
282 struct sk_buff *skb_res,
283 struct net_device *dev,
284 struct netdev_queue *txq)
285 {
286 struct dst_entry *dst = skb_dst(skb);
287 int res;
288
289 if (rcu_access_pointer(txq->qdisc) == &noop_qdisc)
290 return -ENODEV;
291
292 if (!dev->header_ops || !dst)
293 return 0;
294
295 rcu_read_lock();
296 res = __teql_resolve(skb, skb_res, dev, txq, dst);
297 rcu_read_unlock();
298
299 return res;
300 }
301
teql_master_xmit(struct sk_buff * skb,struct net_device * dev)302 static netdev_tx_t teql_master_xmit(struct sk_buff *skb, struct net_device *dev)
303 {
304 struct teql_master *master = netdev_priv(dev);
305 struct Qdisc *start, *q;
306 int busy;
307 int nores;
308 int subq = skb_get_queue_mapping(skb);
309 struct sk_buff *skb_res = NULL;
310
311 restart:
312 nores = 0;
313 busy = 0;
314
315 rcu_read_lock();
316
317 start = rcu_dereference(master->slaves);
318
319 q = start;
320 if (!q)
321 goto drop;
322
323 do {
324 struct net_device *slave = qdisc_dev(q);
325 struct netdev_queue *slave_txq = netdev_get_tx_queue(slave, 0);
326
327 if (rcu_access_pointer(slave_txq->qdisc_sleeping) != q)
328 continue;
329 if (netif_xmit_stopped(netdev_get_tx_queue(slave, subq)) ||
330 !netif_running(slave)) {
331 busy = 1;
332 continue;
333 }
334
335 switch (teql_resolve(skb, skb_res, slave, slave_txq)) {
336 case 0:
337 if (__netif_tx_trylock(slave_txq)) {
338 unsigned int length = qdisc_pkt_len(skb);
339
340 skb->dev = slave;
341 if (!netif_xmit_frozen_or_stopped(slave_txq) &&
342 netdev_start_xmit(skb, slave, slave_txq, false) ==
343 NETDEV_TX_OK) {
344 __netif_tx_unlock(slave_txq);
345 spin_lock(&master->slaves_lock);
346 if (rcu_dereference_protected(master->slaves,
347 lockdep_is_held(&master->slaves_lock)) == q)
348 rcu_assign_pointer(master->slaves,
349 rcu_dereference_protected(NEXT_SLAVE(q),
350 lockdep_is_held(&master->slaves_lock)));
351 spin_unlock(&master->slaves_lock);
352 netif_wake_queue(dev);
353 master->tx_packets++;
354 master->tx_bytes += length;
355 rcu_read_unlock();
356 return NETDEV_TX_OK;
357 }
358 __netif_tx_unlock(slave_txq);
359 }
360 if (netif_xmit_stopped(netdev_get_tx_queue(dev, 0)))
361 busy = 1;
362 break;
363 case 1:
364 spin_lock(&master->slaves_lock);
365 if (rcu_dereference_protected(master->slaves,
366 lockdep_is_held(&master->slaves_lock)) == q)
367 rcu_assign_pointer(master->slaves,
368 rcu_dereference_protected(NEXT_SLAVE(q),
369 lockdep_is_held(&master->slaves_lock)));
370 spin_unlock(&master->slaves_lock);
371 rcu_read_unlock();
372 return NETDEV_TX_OK;
373 default:
374 nores = 1;
375 break;
376 }
377 skb->dev = dev;
378 __skb_pull(skb, skb_network_offset(skb));
379 } while ((q = rcu_dereference(NEXT_SLAVE(q))) != start);
380
381 if (nores && skb_res == NULL) {
382 skb_res = skb;
383 rcu_read_unlock();
384 goto restart;
385 }
386
387 if (busy) {
388 netif_stop_queue(dev);
389 rcu_read_unlock();
390 return NETDEV_TX_BUSY;
391 }
392 master->tx_errors++;
393
394 drop:
395 master->tx_dropped++;
396 rcu_read_unlock();
397 dev_kfree_skb(skb);
398 return NETDEV_TX_OK;
399 }
400
teql_master_open(struct net_device * dev)401 static int teql_master_open(struct net_device *dev)
402 {
403 struct Qdisc *q, *first;
404 struct teql_master *m = netdev_priv(dev);
405 int mtu = 0xFFFE;
406 unsigned int flags = IFF_NOARP | IFF_MULTICAST;
407
408 first = rtnl_dereference(m->slaves);
409 if (!first)
410 return -EUNATCH;
411
412 flags = FMASK;
413
414 q = first;
415 do {
416 struct net_device *slave = qdisc_dev(q);
417
418 if (slave == NULL)
419 return -EUNATCH;
420
421 if (slave->mtu < mtu)
422 mtu = slave->mtu;
423 if (slave->hard_header_len > LL_MAX_HEADER)
424 return -EINVAL;
425
426 /* If all the slaves are BROADCAST, master is BROADCAST
427 If all the slaves are PtP, master is PtP
428 Otherwise, master is NBMA.
429 */
430 if (!(slave->flags&IFF_POINTOPOINT))
431 flags &= ~IFF_POINTOPOINT;
432 if (!(slave->flags&IFF_BROADCAST))
433 flags &= ~IFF_BROADCAST;
434 if (!(slave->flags&IFF_MULTICAST))
435 flags &= ~IFF_MULTICAST;
436 } while ((q = rtnl_dereference(NEXT_SLAVE(q))) != first);
437
438 m->dev->mtu = mtu;
439 m->dev->flags = (m->dev->flags&~FMASK) | flags;
440 netif_start_queue(m->dev);
441 return 0;
442 }
443
teql_master_close(struct net_device * dev)444 static int teql_master_close(struct net_device *dev)
445 {
446 netif_stop_queue(dev);
447 return 0;
448 }
449
teql_master_stats64(struct net_device * dev,struct rtnl_link_stats64 * stats)450 static void teql_master_stats64(struct net_device *dev,
451 struct rtnl_link_stats64 *stats)
452 {
453 struct teql_master *m = netdev_priv(dev);
454
455 stats->tx_packets = m->tx_packets;
456 stats->tx_bytes = m->tx_bytes;
457 stats->tx_errors = m->tx_errors;
458 stats->tx_dropped = m->tx_dropped;
459 }
460
teql_master_mtu(struct net_device * dev,int new_mtu)461 static int teql_master_mtu(struct net_device *dev, int new_mtu)
462 {
463 struct teql_master *m = netdev_priv(dev);
464 struct Qdisc *q, *first;
465
466 first = rtnl_dereference(m->slaves);
467 q = first;
468 if (q) {
469 do {
470 if (new_mtu > qdisc_dev(q)->mtu)
471 return -EINVAL;
472 } while ((q = rtnl_dereference(NEXT_SLAVE(q))) != first);
473 }
474
475 WRITE_ONCE(dev->mtu, new_mtu);
476 return 0;
477 }
478
479 static const struct net_device_ops teql_netdev_ops = {
480 .ndo_open = teql_master_open,
481 .ndo_stop = teql_master_close,
482 .ndo_start_xmit = teql_master_xmit,
483 .ndo_get_stats64 = teql_master_stats64,
484 .ndo_change_mtu = teql_master_mtu,
485 };
486
teql_master_setup(struct net_device * dev)487 static __init void teql_master_setup(struct net_device *dev)
488 {
489 struct teql_master *master = netdev_priv(dev);
490 struct Qdisc_ops *ops = &master->qops;
491
492 spin_lock_init(&master->slaves_lock);
493 master->dev = dev;
494 ops->priv_size = sizeof(struct teql_sched_data);
495
496 ops->enqueue = teql_enqueue;
497 ops->dequeue = teql_dequeue;
498 ops->peek = teql_peek;
499 ops->init = teql_qdisc_init;
500 ops->reset = teql_reset;
501 ops->destroy = teql_destroy;
502 ops->owner = THIS_MODULE;
503
504 dev->netdev_ops = &teql_netdev_ops;
505 dev->type = ARPHRD_VOID;
506 dev->mtu = 1500;
507 dev->min_mtu = 68;
508 dev->max_mtu = 65535;
509 dev->tx_queue_len = 100;
510 dev->flags = IFF_NOARP;
511 dev->hard_header_len = LL_MAX_HEADER;
512 netif_keep_dst(dev);
513 }
514
515 static LIST_HEAD(master_dev_list);
516 static int max_equalizers = 1;
517 module_param(max_equalizers, int, 0);
518 MODULE_PARM_DESC(max_equalizers, "Max number of link equalizers");
519
teql_init(void)520 static int __init teql_init(void)
521 {
522 int i;
523 int err = -ENODEV;
524
525 for (i = 0; i < max_equalizers; i++) {
526 struct net_device *dev;
527 struct teql_master *master;
528
529 dev = alloc_netdev(sizeof(struct teql_master), "teql%d",
530 NET_NAME_UNKNOWN, teql_master_setup);
531 if (!dev) {
532 err = -ENOMEM;
533 break;
534 }
535
536 if ((err = register_netdev(dev))) {
537 free_netdev(dev);
538 break;
539 }
540
541 master = netdev_priv(dev);
542
543 strscpy(master->qops.id, dev->name, IFNAMSIZ);
544 err = register_qdisc(&master->qops);
545
546 if (err) {
547 unregister_netdev(dev);
548 free_netdev(dev);
549 break;
550 }
551
552 list_add_tail(&master->master_list, &master_dev_list);
553 }
554 return i ? 0 : err;
555 }
556
teql_exit(void)557 static void __exit teql_exit(void)
558 {
559 struct teql_master *master, *nxt;
560
561 list_for_each_entry_safe(master, nxt, &master_dev_list, master_list) {
562
563 list_del(&master->master_list);
564
565 unregister_qdisc(&master->qops);
566 unregister_netdev(master->dev);
567 free_netdev(master->dev);
568 }
569 }
570
571 module_init(teql_init);
572 module_exit(teql_exit);
573
574 MODULE_LICENSE("GPL");
575 MODULE_DESCRIPTION("True (or trivial) link equalizer qdisc");
576