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