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