bpf/progs/test_xdp.c

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