1 // SPDX-License-Identifier: GPL-2.0+
3 * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
6 #include <sys/socket.h>
9 #include <net/if_arp.h>
15 #include <libubox/vlist.h>
16 #include <libubox/avl-cmp.h>
17 #include <libubox/uloop.h>
21 static void interface_update_cb(struct vlist_tree
*tree
,
22 struct vlist_node
*node_new
,
23 struct vlist_node
*node_old
);
25 static VLIST_TREE(devices
, avl_strcmp
, interface_update_cb
, true, false);
26 static VLIST_TREE(interfaces
, avl_strcmp
, interface_update_cb
, true, false);
29 #define APPEND(_buf, _ofs, _format, ...) _ofs += snprintf(_buf + _ofs, sizeof(_buf) - _ofs, _format, ##__VA_ARGS__)
31 struct qosify_iface_config
{
32 struct blob_attr
*data
;
38 bool autorate_ingress
;
40 const char *bandwidth_up
;
41 const char *bandwidth_down
;
43 const char *common_opts
;
44 const char *ingress_opts
;
45 const char *egress_opts
;
50 struct vlist_node node
;
52 char ifname
[IFNAMSIZ
];
56 struct blob_attr
*config_data
;
57 struct qosify_iface_config config
;
67 IFACE_ATTR_HOST_ISOLATE
,
68 IFACE_ATTR_AUTORATE_IN
,
69 IFACE_ATTR_INGRESS_OPTS
,
70 IFACE_ATTR_EGRESS_OPTS
,
75 static inline const char *qosify_iface_name(struct qosify_iface
*iface
)
77 return iface
->node
.avl
.key
;
81 iface_config_parse(struct blob_attr
*attr
, struct blob_attr
**tb
)
83 static const struct blobmsg_policy policy
[__IFACE_ATTR_MAX
] = {
84 [IFACE_ATTR_BW_UP
] = { "bandwidth_up", BLOBMSG_TYPE_STRING
},
85 [IFACE_ATTR_BW_DOWN
] = { "bandwidth_down", BLOBMSG_TYPE_STRING
},
86 [IFACE_ATTR_INGRESS
] = { "ingress", BLOBMSG_TYPE_BOOL
},
87 [IFACE_ATTR_EGRESS
] = { "egress", BLOBMSG_TYPE_BOOL
},
88 [IFACE_ATTR_MODE
] = { "mode", BLOBMSG_TYPE_STRING
},
89 [IFACE_ATTR_NAT
] = { "nat", BLOBMSG_TYPE_BOOL
},
90 [IFACE_ATTR_HOST_ISOLATE
] = { "host_isolate", BLOBMSG_TYPE_BOOL
},
91 [IFACE_ATTR_AUTORATE_IN
] = { "autorate_ingress", BLOBMSG_TYPE_BOOL
},
92 [IFACE_ATTR_INGRESS_OPTS
] = { "ingress_options", BLOBMSG_TYPE_STRING
},
93 [IFACE_ATTR_EGRESS_OPTS
] = { "egress_options", BLOBMSG_TYPE_STRING
},
94 [IFACE_ATTR_OPTS
] = { "options", BLOBMSG_TYPE_STRING
},
97 blobmsg_parse(policy
, __IFACE_ATTR_MAX
, tb
, blobmsg_data(attr
), blobmsg_len(attr
));
101 iface_config_equal(struct qosify_iface
*if1
, struct qosify_iface
*if2
)
103 struct blob_attr
*tb1
[__IFACE_ATTR_MAX
], *tb2
[__IFACE_ATTR_MAX
];
106 iface_config_parse(if1
->config_data
, tb1
);
107 iface_config_parse(if2
->config_data
, tb2
);
109 for (i
= 0; i
< __IFACE_ATTR_MAX
; i
++) {
110 if (!!tb1
[i
] != !!tb2
[i
])
116 if (blob_raw_len(tb1
[i
]) != blob_raw_len(tb2
[i
]))
119 if (memcmp(tb1
[i
], tb2
[i
], blob_raw_len(tb1
[i
])) != 0)
126 static const char *check_str(struct blob_attr
*attr
)
128 const char *str
= blobmsg_get_string(attr
);
130 if (strchr(str
, '\''))
137 iface_config_set(struct qosify_iface_config
*cfg
, struct blob_attr
*attr
)
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
= true;
153 if ((cur
= tb
[IFACE_ATTR_BW_UP
]) != NULL
)
154 cfg
->bandwidth_up
= check_str(cur
);
155 if ((cur
= tb
[IFACE_ATTR_BW_DOWN
]) != NULL
)
156 cfg
->bandwidth_down
= check_str(cur
);
157 if ((cur
= tb
[IFACE_ATTR_MODE
]) != NULL
)
158 cfg
->mode
= check_str(cur
);
159 if ((cur
= tb
[IFACE_ATTR_OPTS
]) != NULL
)
160 cfg
->common_opts
= check_str(cur
);
161 if ((cur
= tb
[IFACE_ATTR_EGRESS_OPTS
]) != NULL
)
162 cfg
->egress_opts
= check_str(cur
);
163 if ((cur
= tb
[IFACE_ATTR_INGRESS_OPTS
]) != NULL
)
164 cfg
->ingress_opts
= check_str(cur
);
165 if ((cur
= tb
[IFACE_ATTR_INGRESS
]) != NULL
)
166 cfg
->ingress
= blobmsg_get_bool(cur
);
167 if ((cur
= tb
[IFACE_ATTR_EGRESS
]) != NULL
)
168 cfg
->egress
= blobmsg_get_bool(cur
);
169 if ((cur
= tb
[IFACE_ATTR_NAT
]) != NULL
)
170 cfg
->nat
= blobmsg_get_bool(cur
);
171 if ((cur
= tb
[IFACE_ATTR_HOST_ISOLATE
]) != NULL
)
172 cfg
->host_isolate
= blobmsg_get_bool(cur
);
173 if ((cur
= tb
[IFACE_ATTR_AUTORATE_IN
]) != NULL
)
174 cfg
->autorate_ingress
= blobmsg_get_bool(cur
);
178 interface_ifb_name(struct qosify_iface
*iface
)
180 static char ifname
[IFNAMSIZ
+ 1] = "ifb-";
181 int len
= strlen(iface
->ifname
);
183 if (len
+ 4 < IFNAMSIZ
) {
184 snprintf(ifname
+ 4, IFNAMSIZ
- 4, "%s", iface
->ifname
);
189 ifname
[4] = iface
->ifname
[0];
190 ifname
[5] = iface
->ifname
[1];
191 snprintf(ifname
+ 6, IFNAMSIZ
- 6, "%s", iface
->ifname
+ len
- (IFNAMSIZ
+ 6) - 1);
196 static int run_cmd(char *cmd
, bool ignore
)
198 char *argv
[] = { "sh", "-c", cmd
, NULL
};
212 if (fds
[1] != STDOUT_FILENO
)
213 dup2(fds
[1], STDOUT_FILENO
);
214 if (fds
[1] != STDERR_FILENO
)
215 dup2(fds
[1], STDERR_FILENO
);
216 if (fds
[1] > STDERR_FILENO
)
218 execv("/bin/sh", argv
);
226 f
= fdopen(fds
[0], "r");
232 while (fgets(buf
, sizeof(buf
), f
) != NULL
) {
238 ULOG_WARN("Command: %s\n", cmd
);
241 ULOG_WARN("%s%s", buf
, strchr(buf
, '\n') ? "" : "\n");
247 while (waitpid(pid
, &status
, 0) < 0)
255 prepare_tc_cmd(char *buf
, int len
, const char *type
, const char *cmd
,
256 const char *dev
, const char *extra
)
258 return snprintf(buf
, len
, "tc %s %s dev '%s' %s", type
, cmd
, dev
, extra
);
262 cmd_del_qdisc(const char *ifname
, const char *type
)
266 prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "del", ifname
, type
);
268 return run_cmd(buf
, true);
272 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
274 struct qosify_iface_config
*cfg
= &iface
->config
;
275 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
276 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
280 cmd_del_qdisc(ifname
, "root");
282 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", ifname
, "root handle 1: cake");
284 APPEND(buf
, ofs
, " bandwidth %s", bw
);
286 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
288 if (cfg
->host_isolate
)
289 APPEND(buf
, ofs
, " %snat dual-%shost",
290 cfg
->nat
? "" : "no",
291 egress
? "src" : "dst");
293 APPEND(buf
, ofs
, " flows");
295 APPEND(buf
, ofs
, " %s %s",
296 cfg
->common_opts
? cfg
->common_opts
: "",
297 dir_opts
? dir_opts
: "");
301 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", ifname
, "parent 1: bpf");
302 APPEND(buf
, ofs
, " object-pinned /sys/fs/bpf/qosify_%sgress_%s verbose direct-action",
306 return run_cmd(buf
, false);
310 cmd_del_ingress(struct qosify_iface
*iface
)
314 cmd_del_qdisc(iface
->ifname
, "handle ffff: ingress");
315 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
317 return run_cmd(buf
, true);
322 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
324 const char *ifbdev
= interface_ifb_name(iface
);
328 cmd_del_ingress(iface
);
330 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", iface
->ifname
, " handle ffff: ingress");
333 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
336 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
338 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
341 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " parent ffff:");
342 APPEND(buf
, ofs
, " protocol all prio 10 u32 match u32 0 0 "
343 "flowid 1:1 action mirred egress redirect dev '%s'", ifbdev
);
344 return run_cmd(buf
, false);
348 interface_start(struct qosify_iface
*iface
)
350 struct ifreq ifr
= {};
353 if (!iface
->ifname
[0] || iface
->active
)
356 ULOG_INFO("start interface %s\n", iface
->ifname
);
358 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
359 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
360 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
364 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
366 if (iface
->config
.egress
)
367 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
368 if (iface
->config
.ingress
)
369 cmd_add_ingress(iface
, eth
);
371 iface
->active
= true;
375 interface_stop(struct qosify_iface
*iface
)
377 if (!iface
->ifname
[0] || !iface
->active
)
380 ULOG_INFO("stop interface %s\n", iface
->ifname
);
381 iface
->active
= false;
383 if (iface
->config
.egress
)
384 cmd_del_qdisc(iface
->ifname
, "root");
385 if (iface
->config
.ingress
)
386 cmd_del_ingress(iface
);
390 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
392 iface
->config_data
= blob_memdup(config
);
393 iface_config_set(&iface
->config
, iface
->config_data
);
394 interface_start(iface
);
398 interface_update_cb(struct vlist_tree
*tree
,
399 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
401 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
404 if_new
= container_of(node_new
, struct qosify_iface
, node
);
406 if_old
= container_of(node_old
, struct qosify_iface
, node
);
408 if (if_new
&& if_old
) {
409 if (!iface_config_equal(if_old
, if_new
)) {
410 interface_stop(if_old
);
411 free(if_old
->config_data
);
412 interface_set_config(if_old
, if_new
->config_data
);
420 interface_stop(if_old
);
421 free(if_old
->config_data
);
426 interface_set_config(if_new
, if_new
->config_data
);
430 interface_create(struct blob_attr
*attr
, bool device
)
432 struct qosify_iface
*iface
;
433 const char *name
= blobmsg_name(attr
);
434 int name_len
= strlen(name
);
437 if (strchr(name
, '\''))
440 if (name_len
>= IFNAMSIZ
)
443 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
446 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
447 strcpy(name_buf
, blobmsg_name(attr
));
448 iface
->config_data
= attr
;
449 iface
->device
= device
;
450 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
453 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
455 struct blob_attr
*cur
;
458 vlist_update(&devices
);
459 blobmsg_for_each_attr(cur
, devs
, rem
)
460 interface_create(cur
, true);
461 vlist_flush(&devices
);
463 vlist_update(&interfaces
);
464 blobmsg_for_each_attr(cur
, ifaces
, rem
)
465 interface_create(cur
, false);
466 vlist_flush(&interfaces
);
470 qosify_iface_check_device(struct qosify_iface
*iface
)
472 const char *name
= qosify_iface_name(iface
);
475 ifindex
= if_nametoindex(name
);
477 interface_stop(iface
);
478 iface
->ifname
[0] = 0;
480 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
481 interface_start(iface
);
486 qosify_iface_check_interface(struct qosify_iface
*iface
)
488 const char *name
= qosify_iface_name(iface
);
489 char ifname
[IFNAMSIZ
];
491 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
492 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
493 interface_start(iface
);
495 interface_stop(iface
);
496 iface
->ifname
[0] = 0;
500 static void qos_iface_check_cb(struct uloop_timeout
*t
)
502 struct qosify_iface
*iface
;
504 vlist_for_each_element(&devices
, iface
, node
)
505 qosify_iface_check_device(iface
);
506 vlist_for_each_element(&interfaces
, iface
, node
)
507 qosify_iface_check_interface(iface
);
510 void qosify_iface_check(void)
512 static struct uloop_timeout timer
= {
513 .cb
= qos_iface_check_cb
,
516 uloop_timeout_set(&timer
, 10);
519 void qosify_iface_status(struct blob_buf
*b
)
521 struct qosify_iface
*iface
;
524 c
= blobmsg_open_table(b
, "devices");
525 vlist_for_each_element(&devices
, iface
, node
) {
526 i
= blobmsg_open_table(b
, qosify_iface_name(iface
));
527 blobmsg_add_u8(b
, "active", iface
->active
);
528 blobmsg_close_table(b
, i
);
530 blobmsg_close_table(b
, c
);
532 c
= blobmsg_open_table(b
, "interfaces");
533 vlist_for_each_element(&interfaces
, iface
, node
) {
534 i
= blobmsg_open_table(b
, qosify_iface_name(iface
));
535 blobmsg_add_u8(b
, "active", iface
->active
);
537 blobmsg_add_string(b
, "ifname", iface
->ifname
);
538 blobmsg_close_table(b
, i
);
540 blobmsg_close_table(b
, c
);
543 int qosify_iface_init(void)
545 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
552 void qosify_iface_stop(void)
554 struct qosify_iface
*iface
;
556 vlist_for_each_element(&interfaces
, iface
, node
)
557 interface_stop(iface
);
558 vlist_for_each_element(&devices
, iface
, node
)
559 interface_stop(iface
);