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         int cur_len = skb->len;
 331
 332         if (flow->bulk_timeout)
 333                 return;
 334
 335         if (!config->prio_max_avg_pkt_len)
 336                 return;
 337
 338         if (skb->gso_size && skb->gso_segs > 1)
 339                 cur_len /= skb->gso_segs;
 340
 341         if (ewma(&flow->pkt_len_avg, cur_len) <= config->prio_max_avg_pkt_len)
 342                 *out_val = config->dscp_prio;
 343 }
 344
 345 static __always_inline void
 346 check_flow(struct qosify_flow_config *config, struct __sk_buff *skb,
 347            __u8 *out_val)
 348 {
 349         struct flow_bucket flow_data;
 350         struct flow_bucket *flow;
 351         __u32 hash;
 352
 353         if (!config)
 354                 return;
 355
 356         hash = bpf_get_hash_recalc(skb);
 357         flow = bpf_map_lookup_elem(&flow_map, &hash);
 358         if (!flow) {
 359                 memset(&flow_data, 0, sizeof(flow_data));
 360                 bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY);
 361                 flow = bpf_map_lookup_elem(&flow_map, &hash);
 362                 if (!flow)
 363                         return;
 364         }
 365
 366         check_flow_bulk(config, skb, flow, out_val);
 367         check_flow_prio(config, skb, flow, out_val);
 368 }
 369
 370 static __always_inline struct qosify_ip_map_val *
 371 parse_ipv4(struct qosify_config *config, struct __sk_buff *skb, __u32 *offset,
 372            bool ingress, __u8 *out_val)
 373 {
 374         struct iphdr *iph;
 375         __u8 ipproto;
 376         int hdr_len;
 377         void *key;
 378
 379         iph = skb_ptr(skb, *offset);
 380         if (skb_check(skb, iph + 1))
 381                 return NULL;
 382
 383         hdr_len = iph->ihl * 4;
 384         if (bpf_skb_pull_data(skb, *offset + hdr_len + sizeof(struct udphdr)))
 385                 return NULL;
 386
 387         iph = skb_ptr(skb, *offset);
 388         *offset += hdr_len;
 389
 390         if (skb_check(skb, (void *)(iph + 1)))
 391                 return NULL;
 392
 393         ipproto = iph->protocol;
 394         parse_l4proto(config, skb, *offset, ipproto, ingress, out_val);
 395
 396         if (ingress)
 397                 key = &iph->saddr;
 398         else
 399                 key = &iph->daddr;
 400
 401         return bpf_map_lookup_elem(&ipv4_map, key);
 402 }
 403
 404 static __always_inline struct qosify_ip_map_val *
 405 parse_ipv6(struct qosify_config *config, struct __sk_buff *skb, __u32 *offset,
 406            bool ingress, __u8 *out_val)
 407 {
 408         struct ipv6hdr *iph;
 409         __u8 ipproto;
 410         void *key;
 411
 412         if (bpf_skb_pull_data(skb, *offset + sizeof(*iph) + sizeof(struct udphdr)))
 413                 return NULL;
 414
 415         iph = skb_ptr(skb, *offset);
 416         *offset += sizeof(*iph);
 417
 418         if (skb_check(skb, (void *)(iph + 1)))
 419                 return NULL;
 420
 421         ipproto = iph->nexthdr;
 422         if (ingress)
 423                 key = &iph->saddr;
 424         else
 425                 key = &iph->daddr;
 426
 427         parse_l4proto(config, skb, *offset, ipproto, ingress, out_val);
 428
 429         return bpf_map_lookup_elem(&ipv6_map, key);
 430 }
 431
 432 static __always_inline int
 433 dscp_lookup_class(uint8_t *dscp, bool ingress, struct qosify_class **out_class)
 434 {
 435         struct qosify_class *class;
 436         __u8 fallback_flag;
 437         __u32 key;
 438
 439         if (!(*dscp & QOSIFY_DSCP_CLASS_FLAG))
 440                 return 0;
 441
 442         fallback_flag = *dscp & QOSIFY_DSCP_FALLBACK_FLAG;
 443         key = *dscp & QOSIFY_DSCP_VALUE_MASK;
 444         class = bpf_map_lookup_elem(&class_map, &key);
 445         if (!class)
 446                 return -1;
 447
 448         if (!(class->flags & QOSIFY_CLASS_FLAG_PRESENT))
 449                 return -1;
 450
 451         *dscp = dscp_val(&class->val, ingress);
 452         *dscp |= fallback_flag;
 453         *out_class = class;
 454
 455         return 0;
 456 }
 457
 458 SEC("classifier")
 459 int classify(struct __sk_buff *skb)
 460 {
 461         bool ingress = module_flags & QOSIFY_INGRESS;
 462         struct qosify_config *config;
 463         struct qosify_class *class = NULL;
 464         struct qosify_ip_map_val *ip_val;
 465         __u32 offset = 0;
 466         __u32 iph_offset;
 467         void *iph;
 468         __u8 dscp;
 469         bool force;
 470         int type;
 471
 472         config = get_config();
 473         if (!config)
 474                 return TC_ACT_OK;
 475
 476         if (module_flags & QOSIFY_IP_ONLY)
 477                 type = skb->protocol;
 478         else
 479                 type = parse_ethernet(skb, &offset);
 480
 481         iph_offset = offset;
 482         if (type == bpf_htons(ETH_P_IP))
 483                 ip_val = parse_ipv4(config, skb, &offset, ingress, &dscp);
 484         else if (type == bpf_htons(ETH_P_IPV6))
 485                 ip_val = parse_ipv6(config, skb, &offset, ingress, &dscp);
 486         else
 487                 return TC_ACT_OK;
 488
 489         if (ip_val) {
 490                 if (!ip_val->seen)
 491                         ip_val->seen = 1;
 492                 dscp = ip_val->dscp;
 493         }
 494
 495         if (dscp_lookup_class(&dscp, ingress, &class))
 496                 return TC_ACT_OK;
 497
 498         if (class) {
 499                 check_flow(&class->config, skb, &dscp);
 500
 501                 if (dscp_lookup_class(&dscp, ingress, &class))
 502                         return TC_ACT_OK;
 503         }
 504
 505         dscp &= GENMASK(5, 0);
 506         dscp <<= 2;
 507         force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
 508
 509         if (type == bpf_htons(ETH_P_IP))
 510                 ipv4_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 511         else if (type == bpf_htons(ETH_P_IPV6))
 512                 ipv6_change_dsfield(skb, iph_offset, INET_ECN_MASK, dscp, force);
 513
 514         return TC_ACT_OK;
 515 }
 516
 517 char _license[] SEC("license") = "GPL";