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
 290         if (cfg->host_isolate)
 291                 APPEND(buf, ofs, " %snat dual-%shost",
 292                         cfg->nat ? "" : "no",
 293                         egress ? "src" : "dst");
 294         else
 295                 APPEND(buf, ofs, " flows");
 296
 297         APPEND(buf, ofs, " %s %s",
 298                cfg->common_opts ? cfg->common_opts : "",
 299                dir_opts ? dir_opts : "");
 300
 301         run_cmd(buf, false);
 302
 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",
 305                egress ? "e" : "in",
 306                    eth ? "eth" : "ip");
 307
 308         return run_cmd(buf, false);
 309 }
 310
 311 static int
 312 cmd_del_ingress(struct qosify_iface *iface)
 313 {
 314         char buf[256];
 315
 316         cmd_del_qdisc(iface->ifname, "handle ffff: ingress");
 317         snprintf(buf, sizeof(buf), "ip link del '%s'", interface_ifb_name(iface));
 318
 319         return run_cmd(buf, true);
 320 }
 321
 322
 323 static int
 324 cmd_add_ingress(struct qosify_iface *iface, bool eth)
 325 {
 326         const char *ifbdev = interface_ifb_name(iface);
 327         char buf[256];
 328         int ofs;
 329
 330         cmd_del_ingress(iface);
 331
 332         ofs = prepare_tc_cmd(buf, sizeof(buf), "qdisc", "add", iface->ifname, " handle ffff: ingress");
 333         run_cmd(buf, false);
 334
 335         snprintf(buf, sizeof(buf), "ip link add '%s' type ifb", ifbdev);
 336         run_cmd(buf, false);
 337
 338         cmd_add_qdisc(iface, ifbdev, false, eth);
 339
 340         snprintf(buf, sizeof(buf), "ip link set dev '%s' up", ifbdev);
 341         run_cmd(buf, false);
 342
 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);
 347 }
 348
 349 static void
 350 interface_start(struct qosify_iface *iface)
 351 {
 352         struct ifreq ifr = {};
 353         bool eth;
 354
 355         if (!iface->ifname[0] || iface->active)
 356                 return;
 357
 358         ULOG_INFO("start interface %s\n", iface->ifname);
 359
 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));
 363                 return;
 364         }
 365
 366         eth = ifr.ifr_hwaddr.sa_family == ARPHRD_ETHER;
 367
 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);
 372
 373         iface->active = true;
 374 }
 375
 376 static void
 377 interface_stop(struct qosify_iface *iface)
 378 {
 379         if (!iface->ifname[0] || !iface->active)
 380                 return;
 381
 382         ULOG_INFO("stop interface %s\n", iface->ifname);
 383         iface->active = false;
 384
 385         if (iface->config.egress)
 386                 cmd_del_qdisc(iface->ifname, "root");
 387         if (iface->config.ingress)
 388                 cmd_del_ingress(iface);
 389 }
 390
 391 static void
 392 interface_set_config(struct qosify_iface *iface, struct blob_attr *config)
 393 {
 394         iface->config_data = blob_memdup(config);
 395         iface_config_set(iface, iface->config_data);
 396         interface_start(iface);
 397 }
 398
 399 static void
 400 interface_update_cb(struct vlist_tree *tree,
 401                     struct vlist_node *node_new, struct vlist_node *node_old)
 402 {
 403         struct qosify_iface *if_new = NULL, *if_old = NULL;
 404
 405         if (node_new)
 406                 if_new = container_of(node_new, struct qosify_iface, node);
 407         if (node_old)
 408                 if_old = container_of(node_old, struct qosify_iface, node);
 409
 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);
 415                 }
 416
 417                 free(if_new);
 418                 return;
 419         }
 420
 421         if (if_old) {
 422                 interface_stop(if_old);
 423                 free(if_old->config_data);
 424                 free(if_old);
 425         }
 426
 427         if (if_new)
 428                 interface_set_config(if_new, if_new->config_data);
 429 }
 430
 431 static void
 432 interface_create(struct blob_attr *attr, bool device)
 433 {
 434         struct qosify_iface *iface;
 435         const char *name = blobmsg_name(attr);
 436         int name_len = strlen(name);
 437         char *name_buf;
 438
 439         if (strchr(name, '\''))
 440                 return;
 441
 442         if (name_len >= IFNAMSIZ)
 443                 return;
 444
 445         if (blobmsg_type(attr) != BLOBMSG_TYPE_TABLE)
 446                 return;
 447
 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);
 453 }
 454
 455 void qosify_iface_config_update(struct blob_attr *ifaces, struct blob_attr *devs)
 456 {
 457         struct blob_attr *cur;
 458         int rem;
 459
 460         vlist_update(&devices);
 461         blobmsg_for_each_attr(cur, devs, rem)
 462                 interface_create(cur, true);
 463         vlist_flush(&devices);
 464
 465         vlist_update(&interfaces);
 466         blobmsg_for_each_attr(cur, ifaces, rem)
 467                 interface_create(cur, false);
 468         vlist_flush(&interfaces);
 469 }
 470
 471 static void
 472 qosify_iface_check_device(struct qosify_iface *iface)
 473 {
 474         const char *name = qosify_iface_name(iface);
 475         int ifindex;
 476
 477         ifindex = if_nametoindex(name);
 478         if (!ifindex) {
 479                 interface_stop(iface);
 480                 iface->ifname[0] = 0;
 481         } else {
 482                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", name);
 483                 interface_start(iface);
 484         }
 485 }
 486
 487 static void
 488 qosify_iface_check_interface(struct qosify_iface *iface)
 489 {
 490         const char *name = qosify_iface_name(iface);
 491         char ifname[IFNAMSIZ];
 492
 493         if (qosify_ubus_check_interface(name, ifname, sizeof(ifname)) == 0) {
 494                 snprintf(iface->ifname, sizeof(iface->ifname), "%s", ifname);
 495                 interface_start(iface);
 496         } else {
 497                 interface_stop(iface);
 498                 iface->ifname[0] = 0;
 499         }
 500 }
 501
 502 static void qos_iface_check_cb(struct uloop_timeout *t)
 503 {
 504         struct qosify_iface *iface;
 505
 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);
 510 }
 511
 512 void qosify_iface_check(void)
 513 {
 514         static struct uloop_timeout timer = {
 515                 .cb = qos_iface_check_cb,
 516         };
 517
 518         uloop_timeout_set(&timer, 10);
 519 }
 520
 521 void qosify_iface_status(struct blob_buf *b)
 522 {
 523         struct qosify_iface *iface;
 524         void *c, *i;
 525
 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);
 531         }
 532         blobmsg_close_table(b, c);
 533
 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);
 538                 if (iface->ifname)
 539                         blobmsg_add_string(b, "ifname", iface->ifname);
 540                 blobmsg_close_table(b, i);
 541         }
 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