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 "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 static struct qosify_config *get_config(void)
 100 {
 101         __u32 key = 0;
 102
 103         return bpf_map_lookup_elem(&config, &key);
 104 }
 105
 106 static __always_inline __u32 cur_time(void)
 107 {
 108         __u32 val = bpf_ktime_get_ns() >> 24;
 109
 110         if (!val)
 111                 val = 1;
 112
 113         return val;
 114 }
 115
 116 static __always_inline __u32 ewma(__u32 *avg, __u32 val)
 117 {
 118         if (*avg)
 119                 *avg = (*avg * 3) / 4 + (val << EWMA_SHIFT) / 4;
 120         else
 121                 *avg = val << EWMA_SHIFT;
 122
 123         return *avg >> EWMA_SHIFT;
 124 }
 125
 126 static __always_inline __u8 dscp_val(struct qosify_dscp_val *val, bool ingress)
 127 {
 128         __u8 ival = val->ingress;
 129         __u8 eval = val->egress;
 130
 131         return ingress ? ival : eval;
 132 }
 133
 134 static __always_inline void
 135 ipv4_change_dsfield(struct __sk_buff *skb, __u32 offset,
 136                     __u8 mask, __u8 value, bool force)
 137 {
 138         struct iphdr *iph;
 139         __u32 check;
 140         __u8 dsfield;
 141
 142         iph = skb_ptr(skb, offset, sizeof(*iph));
 143         if (!iph)
 144                 return;
 145
 146         check = bpf_ntohs(iph->check);
 147         if ((iph->tos & mask) && !force)
 148                 return;
 149
 150         dsfield = (iph->tos & mask) | value;
 151         if (iph->tos == dsfield)
 152                 return;
 153
 154         check += iph->tos;
 155         if ((check + 1) >> 16)
 156                 check = (check + 1) & 0xffff;
 157         check -= dsfield;
 158         check += check >> 16;
 159         iph->check = bpf_htons(check);
 160         iph->tos = dsfield;
 161 }
 162
 163 static __always_inline void
 164 ipv6_change_dsfield(struct __sk_buff *skb, __u32 offset,
 165                     __u8 mask, __u8 value, bool force)
 166 {
 167         struct ipv6hdr *ipv6h;
 168         __u16 *p;
 169         __u16 val;
 170
 171         ipv6h = skb_ptr(skb, offset, sizeof(*ipv6h));
 172         if (!ipv6h)
 173                 return;
 174
 175         p = (__u16 *)ipv6h;
 176         if (((*p >> 4) & mask) && !force)
 177                 return;
 178
 179         val = (*p & bpf_htons((((__u16)mask << 4) | 0xf00f))) | bpf_htons((__u16)value << 4);
 180         if (val == *p)
 181                 return;
 182
 183         *p = val;
 184 }
 185
 186 static void
 187 parse_l4proto(struct qosify_config *config, struct skb_parser_info *info,
 188               bool ingress, __u8 *out_val)
 189 {
 190         struct udphdr *udp;
 191         __u32 src, dest, key;
 192         __u8 *value;
 193         __u8 proto = info->proto;
 194
 195         udp = skb_info_ptr(info, sizeof(*udp));
 196         if (!udp)
 197                 return;
 198
 199         if (config && (proto == IPPROTO_ICMP || proto == IPPROTO_ICMPV6)) {
 200                 *out_val = config->dscp_icmp;
 201                 return;
 202         }
 203
 204         src = READ_ONCE(udp->source);
 205         dest = READ_ONCE(udp->dest);
 206         if (ingress)
 207                 key = src;
 208         else
 209                 key = dest;
 210
 211         if (proto == IPPROTO_TCP) {
 212                 value = bpf_map_lookup_elem(&tcp_ports, &key);
 213         } else {
 214                 if (proto != IPPROTO_UDP)
 215                         key = 0;
 216
 217                 value = bpf_map_lookup_elem(&udp_ports, &key);
 218         }
 219
 220         if (value)
 221                 *out_val = *value;
 222 }
 223
 224 static __always_inline void
 225 check_flow_bulk(struct qosify_flow_config *config, struct __sk_buff *skb,
 226                 struct flow_bucket *flow, __u8 *out_val)
 227 {
 228         bool trigger = false;
 229         __s32 delta;
 230         __u32 time;
 231         int segs = 1;
 232
 233         if (!config->bulk_trigger_pps)
 234                 return;
 235
 236         time = cur_time();
 237         if (!flow->last_update)
 238                 goto reset;
 239
 240         delta = time - flow->last_update;
 241         if ((u32)delta > FLOW_TIMEOUT)
 242                 goto reset;
 243
 244         if (skb->gso_segs)
 245                 segs = skb->gso_segs;
 246         flow->pkt_count += segs;
 247         if (flow->pkt_count > config->bulk_trigger_pps) {
 248                 flow->bulk_timeout = config->bulk_trigger_timeout + 1;
 249                 trigger = true;
 250         }
 251
 252         if (delta >= FLOW_CHECK_INTERVAL) {
 253                 if (flow->bulk_timeout && !trigger)
 254                         flow->bulk_timeout--;
 255
 256                 goto clear;
 257         }
 258
 259         goto out;
 260
 261 reset:
 262         flow->pkt_len_avg = 0;
 263 clear:
 264         flow->pkt_count = 1;
 265         flow->last_update = time;
 266 out:
 267         if (flow->bulk_timeout)
 268                 *out_val = config->dscp_bulk;
 269 }
 270
 271 static __always_inline void
 272 check_flow_prio(struct qosify_flow_config *config, struct __sk_buff *skb,
 273                 struct flow_bucket *flow, __u8 *out_val)
 274 {
 275         int cur_len = skb->len;
 276
 277         if (flow->bulk_timeout)
 278                 return;
 279
 280         if (!config->prio_max_avg_pkt_len)
 281                 return;
 282
 283         if (skb->gso_segs > 1)
 284                 cur_len /= skb->gso_segs;
 285
 286         if (ewma(&flow->pkt_len_avg, cur_len) <= config->prio_max_avg_pkt_len)
 287                 *out_val = config->dscp_prio;
 288 }
 289
 290 static __always_inline void
 291 check_flow(struct qosify_flow_config *config, struct __sk_buff *skb,
 292            __u8 *out_val)
 293 {
 294         struct flow_bucket flow_data;
 295         struct flow_bucket *flow;
 296         __u32 hash;
 297
 298         if (!config)
 299                 return;
 300
 301         hash = bpf_get_hash_recalc(skb);
 302         flow = bpf_map_lookup_elem(&flow_map, &hash);
 303         if (!flow) {
 304                 memset(&flow_data, 0, sizeof(flow_data));
 305                 bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY);
 306                 flow = bpf_map_lookup_elem(&flow_map, &hash);
 307                 if (!flow)
 308                         return;
 309         }
 310
 311         check_flow_bulk(config, skb, flow, out_val);
 312         check_flow_prio(config, skb, flow, out_val);
 313 }
 314
 315 static __always_inline struct qosify_ip_map_val *
 316 parse_ipv4(struct qosify_config *config, struct skb_parser_info *info,
 317            bool ingress, __u8 *out_val)
 318 {
 319         struct iphdr *iph;
 320         __u8 ipproto;
 321         int hdr_len;
 322         void *key;
 323
 324         iph = skb_parse_ipv4(info, sizeof(struct udphdr));
 325         if (!iph)
 326                 return NULL;
 327
 328         parse_l4proto(config, info, ingress, out_val);
 329
 330         if (ingress)
 331                 key = &iph->saddr;
 332         else
 333                 key = &iph->daddr;
 334
 335         return bpf_map_lookup_elem(&ipv4_map, key);
 336 }
 337
 338 static __always_inline struct qosify_ip_map_val *
 339 parse_ipv6(struct qosify_config *config, struct skb_parser_info *info,
 340            bool ingress, __u8 *out_val)
 341 {
 342         struct ipv6hdr *iph;
 343         __u8 ipproto;
 344         void *key;
 345
 346         iph = skb_parse_ipv6(info, sizeof(struct udphdr));
 347         if (!iph)
 348                 return NULL;
 349
 350         if (ingress)
 351                 key = &iph->saddr;
 352         else
 353                 key = &iph->daddr;
 354
 355         parse_l4proto(config, info, ingress, out_val);
 356
 357         return bpf_map_lookup_elem(&ipv6_map, key);
 358 }
 359
 360 static __always_inline int
 361 dscp_lookup_class(uint8_t *dscp, bool ingress, struct qosify_class **out_class)
 362 {
 363         struct qosify_class *class;
 364         __u8 fallback_flag;
 365         __u32 key;
 366
 367         if (!(*dscp & QOSIFY_DSCP_CLASS_FLAG))
 368                 return 0;
 369
 370         fallback_flag = *dscp & QOSIFY_DSCP_FALLBACK_FLAG;
 371         key = *dscp & QOSIFY_DSCP_VALUE_MASK;
 372         class = bpf_map_lookup_elem(&class_map, &key);
 373         if (!class)
 374                 return -1;
 375
 376         if (!(class->flags & QOSIFY_CLASS_FLAG_PRESENT))
 377                 return -1;
 378
 379         *dscp = dscp_val(&class->val, ingress);
 380         *dscp |= fallback_flag;
 381         *out_class = class;
 382
 383         return 0;
 384 }
 385
 386 SEC("tc")
 387 int classify(struct __sk_buff *skb)
 388 {
 389         struct skb_parser_info info;
 390         bool ingress = module_flags & QOSIFY_INGRESS;
 391         struct qosify_config *config;
 392         struct qosify_class *class = NULL;
 393         struct qosify_ip_map_val *ip_val;
 394         __u32 iph_offset;
 395         __u8 dscp = 0;
 396         void *iph;
 397         bool force;
 398         int type;
 399
 400         config = get_config();
 401         if (!config)
 402                 return TC_ACT_UNSPEC;
 403
 404         skb_parse_init(&info, skb);
 405         if (module_flags & QOSIFY_IP_ONLY) {
 406                 type = info.proto = skb->protocol;
 407         } else if (skb_parse_ethernet(&info)) {
 408                 skb_parse_vlan(&info);
 409                 skb_parse_vlan(&info);
 410                 type = info.proto;
 411         } else {
 412                 return TC_ACT_UNSPEC;
 413         }
 414
 415         iph_offset = info.offset;
 416         if (type == bpf_htons(ETH_P_IP))
 417                 ip_val = parse_ipv4(config, &info, ingress, &dscp);
 418         else if (type == bpf_htons(ETH_P_IPV6))
 419                 ip_val = parse_ipv6(config, &info, ingress, &dscp);
 420         else
 421                 return TC_ACT_UNSPEC;
 422
 423         if (ip_val) {
 424                 if (!ip_val->seen)
 425                         ip_val->seen = 1;
 426                 dscp = ip_val->dscp;
 427         }
 428
 429         if (dscp_lookup_class(&dscp, ingress, &class))
 430                 return TC_ACT_UNSPEC;
 431
 432         if (class) {
 433                 check_flow(&class->config, skb, &dscp);
 434
 435                 if (dscp_lookup_class(&dscp, ingress, &class))
 436                         return TC_ACT_UNSPEC;
 437         }
 438
 439         dscp &= GENMASK(5, 0);
 440         dscp <<= 2;
 441         force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
 442
 443         if (type == bpf_htons(ETH_P_IP))
 444                 ipv4_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 445         else if (type == bpf_htons(ETH_P_IPV6))
 446                 ipv6_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 447
 448         return TC_ACT_UNSPEC;
 449 }
 450
 451 char _license[] SEC("license") = "GPL";