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