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