1 // SPDX-License-Identifier: GPL-2.0+
3 * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
6 #include <sys/socket.h>
8 #include <net/if_arp.h>
14 #include <libubox/vlist.h>
15 #include <libubox/avl-cmp.h>
16 #include <libubox/uloop.h>
20 static void interface_update_cb(struct vlist_tree
*tree
,
21 struct vlist_node
*node_new
,
22 struct vlist_node
*node_old
);
24 static VLIST_TREE(devices
, avl_strcmp
, interface_update_cb
, true, false);
25 static VLIST_TREE(interfaces
, avl_strcmp
, interface_update_cb
, true, false);
28 #define APPEND(_buf, _ofs, _format, ...) _ofs += snprintf(_buf + _ofs, sizeof(_buf) - _ofs, _format, ##__VA_ARGS__)
30 struct qosify_iface_config
{
31 struct blob_attr
*data
;
37 bool autorate_ingress
;
39 const char *bandwidth_up
;
40 const char *bandwidth_down
;
42 const char *common_opts
;
43 const char *ingress_opts
;
44 const char *egress_opts
;
49 struct vlist_node node
;
51 char ifname
[IFNAMSIZ
];
55 struct blob_attr
*config_data
;
56 struct qosify_iface_config config
;
66 IFACE_ATTR_HOST_ISOLATE
,
67 IFACE_ATTR_AUTORATE_IN
,
68 IFACE_ATTR_INGRESS_OPTS
,
69 IFACE_ATTR_EGRESS_OPTS
,
74 static inline const char *qosify_iface_name(struct qosify_iface
*iface
)
76 return iface
->node
.avl
.key
;
80 iface_config_parse(struct blob_attr
*attr
, struct blob_attr
**tb
)
82 static const struct blobmsg_policy policy
[__IFACE_ATTR_MAX
] = {
83 [IFACE_ATTR_BW_UP
] = { "bandwidth_up", BLOBMSG_TYPE_STRING
},
84 [IFACE_ATTR_BW_DOWN
] = { "bandwidth_down", BLOBMSG_TYPE_STRING
},
85 [IFACE_ATTR_INGRESS
] = { "ingress", BLOBMSG_TYPE_BOOL
},
86 [IFACE_ATTR_EGRESS
] = { "egress", BLOBMSG_TYPE_BOOL
},
87 [IFACE_ATTR_MODE
] = { "mode", BLOBMSG_TYPE_STRING
},
88 [IFACE_ATTR_NAT
] = { "nat", BLOBMSG_TYPE_BOOL
},
89 [IFACE_ATTR_HOST_ISOLATE
] = { "host_isolate", BLOBMSG_TYPE_BOOL
},
90 [IFACE_ATTR_AUTORATE_IN
] = { "autorate_ingress", BLOBMSG_TYPE_BOOL
},
91 [IFACE_ATTR_INGRESS_OPTS
] = { "ingress_options", BLOBMSG_TYPE_STRING
},
92 [IFACE_ATTR_EGRESS_OPTS
] = { "egress_options", BLOBMSG_TYPE_STRING
},
93 [IFACE_ATTR_OPTS
] = { "options", BLOBMSG_TYPE_STRING
},
96 blobmsg_parse(policy
, __IFACE_ATTR_MAX
, tb
, blobmsg_data(attr
), blobmsg_len(attr
));
100 iface_config_equal(struct qosify_iface
*if1
, struct qosify_iface
*if2
)
102 struct blob_attr
*tb1
[__IFACE_ATTR_MAX
], *tb2
[__IFACE_ATTR_MAX
];
105 iface_config_parse(if1
->config_data
, tb1
);
106 iface_config_parse(if2
->config_data
, tb2
);
108 for (i
= 0; i
< __IFACE_ATTR_MAX
; i
++) {
109 if (!!tb1
[i
] != !!tb2
[i
])
115 if (blob_raw_len(tb1
[i
]) != blob_raw_len(tb2
[i
]))
118 if (memcmp(tb1
[i
], tb2
[i
], blob_raw_len(tb1
[i
])) != 0)
125 static const char *check_str(struct blob_attr
*attr
)
127 const char *str
= blobmsg_get_string(attr
);
129 if (strchr(str
, '\''))
136 iface_config_set(struct qosify_iface
*iface
, struct blob_attr
*attr
)
138 struct qosify_iface_config
*cfg
= &iface
->config
;
139 struct blob_attr
*tb
[__IFACE_ATTR_MAX
];
140 struct blob_attr
*cur
;
142 iface_config_parse(attr
, tb
);
144 memset(cfg
, 0, sizeof(*cfg
));
147 cfg
->mode
= "diffserv4";
150 cfg
->host_isolate
= true;
151 cfg
->autorate_ingress
= false;
152 cfg
->nat
= !iface
->device
;
154 if ((cur
= tb
[IFACE_ATTR_BW_UP
]) != NULL
)
155 cfg
->bandwidth_up
= check_str(cur
);
156 if ((cur
= tb
[IFACE_ATTR_BW_DOWN
]) != NULL
)
157 cfg
->bandwidth_down
= check_str(cur
);
158 if ((cur
= tb
[IFACE_ATTR_MODE
]) != NULL
)
159 cfg
->mode
= check_str(cur
);
160 if ((cur
= tb
[IFACE_ATTR_OPTS
]) != NULL
)
161 cfg
->common_opts
= check_str(cur
);
162 if ((cur
= tb
[IFACE_ATTR_EGRESS_OPTS
]) != NULL
)
163 cfg
->egress_opts
= check_str(cur
);
164 if ((cur
= tb
[IFACE_ATTR_INGRESS_OPTS
]) != NULL
)
165 cfg
->ingress_opts
= check_str(cur
);
166 if ((cur
= tb
[IFACE_ATTR_INGRESS
]) != NULL
)
167 cfg
->ingress
= blobmsg_get_bool(cur
);
168 if ((cur
= tb
[IFACE_ATTR_EGRESS
]) != NULL
)
169 cfg
->egress
= blobmsg_get_bool(cur
);
170 if ((cur
= tb
[IFACE_ATTR_NAT
]) != NULL
)
171 cfg
->nat
= blobmsg_get_bool(cur
);
172 if ((cur
= tb
[IFACE_ATTR_HOST_ISOLATE
]) != NULL
)
173 cfg
->host_isolate
= blobmsg_get_bool(cur
);
174 if ((cur
= tb
[IFACE_ATTR_AUTORATE_IN
]) != NULL
)
175 cfg
->autorate_ingress
= blobmsg_get_bool(cur
);
179 interface_ifb_name(struct qosify_iface
*iface
)
181 static char ifname
[IFNAMSIZ
+ 1] = "ifb-";
182 int len
= strlen(iface
->ifname
);
184 if (len
+ 4 < IFNAMSIZ
) {
185 snprintf(ifname
+ 4, IFNAMSIZ
- 4, "%s", iface
->ifname
);
190 ifname
[4] = iface
->ifname
[0];
191 ifname
[5] = iface
->ifname
[1];
192 snprintf(ifname
+ 6, IFNAMSIZ
- 6, "%s", iface
->ifname
+ len
- (IFNAMSIZ
+ 6) - 1);
198 prepare_qdisc_cmd(char *buf
, int len
, const char *dev
, bool add
, const char *type
)
200 return snprintf(buf
, len
, "tc qdisc %s dev '%s' %s",
201 add
? "add" : "del", dev
, type
);
205 prepare_filter_cmd(char *buf
, int len
, const char *dev
, int prio
, bool add
, bool egress
)
207 return snprintf(buf
, len
, "tc filter %s dev '%s' %sgress prio %d",
208 add
? "add" : "del", dev
, egress
? "e" : "in", prio
);
212 cmd_add_bpf_filter(const char *ifname
, int prio
, bool egress
, bool eth
)
217 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), ifname
, prio
, true, egress
);
218 APPEND(buf
, ofs
, " bpf object-pinned /sys/fs/bpf/qosify_%sgress_%s verbose direct-action",
222 return qosify_run_cmd(buf
, false);
226 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
228 struct qosify_iface_config
*cfg
= &iface
->config
;
229 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
230 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
234 ofs
= prepare_qdisc_cmd(buf
, sizeof(buf
), ifname
, true, "clsact");
235 qosify_run_cmd(buf
, true);
237 ofs
= prepare_qdisc_cmd(buf
, sizeof(buf
), ifname
, true, "root cake");
239 APPEND(buf
, ofs
, " bandwidth %s", bw
);
241 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
242 if (!egress
&& cfg
->autorate_ingress
)
243 APPEND(buf
, ofs
, " autorate-ingress");
245 if (cfg
->host_isolate
)
246 APPEND(buf
, ofs
, " %snat dual-%shost",
247 cfg
->nat
? "" : "no",
248 egress
? "src" : "dst");
250 APPEND(buf
, ofs
, " flows");
252 APPEND(buf
, ofs
, " %s %s",
253 cfg
->common_opts
? cfg
->common_opts
: "",
254 dir_opts
? dir_opts
: "");
256 return qosify_run_cmd(buf
, false);
260 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
262 const char *ifbdev
= interface_ifb_name(iface
);
264 int prio
= QOSIFY_PRIO_BASE
;
267 cmd_add_bpf_filter(iface
->ifname
, prio
++, false, eth
);
269 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
270 APPEND(buf
, ofs
, " protocol ip u32 match ip sport 53 0xffff "
271 "flowid 1:1 action mirred egress redirect dev ifb-dns");
272 qosify_run_cmd(buf
, false);
274 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
275 APPEND(buf
, ofs
, " protocol 802.1Q u32 offset plus 4 match ip sport 53 0xffff "
276 "flowid 1:1 action mirred egress redirect dev ifb-dns");
277 qosify_run_cmd(buf
, false);
279 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
280 APPEND(buf
, ofs
, " protocol ipv6 u32 match ip6 sport 53 0xffff "
281 "flowid 1:1 action mirred egress redirect dev ifb-dns");
282 qosify_run_cmd(buf
, false);
284 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
285 APPEND(buf
, ofs
, " protocol ipv6 u32 offset plus 4 match ip6 sport 53 0xffff "
286 "flowid 1:1 action mirred egress redirect dev ifb-dns");
287 qosify_run_cmd(buf
, false);
290 if (!iface
->config
.ingress
)
293 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
294 qosify_run_cmd(buf
, false);
296 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
298 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
299 qosify_run_cmd(buf
, false);
301 ofs
= prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, prio
++, true, false);
302 APPEND(buf
, ofs
, " protocol all u32 match u32 0 0 flowid 1:1"
303 " action mirred egress redirect dev '%s'", ifbdev
);
304 return qosify_run_cmd(buf
, false);
307 static int cmd_add_egress(struct qosify_iface
*iface
, bool eth
)
309 if (!iface
->config
.egress
)
312 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
314 return cmd_add_bpf_filter(iface
->ifname
, QOSIFY_PRIO_BASE
, true, eth
);
318 interface_clear_qdisc(struct qosify_iface
*iface
)
323 prepare_qdisc_cmd(buf
, sizeof(buf
), iface
->ifname
, false, "root");
324 qosify_run_cmd(buf
, true);
326 for (i
= 0; i
< 6; i
++) {
327 prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, QOSIFY_PRIO_BASE
+ i
, false, false);
328 qosify_run_cmd(buf
, true);
331 prepare_filter_cmd(buf
, sizeof(buf
), iface
->ifname
, QOSIFY_PRIO_BASE
, false, true);
332 qosify_run_cmd(buf
, true);
334 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
335 qosify_run_cmd(buf
, true);
339 interface_start(struct qosify_iface
*iface
)
341 struct ifreq ifr
= {};
344 if (!iface
->ifname
[0] || iface
->active
)
347 ULOG_INFO("start interface %s\n", iface
->ifname
);
349 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
350 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
351 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
355 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
357 interface_clear_qdisc(iface
);
358 cmd_add_egress(iface
, eth
);
359 cmd_add_ingress(iface
, eth
);
361 iface
->active
= true;
365 interface_stop(struct qosify_iface
*iface
)
367 if (!iface
->ifname
[0] || !iface
->active
)
370 ULOG_INFO("stop interface %s\n", iface
->ifname
);
371 iface
->active
= false;
373 interface_clear_qdisc(iface
);
377 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
379 iface
->config_data
= blob_memdup(config
);
380 iface_config_set(iface
, iface
->config_data
);
381 interface_start(iface
);
385 interface_update_cb(struct vlist_tree
*tree
,
386 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
388 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
391 if_new
= container_of(node_new
, struct qosify_iface
, node
);
393 if_old
= container_of(node_old
, struct qosify_iface
, node
);
395 if (if_new
&& if_old
) {
396 if (!iface_config_equal(if_old
, if_new
)) {
397 interface_stop(if_old
);
398 free(if_old
->config_data
);
399 interface_set_config(if_old
, if_new
->config_data
);
407 interface_stop(if_old
);
408 free(if_old
->config_data
);
413 interface_set_config(if_new
, if_new
->config_data
);
417 interface_create(struct blob_attr
*attr
, bool device
)
419 struct qosify_iface
*iface
;
420 const char *name
= blobmsg_name(attr
);
421 int name_len
= strlen(name
);
424 if (strchr(name
, '\''))
427 if (name_len
>= IFNAMSIZ
)
430 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
433 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
434 strcpy(name_buf
, blobmsg_name(attr
));
435 iface
->config_data
= attr
;
436 iface
->device
= device
;
437 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
440 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
442 struct blob_attr
*cur
;
445 vlist_update(&devices
);
446 blobmsg_for_each_attr(cur
, devs
, rem
)
447 interface_create(cur
, true);
448 vlist_flush(&devices
);
450 vlist_update(&interfaces
);
451 blobmsg_for_each_attr(cur
, ifaces
, rem
)
452 interface_create(cur
, false);
453 vlist_flush(&interfaces
);
457 qosify_iface_check_device(struct qosify_iface
*iface
)
459 const char *name
= qosify_iface_name(iface
);
462 ifindex
= if_nametoindex(name
);
464 interface_stop(iface
);
465 iface
->ifname
[0] = 0;
467 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
468 interface_start(iface
);
473 qosify_iface_check_interface(struct qosify_iface
*iface
)
475 const char *name
= qosify_iface_name(iface
);
476 char ifname
[IFNAMSIZ
];
478 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
479 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
480 interface_start(iface
);
482 interface_stop(iface
);
483 iface
->ifname
[0] = 0;
487 static void qos_iface_check_cb(struct uloop_timeout
*t
)
489 struct qosify_iface
*iface
;
491 vlist_for_each_element(&devices
, iface
, node
)
492 qosify_iface_check_device(iface
);
493 vlist_for_each_element(&interfaces
, iface
, node
)
494 qosify_iface_check_interface(iface
);
497 void qosify_iface_check(void)
499 static struct uloop_timeout timer
= {
500 .cb
= qos_iface_check_cb
,
503 uloop_timeout_set(&timer
, 10);
507 __qosify_iface_status(struct blob_buf
*b
, struct qosify_iface
*iface
)
511 c
= blobmsg_open_table(b
, qosify_iface_name(iface
));
512 blobmsg_add_u8(b
, "active", iface
->active
);
514 blobmsg_add_string(b
, "ifname", iface
->ifname
);
515 blobmsg_add_u8(b
, "egress", iface
->config
.egress
);
516 blobmsg_add_u8(b
, "ingress", iface
->config
.ingress
);
517 blobmsg_close_table(b
, c
);
521 void qosify_iface_status(struct blob_buf
*b
)
523 struct qosify_iface
*iface
;
526 c
= blobmsg_open_table(b
, "devices");
527 vlist_for_each_element(&devices
, iface
, node
)
528 __qosify_iface_status(b
, iface
);
529 blobmsg_close_table(b
, c
);
531 c
= blobmsg_open_table(b
, "interfaces");
532 vlist_for_each_element(&interfaces
, iface
, node
)
533 __qosify_iface_status(b
, iface
);
534 blobmsg_close_table(b
, c
);
537 int qosify_iface_init(void)
539 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
546 void qosify_iface_stop(void)
548 struct qosify_iface
*iface
;
550 vlist_for_each_element(&interfaces
, iface
, node
)
551 interface_stop(iface
);
552 vlist_for_each_element(&devices
, iface
, node
)
553 interface_stop(iface
);