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

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