map.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name>
   4  */
   5 #include <arpa/inet.h>
   6
   7 #include <errno.h>
   8 #include <stdio.h>
   9 #include <ctype.h>
  10 #include <stdlib.h>
  11 #include <time.h>
  12
  13 #include <libubox/uloop.h>
  14
  15 #include "qosify.h"
  16
  17 static int qosify_map_entry_cmp(const void *k1, const void *k2, void *ptr);
  18
  19 static int qosify_map_fds[__CL_MAP_MAX];
  20 static AVL_TREE(map_data, qosify_map_entry_cmp, false, NULL);
  21 static LIST_HEAD(map_files);
  22 static uint32_t next_timeout;
  23 static uint8_t qosify_dscp_default[2] = { 0xff, 0xff };
  24 int qosify_map_timeout = 3600;
  25 struct qosify_config config;
  26
  27 struct qosify_map_file {
  28         struct list_head list;
  29         char filename[];
  30 };
  31
  32 static const struct {
  33         const char *name;
  34         const char *type_name;
  35 } qosify_map_info[] = {
  36         [CL_MAP_TCP_PORTS] = { "tcp_ports", "tcp_port" },
  37         [CL_MAP_UDP_PORTS] = { "udp_ports", "udp_port" },
  38         [CL_MAP_IPV4_ADDR] = { "ipv4_map", "ipv4_addr" },
  39         [CL_MAP_IPV6_ADDR] = { "ipv6_map", "ipv6_addr" },
  40         [CL_MAP_CONFIG] = { "config", "config" },
  41         [CL_MAP_DNS] = { "dns", "dns" },
  42 };
  43
  44 static const struct {
  45         const char name[5];
  46         uint8_t val;
  47 } codepoints[] = {
  48         { "CS0", 0 },
  49         { "CS1", 8 },
  50         { "CS2", 16 },
  51         { "CS3", 24 },
  52         { "CS4", 32 },
  53         { "CS5", 40 },
  54         { "CS6", 48 },
  55         { "CS7", 56 },
  56         { "AF11", 10 },
  57         { "AF12", 12 },
  58         { "AF13", 14 },
  59         { "AF21", 18 },
  60         { "AF22", 20 },
  61         { "AF22", 22 },
  62         { "AF31", 26 },
  63         { "AF32", 28 },
  64         { "AF33", 30 },
  65         { "AF41", 34 },
  66         { "AF42", 36 },
  67         { "AF43", 38 },
  68         { "EF", 46 },
  69         { "VA", 44 },
  70         { "LE", 1 },
  71 };
  72
  73 static void qosify_map_timer_cb(struct uloop_timeout *t)
  74 {
  75         qosify_map_gc();
  76 }
  77
  78 static struct uloop_timeout qosify_map_timer = {
  79         .cb = qosify_map_timer_cb,
  80 };
  81
  82 static uint32_t qosify_gettime(void)
  83 {
  84         struct timespec ts;
  85
  86         clock_gettime(CLOCK_MONOTONIC, &ts);
  87
  88         return ts.tv_sec;
  89 }
  90
  91 static const char *
  92 qosify_map_path(enum qosify_map_id id)
  93 {
  94         static char path[128];
  95         const char *name;
  96
  97         if (id >= ARRAY_SIZE(qosify_map_info))
  98                 return NULL;
  99
 100         name = qosify_map_info[id].name;
 101         if (!name)
 102                 return NULL;
 103
 104         snprintf(path, sizeof(path), "%s/%s", CLASSIFY_DATA_PATH, name);
 105
 106         return path;
 107 }
 108
 109 static int qosify_map_get_fd(enum qosify_map_id id)
 110 {
 111         const char *path = qosify_map_path(id);
 112         int fd;
 113
 114         if (!path)
 115                 return -1;
 116
 117         fd = bpf_obj_get(path);
 118         if (fd < 0)
 119                 fprintf(stderr, "Failed to open map %s: %s\n", path, strerror(errno));
 120
 121         return fd;
 122 }
 123
 124 static void qosify_map_clear_list(enum qosify_map_id id)
 125 {
 126         int fd = qosify_map_fds[id];
 127         __u32 key[4] = {};
 128
 129         while (bpf_map_get_next_key(fd, &key, &key) != -1)
 130                 bpf_map_delete_elem(fd, &key);
 131 }
 132
 133 static void __qosify_map_set_dscp_default(enum qosify_map_id id, uint8_t val)
 134 {
 135         struct qosify_map_data data = {
 136                 .id = id,
 137         };
 138         int fd = qosify_map_fds[id];
 139         int i;
 140
 141         val |= QOSIFY_DSCP_DEFAULT_FLAG;
 142
 143         for (i = 0; i < (1 << 16); i++) {
 144                 data.addr.port = htons(i);
 145                 if (avl_find(&map_data, &data))
 146                         continue;
 147
 148                 bpf_map_update_elem(fd, &data.addr, &val, BPF_ANY);
 149         }
 150 }
 151
 152 void qosify_map_set_dscp_default(enum qosify_map_id id, uint8_t val)
 153 {
 154         bool udp;
 155
 156         if (id == CL_MAP_TCP_PORTS)
 157                 udp = false;
 158         else if (id == CL_MAP_UDP_PORTS)
 159                 udp = true;
 160         else
 161                 return;
 162
 163         if (qosify_dscp_default[udp] == val)
 164                 return;
 165
 166         qosify_dscp_default[udp] = val;
 167         __qosify_map_set_dscp_default(id, val);
 168 }
 169
 170 int qosify_map_init(void)
 171 {
 172         int i;
 173
 174         for (i = 0; i < CL_MAP_DNS; i++) {
 175                 qosify_map_fds[i] = qosify_map_get_fd(i);
 176                 if (qosify_map_fds[i] < 0)
 177                         return -1;
 178         }
 179
 180         qosify_map_clear_list(CL_MAP_IPV4_ADDR);
 181         qosify_map_clear_list(CL_MAP_IPV6_ADDR);
 182         qosify_map_reset_config();
 183
 184         return 0;
 185 }
 186
 187 static char *str_skip(char *str, bool space)
 188 {
 189         while (*str && isspace(*str) == space)
 190                 str++;
 191
 192         return str;
 193 }
 194
 195 static int
 196 qosify_map_codepoint(const char *val)
 197 {
 198         int i;
 199
 200         for (i = 0; i < ARRAY_SIZE(codepoints); i++)
 201                 if (!strcmp(codepoints[i].name, val))
 202                         return codepoints[i].val;
 203
 204         return 0xff;
 205 }
 206
 207 static int qosify_map_entry_cmp(const void *k1, const void *k2, void *ptr)
 208 {
 209         const struct qosify_map_data *d1 = k1;
 210         const struct qosify_map_data *d2 = k2;
 211
 212         if (d1->id != d2->id)
 213                 return d2->id - d1->id;
 214
 215         if (d1->id == CL_MAP_DNS)
 216                 return strcmp(d1->addr.dns.pattern, d2->addr.dns.pattern);
 217
 218         return memcmp(&d1->addr, &d2->addr, sizeof(d1->addr));
 219 }
 220
 221 static struct qosify_map_entry *
 222 __qosify_map_alloc_entry(struct qosify_map_data *data)
 223 {
 224         struct qosify_map_entry *e;
 225         char *pattern;
 226
 227         if (data->id < CL_MAP_DNS) {
 228                 e = calloc(1, sizeof(*e));
 229                 memcpy(&e->data.addr, &data->addr, sizeof(e->data.addr));
 230
 231                 return e;
 232         }
 233
 234         e = calloc_a(sizeof(*e), &pattern, strlen(data->addr.dns.pattern) + 1);
 235         strcpy(pattern, data->addr.dns.pattern);
 236         e->data.addr.dns.pattern = pattern;
 237         if (regcomp(&e->data.addr.dns.regex, pattern,
 238                     REG_EXTENDED | REG_ICASE | REG_NOSUB)) {
 239                 free(e);
 240                 return NULL;
 241         }
 242
 243         return e;
 244 }
 245
 246 static void __qosify_map_set_entry(struct qosify_map_data *data)
 247 {
 248         int fd = qosify_map_fds[data->id];
 249         struct qosify_map_entry *e;
 250         bool file = data->file;
 251         int32_t delta = 0;
 252         bool add = data->dscp != 0xff;
 253         uint8_t prev_dscp = 0xff;
 254
 255         e = avl_find_element(&map_data, data, e, avl);
 256         if (!e) {
 257                 if (!add)
 258                         return;
 259
 260                 e = __qosify_map_alloc_entry(data);
 261                 if (!e)
 262                         return;
 263
 264                 e->avl.key = &e->data;
 265                 e->data.id = data->id;
 266                 avl_insert(&map_data, &e->avl);
 267         } else {
 268                 prev_dscp = e->data.dscp;
 269         }
 270
 271         if (file)
 272                 e->data.file = add;
 273         else
 274                 e->data.user = add;
 275
 276         if (add) {
 277                 if (file)
 278                         e->data.file_dscp = data->dscp;
 279                 if (!e->data.user || !file)
 280                         e->data.dscp = data->dscp;
 281         } else if (e->data.file && !file) {
 282                 e->data.dscp = e->data.file_dscp;
 283         }
 284
 285         if (e->data.dscp != prev_dscp && data->id < CL_MAP_DNS)
 286                 bpf_map_update_elem(fd, &data->addr, &e->data.dscp, BPF_ANY);
 287
 288         if (add) {
 289                 if (qosify_map_timeout == ~0 || file) {
 290                         e->timeout = ~0;
 291                         return;
 292                 }
 293
 294                 e->timeout = qosify_gettime() + qosify_map_timeout;
 295                 delta = e->timeout - next_timeout;
 296                 if (next_timeout && delta >= 0)
 297                         return;
 298         }
 299
 300         uloop_timeout_set(&qosify_map_timer, 1);
 301 }
 302
 303 static int
 304 qosify_map_set_port(struct qosify_map_data *data, const char *str)
 305 {
 306         unsigned long start_port, end_port;
 307         char *err;
 308         int i;
 309
 310         start_port = end_port = strtoul(str, &err, 0);
 311         if (err && *err) {
 312                 if (*err == '-')
 313                         end_port = strtoul(err + 1, &err, 0);
 314                 if (*err)
 315                         return -1;
 316         }
 317
 318         if (!start_port || end_port < start_port ||
 319             end_port >= 65535)
 320                 return -1;
 321
 322         for (i = start_port; i <= end_port; i++) {
 323                 data->addr.port = htons(i);
 324                 __qosify_map_set_entry(data);
 325         }
 326
 327         return 0;
 328 }
 329
 330 static int
 331 qosify_map_fill_ip(struct qosify_map_data *data, const char *str)
 332 {
 333         int af;
 334
 335         if (data->id == CL_MAP_IPV6_ADDR)
 336                 af = AF_INET6;
 337         else
 338                 af = AF_INET;
 339
 340         if (inet_pton(af, str, &data->addr) != 1)
 341                 return -1;
 342
 343         return 0;
 344 }
 345
 346 int qosify_map_set_entry(enum qosify_map_id id, bool file, const char *str, uint8_t dscp)
 347 {
 348         struct qosify_map_data data = {
 349                 .id = id,
 350                 .file = file,
 351                 .dscp = dscp,
 352         };
 353
 354         switch (id) {
 355         case CL_MAP_DNS:
 356                 data.addr.dns.pattern = str;
 357                 break;
 358         case CL_MAP_TCP_PORTS:
 359         case CL_MAP_UDP_PORTS:
 360                 return qosify_map_set_port(&data, str);
 361         case CL_MAP_IPV4_ADDR:
 362         case CL_MAP_IPV6_ADDR:
 363                 if (qosify_map_fill_ip(&data, str))
 364                         return -1;
 365                 break;
 366         default:
 367                 return -1;
 368         }
 369
 370         __qosify_map_set_entry(&data);
 371
 372         return 0;
 373 }
 374
 375 int qosify_map_dscp_value(const char *val)
 376 {
 377         unsigned long dscp;
 378         char *err;
 379         bool fallback = false;
 380
 381         if (*val == '+') {
 382                 fallback = true;
 383                 val++;
 384         }
 385
 386         dscp = strtoul(val, &err, 0);
 387         if (err && *err)
 388                 dscp = qosify_map_codepoint(val);
 389
 390         if (dscp >= 64)
 391                 return -1;
 392
 393         return dscp + (fallback << 6);
 394 }
 395
 396 static void
 397 qosify_map_dscp_codepoint_str(char *dest, int len, uint8_t dscp)
 398 {
 399         int i;
 400
 401         if (dscp & QOSIFY_DSCP_FALLBACK_FLAG) {
 402                 *(dest++) = '+';
 403                 len--;
 404                 dscp &= ~QOSIFY_DSCP_FALLBACK_FLAG;
 405         }
 406
 407         for (i = 0; i < ARRAY_SIZE(codepoints); i++) {
 408                 if (codepoints[i].val != dscp)
 409                         continue;
 410
 411                 snprintf(dest, len, "%s", codepoints[i].name);
 412                 return;
 413         }
 414
 415         snprintf(dest, len, "0x%x", dscp);
 416 }
 417
 418 static void
 419 qosify_map_parse_line(char *str)
 420 {
 421         const char *key, *value;
 422         int dscp;
 423
 424         str = str_skip(str, true);
 425         key = str;
 426
 427         str = str_skip(str, false);
 428         if (!*str)
 429                 return;
 430
 431         *(str++) = 0;
 432         str = str_skip(str, true);
 433         value = str;
 434
 435         dscp = qosify_map_dscp_value(value);
 436         if (dscp < 0)
 437                 return;
 438
 439         if (!strncmp(key, "dns:", 4))
 440                 qosify_map_set_entry(CL_MAP_DNS, true, key + 4, dscp);
 441         if (!strncmp(key, "tcp:", 4))
 442                 qosify_map_set_entry(CL_MAP_TCP_PORTS, true, key + 4, dscp);
 443         else if (!strncmp(key, "udp:", 4))
 444                 qosify_map_set_entry(CL_MAP_UDP_PORTS, true, key + 4, dscp);
 445         else if (strchr(key, ':'))
 446                 qosify_map_set_entry(CL_MAP_IPV6_ADDR, true, key, dscp);
 447         else if (strchr(key, '.'))
 448                 qosify_map_set_entry(CL_MAP_IPV4_ADDR, true, key, dscp);
 449 }
 450
 451 static int __qosify_map_load_file(const char *file)
 452 {
 453         char line[1024];
 454         char *cur;
 455         FILE *f;
 456
 457         if (!file)
 458                 return 0;
 459
 460         f = fopen(file, "r");
 461         if (!f) {
 462                 fprintf(stderr, "Can't open data file %s\n", file);
 463                 return -1;
 464         }
 465
 466         while (fgets(line, sizeof(line), f)) {
 467                 cur = strchr(line, '#');
 468                 if (cur)
 469                         *cur = 0;
 470
 471                 cur = line + strlen(line);
 472                 if (cur == line)
 473                         continue;
 474
 475                 while (cur > line && isspace(cur[-1]))
 476                         cur--;
 477
 478                 *cur = 0;
 479                 qosify_map_parse_line(line);
 480         }
 481
 482         fclose(f);
 483
 484         return 0;
 485 }
 486
 487 int qosify_map_load_file(const char *file)
 488 {
 489         struct qosify_map_file *f;
 490
 491         if (!file)
 492                 return 0;
 493
 494         f = calloc(1, sizeof(*f) + strlen(file) + 1);
 495         strcpy(f->filename, file);
 496         list_add_tail(&f->list, &map_files);
 497
 498         return __qosify_map_load_file(file);
 499 }
 500
 501 static void qosify_map_reset_file_entries(void)
 502 {
 503         struct qosify_map_entry *e;
 504
 505         avl_for_each_element(&map_data, e, avl)
 506                 e->data.file = false;
 507 }
 508
 509 void qosify_map_clear_files(void)
 510 {
 511         struct qosify_map_file *f, *tmp;
 512
 513         qosify_map_reset_file_entries();
 514
 515         list_for_each_entry_safe(f, tmp, &map_files, list) {
 516                 list_del(&f->list);
 517                 free(f);
 518         }
 519 }
 520
 521 void qosify_map_reset_config(void)
 522 {
 523         qosify_map_clear_files();
 524         qosify_map_set_dscp_default(CL_MAP_TCP_PORTS, 0);
 525         qosify_map_set_dscp_default(CL_MAP_UDP_PORTS, 0);
 526         qosify_map_timeout = 3600;
 527
 528         memset(&config, 0, sizeof(config));
 529         config.dscp_prio = 0xff;
 530         config.dscp_bulk = 0xff;
 531         config.dscp_icmp = 0xff;
 532 }
 533
 534 void qosify_map_reload(void)
 535 {
 536         struct qosify_map_file *f;
 537
 538         qosify_map_reset_file_entries();
 539
 540         list_for_each_entry(f, &map_files, list)
 541                 __qosify_map_load_file(f->filename);
 542
 543         qosify_map_gc();
 544 }
 545
 546 static void qosify_map_free_entry(struct qosify_map_entry *e)
 547 {
 548         int fd = qosify_map_fds[e->data.id];
 549
 550         avl_delete(&map_data, &e->avl);
 551         if (e->data.id < CL_MAP_DNS)
 552                 bpf_map_delete_elem(fd, &e->data.addr);
 553         free(e);
 554 }
 555
 556 void qosify_map_gc(void)
 557 {
 558         struct qosify_map_entry *e, *tmp;
 559         int32_t timeout = 0;
 560         uint32_t cur_time = qosify_gettime();
 561
 562         next_timeout = 0;
 563         avl_for_each_element_safe(&map_data, e, avl, tmp) {
 564                 int32_t cur_timeout;
 565
 566                 if (e->data.user && e->timeout != ~0) {
 567                         cur_timeout = e->timeout - cur_time;
 568                         if (cur_timeout <= 0) {
 569                                 e->data.user = false;
 570                                 e->data.dscp = e->data.file_dscp;
 571                         } else if (!timeout || cur_timeout < timeout) {
 572                                 timeout = cur_timeout;
 573                                 next_timeout = e->timeout;
 574                         }
 575                 }
 576
 577                 if (e->data.file || e->data.user)
 578                         continue;
 579
 580                 qosify_map_free_entry(e);
 581         }
 582
 583         if (!timeout)
 584                 return;
 585
 586         uloop_timeout_set(&qosify_map_timer, timeout * 1000);
 587 }
 588
 589
 590 int qosify_map_add_dns_host(const char *host, const char *addr, const char *type, int ttl)
 591 {
 592         struct qosify_map_data data = {
 593                 .id = CL_MAP_DNS,
 594                 .addr.dns.pattern = "",
 595         };
 596         struct qosify_map_entry *e;
 597         int prev_timeout = qosify_map_timeout;
 598
 599         e = avl_find_ge_element(&map_data, &data, e, avl);
 600         if (!e)
 601                 return 0;
 602
 603         memset(&data, 0, sizeof(data));
 604         data.user = true;
 605         if (!strcmp(type, "A"))
 606                 data.id = CL_MAP_IPV4_ADDR;
 607         else if (!strcmp(type, "AAAA"))
 608                 data.id = CL_MAP_IPV6_ADDR;
 609         else
 610                 return 0;
 611
 612         if (qosify_map_fill_ip(&data, addr))
 613                 return -1;
 614
 615         avl_for_element_to_last(&map_data, e, e, avl) {
 616                 regex_t *regex = &e->data.addr.dns.regex;
 617
 618                 if (e->data.id != CL_MAP_DNS)
 619                         return 0;
 620
 621                 if (regexec(regex, host, 0, NULL, 0) != 0)
 622                         continue;
 623
 624                 if (ttl)
 625                         qosify_map_timeout = ttl;
 626                 data.dscp = e->data.dscp;
 627                 __qosify_map_set_entry(&data);
 628                 qosify_map_timeout = prev_timeout;
 629         }
 630
 631         return 0;
 632 }
 633
 634
 635 void qosify_map_dump(struct blob_buf *b)
 636 {
 637         struct qosify_map_entry *e;
 638         uint32_t cur_time = qosify_gettime();
 639         int buf_len = INET6_ADDRSTRLEN + 1;
 640         char *buf;
 641         void *a;
 642         int af;
 643
 644         a = blobmsg_open_array(b, "entries");
 645         avl_for_each_element(&map_data, e, avl) {
 646                 void *c;
 647
 648                 if (!e->data.file && !e->data.user)
 649                         continue;
 650
 651                 c = blobmsg_open_table(b, NULL);
 652                 if (e->data.user && e->timeout != ~0) {
 653                         int32_t cur_timeout = e->timeout - cur_time;
 654
 655                         if (cur_timeout < 0)
 656                                 cur_timeout = 0;
 657
 658                         blobmsg_add_u32(b, "timeout", cur_timeout);
 659                 }
 660
 661                 blobmsg_add_u8(b, "file", e->data.file);
 662                 blobmsg_add_u8(b, "user", e->data.user);
 663
 664                 buf = blobmsg_alloc_string_buffer(b, "dscp", buf_len);
 665                 qosify_map_dscp_codepoint_str(buf, buf_len, e->data.dscp);
 666                 blobmsg_add_string_buffer(b);
 667
 668                 blobmsg_add_string(b, "type", qosify_map_info[e->data.id].type_name);
 669
 670                 switch (e->data.id) {
 671                 case CL_MAP_TCP_PORTS:
 672                 case CL_MAP_UDP_PORTS:
 673                         blobmsg_printf(b, "addr", "%d", ntohs(e->data.addr.port));
 674                         break;
 675                 case CL_MAP_IPV4_ADDR:
 676                 case CL_MAP_IPV6_ADDR:
 677                         buf = blobmsg_alloc_string_buffer(b, "addr", buf_len);
 678                         af = e->data.id == CL_MAP_IPV6_ADDR ? AF_INET6 : AF_INET;
 679                         inet_ntop(af, &e->data.addr, buf, buf_len);
 680                         blobmsg_add_string_buffer(b);
 681                         break;
 682                 case CL_MAP_DNS:
 683                         blobmsg_add_string(b, "addr", e->data.addr.dns.pattern);
 684                         break;
 685                 default:
 686                         *buf = 0;
 687                         break;
 688                 }
 689                 blobmsg_close_table(b, c);
 690         }
 691         blobmsg_close_array(b, a);
 692 }
 693
 694 void qosify_map_update_config(void)
 695 {
 696         int fd = qosify_map_fds[CL_MAP_CONFIG];
 697         uint32_t key = 0;
 698
 699         bpf_map_update_elem(fd, &key, &config, BPF_ANY);
 700 }