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         __u8 dscp;
  37         __u8 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 typedef 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 } 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, __u8);
  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, __u8);
  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 void
 145 ipv4_change_dsfield(struct iphdr *iph, __u8 mask, __u8 value, bool force)
 146 {
 147         __u32 check = bpf_ntohs(iph->check);
 148         __u8 dsfield;
 149
 150         if ((iph->tos & mask) && !force)
 151                 return;
 152
 153         dsfield = (iph->tos & mask) | value;
 154         if (iph->tos == dsfield)
 155                 return;
 156
 157         check += iph->tos;
 158         if ((check + 1) >> 16)
 159                 check = (check + 1) & 0xffff;
 160         check -= dsfield;
 161         check += check >> 16;
 162         iph->check = bpf_htons(check);
 163         iph->tos = dsfield;
 164 }
 165
 166 static __always_inline void
 167 ipv6_change_dsfield(struct ipv6hdr *ipv6h, __u8 mask, __u8 value, bool force)
 168 {
 169         __u16 *p = (__u16 *)ipv6h;
 170         __u16 val;
 171
 172         if (((*p >> 4) & mask) && !force)
 173                 return;
 174
 175         val = (*p & bpf_htons((((__u16)mask << 4) | 0xf00f))) | bpf_htons((__u16)value << 4);
 176         if (val == *p)
 177                 return;
 178
 179         *p = val;
 180 }
 181
 182 static __always_inline int
 183 parse_ethernet(struct __sk_buff *skb, __u32 *offset)
 184 {
 185         struct ethhdr *eth;
 186         __u16 h_proto;
 187         int i;
 188
 189         eth = skb_ptr(skb, *offset);
 190         if (skb_check(skb, eth + 1))
 191                 return -1;
 192
 193         h_proto = eth->h_proto;
 194         *offset += sizeof(*eth);
 195
 196 #pragma unroll
 197         for (i = 0; i < 2; i++) {
 198                 struct vlan_hdr *vlh = skb_ptr(skb, *offset);
 199
 200                 if (!proto_is_vlan(h_proto))
 201                         break;
 202
 203                 if (skb_check(skb, vlh + 1))
 204                         return -1;
 205
 206                 h_proto = vlh->h_vlan_encapsulated_proto;
 207                 *offset += sizeof(*vlh);
 208         }
 209
 210         return h_proto;
 211 }
 212
 213 static void
 214 parse_l4proto(struct qosify_config *config, struct __sk_buff *skb,
 215               __u32 offset, __u8 proto, __u8 *dscp_out)
 216 {
 217         struct udphdr *udp;
 218         __u32 src, dest, key;
 219         __u8 *value;
 220
 221         udp = skb_ptr(skb, offset);
 222         if (skb_check(skb, &udp->len))
 223                 return;
 224
 225         if (config && (proto == IPPROTO_ICMP || proto == IPPROTO_ICMPV6)) {
 226                 *dscp_out = config->dscp_icmp;
 227                 return;
 228         }
 229
 230         src = udp->source;
 231         dest = udp->dest;
 232
 233         if (module_flags & QOSIFY_INGRESS)
 234                 key = src;
 235         else
 236                 key = dest;
 237
 238         if (proto == IPPROTO_TCP) {
 239                 value = bpf_map_lookup_elem(&tcp_ports, &key);
 240         } else {
 241                 if (proto != IPPROTO_UDP)
 242                         key = 0;
 243
 244                 value = bpf_map_lookup_elem(&udp_ports, &key);
 245         }
 246
 247         if (!value)
 248                 return;
 249
 250         *dscp_out = *value;
 251 }
 252
 253 static void
 254 check_flow(struct qosify_config *config, struct __sk_buff *skb,
 255            uint8_t *dscp)
 256 {
 257         struct flow_bucket flow_data;
 258         struct flow_bucket *flow;
 259         __s32 delta;
 260         __u32 hash;
 261         __u32 time;
 262
 263         if (!(*dscp & QOSIFY_DSCP_DEFAULT_FLAG))
 264                 return;
 265
 266         if (!config)
 267                 return;
 268
 269         if (!config->bulk_trigger_pps &&
 270             !config->prio_max_avg_pkt_len)
 271                 return;
 272
 273         time = cur_time();
 274         hash = bpf_get_hash_recalc(skb);
 275         flow = bpf_map_lookup_elem(&flow_map, &hash);
 276         if (!flow) {
 277                 memset(&flow_data, 0, sizeof(flow_data));
 278                 bpf_map_update_elem(&flow_map, &hash, &flow_data, BPF_ANY);
 279                 flow = bpf_map_lookup_elem(&flow_map, &hash);
 280                 if (!flow)
 281                         return;
 282         }
 283
 284         if (!flow->last_update)
 285                 goto reset;
 286
 287         delta = time - flow->last_update;
 288         if ((u32)delta > FLOW_TIMEOUT)
 289                 goto reset;
 290
 291         if (delta >= FLOW_CHECK_INTERVAL) {
 292                 if (flow->bulk_timeout) {
 293                         flow->bulk_timeout--;
 294                         if (!flow->bulk_timeout)
 295                                 flow->dscp = 0xff;
 296                 }
 297
 298                 goto clear;
 299         }
 300
 301         if (flow->pkt_count < 0xffff)
 302                 flow->pkt_count++;
 303
 304         if (config->bulk_trigger_pps &&
 305             flow->pkt_count > config->bulk_trigger_pps) {
 306                 flow->dscp = config->dscp_bulk;
 307                 flow->bulk_timeout = config->bulk_trigger_timeout;
 308         }
 309
 310 out:
 311         if (config->prio_max_avg_pkt_len &&
 312             flow->dscp != config->dscp_bulk) {
 313                 if (ewma(&flow->pkt_len_avg, skb->len) <
 314                     config->prio_max_avg_pkt_len)
 315                         flow->dscp = config->dscp_prio;
 316                 else
 317                         flow->dscp = 0xff;
 318         }
 319
 320         if (flow->dscp != 0xff)
 321                 *dscp = flow->dscp;
 322
 323         return;
 324
 325 reset:
 326         flow->dscp = 0xff;
 327         flow->pkt_len_avg = 0;
 328 clear:
 329         flow->pkt_count = 1;
 330         flow->last_update = time;
 331
 332         goto out;
 333 }
 334
 335 static __always_inline void
 336 parse_ipv4(struct __sk_buff *skb, __u32 *offset)
 337 {
 338         struct qosify_config *config;
 339         const __u32 zero_port = 0;
 340         struct iphdr *iph;
 341         __u8 dscp = 0xff;
 342         __u8 *value;
 343         __u8 ipproto;
 344         int hdr_len;
 345         void *key;
 346         bool force;
 347
 348         config = get_config();
 349
 350         iph = skb_ptr(skb, *offset);
 351         if (skb_check(skb, iph + 1))
 352                 return;
 353
 354         hdr_len = iph->ihl * 4;
 355         if (bpf_skb_pull_data(skb, *offset + hdr_len + sizeof(struct udphdr)))
 356                 return;
 357
 358         iph = skb_ptr(skb, *offset);
 359         *offset += hdr_len;
 360
 361         if (skb_check(skb, (void *)(iph + 1)))
 362                 return;
 363
 364         ipproto = iph->protocol;
 365         parse_l4proto(config, skb, *offset, ipproto, &dscp);
 366
 367         if (module_flags & QOSIFY_INGRESS)
 368                 key = &iph->saddr;
 369         else
 370                 key = &iph->daddr;
 371
 372         value = bpf_map_lookup_elem(&ipv4_map, key);
 373         /* use udp port 0 entry as fallback for non-tcp/udp */
 374         if (!value && dscp == 0xff)
 375                 value = bpf_map_lookup_elem(&udp_ports, &zero_port);
 376         if (value)
 377                 dscp = *value;
 378
 379         check_flow(config, skb, &dscp);
 380
 381         force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
 382         dscp &= GENMASK(5, 0);
 383
 384         ipv4_change_dsfield(iph, INET_ECN_MASK, dscp << 2, force);
 385 }
 386
 387 static __always_inline void
 388 parse_ipv6(struct __sk_buff *skb, __u32 *offset)
 389 {
 390         struct qosify_config *config;
 391         const __u32 zero_port = 0;
 392         struct ipv6hdr *iph;
 393         __u8 dscp = 0;
 394         __u8 *value;
 395         __u8 ipproto;
 396         void *key;
 397         bool force;
 398
 399         config = get_config();
 400
 401         if (bpf_skb_pull_data(skb, *offset + sizeof(*iph) + sizeof(struct udphdr)))
 402                 return;
 403
 404         iph = skb_ptr(skb, *offset);
 405         *offset += sizeof(*iph);
 406
 407         if (skb_check(skb, (void *)(iph + 1)))
 408                 return;
 409
 410         ipproto = iph->nexthdr;
 411         if (module_flags & QOSIFY_INGRESS)
 412                 key = &iph->saddr;
 413         else
 414                 key = &iph->daddr;
 415
 416         parse_l4proto(config, skb, *offset, ipproto, &dscp);
 417
 418         value = bpf_map_lookup_elem(&ipv6_map, key);
 419
 420         /* use udp port 0 entry as fallback for non-tcp/udp */
 421         if (!value)
 422                 value = bpf_map_lookup_elem(&udp_ports, &zero_port);
 423         if (value)
 424                 dscp = *value;
 425
 426         check_flow(config, skb, &dscp);
 427
 428         force = !(dscp & QOSIFY_DSCP_FALLBACK_FLAG);
 429         dscp &= GENMASK(5, 0);
 430
 431         ipv6_change_dsfield(iph, INET_ECN_MASK, dscp << 2, force);
 432 }
 433
 434 SEC("classifier")
 435 int classify(struct __sk_buff *skb)
 436 {
 437         __u32 offset = 0;
 438         int type;
 439
 440         if (module_flags & QOSIFY_IP_ONLY)
 441                 type = skb->protocol;
 442         else
 443                 type = parse_ethernet(skb, &offset);
 444
 445         if (type == bpf_htons(ETH_P_IP))
 446                 parse_ipv4(skb, &offset);
 447         else if (type == bpf_htons(ETH_P_IPV6))
 448                 parse_ipv6(skb, &offset);
 449
 450         return TC_ACT_OK;
 451 }
 452
 453 char _license[] SEC("license") = "GPL";