qosify-bpf.c

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