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_del_qdisc(const char *ifname
, const char *type
)
209 prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "del", ifname
, type
);
211 return qosify_run_cmd(buf
, true);
215 cmd_add_qdisc(struct qosify_iface
*iface
, const char *ifname
, bool egress
, bool eth
)
217 struct qosify_iface_config
*cfg
= &iface
->config
;
218 const char *bw
= egress
? cfg
->bandwidth_up
: cfg
->bandwidth_down
;
219 const char *dir_opts
= egress
? cfg
->egress_opts
: cfg
->ingress_opts
;
223 cmd_del_qdisc(ifname
, "root");
225 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", ifname
, "root handle 1: cake");
227 APPEND(buf
, ofs
, " bandwidth %s", bw
);
229 APPEND(buf
, ofs
, " %s %sgress", cfg
->mode
, egress
? "e" : "in");
230 if (!egress
&& cfg
->autorate_ingress
)
231 APPEND(buf
, ofs
, " autorate-ingress");
233 if (cfg
->host_isolate
)
234 APPEND(buf
, ofs
, " %snat dual-%shost",
235 cfg
->nat
? "" : "no",
236 egress
? "src" : "dst");
238 APPEND(buf
, ofs
, " flows");
240 APPEND(buf
, ofs
, " %s %s",
241 cfg
->common_opts
? cfg
->common_opts
: "",
242 dir_opts
? dir_opts
: "");
244 qosify_run_cmd(buf
, false);
246 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", ifname
, "parent 1: bpf");
247 APPEND(buf
, ofs
, " object-pinned /sys/fs/bpf/qosify_%sgress_%s verbose direct-action",
251 return qosify_run_cmd(buf
, false);
255 cmd_del_ingress(struct qosify_iface
*iface
)
259 cmd_del_qdisc(iface
->ifname
, "handle ffff: ingress");
260 snprintf(buf
, sizeof(buf
), "ip link del '%s'", interface_ifb_name(iface
));
262 return qosify_run_cmd(buf
, true);
267 cmd_add_ingress(struct qosify_iface
*iface
, bool eth
)
269 const char *ifbdev
= interface_ifb_name(iface
);
273 cmd_del_ingress(iface
);
275 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "qdisc", "add", iface
->ifname
, " handle ffff: ingress");
276 qosify_run_cmd(buf
, false);
278 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " parent ffff:");
279 APPEND(buf
, ofs
, " protocol ip prio 5 u32 match ip sport 53 0xffff "
280 "flowid 1:1 action mirred egress redirect dev ifb-dns");
281 qosify_run_cmd(buf
, false);
283 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " parent ffff:");
284 APPEND(buf
, ofs
, " protocol ipv6 prio 6 u32 match ip6 sport 53 0xffff "
285 "flowid 1:1 action mirred egress redirect dev ifb-dns");
286 qosify_run_cmd(buf
, false);
288 if (!iface
->config
.ingress
)
291 snprintf(buf
, sizeof(buf
), "ip link add '%s' type ifb", ifbdev
);
292 qosify_run_cmd(buf
, false);
294 cmd_add_qdisc(iface
, ifbdev
, false, eth
);
296 snprintf(buf
, sizeof(buf
), "ip link set dev '%s' up", ifbdev
);
297 qosify_run_cmd(buf
, false);
299 ofs
= prepare_tc_cmd(buf
, sizeof(buf
), "filter", "add", iface
->ifname
, " parent ffff:");
300 APPEND(buf
, ofs
, " protocol all prio 10 u32 match u32 0 0 "
301 "flowid 1:1 action mirred egress redirect dev '%s'", ifbdev
);
302 return qosify_run_cmd(buf
, false);
306 interface_start(struct qosify_iface
*iface
)
308 struct ifreq ifr
= {};
311 if (!iface
->ifname
[0] || iface
->active
)
314 ULOG_INFO("start interface %s\n", iface
->ifname
);
316 strncpy(ifr
.ifr_name
, iface
->ifname
, sizeof(ifr
.ifr_name
));
317 if (ioctl(socket_fd
, SIOCGIFHWADDR
, &ifr
) < 0) {
318 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface
->ifname
, strerror(errno
));
322 eth
= ifr
.ifr_hwaddr
.sa_family
== ARPHRD_ETHER
;
324 if (iface
->config
.egress
)
325 cmd_add_qdisc(iface
, iface
->ifname
, true, eth
);
326 cmd_add_ingress(iface
, eth
);
328 iface
->active
= true;
332 interface_stop(struct qosify_iface
*iface
)
334 if (!iface
->ifname
[0] || !iface
->active
)
337 ULOG_INFO("stop interface %s\n", iface
->ifname
);
338 iface
->active
= false;
340 if (iface
->config
.egress
)
341 cmd_del_qdisc(iface
->ifname
, "root");
342 if (iface
->config
.ingress
)
343 cmd_del_ingress(iface
);
347 interface_set_config(struct qosify_iface
*iface
, struct blob_attr
*config
)
349 iface
->config_data
= blob_memdup(config
);
350 iface_config_set(iface
, iface
->config_data
);
351 interface_start(iface
);
355 interface_update_cb(struct vlist_tree
*tree
,
356 struct vlist_node
*node_new
, struct vlist_node
*node_old
)
358 struct qosify_iface
*if_new
= NULL
, *if_old
= NULL
;
361 if_new
= container_of(node_new
, struct qosify_iface
, node
);
363 if_old
= container_of(node_old
, struct qosify_iface
, node
);
365 if (if_new
&& if_old
) {
366 if (!iface_config_equal(if_old
, if_new
)) {
367 interface_stop(if_old
);
368 free(if_old
->config_data
);
369 interface_set_config(if_old
, if_new
->config_data
);
377 interface_stop(if_old
);
378 free(if_old
->config_data
);
383 interface_set_config(if_new
, if_new
->config_data
);
387 interface_create(struct blob_attr
*attr
, bool device
)
389 struct qosify_iface
*iface
;
390 const char *name
= blobmsg_name(attr
);
391 int name_len
= strlen(name
);
394 if (strchr(name
, '\''))
397 if (name_len
>= IFNAMSIZ
)
400 if (blobmsg_type(attr
) != BLOBMSG_TYPE_TABLE
)
403 iface
= calloc_a(sizeof(*iface
), &name_buf
, name_len
+ 1);
404 strcpy(name_buf
, blobmsg_name(attr
));
405 iface
->config_data
= attr
;
406 iface
->device
= device
;
407 vlist_add(device
? &devices
: &interfaces
, &iface
->node
, name_buf
);
410 void qosify_iface_config_update(struct blob_attr
*ifaces
, struct blob_attr
*devs
)
412 struct blob_attr
*cur
;
415 vlist_update(&devices
);
416 blobmsg_for_each_attr(cur
, devs
, rem
)
417 interface_create(cur
, true);
418 vlist_flush(&devices
);
420 vlist_update(&interfaces
);
421 blobmsg_for_each_attr(cur
, ifaces
, rem
)
422 interface_create(cur
, false);
423 vlist_flush(&interfaces
);
427 qosify_iface_check_device(struct qosify_iface
*iface
)
429 const char *name
= qosify_iface_name(iface
);
432 ifindex
= if_nametoindex(name
);
434 interface_stop(iface
);
435 iface
->ifname
[0] = 0;
437 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", name
);
438 interface_start(iface
);
443 qosify_iface_check_interface(struct qosify_iface
*iface
)
445 const char *name
= qosify_iface_name(iface
);
446 char ifname
[IFNAMSIZ
];
448 if (qosify_ubus_check_interface(name
, ifname
, sizeof(ifname
)) == 0) {
449 snprintf(iface
->ifname
, sizeof(iface
->ifname
), "%s", ifname
);
450 interface_start(iface
);
452 interface_stop(iface
);
453 iface
->ifname
[0] = 0;
457 static void qos_iface_check_cb(struct uloop_timeout
*t
)
459 struct qosify_iface
*iface
;
461 vlist_for_each_element(&devices
, iface
, node
)
462 qosify_iface_check_device(iface
);
463 vlist_for_each_element(&interfaces
, iface
, node
)
464 qosify_iface_check_interface(iface
);
467 void qosify_iface_check(void)
469 static struct uloop_timeout timer
= {
470 .cb
= qos_iface_check_cb
,
473 uloop_timeout_set(&timer
, 10);
477 __qosify_iface_status(struct blob_buf
*b
, struct qosify_iface
*iface
)
481 c
= blobmsg_open_table(b
, qosify_iface_name(iface
));
482 blobmsg_add_u8(b
, "active", iface
->active
);
484 blobmsg_add_string(b
, "ifname", iface
->ifname
);
485 blobmsg_add_u8(b
, "egress", iface
->config
.egress
);
486 blobmsg_add_u8(b
, "ingress", iface
->config
.ingress
);
487 blobmsg_close_table(b
, c
);
491 void qosify_iface_status(struct blob_buf
*b
)
493 struct qosify_iface
*iface
;
496 c
= blobmsg_open_table(b
, "devices");
497 vlist_for_each_element(&devices
, iface
, node
)
498 __qosify_iface_status(b
, iface
);
499 blobmsg_close_table(b
, c
);
501 c
= blobmsg_open_table(b
, "interfaces");
502 vlist_for_each_element(&interfaces
, iface
, node
)
503 __qosify_iface_status(b
, iface
);
504 blobmsg_close_table(b
, c
);
507 int qosify_iface_init(void)
509 socket_fd
= socket(AF_UNIX
, SOCK_DGRAM
, 0);
516 void qosify_iface_stop(void)
518 struct qosify_iface
*iface
;
520 vlist_for_each_element(&interfaces
, iface
, node
)
521 interface_stop(iface
);
522 vlist_for_each_element(&devices
, iface
, node
)
523 interface_stop(iface
);