1 // SPDX-License-Identifier: GPL-2.0+
3 * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
7 #include <sys/socket.h>
9 #include <net/if_arp.h>
11 #include <netinet/if_ether.h>
16 #include <netlink/msg.h>
17 #include <netlink/attr.h>
18 #include <netlink/socket.h>
20 #include <linux/rtnetlink.h>
21 #include <linux/pkt_cls.h>
23 #include <libubox/vlist.h>
24 #include <libubox/avl-cmp.h>
25 #include <libubox/uloop.h>
29 static void interface_update_cb(struct vlist_tree
*tree
,
30 struct vlist_node
*node_new
,
31 struct vlist_node
*node_old
);
33 static VLIST_TREE(devices
, avl_strcmp
, interface_update_cb
, true, false);
34 static VLIST_TREE(interfaces
, avl_strcmp
, interface_update_cb
, true, false);
36 static struct nl_sock
*rtnl_sock
;
38 #define APPEND(_buf, _ofs, _format, ...) _ofs += snprintf(_buf + _ofs, sizeof(_buf) - _ofs, _format, ##__VA_ARGS__)
40 struct qosify_iface_config
{
41 struct blob_attr
*data
;
47 bool autorate_ingress
;
49 const char *bandwidth_up
;
50 const char *bandwidth_down
;
52 const char *common_opts
;
53 const char *ingress_opts
;
54 const char *egress_opts
;
59 struct vlist_node node
;
61 char ifname
[IFNAMSIZ
];
65 struct blob_attr
*config_data
;
66 struct qosify_iface_config config
;
76 IFACE_ATTR_HOST_ISOLATE
,
77 IFACE_ATTR_AUTORATE_IN
,
78 IFACE_ATTR_INGRESS_OPTS
,
79 IFACE_ATTR_EGRESS_OPTS
,
84 static inline const char *qosify_iface_name(struct qosify_iface
*iface
)
86 return iface
->node
.avl
.key
;
90 iface_config_parse(struct blob_attr
*attr
, struct blob_attr
**tb
)
92 static const struct blobmsg_policy policy
[__IFACE_ATTR_MAX
] = {
93 [IFACE_ATTR_BW_UP
] = { "bandwidth_up", BLOBMSG_TYPE_STRING
},
94 [IFACE_ATTR_BW_DOWN
] = { "bandwidth_down", BLOBMSG_TYPE_STRING
},
95 [IFACE_ATTR_INGRESS
] = { "ingress", BLOBMSG_TYPE_BOOL
},
96 [IFACE_ATTR_EGRESS
] = { "egress", BLOBMSG_TYPE_BOOL
},
97 [IFACE_ATTR_MODE
] = { "mode", BLOBMSG_TYPE_STRING
},
98 [IFACE_ATTR_NAT
] = { "nat", BLOBMSG_TYPE_BOOL
},
99 [IFACE_ATTR_HOST_ISOLATE
] = { "host_isolate", BLOBMSG_TYPE_BOOL
},
100 [IFACE_ATTR_AUTORATE_IN
] = { "autorate_ingress", BLOBMSG_TYPE_BOOL
},
101 [IFACE_ATTR_INGRESS_OPTS
] = { "ingress_options", BLOBMSG_TYPE_STRING
},
102 [IFACE_ATTR_EGRESS_OPTS
] = { "egress_options", BLOBMSG_TYPE_STRING
},
103 [IFACE_ATTR_OPTS
] = { "options", BLOBMSG_TYPE_STRING
},
106 blobmsg_parse(policy
, __IFACE_ATTR_MAX
, tb
, blobmsg_data(attr
), blobmsg_len(attr
));
110 iface_config_equal(struct qosify_iface
*if1
, struct qosify_iface
*if2
)
112 struct blob_attr
*tb1
[__IFACE_ATTR_MAX
], *tb2
[__IFACE_ATTR_MAX
];
115 iface_config_parse(if1
->config_data
, tb1
);
116 iface_config_parse(if2
->config_data
, tb2
);
118 for (i
= 0; i
< __IFACE_ATTR_MAX
; i
++) {
119 if (!!tb1
[i
] != !!tb2
[i
])
125 if (blob_raw_len(tb1
[i
]) != blob_raw_len(tb2
[i
]))
128 if (memcmp(tb1
[i
], tb2
[i
], blob_raw_len(tb1
[i
])) != 0)
135 static const char *check_str(struct blob_attr
*attr
)
137 const char *str
= blobmsg_get_string(attr
);
139 if (strchr(str
, '\''))
146 iface_config_set(struct qosify_iface
*iface
, struct blob_attr
*attr
)
148 struct qosify_iface_config
*cfg
= &iface
->config
;
149 struct blob_attr
*tb
[__IFACE_ATTR_MAX
];
150 struct blob_attr
*cur
;
152 iface_config_parse(attr
, tb
);
154 memset(cfg
, 0, sizeof(*cfg
));
157 cfg
->mode
= "diffserv4";
160 cfg
->host_isolate
= true;
161 cfg
->autorate_ingress
= false;
162 cfg
->nat
= !iface
->device
;
164 if ((cur
= tb
[IFACE_ATTR_BW_UP
]) != NULL
)
165 cfg
->bandwidth_up
= check_str(cur
);
166 if ((cur
= tb
[IFACE_ATTR_BW_DOWN
]) != NULL
)
167 cfg
->bandwidth_down
= check_str(cur
);
168 if ((cur
= tb
[IFACE_ATTR_MODE
]) != NULL
)
169 cfg
->mode
= check_str(cur
);
170 if ((cur
= tb
[IFACE_ATTR_OPTS
]) != NULL
)
171 cfg
->common_opts
= check_str(cur
);
172 if ((cur
= tb
[IFACE_ATTR_EGRESS_OPTS
]) != NULL
)
173 cfg
->egress_opts
= check_str(cur
);
174 if ((cur
= tb
[IFACE_ATTR_INGRESS_OPTS
]) != NULL
)
175 cfg
->ingress_opts
= check_str(cur
);
176 if ((cur
= tb
[IFACE_ATTR_INGRESS
]) != NULL
)
177 cfg
->ingress
= blobmsg_get_bool(cur
);
178 if ((cur
= tb
[IFACE_ATTR_EGRESS
]) != NULL
)
179 cfg
->egress
= blobmsg_get_bool(cur
);
180 if ((cur
= tb
[IFACE_ATTR_NAT
]) != NULL
)
181 cfg
->nat
= blobmsg_get_bool(cur
);
182 if ((cur
= tb
[IFACE_ATTR_HOST_ISOLATE
]) != NULL
)
183 cfg
->host_isolate
= blobmsg_get_bool(cur
);
184 if ((cur
= tb
[IFACE_ATTR_AUTORATE_IN
]) != NULL
)
185 cfg
->autorate_ingress
= blobmsg_get_bool(cur
);
189 interface_ifb_name(struct qosify_iface
*iface
)
191 static char ifname
[IFNAMSIZ
+ 1] = "ifb-";
192 int len
= strlen(iface
->ifname
);
194 if (len
+ 4 < IFNAMSIZ
) {
195 snprintf(ifname
+ 4, IFNAMSIZ
- 4, "%s", iface
->ifname
);
200 ifname
[4] = iface
->ifname
[0];
201 ifname
[5] = iface
->ifname
[1];
202 snprintf(ifname
+ 6, IFNAMSIZ
- 6, "%s", iface
->ifname
+ len
- (IFNAMSIZ
+ 6) - 1);
208 prepare_qdisc_cmd(char *buf
, int len
, const char *dev
, bool add
, const char *type
)
210 return snprintf(buf
, len
, "tc qdisc %s dev '%s' %s",
211 add
? "add" : "del", dev
, type
);
215 prepare_filter_cmd(char *buf
, int len
, const char *dev
, int prio
, bool add
, bool egress
)
217 return snprintf(buf
, len
, "tc filter %s dev '%s' %sgress prio %d",
218 add
? "add" : "del", dev
, egress
? "e" : "in", prio
);
222 cmd_add_bpf_filter(const char *ifname
, int prio
, bool egress
, bool eth
)
224 struct tcmsg tcmsg
= {
225 .tcm_family
= AF_UNSPEC
,
226 .tcm_ifindex
= if_nametoindex(ifname
),
234 suffix
= qosify_get_program(!egress
* QOSIFY_INGRESS
+ !eth
* QOSIFY_IP_ONLY
, &prog_fd
);
238 snprintf(name
, sizeof(name
), "qosify_%s", suffix
);
241 tcmsg
.tcm_parent
= TC_H_MAKE(TC_H_CLSACT
, TC_H_MIN_EGRESS
);
243 tcmsg
.tcm_parent
= TC_H_MAKE(TC_H_CLSACT
, TC_H_MIN_INGRESS
);
245 tcmsg
.tcm_info
= TC_H_MAKE(prio
<< 16, htons(ETH_P_ALL
));
247 msg
= nlmsg_alloc_simple(RTM_NEWTFILTER
, NLM_F_REQUEST
| NLM_F_CREATE
| NLM_F_EXCL
);
248 nlmsg_append(msg
, &tcmsg
, sizeof(tcmsg
), NLMSG_ALIGNTO
);
249 nla_put_string(msg
, TCA_KIND
, "bpf");
251 opts
= nla_nest_start(msg
, TCA_OPTIONS
);
252 nla_put_u32(msg
, TCA_BPF_FD
, prog_fd
);
253 nla_put_string(msg
, TCA_BPF_NAME
, name
);
254 nla_put_u32(msg
, TCA_BPF_FLAGS
, TCA_BPF_FLAG_ACT_DIRECT
);
255 nla_put_u32(msg
, TCA_BPF_FLAGS_GEN
, TCA_CLS_FLAGS_SKIP_HW
);
256 nla_nest_end(msg
, opts
);
258 nl_send_auto_complete(rtnl_sock
, msg
);
261 return nl_wait_for_ack(rtnl_sock
);
265 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
267 struct qosify_iface_config
*cfg
= &iface
->config
;
268 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
269 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
273 ofs
= prepare_qdisc_cmd(buf
, sizeof(buf
), ifname
, true, "clsact");
274 qosify_run_cmd(buf
, true);
276 ofs
= prepare_qdisc_cmd(buf
, sizeof(buf
), ifname
, true, "root cake");
278 APPEND(buf
, ofs
, " bandwidth %s", bw
);
280 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
281 if (!egress
&& cfg
->autorate_ingress
)
282 APPEND(buf
, ofs
, " autorate-ingress");
284 if (cfg
->host_isolate
)
285 APPEND(buf
, ofs
, " %snat dual-%shost",
286 cfg
->nat
? "" : "no",
287 egress
? "src" : "dst");
289 APPEND(buf
, ofs
, " flows");
291 APPEND(buf
, ofs
, " %s %s",
292 cfg
->common_opts
? cfg
->common_opts
: "",
293 dir_opts
? dir_opts
: "");
295 return qosify_run_cmd(buf
, false);
299 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
301 const char *ifbdev
= interface_ifb_name(iface
);
303 int prio
= QOSIFY_PRIO_BASE
;
306 cmd_add_bpf_filter(iface
->ifname
, prio
++, false, eth
);
308 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
309 APPEND(buf
, ofs
, " protocol ip u32 match ip sport 53 0xffff "
310 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME
);
311 qosify_run_cmd(buf
, false);
313 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
314 APPEND(buf
, ofs
, " protocol 802.1Q u32 offset plus 4 match ip sport 53 0xffff "
315 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME
);
316 qosify_run_cmd(buf
, false);
318 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
319 APPEND(buf
, ofs
, " protocol ipv6 u32 match ip6 sport 53 0xffff "
320 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME
);
321 qosify_run_cmd(buf
, false);
323 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
324 APPEND(buf
, ofs
, " protocol ipv6 u32 offset plus 4 match ip6 sport 53 0xffff "
325 "flowid 1:1 action mirred egress redirect dev " QOSIFY_DNS_IFNAME
);
326 qosify_run_cmd(buf
, false);
329 if (!iface
->config
.ingress
)
332 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
333 qosify_run_cmd(buf
, false);
335 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
337 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
338 qosify_run_cmd(buf
, false);
340 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
341 APPEND(buf
, ofs
, " protocol all u32 match u32 0 0 flowid 1:1"
342 " action mirred egress redirect dev '%s'", ifbdev
);
343 return qosify_run_cmd(buf
, false);
346 static int cmd_add_egress(struct qosify_iface
*iface
, bool eth
)
348 if (!iface
->config
.egress
)
351 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
353 return cmd_add_bpf_filter(iface
->ifname
, QOSIFY_PRIO_BASE
, true, eth
);
357 interface_clear_qdisc(struct qosify_iface
*iface
)
362 prepare_qdisc_cmd(buf
, sizeof(buf
), iface
->ifname
, false, "root");
363 qosify_run_cmd(buf
, true);
365 for (i
= 0; i
< 6; i
++) {
366 prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, QOSIFY_PRIO_BASE
+ i
, false, false);
367 qosify_run_cmd(buf
, true);
370 prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, QOSIFY_PRIO_BASE
, false, true);
371 qosify_run_cmd(buf
, true);
373 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
374 qosify_run_cmd(buf
, true);
378 interface_start(struct qosify_iface
*iface
)
380 struct ifreq ifr
= {};
383 if (!iface
->ifname
[0] || iface
->active
)
386 ULOG_INFO("start interface %s\n", iface
->ifname
);
388 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
389 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
390 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
394 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
396 interface_clear_qdisc(iface
);
397 cmd_add_egress(iface
, eth
);
398 cmd_add_ingress(iface
, eth
);
400 iface
->active
= true;
404 interface_stop(struct qosify_iface
*iface
)
406 if (!iface
->ifname
[0] || !iface
->active
)
409 ULOG_INFO("stop interface %s\n", iface
->ifname
);
410 iface
->active
= false;
412 interface_clear_qdisc(iface
);
416 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
418 iface
->config_data
= blob_memdup(config
);
419 iface_config_set(iface
, iface
->config_data
);
420 interface_start(iface
);
424 interface_update_cb(struct vlist_tree
*tree
,
425 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
427 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
430 if_new
= container_of(node_new
, struct qosify_iface
, node
);
432 if_old
= container_of(node_old
, struct qosify_iface
, node
);
434 if (if_new
&& if_old
) {
435 if (!iface_config_equal(if_old
, if_new
)) {
436 interface_stop(if_old
);
437 free(if_old
->config_data
);
438 interface_set_config(if_old
, if_new
->config_data
);
446 interface_stop(if_old
);
447 free(if_old
->config_data
);
452 interface_set_config(if_new
, if_new
->config_data
);
456 interface_create(struct blob_attr
*attr
, bool device
)
458 struct qosify_iface
*iface
;
459 const char *name
= blobmsg_name(attr
);
460 int name_len
= strlen(name
);
463 if (strchr(name
, '\''))
466 if (name_len
>= IFNAMSIZ
)
469 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
472 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
473 strcpy(name_buf
, blobmsg_name(attr
));
474 iface
->config_data
= attr
;
475 iface
->device
= device
;
476 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
479 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
481 struct blob_attr
*cur
;
484 vlist_update(&devices
);
485 blobmsg_for_each_attr(cur
, devs
, rem
)
486 interface_create(cur
, true);
487 vlist_flush(&devices
);
489 vlist_update(&interfaces
);
490 blobmsg_for_each_attr(cur
, ifaces
, rem
)
491 interface_create(cur
, false);
492 vlist_flush(&interfaces
);
496 qosify_iface_check_device(struct qosify_iface
*iface
)
498 const char *name
= qosify_iface_name(iface
);
501 ifindex
= if_nametoindex(name
);
503 interface_stop(iface
);
504 iface
->ifname
[0] = 0;
506 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
507 interface_start(iface
);
512 qosify_iface_check_interface(struct qosify_iface
*iface
)
514 const char *name
= qosify_iface_name(iface
);
515 char ifname
[IFNAMSIZ
];
517 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
518 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
519 interface_start(iface
);
521 interface_stop(iface
);
522 iface
->ifname
[0] = 0;
526 static void qos_iface_check_cb(struct uloop_timeout
*t
)
528 struct qosify_iface
*iface
;
530 vlist_for_each_element(&devices
, iface
, node
)
531 qosify_iface_check_device(iface
);
532 vlist_for_each_element(&interfaces
, iface
, node
)
533 qosify_iface_check_interface(iface
);
534 qosify_ubus_update_bridger(false);
537 void qosify_iface_check(void)
539 static struct uloop_timeout timer
= {
540 .cb
= qos_iface_check_cb
,
543 uloop_timeout_set(&timer
, 10);
547 __qosify_iface_status(struct blob_buf
*b
, struct qosify_iface
*iface
)
551 c
= blobmsg_open_table(b
, qosify_iface_name(iface
));
552 blobmsg_add_u8(b
, "active", iface
->active
);
553 if (iface
->ifname
[0])
554 blobmsg_add_string(b
, "ifname", iface
->ifname
);
555 blobmsg_add_u8(b
, "egress", iface
->config
.egress
);
556 blobmsg_add_u8(b
, "ingress", iface
->config
.ingress
);
557 blobmsg_close_table(b
, c
);
561 void qosify_iface_status(struct blob_buf
*b
)
563 struct qosify_iface
*iface
;
566 c
= blobmsg_open_table(b
, "devices");
567 vlist_for_each_element(&devices
, iface
, node
)
568 __qosify_iface_status(b
, iface
);
569 blobmsg_close_table(b
, c
);
571 c
= blobmsg_open_table(b
, "interfaces");
572 vlist_for_each_element(&interfaces
, iface
, node
)
573 __qosify_iface_status(b
, iface
);
574 blobmsg_close_table(b
, c
);
578 qosify_nl_error_cb(struct sockaddr_nl
*nla
, struct nlmsgerr
*err
,
581 struct nlmsghdr
*nlh
= (struct nlmsghdr
*) err
- 1;
582 struct nlattr
*tb
[NLMSGERR_ATTR_MAX
+ 1];
583 struct nlattr
*attrs
;
584 int ack_len
= sizeof(*nlh
) + sizeof(int) + sizeof(*nlh
);
585 int len
= nlh
->nlmsg_len
;
586 const char *errstr
= "(unknown)";
588 if (!(nlh
->nlmsg_flags
& NLM_F_ACK_TLVS
))
591 if (!(nlh
->nlmsg_flags
& NLM_F_CAPPED
))
592 ack_len
+= err
->msg
.nlmsg_len
- sizeof(*nlh
);
594 attrs
= (void *) ((unsigned char *) nlh
+ ack_len
);
597 nla_parse(tb
, NLMSGERR_ATTR_MAX
, attrs
, len
, NULL
);
598 if (tb
[NLMSGERR_ATTR_MSG
])
599 errstr
= nla_data(tb
[NLMSGERR_ATTR_MSG
]);
601 ULOG_ERR("Netlink error(%d): %s\n", err
->error
, errstr
);
607 __qosify_iface_get_device(struct blob_buf
*b
, struct qosify_iface
*iface
)
609 if (!iface
->ifname
[0] || !iface
->active
)
612 blobmsg_add_string(b
, NULL
, iface
->ifname
);
615 void qosify_iface_get_devices(struct blob_buf
*b
)
617 struct qosify_iface
*iface
;
619 vlist_for_each_element(&devices
, iface
, node
)
620 __qosify_iface_get_device(b
, iface
);
621 vlist_for_each_element(&interfaces
, iface
, node
)
622 __qosify_iface_get_device(b
, iface
);
625 int qosify_iface_init(void)
629 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
633 rtnl_sock
= nl_socket_alloc();
637 if (nl_connect(rtnl_sock
, NETLINK_ROUTE
))
640 nl_cb_err(nl_socket_get_cb(rtnl_sock
), NL_CB_CUSTOM
,
641 qosify_nl_error_cb
, NULL
);
643 fd
= nl_socket_get_fd(rtnl_sock
);
645 setsockopt(fd
, SOL_NETLINK
, NETLINK_EXT_ACK
, &opt
, sizeof(opt
));
648 setsockopt(fd
, SOL_NETLINK
, NETLINK_CAP_ACK
, &opt
, sizeof(opt
));
653 void qosify_iface_stop(void)
655 struct qosify_iface
*iface
;
657 vlist_for_each_element(&interfaces
, iface
, node
)
658 interface_stop(iface
);
659 vlist_for_each_element(&devices
, iface
, node
)
660 interface_stop(iface
);
662 nl_socket_free(rtnl_sock
);