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_tc_cmd(char *buf
, int len
, const char *type
, const char *cmd
,
199 const char *dev
, const char *extra
)
201 return snprintf(buf
, len
, "tc %s %s dev '%s' %s", type
, cmd
, dev
, extra
);
205 __cmd_add_del_qdisc(const char *ifname
, const char *type
, bool add
)
209 prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", add
? "add" : "del", ifname
, type
);
211 return qosify_run_cmd(buf
, true);
215 cmd_del_qdisc(const char *ifname
, const char *type
)
217 return __cmd_add_del_qdisc(ifname
, type
, false);
221 cmd_add_bpf_filter(const char *ifname
, bool egress
, bool eth
)
226 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", ifname
, egress
? "egress" : "ingress");
227 APPEND(buf
, ofs
, " prio 10 bpf object-pinned /sys/fs/bpf/qosify_%sgress_%s verbose direct-action",
231 return qosify_run_cmd(buf
, false);
235 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
237 struct qosify_iface_config
*cfg
= &iface
->config
;
238 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
239 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
243 __cmd_add_del_qdisc(ifname
, "clsact", true);
245 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", ifname
, "root cake");
247 APPEND(buf
, ofs
, " bandwidth %s", bw
);
249 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
250 if (!egress
&& cfg
->autorate_ingress
)
251 APPEND(buf
, ofs
, " autorate-ingress");
253 if (cfg
->host_isolate
)
254 APPEND(buf
, ofs
, " %snat dual-%shost",
255 cfg
->nat
? "" : "no",
256 egress
? "src" : "dst");
258 APPEND(buf
, ofs
, " flows");
260 APPEND(buf
, ofs
, " %s %s",
261 cfg
->common_opts
? cfg
->common_opts
: "",
262 dir_opts
? dir_opts
: "");
264 return qosify_run_cmd(buf
, false);
268 cmd_del_ingress(struct qosify_iface
*iface
)
272 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
274 return qosify_run_cmd(buf
, true);
278 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
280 const char *ifbdev
= interface_ifb_name(iface
);
284 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " ingress");
285 APPEND(buf
, ofs
, " protocol ip prio 5 u32 match ip sport 53 0xffff "
286 "flowid 1:1 action mirred egress redirect dev ifb-dns");
287 qosify_run_cmd(buf
, false);
289 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " ingress");
290 APPEND(buf
, ofs
, " protocol 802.1Q prio 6 u32 offset plus 4 match ip sport 53 0xffff "
291 "flowid 1:1 action mirred egress redirect dev ifb-dns");
292 qosify_run_cmd(buf
, false);
294 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " ingress");
295 APPEND(buf
, ofs
, " protocol ipv6 prio 7 u32 match ip6 sport 53 0xffff "
296 "flowid 1:1 action mirred egress redirect dev ifb-dns");
297 qosify_run_cmd(buf
, false);
299 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " ingress");
300 APPEND(buf
, ofs
, " protocol ipv6 prio 8 u32 offset plus 4 match ip6 sport 53 0xffff "
301 "flowid 1:1 action mirred egress redirect dev ifb-dns");
302 qosify_run_cmd(buf
, false);
304 cmd_add_bpf_filter(iface
->ifname
, false, eth
);
306 if (!iface
->config
.ingress
)
309 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
310 qosify_run_cmd(buf
, false);
312 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
314 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
315 qosify_run_cmd(buf
, false);
317 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " ingress");
318 APPEND(buf
, ofs
, " protocol all prio 20 u32 match u32 0 0 "
319 "flowid 1:1 action mirred egress redirect dev '%s'", ifbdev
);
320 return qosify_run_cmd(buf
, false);
323 static int cmd_add_egress(struct qosify_iface
*iface
, bool eth
)
325 if (!iface
->config
.egress
)
328 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
330 return cmd_add_bpf_filter(iface
->ifname
, true, eth
);
334 interface_clear_qdisc(struct qosify_iface
*iface
)
336 cmd_del_qdisc(iface
->ifname
, "root");
337 cmd_del_qdisc(iface
->ifname
, "clsact");
338 cmd_del_ingress(iface
);
342 interface_start(struct qosify_iface
*iface
)
344 struct ifreq ifr
= {};
347 if (!iface
->ifname
[0] || iface
->active
)
350 ULOG_INFO("start interface %s\n", iface
->ifname
);
352 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
353 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
354 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
358 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
360 interface_clear_qdisc(iface
);
361 cmd_add_egress(iface
, eth
);
362 cmd_add_ingress(iface
, eth
);
364 iface
->active
= true;
368 interface_stop(struct qosify_iface
*iface
)
370 if (!iface
->ifname
[0] || !iface
->active
)
373 ULOG_INFO("stop interface %s\n", iface
->ifname
);
374 iface
->active
= false;
376 interface_clear_qdisc(iface
);
380 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
382 iface
->config_data
= blob_memdup(config
);
383 iface_config_set(iface
, iface
->config_data
);
384 interface_start(iface
);
388 interface_update_cb(struct vlist_tree
*tree
,
389 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
391 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
394 if_new
= container_of(node_new
, struct qosify_iface
, node
);
396 if_old
= container_of(node_old
, struct qosify_iface
, node
);
398 if (if_new
&& if_old
) {
399 if (!iface_config_equal(if_old
, if_new
)) {
400 interface_stop(if_old
);
401 free(if_old
->config_data
);
402 interface_set_config(if_old
, if_new
->config_data
);
410 interface_stop(if_old
);
411 free(if_old
->config_data
);
416 interface_set_config(if_new
, if_new
->config_data
);
420 interface_create(struct blob_attr
*attr
, bool device
)
422 struct qosify_iface
*iface
;
423 const char *name
= blobmsg_name(attr
);
424 int name_len
= strlen(name
);
427 if (strchr(name
, '\''))
430 if (name_len
>= IFNAMSIZ
)
433 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
436 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
437 strcpy(name_buf
, blobmsg_name(attr
));
438 iface
->config_data
= attr
;
439 iface
->device
= device
;
440 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
443 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
445 struct blob_attr
*cur
;
448 vlist_update(&devices
);
449 blobmsg_for_each_attr(cur
, devs
, rem
)
450 interface_create(cur
, true);
451 vlist_flush(&devices
);
453 vlist_update(&interfaces
);
454 blobmsg_for_each_attr(cur
, ifaces
, rem
)
455 interface_create(cur
, false);
456 vlist_flush(&interfaces
);
460 qosify_iface_check_device(struct qosify_iface
*iface
)
462 const char *name
= qosify_iface_name(iface
);
465 ifindex
= if_nametoindex(name
);
467 interface_stop(iface
);
468 iface
->ifname
[0] = 0;
470 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
471 interface_start(iface
);
476 qosify_iface_check_interface(struct qosify_iface
*iface
)
478 const char *name
= qosify_iface_name(iface
);
479 char ifname
[IFNAMSIZ
];
481 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
482 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
483 interface_start(iface
);
485 interface_stop(iface
);
486 iface
->ifname
[0] = 0;
490 static void qos_iface_check_cb(struct uloop_timeout
*t
)
492 struct qosify_iface
*iface
;
494 vlist_for_each_element(&devices
, iface
, node
)
495 qosify_iface_check_device(iface
);
496 vlist_for_each_element(&interfaces
, iface
, node
)
497 qosify_iface_check_interface(iface
);
500 void qosify_iface_check(void)
502 static struct uloop_timeout timer
= {
503 .cb
= qos_iface_check_cb
,
506 uloop_timeout_set(&timer
, 10);
510 __qosify_iface_status(struct blob_buf
*b
, struct qosify_iface
*iface
)
514 c
= blobmsg_open_table(b
, qosify_iface_name(iface
));
515 blobmsg_add_u8(b
, "active", iface
->active
);
517 blobmsg_add_string(b
, "ifname", iface
->ifname
);
518 blobmsg_add_u8(b
, "egress", iface
->config
.egress
);
519 blobmsg_add_u8(b
, "ingress", iface
->config
.ingress
);
520 blobmsg_close_table(b
, c
);
524 void qosify_iface_status(struct blob_buf
*b
)
526 struct qosify_iface
*iface
;
529 c
= blobmsg_open_table(b
, "devices");
530 vlist_for_each_element(&devices
, iface
, node
)
531 __qosify_iface_status(b
, iface
);
532 blobmsg_close_table(b
, c
);
534 c
= blobmsg_open_table(b
, "interfaces");
535 vlist_for_each_element(&interfaces
, iface
, node
)
536 __qosify_iface_status(b
, iface
);
537 blobmsg_close_table(b
, c
);
540 int qosify_iface_init(void)
542 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
549 void qosify_iface_stop(void)
551 struct qosify_iface
*iface
;
553 vlist_for_each_element(&interfaces
, iface
, node
)
554 interface_stop(iface
);
555 vlist_for_each_element(&devices
, iface
, node
)
556 interface_stop(iface
);