tools/testing/selftests/bpf/progs/test_xdp_dynptr.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2022 Meta */
   3 #include <stddef.h>
   4 #include <string.h>
   5 #include <stdbool.h>
   6 #include <linux/bpf.h>
   7 #include <linux/if_ether.h>
   8 #include <linux/if_packet.h>
   9 #include <linux/ip.h>
  10 #include <linux/ipv6.h>
  11 #include <linux/in.h>
  12 #include <linux/udp.h>
  13 #include <linux/tcp.h>
  14 #include <linux/pkt_cls.h>
  15 #include <sys/socket.h>
  16 #include <bpf/bpf_helpers.h>
  17 #include <bpf/bpf_endian.h>
  18 #include "test_iptunnel_common.h"
  19 #include "bpf_kfuncs.h"
  20
  21 const size_t tcphdr_sz = sizeof(struct tcphdr);
  22 const size_t udphdr_sz = sizeof(struct udphdr);
  23 const size_t ethhdr_sz = sizeof(struct ethhdr);
  24 const size_t iphdr_sz = sizeof(struct iphdr);
  25 const size_t ipv6hdr_sz = sizeof(struct ipv6hdr);
  26
  27 struct {
  28         __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY);
  29         __uint(max_entries, 256);
  30         __type(key, __u32);
  31         __type(value, __u64);
  32 } rxcnt SEC(".maps");
  33
  34 struct {
  35         __uint(type, BPF_MAP_TYPE_HASH);
  36         __uint(max_entries, MAX_IPTNL_ENTRIES);
  37         __type(key, struct vip);
  38         __type(value, struct iptnl_info);
  39 } vip2tnl SEC(".maps");
  40
  41 static __always_inline void count_tx(__u32 protocol)
  42 {
  43         __u64 *rxcnt_count;
  44
  45         rxcnt_count = bpf_map_lookup_elem(&rxcnt, &protocol);
  46         if (rxcnt_count)
  47                 *rxcnt_count += 1;
  48 }
  49
  50 static __always_inline int get_dport(void *trans_data, __u8 protocol)
  51 {
  52         struct tcphdr *th;
  53         struct udphdr *uh;
  54
  55         switch (protocol) {
  56         case IPPROTO_TCP:
  57                 th = (struct tcphdr *)trans_data;
  58                 return th->dest;
  59         case IPPROTO_UDP:
  60                 uh = (struct udphdr *)trans_data;
  61                 return uh->dest;
  62         default:
  63                 return 0;
  64         }
  65 }
  66
  67 static __always_inline void set_ethhdr(struct ethhdr *new_eth,
  68                                        const struct ethhdr *old_eth,
  69                                        const struct iptnl_info *tnl,
  70                                        __be16 h_proto)
  71 {
  72         memcpy(new_eth->h_source, old_eth->h_dest, sizeof(new_eth->h_source));
  73         memcpy(new_eth->h_dest, tnl->dmac, sizeof(new_eth->h_dest));
  74         new_eth->h_proto = h_proto;
  75 }
  76
  77 static __always_inline int handle_ipv4(struct xdp_md *xdp, struct bpf_dynptr *xdp_ptr)
  78 {
  79         __u8 eth_buffer[ethhdr_sz + iphdr_sz + ethhdr_sz];
  80         __u8 iph_buffer_tcp[iphdr_sz + tcphdr_sz];
  81         __u8 iph_buffer_udp[iphdr_sz + udphdr_sz];
  82         struct bpf_dynptr new_xdp_ptr;
  83         struct iptnl_info *tnl;
  84         struct ethhdr *new_eth;
  85         struct ethhdr *old_eth;
  86         struct iphdr *iph;
  87         __u16 *next_iph;
  88         __u16 payload_len;
  89         struct vip vip = {};
  90         int dport;
  91         __u32 csum = 0;
  92         int i;
  93
  94         __builtin_memset(eth_buffer, 0, sizeof(eth_buffer));
  95         __builtin_memset(iph_buffer_tcp, 0, sizeof(iph_buffer_tcp));
  96         __builtin_memset(iph_buffer_udp, 0, sizeof(iph_buffer_udp));
  97
  98         if (ethhdr_sz + iphdr_sz + tcphdr_sz > xdp->data_end - xdp->data)
  99                 iph = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, iph_buffer_udp, sizeof(iph_buffer_udp));
 100         else
 101                 iph = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, iph_buffer_tcp, sizeof(iph_buffer_tcp));
 102
 103         if (!iph)
 104                 return XDP_DROP;
 105
 106         dport = get_dport(iph + 1, iph->protocol);
 107         if (dport == -1)
 108                 return XDP_DROP;
 109
 110         vip.protocol = iph->protocol;
 111         vip.family = AF_INET;
 112         vip.daddr.v4 = iph->daddr;
 113         vip.dport = dport;
 114         payload_len = bpf_ntohs(iph->tot_len);
 115
 116         tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
 117         /* It only does v4-in-v4 */
 118         if (!tnl || tnl->family != AF_INET)
 119                 return XDP_PASS;
 120
 121         if (bpf_xdp_adjust_head(xdp, 0 - (int)iphdr_sz))
 122                 return XDP_DROP;
 123
 124         bpf_dynptr_from_xdp(xdp, 0, &new_xdp_ptr);
 125         new_eth = bpf_dynptr_slice_rdwr(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer));
 126         if (!new_eth)
 127                 return XDP_DROP;
 128
 129         iph = (struct iphdr *)(new_eth + 1);
 130         old_eth = (struct ethhdr *)(iph + 1);
 131
 132         set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IP));
 133
 134         if (new_eth == eth_buffer)
 135                 bpf_dynptr_write(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer), 0);
 136
 137         iph->version = 4;
 138         iph->ihl = iphdr_sz >> 2;
 139         iph->frag_off = 0;
 140         iph->protocol = IPPROTO_IPIP;
 141         iph->check = 0;
 142         iph->tos = 0;
 143         iph->tot_len = bpf_htons(payload_len + iphdr_sz);
 144         iph->daddr = tnl->daddr.v4;
 145         iph->saddr = tnl->saddr.v4;
 146         iph->ttl = 8;
 147
 148         next_iph = (__u16 *)iph;
 149         for (i = 0; i < iphdr_sz >> 1; i++)
 150                 csum += *next_iph++;
 151
 152         iph->check = ~((csum & 0xffff) + (csum >> 16));
 153
 154         count_tx(vip.protocol);
 155
 156         return XDP_TX;
 157 }
 158
 159 static __always_inline int handle_ipv6(struct xdp_md *xdp, struct bpf_dynptr *xdp_ptr)
 160 {
 161         __u8 eth_buffer[ethhdr_sz + ipv6hdr_sz + ethhdr_sz];
 162         __u8 ip6h_buffer_tcp[ipv6hdr_sz + tcphdr_sz];
 163         __u8 ip6h_buffer_udp[ipv6hdr_sz + udphdr_sz];
 164         struct bpf_dynptr new_xdp_ptr;
 165         struct iptnl_info *tnl;
 166         struct ethhdr *new_eth;
 167         struct ethhdr *old_eth;
 168         struct ipv6hdr *ip6h;
 169         __u16 payload_len;
 170         struct vip vip = {};
 171         int dport;
 172
 173         __builtin_memset(eth_buffer, 0, sizeof(eth_buffer));
 174         __builtin_memset(ip6h_buffer_tcp, 0, sizeof(ip6h_buffer_tcp));
 175         __builtin_memset(ip6h_buffer_udp, 0, sizeof(ip6h_buffer_udp));
 176
 177         if (ethhdr_sz + iphdr_sz + tcphdr_sz > xdp->data_end - xdp->data)
 178                 ip6h = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, ip6h_buffer_udp, sizeof(ip6h_buffer_udp));
 179         else
 180                 ip6h = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, ip6h_buffer_tcp, sizeof(ip6h_buffer_tcp));
 181
 182         if (!ip6h)
 183                 return XDP_DROP;
 184
 185         dport = get_dport(ip6h + 1, ip6h->nexthdr);
 186         if (dport == -1)
 187                 return XDP_DROP;
 188
 189         vip.protocol = ip6h->nexthdr;
 190         vip.family = AF_INET6;
 191         memcpy(vip.daddr.v6, ip6h->daddr.s6_addr32, sizeof(vip.daddr));
 192         vip.dport = dport;
 193         payload_len = ip6h->payload_len;
 194
 195         tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
 196         /* It only does v6-in-v6 */
 197         if (!tnl || tnl->family != AF_INET6)
 198                 return XDP_PASS;
 199
 200         if (bpf_xdp_adjust_head(xdp, 0 - (int)ipv6hdr_sz))
 201                 return XDP_DROP;
 202
 203         bpf_dynptr_from_xdp(xdp, 0, &new_xdp_ptr);
 204         new_eth = bpf_dynptr_slice_rdwr(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer));
 205         if (!new_eth)
 206                 return XDP_DROP;
 207
 208         ip6h = (struct ipv6hdr *)(new_eth + 1);
 209         old_eth = (struct ethhdr *)(ip6h + 1);
 210
 211         set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IPV6));
 212
 213         if (new_eth == eth_buffer)
 214                 bpf_dynptr_write(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer), 0);
 215
 216         ip6h->version = 6;
 217         ip6h->priority = 0;
 218         memset(ip6h->flow_lbl, 0, sizeof(ip6h->flow_lbl));
 219         ip6h->payload_len = bpf_htons(bpf_ntohs(payload_len) + ipv6hdr_sz);
 220         ip6h->nexthdr = IPPROTO_IPV6;
 221         ip6h->hop_limit = 8;
 222         memcpy(ip6h->saddr.s6_addr32, tnl->saddr.v6, sizeof(tnl->saddr.v6));
 223         memcpy(ip6h->daddr.s6_addr32, tnl->daddr.v6, sizeof(tnl->daddr.v6));
 224
 225         count_tx(vip.protocol);
 226
 227         return XDP_TX;
 228 }
 229
 230 SEC("xdp")
 231 int _xdp_tx_iptunnel(struct xdp_md *xdp)
 232 {
 233         __u8 buffer[ethhdr_sz];
 234         struct bpf_dynptr ptr;
 235         struct ethhdr *eth;
 236         __u16 h_proto;
 237
 238         __builtin_memset(buffer, 0, sizeof(buffer));
 239
 240         bpf_dynptr_from_xdp(xdp, 0, &ptr);
 241         eth = bpf_dynptr_slice(&ptr, 0, buffer, sizeof(buffer));
 242         if (!eth)
 243                 return XDP_DROP;
 244
 245         h_proto = eth->h_proto;
 246
 247         if (h_proto == bpf_htons(ETH_P_IP))
 248                 return handle_ipv4(xdp, &ptr);
 249         else if (h_proto == bpf_htons(ETH_P_IPV6))
 250
 251                 return handle_ipv6(xdp, &ptr);
 252         else
 253                 return XDP_DROP;
 254 }
 255
 256 char _license[] SEC("license") = "GPL";