1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Copyright (C) 2021 Felix Fietkau <nbd@nbd.name> 4 */ 5 #define KBUILD_MODNAME "foo" 6 #include <uapi/linux/bpf.h> 7 #include <uapi/linux/if_ether.h> 8 #include <uapi/linux/if_packet.h> 9 #include <uapi/linux/ip.h> 10 #include <uapi/linux/ipv6.h> 11 #include <uapi/linux/in.h> 12 #include <uapi/linux/tcp.h> 13 #include <uapi/linux/udp.h> 14 #include <uapi/linux/filter.h> 15 #include <uapi/linux/pkt_cls.h> 16 #include <linux/ip.h> 17 #include <net/ipv6.h> 18 #include <bpf/bpf_helpers.h> 19 #include <bpf/bpf_endian.h> 20 #include "bpf_skb_utils.h" 21 #include "qosify-bpf.h" 22 23 #define INET_ECN_MASK 3 24 25 #define FLOW_CHECK_INTERVAL ((u32)((1000000000ULL) >> 24)) 26 #define FLOW_TIMEOUT ((u32)((30ULL * 1000000000ULL) >> 24)) 27 #define FLOW_BULK_TIMEOUT 5 28 29 #define EWMA_SHIFT 12 30 31 const volatile static uint32_t module_flags = 0; 32 33 struct flow_bucket { 34 __u32 last_update; 35 __u32 pkt_len_avg; 36 __u32 pkt_count; 37 __u32 bulk_timeout; 38 }; 39 40 struct { 41 __uint(type, BPF_MAP_TYPE_ARRAY); 42 __uint(pinning, 1); 43 __type(key, __u32); 44 __type(value, struct qosify_config); 45 __uint(max_entries, 1); 46 } config SEC(".maps"); 47 48 struct { 49 __uint(type, BPF_MAP_TYPE_ARRAY); 50 __uint(pinning, 1); 51 __type(key, __u32); 52 __type(value, __u8); 53 __uint(max_entries, 1 << 16); 54 } tcp_ports SEC(".maps"); 55 56 struct { 57 __uint(type, BPF_MAP_TYPE_ARRAY); 58 __uint(pinning, 1); 59 __type(key, __u32); 60 __type(value, __u8); 61 __uint(max_entries, 1 << 16); 62 } udp_ports SEC(".maps"); 63 64 struct { 65 __uint(type, BPF_MAP_TYPE_LRU_HASH); 66 __uint(pinning, 1); 67 __type(key, __u32); 68 __type(value, struct flow_bucket); 69 __uint(max_entries, QOSIFY_FLOW_BUCKETS); 70 } flow_map SEC(".maps"); 71 72 struct { 73 __uint(type, BPF_MAP_TYPE_HASH); 74 __uint(pinning, 1); 75 __uint(key_size, sizeof(struct in_addr)); 76 __type(value, struct qosify_ip_map_val); 77 __uint(max_entries, 100000); 78 __uint(map_flags, BPF_F_NO_PREALLOC); 79 } ipv4_map SEC(".maps"); 80 81 struct { 82 __uint(type, BPF_MAP_TYPE_HASH); 83 __uint(pinning, 1); 84 __uint(key_size, sizeof(struct in6_addr)); 85 __type(value, struct qosify_ip_map_val); 86 __uint(max_entries, 100000); 87 __uint(map_flags, BPF_F_NO_PREALLOC); 88 } ipv6_map SEC(".maps"); 89 90 struct { 91 __uint(type, BPF_MAP_TYPE_ARRAY); 92 __uint(pinning, 1); 93 __type(key, __u32); 94 __type(value, struct qosify_class); 95 __uint(max_entries, QOSIFY_MAX_CLASS_ENTRIES + 96 QOSIFY_DEFAULT_CLASS_ENTRIES); 97 } class_map SEC(".maps"); 98 99 struct { 100 __uint(type, BPF_MAP_TYPE_ARRAY); 101 __uint(pinning, 1); 102 __type(key, __u32); 103 __type(value, struct qosify_dscp_stats); 104 __uint(max_entries, QOSIFY_DSCP_MAX); 105 } dscp_stats SEC(".maps"); 106 107 struct { 108 __uint(type, BPF_MAP_TYPE_PERCPU_HASH); 109 __uint(pinning, 1); 110 __type(key, __u32); 111 __type(value, struct qosify_pattern_stats); 112 __uint(max_entries, QOSIFY_PATTERN_STATS_MAX); 113 } pattern_stats SEC(".maps"); 114 115 static struct qosify_config *get_config(void) 116 { 117 __u32 key = 0; 118 119 return bpf_map_lookup_elem(&config, &key); 120 } 121 122 static __always_inline __u32 cur_time(void) 123 { 124 __u32 val = bpf_ktime_get_ns() >> 24; 125 126 if (!val) 127 val = 1; 128 129 return val; 130 } 131 132 static __always_inline __u32 ewma(__u32 *avg, __u32 val) 133 { 134 if (*avg) 135 *avg = (*avg * 3) / 4 + (val << EWMA_SHIFT) / 4; 136 else 137 *avg = val << EWMA_SHIFT; 138 139 return *avg >> EWMA_SHIFT; 140 } 141 142 static __always_inline __u8 dscp_val(struct qosify_dscp_val *val, bool ingress) 143 { 144 __u8 ival = val->ingress; 145 __u8 eval = val->egress; 146 147 return ingress ? ival : eval; 148 } 149 150 static __always_inline void 151 ipv4_change_dsfield(struct __sk_buff *skb, __u32 offset, 152 __u8 mask, __u8 value, bool force) 153 { 154 struct iphdr *iph; 155 __u32 check; 156 __u8 dsfield; 157 158 iph = skb_ptr(skb, offset, sizeof(*iph)); 159 if (!iph) 160 return; 161 162 check = bpf_ntohs(iph->check); 163 if ((iph->tos & mask) && !force) 164 return; 165 166 dsfield = (iph->tos & mask) | value; 167 if (iph->tos == dsfield) 168 return; 169 170 check += iph->tos; 171 if ((check + 1) >> 16) 172 check = (check + 1) & 0xffff; 173 check -= dsfield; 174 check += check >> 16; 175 iph->check = bpf_htons(check); 176 iph->tos = dsfield; 177 } 178 179 static __always_inline void 180 ipv6_change_dsfield(struct __sk_buff *skb, __u32 offset, 181 __u8 mask, __u8 value, bool force) 182 { 183 struct ipv6hdr *ipv6h; 184 __u16 *p; 185 __u16 val; 186 187 ipv6h = skb_ptr(skb, offset, sizeof(*ipv6h)); 188 if (!ipv6h) 189 return; 190 191 p = (__u16 *)ipv6h; 192 if (((*p >> 4) & mask) && !force) 193 return; 194 195 val = (*p & bpf_htons((((__u16)mask << 4) | 0xf00f))) | bpf_htons((__u16)value << 4); 196 if (val == *p) 197 return; 198 199 *p = val; 200 } 201 202 static __always_inline __u8 203 ipv4_get_dscp(struct __sk_buff *skb, __u32 offset) 204 { 205 struct iphdr *iph = skb_ptr(skb, offset, sizeof(*iph)); 206 207 if (!iph) 208 return 0; 209 210 return iph->tos >> 2; 211 } 212 213 static __always_inline __u8 214 ipv6_get_dscp(struct __sk_buff *skb, __u32 offset) 215 { 216 struct ipv6hdr *ip6h = skb_ptr(skb, offset, sizeof(*ip6h)); 217 218 if (!ip6h) 219 return 0; 220 221 return (bpf_ntohs(*(__u16 *)ip6h) >> 6) & 0x3f; 222 } 223 224 static void 225 parse_l4proto(struct qosify_config *config, struct skb_parser_info *info, 226 bool ingress, __u8 *out_val) 227 { 228 struct udphdr *udp; 229 __u32 src, dest, key; 230 __u8 *value; 231 __u8 proto = info->proto; 232 233 udp = skb_info_ptr(info, sizeof(*udp)); 234 if (!udp) 235 return; 236 237 if (config && (proto == IPPROTO_ICMP || proto == IPPROTO_ICMPV6)) { 238 *out_val = config->dscp_icmp; 239 return; 240 } 241 242 src = READ_ONCE(udp->source); 243 dest = READ_ONCE(udp->dest); 244 if (ingress) 245 key = src; 246 else 247 key = dest; 248 249 if (proto == IPPROTO_TCP) { 250 value = bpf_map_lookup_elem(&tcp_ports, &key); 251 } else { 252 if (proto != IPPROTO_UDP) 253 key = 0; 254 255 value = bpf_map_lookup_elem(&udp_ports, &key); 256 } 257 258 if (value) 259 *out_val = *value; 260 } 261 262 static __always_inline bool 263 check_flow_bulk(struct qosify_flow_config *config, struct __sk_buff *skb, 264 struct flow_bucket *flow, __u8 *out_val) 265 { 266 bool trigger = false; 267 __s32 delta; 268 __u32 time; 269 int segs = 1; 270 bool ret = false; 271 272 if (!config->bulk_trigger_pps) 273 return false; 274 275 time = cur_time(); 276 if (!flow->last_update) 277 goto reset; 278 279 delta = time - flow->last_update; 280 if ((u32)delta > FLOW_TIMEOUT) 281 goto reset; 282 283 if (skb->gso_segs) 284 segs = skb->gso_segs; 285 flow->pkt_count += segs; 286 if (flow->pkt_count > config->bulk_trigger_pps) { 287 flow->bulk_timeout = config->bulk_trigger_timeout + 1; 288 trigger = true; 289 } 290 291 if (delta >= FLOW_CHECK_INTERVAL) { 292 if (flow->bulk_timeout && !trigger) 293 flow->bulk_timeout--; 294 295 goto clear; 296 } 297 298 goto out; 299 300 reset: 301 flow->pkt_len_avg = 0; 302 clear: 303 flow->pkt_count = 1; 304 flow->last_update = time; 305 out: 306 if (flow->bulk_timeout) { 307 *out_val = config->dscp_bulk; 308 return true; 309 } 310 311 return false; 312 } 313 314 static __always_inline bool 315 check_flow_prio(struct qosify_flow_config *config, struct __sk_buff *skb, 316 struct flow_bucket *flow, __u8 *out_val) 317 { 318 int cur_len = skb->len; 319 320 if (flow->bulk_timeout) 321 return false; 322 323 if (!config->prio_max_avg_pkt_len) 324 return false; 325 326 if (skb->gso_segs > 1) 327 cur_len /= skb->gso_segs; 328 329 if (ewma(&flow->pkt_len_avg, cur_len) <= config->prio_max_avg_pkt_len) { 330 *out_val = config->dscp_prio; 331 return true; 332 } 333 334 return false; 335 } 336 337 static __always_inline bool 338 check_flow(struct qosify_flow_config *config, struct __sk_buff *skb, 339 __u8 *out_val) 340 { 341 struct flow_bucket flow_data; 342 struct flow_bucket *flow; 343 __u32 hash; 344 bool ret = false; 345 346 if (!config) 347 return false; 348 349 if (!config->prio_max_avg_pkt_len && !config->bulk_trigger_pps) 350 return false; 351 352 hash = bpf_get_hash_recalc(skb); 353 flow = bpf_map_lookup_elem(&flow_map, &hash); 354 if (!flow) { 355 memset(&flow_data, 0, sizeof(flow_data)); 356 bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY); 357 flow = bpf_map_lookup_elem(&flow_map, &hash); 358 if (!flow) 359 return false; 360 } 361 362 ret |= check_flow_bulk(config, skb, flow, out_val); 363 ret |= check_flow_prio(config, skb, flow, out_val); 364 365 return ret; 366 } 367 368 static __always_inline struct qosify_ip_map_val * 369 parse_ipv4(struct qosify_config *config, struct skb_parser_info *info, 370 bool ingress, __u8 *out_val) 371 { 372 struct iphdr *iph; 373 __u8 ipproto; 374 int hdr_len; 375 void *key; 376 377 iph = skb_parse_ipv4(info, sizeof(struct udphdr)); 378 if (!iph) 379 return NULL; 380 381 parse_l4proto(config, info, ingress, out_val); 382 383 if (ingress) 384 key = &iph->saddr; 385 else 386 key = &iph->daddr; 387 388 return bpf_map_lookup_elem(&ipv4_map, key); 389 } 390 391 static __always_inline struct qosify_ip_map_val * 392 parse_ipv6(struct qosify_config *config, struct skb_parser_info *info, 393 bool ingress, __u8 *out_val) 394 { 395 struct ipv6hdr *iph; 396 __u8 ipproto; 397 void *key; 398 399 iph = skb_parse_ipv6(info, sizeof(struct udphdr)); 400 if (!iph) 401 return NULL; 402 403 if (ingress) 404 key = &iph->saddr; 405 else 406 key = &iph->daddr; 407 408 parse_l4proto(config, info, ingress, out_val); 409 410 return bpf_map_lookup_elem(&ipv6_map, key); 411 } 412 413 static __always_inline __u32 skb_packets(struct __sk_buff *skb) 414 { 415 return skb->gso_segs ? skb->gso_segs : 1; 416 } 417 418 static __always_inline void 419 account_pattern(__u32 pattern_id, __u32 pkt_count, __u32 pkt_len) 420 { 421 struct qosify_pattern_stats *stats; 422 423 if (!pattern_id) 424 return; 425 426 stats = bpf_map_lookup_elem(&pattern_stats, &pattern_id); 427 if (!stats) 428 return; 429 430 stats->packets += pkt_count; 431 stats->bytes += pkt_len; 432 } 433 434 static __always_inline void 435 account_dscp(__u8 dscp, __u32 pkt_count, __u32 pkt_len) 436 { 437 struct qosify_dscp_stats *stats; 438 __u32 key = dscp & GENMASK(5, 0); 439 440 stats = bpf_map_lookup_elem(&dscp_stats, &key); 441 if (!stats) 442 return; 443 444 __sync_fetch_and_add(&stats->packets, pkt_count); 445 __sync_fetch_and_add(&stats->bytes, pkt_len); 446 } 447 448 static __always_inline int 449 dscp_lookup_class(uint8_t *dscp, bool ingress, struct qosify_class **out_class, 450 bool counter, __u32 pkt_count, __u32 pkt_len) 451 { 452 struct qosify_class *class; 453 __u8 fallback_flag; 454 bool present; 455 __u32 key; 456 457 if (!(*dscp & QOSIFY_DSCP_CLASS_FLAG)) 458 return 0; 459 460 fallback_flag = *dscp & QOSIFY_DSCP_FALLBACK_FLAG; 461 key = *dscp & QOSIFY_DSCP_VALUE_MASK; 462 class = bpf_map_lookup_elem(&class_map, &key); 463 if (!class) 464 return -1; 465 466 present = class->flags & QOSIFY_CLASS_FLAG_PRESENT; 467 if (!present && key < QOSIFY_MAX_CLASS_ENTRIES) 468 return -1; 469 470 if (counter) { 471 __sync_fetch_and_add(&class->packets, pkt_count); 472 __sync_fetch_and_add(&class->bytes, pkt_len); 473 } 474 475 /* default classes are counted but keep the packet unmarked */ 476 if (!present) 477 return -1; 478 479 *dscp = dscp_val(&class->val, ingress); 480 *dscp |= fallback_flag; 481 *out_class = class; 482 483 return 0; 484 } 485 486 SEC("tc") 487 int classify(struct __sk_buff *skb) 488 { 489 struct skb_parser_info info; 490 bool ingress = module_flags & QOSIFY_INGRESS; 491 struct qosify_config *config; 492 struct qosify_class *class = NULL; 493 struct qosify_ip_map_val *ip_val; 494 __u32 packets = skb_packets(skb); 495 __u32 iph_offset; 496 __u8 dscp = 0; 497 bool force; 498 int type; 499 500 config = get_config(); 501 if (!config) 502 return TC_ACT_UNSPEC; 503 504 skb_parse_init(&info, skb); 505 if (module_flags & QOSIFY_IP_ONLY) { 506 type = info.proto = skb->protocol; 507 } else if (skb_parse_ethernet(&info)) { 508 skb_parse_vlan(&info); 509 skb_parse_vlan(&info); 510 type = info.proto; 511 } else { 512 return TC_ACT_UNSPEC; 513 } 514 515 iph_offset = info.offset; 516 if (type == bpf_htons(ETH_P_IP)) 517 ip_val = parse_ipv4(config, &info, ingress, &dscp); 518 else if (type == bpf_htons(ETH_P_IPV6)) 519 ip_val = parse_ipv6(config, &info, ingress, &dscp); 520 else 521 return TC_ACT_UNSPEC; 522 523 if (ip_val) { 524 if (!ip_val->seen) 525 ip_val->seen = 1; 526 account_pattern(ip_val->pattern_id, packets, skb->len); 527 dscp = ip_val->dscp; 528 } 529 530 if (!dscp_lookup_class(&dscp, ingress, &class, true, packets, skb->len)) { 531 if (class && 532 check_flow(&class->config, skb, &dscp) && 533 dscp_lookup_class(&dscp, ingress, &class, false, 0, 0)) 534 goto out; 535 536 dscp &= GENMASK(5, 0); 537 dscp <<= 2; 538 force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG); 539 540 if (type == bpf_htons(ETH_P_IP)) 541 ipv4_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force); 542 else if (type == bpf_htons(ETH_P_IPV6)) 543 ipv6_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force); 544 } 545 546 out: 547 if (type == bpf_htons(ETH_P_IP)) 548 dscp = ipv4_get_dscp(skb, iph_offset); 549 else 550 dscp = ipv6_get_dscp(skb, iph_offset); 551 552 account_dscp(dscp, packets, skb->len); 553 554 return TC_ACT_UNSPEC; 555 } 556 557 char _license[] SEC("license") = "GPL"; 558
This page was automatically generated by LXR 0.3.1. • OpenWrt