bpf/progs/test_xdp_loop.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 // Copyright (c) 2019 Facebook
0003 #include <stddef.h>
0004 #include <string.h>
0005 #include <linux/bpf.h>
0006 #include <linux/if_ether.h>
0007 #include <linux/if_packet.h>
0008 #include <linux/ip.h>
0009 #include <linux/ipv6.h>
0010 #include <linux/in.h>
0011 #include <linux/udp.h>
0012 #include <linux/tcp.h>
0013 #include <linux/pkt_cls.h>
0014 #include <sys/socket.h>
0015 #include <bpf/bpf_helpers.h>
0016 #include <bpf/bpf_endian.h>
0017 #include "test_iptunnel_common.h"
0018
0019 struct {
0020     __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY);
0021     __uint(max_entries, 256);
0022     __type(key, __u32);
0023     __type(value, __u64);
0024 } rxcnt SEC(".maps");
0025
0026 struct {
0027     __uint(type, BPF_MAP_TYPE_HASH);
0028     __uint(max_entries, MAX_IPTNL_ENTRIES);
0029     __type(key, struct vip);
0030     __type(value, struct iptnl_info);
0031 } vip2tnl SEC(".maps");
0032
0033 static __always_inline void count_tx(__u32 protocol)
0034 {
0035     __u64 *rxcnt_count;
0036
0037     rxcnt_count = bpf_map_lookup_elem(&rxcnt, &protocol);
0038     if (rxcnt_count)
0039         *rxcnt_count += 1;
0040 }
0041
0042 static __always_inline int get_dport(void *trans_data, void *data_end,
0043                      __u8 protocol)
0044 {
0045     struct tcphdr *th;
0046     struct udphdr *uh;
0047
0048     switch (protocol) {
0049     case IPPROTO_TCP:
0050         th = (struct tcphdr *)trans_data;
0051         if (th + 1 > data_end)
0052             return -1;
0053         return th->dest;
0054     case IPPROTO_UDP:
0055         uh = (struct udphdr *)trans_data;
0056         if (uh + 1 > data_end)
0057             return -1;
0058         return uh->dest;
0059     default:
0060         return 0;
0061     }
0062 }
0063
0064 static __always_inline void set_ethhdr(struct ethhdr *new_eth,
0065                        const struct ethhdr *old_eth,
0066                        const struct iptnl_info *tnl,
0067                        __be16 h_proto)
0068 {
0069     memcpy(new_eth->h_source, old_eth->h_dest, sizeof(new_eth->h_source));
0070     memcpy(new_eth->h_dest, tnl->dmac, sizeof(new_eth->h_dest));
0071     new_eth->h_proto = h_proto;
0072 }
0073
0074 static __always_inline int handle_ipv4(struct xdp_md *xdp)
0075 {
0076     void *data_end = (void *)(long)xdp->data_end;
0077     void *data = (void *)(long)xdp->data;
0078     struct iptnl_info *tnl;
0079     struct ethhdr *new_eth;
0080     struct ethhdr *old_eth;
0081     struct iphdr *iph = data + sizeof(struct ethhdr);
0082     __u16 *next_iph;
0083     __u16 payload_len;
0084     struct vip vip = {};
0085     int dport;
0086     __u32 csum = 0;
0087     int i;
0088
0089     if (iph + 1 > data_end)
0090         return XDP_DROP;
0091
0092     dport = get_dport(iph + 1, data_end, iph->protocol);
0093     if (dport == -1)
0094         return XDP_DROP;
0095
0096     vip.protocol = iph->protocol;
0097     vip.family = AF_INET;
0098     vip.daddr.v4 = iph->daddr;
0099     vip.dport = dport;
0100     payload_len = bpf_ntohs(iph->tot_len);
0101
0102     tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
0103     /* It only does v4-in-v4 */
0104     if (!tnl || tnl->family != AF_INET)
0105         return XDP_PASS;
0106
0107     if (bpf_xdp_adjust_head(xdp, 0 - (int)sizeof(struct iphdr)))
0108         return XDP_DROP;
0109
0110     data = (void *)(long)xdp->data;
0111     data_end = (void *)(long)xdp->data_end;
0112
0113     new_eth = data;
0114     iph = data + sizeof(*new_eth);
0115     old_eth = data + sizeof(*iph);
0116
0117     if (new_eth + 1 > data_end ||
0118         old_eth + 1 > data_end ||
0119         iph + 1 > data_end)
0120         return XDP_DROP;
0121
0122     set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IP));
0123
0124     iph->version = 4;
0125     iph->ihl = sizeof(*iph) >> 2;
0126     iph->frag_off = 0;
0127     iph->protocol = IPPROTO_IPIP;
0128     iph->check = 0;
0129     iph->tos = 0;
0130     iph->tot_len = bpf_htons(payload_len + sizeof(*iph));
0131     iph->daddr = tnl->daddr.v4;
0132     iph->saddr = tnl->saddr.v4;
0133     iph->ttl = 8;
0134
0135     next_iph = (__u16 *)iph;
0136 #pragma clang loop unroll(disable)
0137     for (i = 0; i < sizeof(*iph) >> 1; i++)
0138         csum += *next_iph++;
0139
0140     iph->check = ~((csum & 0xffff) + (csum >> 16));
0141
0142     count_tx(vip.protocol);
0143
0144     return XDP_TX;
0145 }
0146
0147 static __always_inline int handle_ipv6(struct xdp_md *xdp)
0148 {
0149     void *data_end = (void *)(long)xdp->data_end;
0150     void *data = (void *)(long)xdp->data;
0151     struct iptnl_info *tnl;
0152     struct ethhdr *new_eth;
0153     struct ethhdr *old_eth;
0154     struct ipv6hdr *ip6h = data + sizeof(struct ethhdr);
0155     __u16 payload_len;
0156     struct vip vip = {};
0157     int dport;
0158
0159     if (ip6h + 1 > data_end)
0160         return XDP_DROP;
0161
0162     dport = get_dport(ip6h + 1, data_end, ip6h->nexthdr);
0163     if (dport == -1)
0164         return XDP_DROP;
0165
0166     vip.protocol = ip6h->nexthdr;
0167     vip.family = AF_INET6;
0168     memcpy(vip.daddr.v6, ip6h->daddr.s6_addr32, sizeof(vip.daddr));
0169     vip.dport = dport;
0170     payload_len = ip6h->payload_len;
0171
0172     tnl = bpf_map_lookup_elem(&vip2tnl, &vip);
0173     /* It only does v6-in-v6 */
0174     if (!tnl || tnl->family != AF_INET6)
0175         return XDP_PASS;
0176
0177     if (bpf_xdp_adjust_head(xdp, 0 - (int)sizeof(struct ipv6hdr)))
0178         return XDP_DROP;
0179
0180     data = (void *)(long)xdp->data;
0181     data_end = (void *)(long)xdp->data_end;
0182
0183     new_eth = data;
0184     ip6h = data + sizeof(*new_eth);
0185     old_eth = data + sizeof(*ip6h);
0186
0187     if (new_eth + 1 > data_end || old_eth + 1 > data_end ||
0188         ip6h + 1 > data_end)
0189         return XDP_DROP;
0190
0191     set_ethhdr(new_eth, old_eth, tnl, bpf_htons(ETH_P_IPV6));
0192
0193     ip6h->version = 6;
0194     ip6h->priority = 0;
0195     memset(ip6h->flow_lbl, 0, sizeof(ip6h->flow_lbl));
0196     ip6h->payload_len = bpf_htons(bpf_ntohs(payload_len) + sizeof(*ip6h));
0197     ip6h->nexthdr = IPPROTO_IPV6;
0198     ip6h->hop_limit = 8;
0199     memcpy(ip6h->saddr.s6_addr32, tnl->saddr.v6, sizeof(tnl->saddr.v6));
0200     memcpy(ip6h->daddr.s6_addr32, tnl->daddr.v6, sizeof(tnl->daddr.v6));
0201
0202     count_tx(vip.protocol);
0203
0204     return XDP_TX;
0205 }
0206
0207 SEC("xdp")
0208 int _xdp_tx_iptunnel(struct xdp_md *xdp)
0209 {
0210     void *data_end = (void *)(long)xdp->data_end;
0211     void *data = (void *)(long)xdp->data;
0212     struct ethhdr *eth = data;
0213     __u16 h_proto;
0214
0215     if (eth + 1 > data_end)
0216         return XDP_DROP;
0217
0218     h_proto = eth->h_proto;
0219
0220     if (h_proto == bpf_htons(ETH_P_IP))
0221         return handle_ipv4(xdp);
0222     else if (h_proto == bpf_htons(ETH_P_IPV6))
0223
0224         return handle_ipv6(xdp);
0225     else
0226         return XDP_DROP;
0227 }
0228
0229 char _license[] SEC("license") = "GPL";