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         if (ingress)
 262                 key = udp->source;
 263         else
 264                 key = udp->dest;
 265
 266         if (proto == IPPROTO_TCP) {
 267                 value = bpf_map_lookup_elem(&tcp_ports, &key);
 268         } else {
 269                 if (proto != IPPROTO_UDP)
 270                         key = 0;
 271
 272                 value = bpf_map_lookup_elem(&udp_ports, &key);
 273         }
 274
 275         if (value)
 276                 *out_val = *value;
 277 }
 278
 279 static __always_inline void
 280 check_flow_bulk(struct qosify_flow_config *config, struct __sk_buff *skb,
 281                 struct flow_bucket *flow, __u8 *out_val)
 282 {
 283         bool trigger = false;
 284         __s32 delta;
 285         __u32 time;
 286         int segs = 1;
 287
 288         if (!config->bulk_trigger_pps)
 289                 return;
 290
 291         time = cur_time();
 292         if (!flow->last_update)
 293                 goto reset;
 294
 295         delta = time - flow->last_update;
 296         if ((u32)delta > FLOW_TIMEOUT)
 297                 goto reset;
 298
 299         if (skb->gso_size && skb->gso_segs)
 300                 segs = skb->gso_segs;
 301         flow->pkt_count += segs;
 302         if (flow->pkt_count > config->bulk_trigger_pps) {
 303                 flow->bulk_timeout = config->bulk_trigger_timeout + 1;
 304                 trigger = true;
 305         }
 306
 307         if (delta >= FLOW_CHECK_INTERVAL) {
 308                 if (flow->bulk_timeout && !trigger)
 309                         flow->bulk_timeout--;
 310
 311                 goto clear;
 312         }
 313
 314         goto out;
 315
 316 reset:
 317         flow->pkt_len_avg = 0;
 318 clear:
 319         flow->pkt_count = 1;
 320         flow->last_update = time;
 321 out:
 322         if (flow->bulk_timeout)
 323                 *out_val = config->dscp_bulk;
 324 }
 325
 326 static __always_inline void
 327 check_flow_prio(struct qosify_flow_config *config, struct __sk_buff *skb,
 328                 struct flow_bucket *flow, __u8 *out_val)
 329 {
 330         if (flow->bulk_timeout)
 331                 return;
 332
 333         if (config->prio_max_avg_pkt_len &&
 334             ewma(&flow->pkt_len_avg, skb->len) <= config->prio_max_avg_pkt_len)
 335                 *out_val = config->dscp_prio;
 336 }
 337
 338 static __always_inline void
 339 check_flow(struct qosify_flow_config *config, struct __sk_buff *skb,
 340            __u8 *out_val)
 341 {
 342         struct flow_bucket flow_data;
 343         struct flow_bucket *flow;
 344         __u32 hash;
 345
 346         if (!config)
 347                 return;
 348
 349         hash = bpf_get_hash_recalc(skb);
 350         flow = bpf_map_lookup_elem(&flow_map, &hash);
 351         if (!flow) {
 352                 memset(&flow_data, 0, sizeof(flow_data));
 353                 bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY);
 354                 flow = bpf_map_lookup_elem(&flow_map, &hash);
 355                 if (!flow)
 356                         return;
 357         }
 358
 359         check_flow_bulk(config, skb, flow, out_val);
 360         check_flow_prio(config, skb, flow, out_val);
 361 }
 362
 363 static __always_inline struct qosify_ip_map_val *
 364 parse_ipv4(struct qosify_config *config, struct __sk_buff *skb, __u32 *offset,
 365            bool ingress, __u8 *out_val)
 366 {
 367         struct iphdr *iph;
 368         __u8 ipproto;
 369         int hdr_len;
 370         void *key;
 371
 372         iph = skb_ptr(skb, *offset);
 373         if (skb_check(skb, iph + 1))
 374                 return NULL;
 375
 376         hdr_len = iph->ihl * 4;
 377         if (bpf_skb_pull_data(skb, *offset + hdr_len + sizeof(struct udphdr)))
 378                 return NULL;
 379
 380         iph = skb_ptr(skb, *offset);
 381         *offset += hdr_len;
 382
 383         if (skb_check(skb, (void *)(iph + 1)))
 384                 return NULL;
 385
 386         ipproto = iph->protocol;
 387         parse_l4proto(config, skb, *offset, ipproto, ingress, out_val);
 388
 389         if (ingress)
 390                 key = &iph->saddr;
 391         else
 392                 key = &iph->daddr;
 393
 394         return bpf_map_lookup_elem(&ipv4_map, key);
 395 }
 396
 397 static __always_inline struct qosify_ip_map_val *
 398 parse_ipv6(struct qosify_config *config, struct __sk_buff *skb, __u32 *offset,
 399            bool ingress, __u8 *out_val)
 400 {
 401         struct ipv6hdr *iph;
 402         __u8 ipproto;
 403         void *key;
 404
 405         if (bpf_skb_pull_data(skb, *offset + sizeof(*iph) + sizeof(struct udphdr)))
 406                 return NULL;
 407
 408         iph = skb_ptr(skb, *offset);
 409         *offset += sizeof(*iph);
 410
 411         if (skb_check(skb, (void *)(iph + 1)))
 412                 return NULL;
 413
 414         ipproto = iph->nexthdr;
 415         if (ingress)
 416                 key = &iph->saddr;
 417         else
 418                 key = &iph->daddr;
 419
 420         parse_l4proto(config, skb, *offset, ipproto, ingress, out_val);
 421
 422         return bpf_map_lookup_elem(&ipv6_map, key);
 423 }
 424
 425 static __always_inline int
 426 dscp_lookup_class(uint8_t *dscp, bool ingress, struct qosify_class **out_class)
 427 {
 428         struct qosify_class *class;
 429         __u8 fallback_flag;
 430         __u32 key;
 431
 432         if (!(*dscp & QOSIFY_DSCP_CLASS_FLAG))
 433                 return 0;
 434
 435         fallback_flag = *dscp & QOSIFY_DSCP_FALLBACK_FLAG;
 436         key = *dscp & QOSIFY_DSCP_VALUE_MASK;
 437         class = bpf_map_lookup_elem(&class_map, &key);
 438         if (!class)
 439                 return -1;
 440
 441         if (!(class->flags & QOSIFY_CLASS_FLAG_PRESENT))
 442                 return -1;
 443
 444         *dscp = dscp_val(&class->val, ingress);
 445         *dscp |= fallback_flag;
 446         *out_class = class;
 447
 448         return 0;
 449 }
 450
 451 SEC("classifier")
 452 int classify(struct __sk_buff *skb)
 453 {
 454         bool ingress = module_flags & QOSIFY_INGRESS;
 455         struct qosify_config *config;
 456         struct qosify_class *class = NULL;
 457         struct qosify_ip_map_val *ip_val;
 458         __u32 offset = 0;
 459         __u32 iph_offset;
 460         void *iph;
 461         __u8 dscp;
 462         bool force;
 463         int type;
 464
 465         config = get_config();
 466         if (!config)
 467                 return TC_ACT_OK;
 468
 469         if (module_flags & QOSIFY_IP_ONLY)
 470                 type = skb->protocol;
 471         else
 472                 type = parse_ethernet(skb, &offset);
 473
 474         iph_offset = offset;
 475         if (type == bpf_htons(ETH_P_IP))
 476                 ip_val = parse_ipv4(config, skb, &offset, ingress, &dscp);
 477         else if (type == bpf_htons(ETH_P_IPV6))
 478                 ip_val = parse_ipv6(config, skb, &offset, ingress, &dscp);
 479         else
 480                 return TC_ACT_OK;
 481
 482         if (ip_val) {
 483                 if (!ip_val->seen)
 484                         ip_val->seen = 1;
 485                 dscp = ip_val->dscp;
 486         }
 487
 488         if (dscp_lookup_class(&dscp, ingress, &class))
 489                 return TC_ACT_OK;
 490
 491         if (class) {
 492                 check_flow(&class->config, skb, &dscp);
 493
 494                 if (dscp_lookup_class(&dscp, ingress, &class))
 495                         return TC_ACT_OK;
 496         }
 497
 498         dscp &= GENMASK(5, 0);
 499         dscp <<= 2;
 500         force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
 501
 502         if (type == bpf_htons(ETH_P_IP))
 503                 ipv4_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 504         else if (type == bpf_htons(ETH_P_IPV6))
 505                 ipv6_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 506
 507         return TC_ACT_OK;
 508 }
 509
 510 char _license[] SEC("license") = "GPL";