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