interface.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
   4  */
   5 #include <sys/types.h>
   6 #include <sys/socket.h>
   7 #include <sys/ioctl.h>
   8 #include <net/if_arp.h>
   9 #include <net/if.h>
  10
  11 #include <unistd.h>
  12 #include <errno.h>
  13
  14 #include <libubox/vlist.h>
  15 #include <libubox/avl-cmp.h>
  16 #include <libubox/uloop.h>
  17
  18 #include "qosify.h"
  19
  20 static void interface_update_cb(struct vlist_tree *tree,
  21                                 struct vlist_node *node_new,
  22                                 struct vlist_node *node_old);
  23
  24 static VLIST_TREE(devices, avl_strcmp, interface_update_cb, true, false);
  25 static VLIST_TREE(interfaces, avl_strcmp, interface_update_cb, true, false);
  26 static int socket_fd;
  27
  28 #define APPEND(_buf, _ofs, _format, ...) _ofs += snprintf(_buf + _ofs, sizeof(_buf) - _ofs, _format, ##__VA_ARGS__)
  29
  30 struct qosify_iface_config {
  31         struct blob_attr *data;
  32
  33         bool ingress;
  34         bool egress;
  35         bool nat;
  36         bool host_isolate;
  37         bool autorate_ingress;
  38
  39         const char *bandwidth_up;
  40         const char *bandwidth_down;
  41         const char *mode;
  42         const char *common_opts;
  43         const char *ingress_opts;
  44         const char *egress_opts;
  45 };
  46
  47
  48 struct qosify_iface {
  49         struct vlist_node node;
  50
  51         char ifname[IFNAMSIZ];
  52         bool active;
  53
  54         bool device;
  55         struct blob_attr *config_data;
  56         struct qosify_iface_config config;
  57 };
  58
  59 enum {
  60         IFACE_ATTR_BW_UP,
  61         IFACE_ATTR_BW_DOWN,
  62         IFACE_ATTR_INGRESS,
  63         IFACE_ATTR_EGRESS,
  64         IFACE_ATTR_MODE,
  65         IFACE_ATTR_NAT,
  66         IFACE_ATTR_HOST_ISOLATE,
  67         IFACE_ATTR_AUTORATE_IN,
  68         IFACE_ATTR_INGRESS_OPTS,
  69         IFACE_ATTR_EGRESS_OPTS,
  70         IFACE_ATTR_OPTS,
  71         __IFACE_ATTR_MAX
  72 };
  73
  74 static inline const char *qosify_iface_name(struct qosify_iface *iface)
  75 {
  76         return iface->node.avl.key;
  77 }
  78
  79 static void
  80 iface_config_parse(struct blob_attr *attr, struct blob_attr **tb)
  81 {
  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 },
  94         };
  95
  96         blobmsg_parse(policy, __IFACE_ATTR_MAX, tb, blobmsg_data(attr), blobmsg_len(attr));
  97 }
  98
  99 static bool
 100 iface_config_equal(struct qosify_iface *if1, struct qosify_iface *if2)
 101 {
 102         struct blob_attr *tb1[__IFACE_ATTR_MAX], *tb2[__IFACE_ATTR_MAX];
 103         int i;
 104
 105         iface_config_parse(if1->config_data, tb1);
 106         iface_config_parse(if2->config_data, tb2);
 107
 108         for (i = 0; i < __IFACE_ATTR_MAX; i++) {
 109                 if (!!tb1[i] != !!tb2[i])
 110                         return false;
 111
 112                 if (!tb1[i])
 113                         continue;
 114
 115                 if (blob_raw_len(tb1[i]) != blob_raw_len(tb2[i]))
 116                         return false;
 117
 118                 if (memcmp(tb1[i], tb2[i], blob_raw_len(tb1[i])) != 0)
 119                         return false;
 120         }
 121
 122         return true;
 123 }
 124
 125 static const char *check_str(struct blob_attr *attr)
 126 {
 127         const char *str = blobmsg_get_string(attr);
 128
 129         if (strchr(str, '\''))
 130                 return NULL;
 131
 132         return str;
 133 }
 134
 135 static void
 136 iface_config_set(struct qosify_iface *iface, struct blob_attr *attr)
 137 {
 138         struct qosify_iface_config *cfg = &iface->config;
 139         struct blob_attr *tb[__IFACE_ATTR_MAX];
 140         struct blob_attr *cur;
 141
 142         iface_config_parse(attr, tb);
 143
 144         memset(cfg, 0, sizeof(*cfg));
 145
 146         /* defaults */
 147         cfg->mode = "diffserv4";
 148         cfg->ingress = true;
 149         cfg->egress = true;
 150         cfg->host_isolate = true;
 151         cfg->autorate_ingress = false;
 152         cfg->nat = !iface->device;
 153
 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);
 176 }
 177
 178 static const char *
 179 interface_ifb_name(struct qosify_iface *iface)
 180 {
 181         static char ifname[IFNAMSIZ + 1] = "ifb-";
 182         int len = strlen(iface->ifname);
 183
 184         if (len + 4 < IFNAMSIZ) {
 185                 snprintf(ifname + 4, IFNAMSIZ - 4, "%s", iface->ifname);
 186
 187                 return ifname;
 188         }
 189
 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);
 193
 194         return ifname;
 195 }
 196
 197 static int
 198 prepare_tc_cmd(char *buf, int len, const char *type, const char *cmd,
 199                const char *dev, const char *extra)
 200 {
 201         return snprintf(buf, len, "tc %s %s dev '%s' %s", type, cmd, dev, extra);
 202 }
 203
 204 static int
 205 __cmd_add_del_qdisc(const char *ifname, const char *type, bool add)
 206 {
 207         char buf[64];
 208
 209         prepare_tc_cmd(buf, sizeof(buf), "qdisc", add ? "add" : "del", ifname, type);
 210
 211         return qosify_run_cmd(buf, true);
 212 }
 213
 214 static int
 215 cmd_del_qdisc(const char *ifname, const char *type)
 216 {
 217         return __cmd_add_del_qdisc(ifname, type, false);
 218 }
 219
 220 static int
 221 cmd_add_bpf_filter(const char *ifname, bool egress, bool eth)
 222 {
 223         char buf[512];
 224         int ofs;
 225
 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",
 228                egress ? "e" : "in",
 229                    eth ? "eth" : "ip");
 230
 231         return qosify_run_cmd(buf, false);
 232 }
 233
 234 static int
 235 cmd_add_qdisc(struct qosify_iface *iface, const char *ifname, bool egress, bool eth)
 236 {
 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;
 240         char buf[512];
 241         int ofs;
 242
 243         __cmd_add_del_qdisc(ifname, "clsact", true);
 244
 245         ofs = prepare_tc_cmd(buf, sizeof(buf), "qdisc", "add", ifname, "root cake");
 246         if (bw)
 247                 APPEND(buf, ofs, " bandwidth %s", bw);
 248
 249         APPEND(buf, ofs, " %s %sgress", cfg->mode, egress ? "e" : "in");
 250         if (!egress && cfg->autorate_ingress)
 251                 APPEND(buf, ofs, " autorate-ingress");
 252
 253         if (cfg->host_isolate)
 254                 APPEND(buf, ofs, " %snat dual-%shost",
 255                         cfg->nat ? "" : "no",
 256                         egress ? "src" : "dst");
 257         else
 258                 APPEND(buf, ofs, " flows");
 259
 260         APPEND(buf, ofs, " %s %s",
 261                cfg->common_opts ? cfg->common_opts : "",
 262                dir_opts ? dir_opts : "");
 263
 264         return qosify_run_cmd(buf, false);
 265 }
 266
 267 static int
 268 cmd_del_ingress(struct qosify_iface *iface)
 269 {
 270         char buf[256];
 271
 272         snprintf(buf, sizeof(buf), "ip link del '%s'", interface_ifb_name(iface));
 273
 274         return qosify_run_cmd(buf, true);
 275 }
 276
 277 static int
 278 cmd_add_ingress(struct qosify_iface *iface, bool eth)
 279 {
 280         const char *ifbdev = interface_ifb_name(iface);
 281         char buf[256];
 282         int ofs;
 283
 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);
 288
 289         ofs = prepare_tc_cmd(buf, sizeof(buf), "filter", "add", iface->ifname, " ingress");
 290         APPEND(buf, ofs, " protocol ip prio 5 u32 match ip sport 53 0xffff "
 291                          "flowid 1:1 action mirred egress redirect dev ifb-dns");
 292         qosify_run_cmd(buf, false);
 293
 294         ofs = prepare_tc_cmd(buf, sizeof(buf), "filter", "add", iface->ifname, " ingress");
 295         APPEND(buf, ofs, " protocol 802.1Q prio 6 u32 offset plus 4 match ip sport 53 0xffff "
 296                          "flowid 1:1 action mirred egress redirect dev ifb-dns");
 297         qosify_run_cmd(buf, false);
 298
 299         ofs = prepare_tc_cmd(buf, sizeof(buf), "filter", "add", iface->ifname, " ingress");
 300         APPEND(buf, ofs, " protocol ipv6 prio 7 u32 match ip6 sport 53 0xffff "
 301                          "flowid 1:1 action mirred egress redirect dev ifb-dns");
 302         qosify_run_cmd(buf, false);
 303
 304         ofs = prepare_tc_cmd(buf, sizeof(buf), "filter", "add", iface->ifname, " ingress");
 305         APPEND(buf, ofs, " protocol ipv6 prio 8 u32 offset plus 4 match ip6 sport 53 0xffff "
 306                          "flowid 1:1 action mirred egress redirect dev ifb-dns");
 307         qosify_run_cmd(buf, false);
 308
 309         cmd_add_bpf_filter(iface->ifname, false, eth);
 310
 311         if (!iface->config.ingress)
 312                 return 0;
 313
 314         snprintf(buf, sizeof(buf), "ip link add '%s' type ifb", ifbdev);
 315         qosify_run_cmd(buf, false);
 316
 317         cmd_add_qdisc(iface, ifbdev, false, eth);
 318
 319         snprintf(buf, sizeof(buf), "ip link set dev '%s' up", ifbdev);
 320         qosify_run_cmd(buf, false);
 321
 322         ofs = prepare_tc_cmd(buf, sizeof(buf), "filter", "add", iface->ifname, " ingress");
 323         APPEND(buf, ofs, " protocol all prio 20 u32 match u32 0 0 "
 324                          "flowid 1:1 action mirred egress redirect dev '%s'", ifbdev);
 325         return qosify_run_cmd(buf, false);
 326 }
 327
 328 static int cmd_add_egress(struct qosify_iface *iface, bool eth)
 329 {
 330         if (!iface->config.egress)
 331                 return 0;
 332
 333         cmd_add_qdisc(iface, iface->ifname, true, eth);
 334
 335         return cmd_add_bpf_filter(iface->ifname, true, eth);
 336 }
 337
 338 static void
 339 interface_clear_qdisc(struct qosify_iface *iface)
 340 {
 341         cmd_del_qdisc(iface->ifname, "root");
 342         cmd_del_qdisc(iface->ifname, "clsact");
 343         cmd_del_ingress(iface);
 344 }
 345
 346 static void
 347 interface_start(struct qosify_iface *iface)
 348 {
 349         struct ifreq ifr = {};
 350         bool eth;
 351
 352         if (!iface->ifname[0] || iface->active)
 353                 return;
 354
 355         ULOG_INFO("start interface %s\n", iface->ifname);
 356
 357         strncpy(ifr.ifr_name, iface->ifname, sizeof(ifr.ifr_name));
 358         if (ioctl(socket_fd, SIOCGIFHWADDR, &ifr) < 0) {
 359                 ULOG_ERR("ioctl(SIOCGIFHWADDR, %s) failed: %s\n", iface->ifname, strerror(errno));
 360                 return;
 361         }
 362
 363         eth = ifr.ifr_hwaddr.sa_family == ARPHRD_ETHER;
 364
 365         interface_clear_qdisc(iface);
 366         cmd_add_egress(iface, eth);
 367         cmd_add_ingress(iface, eth);
 368
 369         iface->active = true;
 370 }
 371
 372 static void
 373 interface_stop(struct qosify_iface *iface)
 374 {
 375         if (!iface->ifname[0] || !iface->active)
 376                 return;
 377
 378         ULOG_INFO("stop interface %s\n", iface->ifname);
 379         iface->active = false;
 380
 381         interface_clear_qdisc(iface);
 382 }
 383
 384 static void
 385 interface_set_config(struct qosify_iface *iface, struct blob_attr *config)
 386 {
 387         iface->config_data = blob_memdup(config);
 388         iface_config_set(iface, iface->config_data);
 389         interface_start(iface);
 390 }
 391
 392 static void
 393 interface_update_cb(struct vlist_tree *tree,
 394                     struct vlist_node *node_new, struct vlist_node *node_old)
 395 {
 396         struct qosify_iface *if_new = NULL, *if_old = NULL;
 397
 398         if (node_new)
 399                 if_new = container_of(node_new, struct qosify_iface, node);
 400         if (node_old)
 401                 if_old = container_of(node_old, struct qosify_iface, node);
 402
 403         if (if_new && if_old) {
 404                 if (!iface_config_equal(if_old, if_new)) {
 405                         interface_stop(if_old);
 406                         free(if_old->config_data);
 407                         interface_set_config(if_old, if_new->config_data);
 408                 }
 409
 410                 free(if_new);
 411                 return;
 412         }
 413
 414         if (if_old) {
 415                 interface_stop(if_old);
 416                 free(if_old->config_data);
 417                 free(if_old);
 418         }
 419
 420         if (if_new)
 421                 interface_set_config(if_new, if_new->config_data);
 422 }
 423
 424 static void
 425 interface_create(struct blob_attr *attr, bool device)
 426 {
 427         struct qosify_iface *iface;
 428         const char *name = blobmsg_name(attr);
 429         int name_len = strlen(name);
 430         char *name_buf;
 431
 432         if (strchr(name, '\''))
 433                 return;
 434
 435         if (name_len >= IFNAMSIZ)
 436                 return;
 437
 438         if (blobmsg_type(attr) != BLOBMSG_TYPE_TABLE)
 439                 return;
 440
 441         iface = calloc_a(sizeof(*iface), &name_buf, name_len + 1);
 442         strcpy(name_buf, blobmsg_name(attr));
 443         iface->config_data = attr;
 444         iface->device = device;
 445         vlist_add(device ? &devices : &interfaces, &iface->node, name_buf);
 446 }
 447
 448 void qosify_iface_config_update(struct blob_attr *ifaces, struct blob_attr *devs)
 449 {
 450         struct blob_attr *cur;
 451         int rem;
 452
 453         vlist_update(&devices);
 454         blobmsg_for_each_attr(cur, devs, rem)
 455                 interface_create(cur, true);
 456         vlist_flush(&devices);
 457
 458         vlist_update(&interfaces);
 459         blobmsg_for_each_attr(cur, ifaces, rem)
 460                 interface_create(cur, false);
 461         vlist_flush(&interfaces);
 462 }
 463
 464 static void
 465 qosify_iface_check_device(struct qosify_iface *iface)
 466 {
 467         const char *name = qosify_iface_name(iface);
 468         int ifindex;
 469
 470         ifindex = if_nametoindex(name);
 471         if (!ifindex) {
 472                 interface_stop(iface);
 473                 iface->ifname[0] = 0;
 474         } else {
 475                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", name);
 476                 interface_start(iface);
 477         }
 478 }
 479
 480 static void
 481 qosify_iface_check_interface(struct qosify_iface *iface)
 482 {
 483         const char *name = qosify_iface_name(iface);
 484         char ifname[IFNAMSIZ];
 485
 486         if (qosify_ubus_check_interface(name, ifname, sizeof(ifname)) == 0) {
 487                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", ifname);
 488                 interface_start(iface);
 489         } else {
 490                 interface_stop(iface);
 491                 iface->ifname[0] = 0;
 492         }
 493 }
 494
 495 static void qos_iface_check_cb(struct uloop_timeout *t)
 496 {
 497         struct qosify_iface *iface;
 498
 499         vlist_for_each_element(&devices, iface, node)
 500                 qosify_iface_check_device(iface);
 501         vlist_for_each_element(&interfaces, iface, node)
 502                 qosify_iface_check_interface(iface);
 503 }
 504
 505 void qosify_iface_check(void)
 506 {
 507         static struct uloop_timeout timer = {
 508                 .cb = qos_iface_check_cb,
 509         };
 510
 511         uloop_timeout_set(&timer, 10);
 512 }
 513
 514 static void
 515 __qosify_iface_status(struct blob_buf *b, struct qosify_iface *iface)
 516 {
 517         void *c;
 518
 519         c = blobmsg_open_table(b, qosify_iface_name(iface));
 520         blobmsg_add_u8(b, "active", iface->active);
 521         if (iface->ifname)
 522                 blobmsg_add_string(b, "ifname", iface->ifname);
 523         blobmsg_add_u8(b, "egress", iface->config.egress);
 524         blobmsg_add_u8(b, "ingress", iface->config.ingress);
 525         blobmsg_close_table(b, c);
 526
 527 }
 528
 529 void qosify_iface_status(struct blob_buf *b)
 530 {
 531         struct qosify_iface *iface;
 532         void *c;
 533
 534         c = blobmsg_open_table(b, "devices");
 535         vlist_for_each_element(&devices, iface, node)
 536                 __qosify_iface_status(b, iface);
 537         blobmsg_close_table(b, c);
 538
 539         c = blobmsg_open_table(b, "interfaces");
 540         vlist_for_each_element(&interfaces, iface, node)
 541                 __qosify_iface_status(b, iface);
 542         blobmsg_close_table(b, c);
 543 }
 544
 545 int qosify_iface_init(void)
 546 {
 547         socket_fd = socket(AF_UNIX, SOCK_DGRAM, 0);
 548         if (socket < 0)
 549                 return -1;
 550
 551         return 0;
 552 }
 553
 554 void qosify_iface_stop(void)
 555 {
 556         struct qosify_iface *iface;
 557
 558         vlist_for_each_element(&interfaces, iface, node)
 559                 interface_stop(iface);
 560         vlist_for_each_element(&devices, iface, node)
 561                 interface_stop(iface);
 562 }
 563