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