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