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_config *cfg, struct blob_attr *attr)
 138 {
 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 = true;
 152
 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);
 175 }
 176
 177 static const char *
 178 interface_ifb_name(struct qosify_iface *iface)
 179 {
 180         static char ifname[IFNAMSIZ + 1] = "ifb-";
 181         int len = strlen(iface->ifname);
 182
 183         if (len + 4 < IFNAMSIZ) {
 184                 snprintf(ifname + 4, IFNAMSIZ - 4, "%s", iface->ifname);
 185
 186                 return ifname;
 187         }
 188
 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);
 192
 193         return ifname;
 194 }
 195
 196 static int run_cmd(char *cmd, bool ignore)
 197 {
 198         char *argv[] = { "sh", "-c", cmd, NULL };
 199         bool first = true;
 200         int status = -1;
 201         char buf[512];
 202         int fds[2];
 203         FILE *f;
 204         int pid;
 205
 206         if (pipe(fds))
 207                 return -1;
 208
 209         pid = fork();
 210         if (!pid) {
 211                 close(fds[0]);
 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)
 217                         close(fds[1]);
 218                 execv("/bin/sh", argv);
 219                 exit(1);
 220         }
 221
 222         if (pid < 0)
 223                 return -1;
 224
 225         close(fds[1]);
 226         f = fdopen(fds[0], "r");
 227         if (!f) {
 228                 close(fds[0]);
 229                 goto out;
 230         }
 231
 232         while (fgets(buf, sizeof(buf), f) != NULL) {
 233                 if (!strlen(buf))
 234                         break;
 235                 if (ignore)
 236                         continue;
 237                 if (first) {
 238                         ULOG_WARN("Command: %s\n", cmd);
 239                         first = false;
 240                 }
 241                 ULOG_WARN("%s%s", buf, strchr(buf, '\n') ? "" : "\n");
 242         }
 243
 244         fclose(f);
 245
 246 out:
 247         while (waitpid(pid, &status, 0) < 0)
 248                 if (errno != EINTR)
 249                         break;
 250
 251         return status;
 252 }
 253
 254 static int
 255 prepare_tc_cmd(char *buf, int len, const char *type, const char *cmd,
 256                const char *dev, const char *extra)
 257 {
 258         return snprintf(buf, len, "tc %s %s dev '%s' %s", type, cmd, dev, extra);
 259 }
 260
 261 static int
 262 cmd_del_qdisc(const char *ifname, const char *type)
 263 {
 264         char buf[64];
 265
 266         prepare_tc_cmd(buf, sizeof(buf), "qdisc", "del", ifname, type);
 267
 268         return run_cmd(buf, true);
 269 }
 270
 271 static int
 272 cmd_add_qdisc(struct qosify_iface *iface, const char *ifname, bool egress, bool eth)
 273 {
 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;
 277         char buf[512];
 278         int ofs;
 279
 280         cmd_del_qdisc(ifname, "root");
 281
 282         ofs = prepare_tc_cmd(buf, sizeof(buf), "qdisc", "add", ifname, "root handle 1: cake");
 283         if (bw)
 284                 APPEND(buf, ofs, " bandwidth %s", bw);
 285
 286         APPEND(buf, ofs, " %s %sgress", cfg->mode, egress ? "e" : "in");
 287
 288         if (cfg->host_isolate)
 289                 APPEND(buf, ofs, " %snat dual-%shost",
 290                         cfg->nat ? "" : "no",
 291                         egress ? "src" : "dst");
 292         else
 293                 APPEND(buf, ofs, " flows");
 294
 295         APPEND(buf, ofs, " %s %s",
 296                cfg->common_opts ? cfg->common_opts : "",
 297                dir_opts ? dir_opts : "");
 298
 299         run_cmd(buf, false);
 300
 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",
 303                egress ? "e" : "in",
 304                    eth ? "eth" : "ip");
 305
 306         return run_cmd(buf, false);
 307 }
 308
 309 static int
 310 cmd_del_ingress(struct qosify_iface *iface)
 311 {
 312         char buf[256];
 313
 314         cmd_del_qdisc(iface->ifname, "handle ffff: ingress");
 315         snprintf(buf, sizeof(buf), "ip link del '%s'", interface_ifb_name(iface));
 316
 317         return run_cmd(buf, true);
 318 }
 319
 320
 321 static int
 322 cmd_add_ingress(struct qosify_iface *iface, bool eth)
 323 {
 324         const char *ifbdev = interface_ifb_name(iface);
 325         char buf[256];
 326         int ofs;
 327
 328         cmd_del_ingress(iface);
 329
 330         ofs = prepare_tc_cmd(buf, sizeof(buf), "qdisc", "add", iface->ifname, " handle ffff: ingress");
 331         run_cmd(buf, false);
 332
 333         snprintf(buf, sizeof(buf), "ip link add '%s' type ifb", ifbdev);
 334         run_cmd(buf, false);
 335
 336         cmd_add_qdisc(iface, ifbdev, false, eth);
 337
 338         snprintf(buf, sizeof(buf), "ip link set dev '%s' up", ifbdev);
 339         run_cmd(buf, false);
 340
 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);
 345 }
 346
 347 static void
 348 interface_start(struct qosify_iface *iface)
 349 {
 350         struct ifreq ifr = {};
 351         bool eth;
 352
 353         if (!iface->ifname[0] || iface->active)
 354                 return;
 355
 356         ULOG_INFO("start interface %s\n", iface->ifname);
 357
 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));
 361                 return;
 362         }
 363
 364         eth = ifr.ifr_hwaddr.sa_family == ARPHRD_ETHER;
 365
 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);
 370
 371         iface->active = true;
 372 }
 373
 374 static void
 375 interface_stop(struct qosify_iface *iface)
 376 {
 377         if (!iface->ifname[0] || !iface->active)
 378                 return;
 379
 380         ULOG_INFO("stop interface %s\n", iface->ifname);
 381         iface->active = false;
 382
 383         if (iface->config.egress)
 384                 cmd_del_qdisc(iface->ifname, "root");
 385         if (iface->config.ingress)
 386                 cmd_del_ingress(iface);
 387 }
 388
 389 static void
 390 interface_set_config(struct qosify_iface *iface, struct blob_attr *config)
 391 {
 392         iface->config_data = blob_memdup(config);
 393         iface_config_set(&iface->config, iface->config_data);
 394         interface_start(iface);
 395 }
 396
 397 static void
 398 interface_update_cb(struct vlist_tree *tree,
 399                     struct vlist_node *node_new, struct vlist_node *node_old)
 400 {
 401         struct qosify_iface *if_new = NULL, *if_old = NULL;
 402
 403         if (node_new)
 404                 if_new = container_of(node_new, struct qosify_iface, node);
 405         if (node_old)
 406                 if_old = container_of(node_old, struct qosify_iface, node);
 407
 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);
 413                 }
 414
 415                 free(if_new);
 416                 return;
 417         }
 418
 419         if (if_old) {
 420                 interface_stop(if_old);
 421                 free(if_old->config_data);
 422                 free(if_old);
 423         }
 424
 425         if (if_new)
 426                 interface_set_config(if_new, if_new->config_data);
 427 }
 428
 429 static void
 430 interface_create(struct blob_attr *attr, bool device)
 431 {
 432         struct qosify_iface *iface;
 433         const char *name = blobmsg_name(attr);
 434         int name_len = strlen(name);
 435         char *name_buf;
 436
 437         if (strchr(name, '\''))
 438                 return;
 439
 440         if (name_len >= IFNAMSIZ)
 441                 return;
 442
 443         if (blobmsg_type(attr) != BLOBMSG_TYPE_TABLE)
 444                 return;
 445
 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);
 451 }
 452
 453 void qosify_iface_config_update(struct blob_attr *ifaces, struct blob_attr *devs)
 454 {
 455         struct blob_attr *cur;
 456         int rem;
 457
 458         vlist_update(&devices);
 459         blobmsg_for_each_attr(cur, devs, rem)
 460                 interface_create(cur, true);
 461         vlist_flush(&devices);
 462
 463         vlist_update(&interfaces);
 464         blobmsg_for_each_attr(cur, ifaces, rem)
 465                 interface_create(cur, false);
 466         vlist_flush(&interfaces);
 467 }
 468
 469 static void
 470 qosify_iface_check_device(struct qosify_iface *iface)
 471 {
 472         const char *name = qosify_iface_name(iface);
 473         int ifindex;
 474
 475         ifindex = if_nametoindex(name);
 476         if (!ifindex) {
 477                 interface_stop(iface);
 478                 iface->ifname[0] = 0;
 479         } else {
 480                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", name);
 481                 interface_start(iface);
 482         }
 483 }
 484
 485 static void
 486 qosify_iface_check_interface(struct qosify_iface *iface)
 487 {
 488         const char *name = qosify_iface_name(iface);
 489         char ifname[IFNAMSIZ];
 490
 491         if (qosify_ubus_check_interface(name, ifname, sizeof(ifname)) == 0) {
 492                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", ifname);
 493                 interface_start(iface);
 494         } else {
 495                 interface_stop(iface);
 496                 iface->ifname[0] = 0;
 497         }
 498 }
 499
 500 static void qos_iface_check_cb(struct uloop_timeout *t)
 501 {
 502         struct qosify_iface *iface;
 503
 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);
 508 }
 509
 510 void qosify_iface_check(void)
 511 {
 512         static struct uloop_timeout timer = {
 513                 .cb = qos_iface_check_cb,
 514         };
 515
 516         uloop_timeout_set(&timer, 10);
 517 }
 518
 519 void qosify_iface_status(struct blob_buf *b)
 520 {
 521         struct qosify_iface *iface;
 522         void *c, *i;
 523
 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);
 529         }
 530         blobmsg_close_table(b, c);
 531
 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);
 536                 if (iface->ifname)
 537                         blobmsg_add_string(b, "ifname", iface->ifname);
 538                 blobmsg_close_table(b, i);
 539         }
 540         blobmsg_close_table(b, c);
 541 }
 542
 543 int qosify_iface_init(void)
 544 {
 545         socket_fd = socket(AF_UNIX, SOCK_DGRAM, 0);
 546         if (socket < 0)
 547                 return -1;
 548
 549         return 0;
 550 }
 551
 552 void qosify_iface_stop(void)
 553 {
 554         struct qosify_iface *iface;
 555
 556         vlist_for_each_element(&interfaces, iface, node)
 557                 interface_stop(iface);
 558         vlist_for_each_element(&devices, iface, node)
 559                 interface_stop(iface);
 560 }
 561