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
*iface
, struct blob_attr
*attr
)
139 struct qosify_iface_config
*cfg
= &iface
->config
;
140 struct blob_attr
*tb
[__IFACE_ATTR_MAX
];
141 struct blob_attr
*cur
;
143 iface_config_parse(attr
, tb
);
145 memset(cfg
, 0, sizeof(*cfg
));
148 cfg
->mode
= "diffserv4";
151 cfg
->host_isolate
= true;
152 cfg
->autorate_ingress
= true;
153 cfg
->nat
= !iface
->device
;
155 if ((cur
= tb
[IFACE_ATTR_BW_UP
]) != NULL
)
156 cfg
->bandwidth_up
= check_str(cur
);
157 if ((cur
= tb
[IFACE_ATTR_BW_DOWN
]) != NULL
)
158 cfg
->bandwidth_down
= check_str(cur
);
159 if ((cur
= tb
[IFACE_ATTR_MODE
]) != NULL
)
160 cfg
->mode
= check_str(cur
);
161 if ((cur
= tb
[IFACE_ATTR_OPTS
]) != NULL
)
162 cfg
->common_opts
= check_str(cur
);
163 if ((cur
= tb
[IFACE_ATTR_EGRESS_OPTS
]) != NULL
)
164 cfg
->egress_opts
= check_str(cur
);
165 if ((cur
= tb
[IFACE_ATTR_INGRESS_OPTS
]) != NULL
)
166 cfg
->ingress_opts
= check_str(cur
);
167 if ((cur
= tb
[IFACE_ATTR_INGRESS
]) != NULL
)
168 cfg
->ingress
= blobmsg_get_bool(cur
);
169 if ((cur
= tb
[IFACE_ATTR_EGRESS
]) != NULL
)
170 cfg
->egress
= blobmsg_get_bool(cur
);
171 if ((cur
= tb
[IFACE_ATTR_NAT
]) != NULL
)
172 cfg
->nat
= blobmsg_get_bool(cur
);
173 if ((cur
= tb
[IFACE_ATTR_HOST_ISOLATE
]) != NULL
)
174 cfg
->host_isolate
= blobmsg_get_bool(cur
);
175 if ((cur
= tb
[IFACE_ATTR_AUTORATE_IN
]) != NULL
)
176 cfg
->autorate_ingress
= blobmsg_get_bool(cur
);
180 interface_ifb_name(struct qosify_iface
*iface
)
182 static char ifname
[IFNAMSIZ
+ 1] = "ifb-";
183 int len
= strlen(iface
->ifname
);
185 if (len
+ 4 < IFNAMSIZ
) {
186 snprintf(ifname
+ 4, IFNAMSIZ
- 4, "%s", iface
->ifname
);
191 ifname
[4] = iface
->ifname
[0];
192 ifname
[5] = iface
->ifname
[1];
193 snprintf(ifname
+ 6, IFNAMSIZ
- 6, "%s", iface
->ifname
+ len
- (IFNAMSIZ
+ 6) - 1);
198 static int run_cmd(char *cmd
, bool ignore
)
200 char *argv
[] = { "sh", "-c", cmd
, NULL
};
214 if (fds
[1] != STDOUT_FILENO
)
215 dup2(fds
[1], STDOUT_FILENO
);
216 if (fds
[1] != STDERR_FILENO
)
217 dup2(fds
[1], STDERR_FILENO
);
218 if (fds
[1] > STDERR_FILENO
)
220 execv("/bin/sh", argv
);
228 f
= fdopen(fds
[0], "r");
234 while (fgets(buf
, sizeof(buf
), f
) != NULL
) {
240 ULOG_WARN("Command: %s\n", cmd
);
243 ULOG_WARN("%s%s", buf
, strchr(buf
, '\n') ? "" : "\n");
249 while (waitpid(pid
, &status
, 0) < 0)
257 prepare_tc_cmd(char *buf
, int len
, const char *type
, const char *cmd
,
258 const char *dev
, const char *extra
)
260 return snprintf(buf
, len
, "tc %s %s dev '%s' %s", type
, cmd
, dev
, extra
);
264 cmd_del_qdisc(const char *ifname
, const char *type
)
268 prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "del", ifname
, type
);
270 return run_cmd(buf
, true);
274 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
276 struct qosify_iface_config
*cfg
= &iface
->config
;
277 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
278 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
282 cmd_del_qdisc(ifname
, "root");
284 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", ifname
, "root handle 1: cake");
286 APPEND(buf
, ofs
, " bandwidth %s", bw
);
288 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
290 if (cfg
->host_isolate
)
291 APPEND(buf
, ofs
, " %snat dual-%shost",
292 cfg
->nat
? "" : "no",
293 egress
? "src" : "dst");
295 APPEND(buf
, ofs
, " flows");
297 APPEND(buf
, ofs
, " %s %s",
298 cfg
->common_opts
? cfg
->common_opts
: "",
299 dir_opts
? dir_opts
: "");
303 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", ifname
, "parent 1: bpf");
304 APPEND(buf
, ofs
, " object-pinned /sys/fs/bpf/qosify_%sgress_%s verbose direct-action",
308 return run_cmd(buf
, false);
312 cmd_del_ingress(struct qosify_iface
*iface
)
316 cmd_del_qdisc(iface
->ifname
, "handle ffff: ingress");
317 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
319 return run_cmd(buf
, true);
324 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
326 const char *ifbdev
= interface_ifb_name(iface
);
330 cmd_del_ingress(iface
);
332 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", iface
->ifname
, " handle ffff: ingress");
335 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
338 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
340 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
343 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " parent ffff:");
344 APPEND(buf
, ofs
, " protocol all prio 10 u32 match u32 0 0 "
345 "flowid 1:1 action mirred egress redirect dev '%s'", ifbdev
);
346 return run_cmd(buf
, false);
350 interface_start(struct qosify_iface
*iface
)
352 struct ifreq ifr
= {};
355 if (!iface
->ifname
[0] || iface
->active
)
358 ULOG_INFO("start interface %s\n", iface
->ifname
);
360 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
361 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
362 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
366 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
368 if (iface
->config
.egress
)
369 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
370 if (iface
->config
.ingress
)
371 cmd_add_ingress(iface
, eth
);
373 iface
->active
= true;
377 interface_stop(struct qosify_iface
*iface
)
379 if (!iface
->ifname
[0] || !iface
->active
)
382 ULOG_INFO("stop interface %s\n", iface
->ifname
);
383 iface
->active
= false;
385 if (iface
->config
.egress
)
386 cmd_del_qdisc(iface
->ifname
, "root");
387 if (iface
->config
.ingress
)
388 cmd_del_ingress(iface
);
392 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
394 iface
->config_data
= blob_memdup(config
);
395 iface_config_set(iface
, iface
->config_data
);
396 interface_start(iface
);
400 interface_update_cb(struct vlist_tree
*tree
,
401 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
403 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
406 if_new
= container_of(node_new
, struct qosify_iface
, node
);
408 if_old
= container_of(node_old
, struct qosify_iface
, node
);
410 if (if_new
&& if_old
) {
411 if (!iface_config_equal(if_old
, if_new
)) {
412 interface_stop(if_old
);
413 free(if_old
->config_data
);
414 interface_set_config(if_old
, if_new
->config_data
);
422 interface_stop(if_old
);
423 free(if_old
->config_data
);
428 interface_set_config(if_new
, if_new
->config_data
);
432 interface_create(struct blob_attr
*attr
, bool device
)
434 struct qosify_iface
*iface
;
435 const char *name
= blobmsg_name(attr
);
436 int name_len
= strlen(name
);
439 if (strchr(name
, '\''))
442 if (name_len
>= IFNAMSIZ
)
445 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
448 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
449 strcpy(name_buf
, blobmsg_name(attr
));
450 iface
->config_data
= attr
;
451 iface
->device
= device
;
452 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
455 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
457 struct blob_attr
*cur
;
460 vlist_update(&devices
);
461 blobmsg_for_each_attr(cur
, devs
, rem
)
462 interface_create(cur
, true);
463 vlist_flush(&devices
);
465 vlist_update(&interfaces
);
466 blobmsg_for_each_attr(cur
, ifaces
, rem
)
467 interface_create(cur
, false);
468 vlist_flush(&interfaces
);
472 qosify_iface_check_device(struct qosify_iface
*iface
)
474 const char *name
= qosify_iface_name(iface
);
477 ifindex
= if_nametoindex(name
);
479 interface_stop(iface
);
480 iface
->ifname
[0] = 0;
482 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
483 interface_start(iface
);
488 qosify_iface_check_interface(struct qosify_iface
*iface
)
490 const char *name
= qosify_iface_name(iface
);
491 char ifname
[IFNAMSIZ
];
493 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
494 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
495 interface_start(iface
);
497 interface_stop(iface
);
498 iface
->ifname
[0] = 0;
502 static void qos_iface_check_cb(struct uloop_timeout
*t
)
504 struct qosify_iface
*iface
;
506 vlist_for_each_element(&devices
, iface
, node
)
507 qosify_iface_check_device(iface
);
508 vlist_for_each_element(&interfaces
, iface
, node
)
509 qosify_iface_check_interface(iface
);
512 void qosify_iface_check(void)
514 static struct uloop_timeout timer
= {
515 .cb
= qos_iface_check_cb
,
518 uloop_timeout_set(&timer
, 10);
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 i
= blobmsg_open_table(b
, qosify_iface_name(iface
));
529 blobmsg_add_u8(b
, "active", iface
->active
);
530 blobmsg_close_table(b
, i
);
532 blobmsg_close_table(b
, c
);
534 c
= blobmsg_open_table(b
, "interfaces");
535 vlist_for_each_element(&interfaces
, iface
, node
) {
536 i
= blobmsg_open_table(b
, qosify_iface_name(iface
));
537 blobmsg_add_u8(b
, "active", iface
->active
);
539 blobmsg_add_string(b
, "ifname", iface
->ifname
);
540 blobmsg_close_table(b
, i
);
542 blobmsg_close_table(b
, c
);
545 int qosify_iface_init(void)
547 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
554 void qosify_iface_stop(void)
556 struct qosify_iface
*iface
;
558 vlist_for_each_element(&interfaces
, iface
, node
)
559 interface_stop(iface
);
560 vlist_for_each_element(&devices
, iface
, node
)
561 interface_stop(iface
);