1 // SPDX-License-Identifier: GPL-2.0 2 3 /* In-place tunneling */ 4 5 #include <stdbool.h> 6 #include <string.h> 7 8 #include <linux/stddef.h> 9 #include <linux/bpf.h> 10 #include <linux/if_ether.h> 11 #include <linux/in.h> 12 #include <linux/ip.h> 13 #include <linux/ipv6.h> 14 #include <linux/tcp.h> 15 #include <linux/pkt_cls.h> 16 #include <linux/types.h> 17 18 #include "bpf_endian.h" 19 #include "bpf_helpers.h" 20 21 static const int cfg_port = 8000; 22 23 struct grev4hdr { 24 struct iphdr ip; 25 __be16 flags; 26 __be16 protocol; 27 } __attribute__((packed)); 28 29 struct grev6hdr { 30 struct ipv6hdr ip; 31 __be16 flags; 32 __be16 protocol; 33 } __attribute__((packed)); 34 35 static __always_inline void set_ipv4_csum(struct iphdr *iph) 36 { 37 __u16 *iph16 = (__u16 *)iph; 38 __u32 csum; 39 int i; 40 41 iph->check = 0; 42 43 #pragma clang loop unroll(full) 44 for (i = 0, csum = 0; i < sizeof(*iph) >> 1; i++) 45 csum += *iph16++; 46 47 iph->check = ~((csum & 0xffff) + (csum >> 16)); 48 } 49 50 static __always_inline int encap_ipv4(struct __sk_buff *skb, bool with_gre) 51 { 52 struct grev4hdr h_outer; 53 struct iphdr iph_inner; 54 struct tcphdr tcph; 55 __u64 flags; 56 int olen; 57 58 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner, 59 sizeof(iph_inner)) < 0) 60 return TC_ACT_OK; 61 62 /* filter only packets we want */ 63 if (iph_inner.ihl != 5 || iph_inner.protocol != IPPROTO_TCP) 64 return TC_ACT_OK; 65 66 if (bpf_skb_load_bytes(skb, ETH_HLEN + sizeof(iph_inner), 67 &tcph, sizeof(tcph)) < 0) 68 return TC_ACT_OK; 69 70 if (tcph.dest != __bpf_constant_htons(cfg_port)) 71 return TC_ACT_OK; 72 73 flags = BPF_F_ADJ_ROOM_FIXED_GSO | BPF_F_ADJ_ROOM_ENCAP_L3_IPV4; 74 if (with_gre) { 75 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_GRE; 76 olen = sizeof(h_outer); 77 } else { 78 olen = sizeof(h_outer.ip); 79 } 80 81 /* add room between mac and network header */ 82 if (bpf_skb_adjust_room(skb, olen, BPF_ADJ_ROOM_MAC, flags)) 83 return TC_ACT_SHOT; 84 85 /* prepare new outer network header */ 86 h_outer.ip = iph_inner; 87 h_outer.ip.tot_len = bpf_htons(olen + 88 bpf_htons(h_outer.ip.tot_len)); 89 if (with_gre) { 90 h_outer.ip.protocol = IPPROTO_GRE; 91 h_outer.protocol = bpf_htons(ETH_P_IP); 92 h_outer.flags = 0; 93 } else { 94 h_outer.ip.protocol = IPPROTO_IPIP; 95 } 96 97 set_ipv4_csum((void *)&h_outer.ip); 98 99 /* store new outer network header */ 100 if (bpf_skb_store_bytes(skb, ETH_HLEN, &h_outer, olen, 101 BPF_F_INVALIDATE_HASH) < 0) 102 return TC_ACT_SHOT; 103 104 return TC_ACT_OK; 105 } 106 107 static __always_inline int encap_ipv6(struct __sk_buff *skb, bool with_gre) 108 { 109 struct ipv6hdr iph_inner; 110 struct grev6hdr h_outer; 111 struct tcphdr tcph; 112 __u64 flags; 113 int olen; 114 115 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner, 116 sizeof(iph_inner)) < 0) 117 return TC_ACT_OK; 118 119 /* filter only packets we want */ 120 if (bpf_skb_load_bytes(skb, ETH_HLEN + sizeof(iph_inner), 121 &tcph, sizeof(tcph)) < 0) 122 return TC_ACT_OK; 123 124 if (tcph.dest != __bpf_constant_htons(cfg_port)) 125 return TC_ACT_OK; 126 127 flags = BPF_F_ADJ_ROOM_FIXED_GSO | BPF_F_ADJ_ROOM_ENCAP_L3_IPV6; 128 if (with_gre) { 129 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_GRE; 130 olen = sizeof(h_outer); 131 } else { 132 olen = sizeof(h_outer.ip); 133 } 134 135 136 /* add room between mac and network header */ 137 if (bpf_skb_adjust_room(skb, olen, BPF_ADJ_ROOM_MAC, flags)) 138 return TC_ACT_SHOT; 139 140 /* prepare new outer network header */ 141 h_outer.ip = iph_inner; 142 h_outer.ip.payload_len = bpf_htons(olen + 143 bpf_ntohs(h_outer.ip.payload_len)); 144 if (with_gre) { 145 h_outer.ip.nexthdr = IPPROTO_GRE; 146 h_outer.protocol = bpf_htons(ETH_P_IPV6); 147 h_outer.flags = 0; 148 } else { 149 h_outer.ip.nexthdr = IPPROTO_IPV6; 150 } 151 152 /* store new outer network header */ 153 if (bpf_skb_store_bytes(skb, ETH_HLEN, &h_outer, olen, 154 BPF_F_INVALIDATE_HASH) < 0) 155 return TC_ACT_SHOT; 156 157 return TC_ACT_OK; 158 } 159 160 SEC("encap_ipip") 161 int __encap_ipip(struct __sk_buff *skb) 162 { 163 if (skb->protocol == __bpf_constant_htons(ETH_P_IP)) 164 return encap_ipv4(skb, false); 165 else 166 return TC_ACT_OK; 167 } 168 169 SEC("encap_gre") 170 int __encap_gre(struct __sk_buff *skb) 171 { 172 if (skb->protocol == __bpf_constant_htons(ETH_P_IP)) 173 return encap_ipv4(skb, true); 174 else 175 return TC_ACT_OK; 176 } 177 178 SEC("encap_ip6tnl") 179 int __encap_ip6tnl(struct __sk_buff *skb) 180 { 181 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6)) 182 return encap_ipv6(skb, false); 183 else 184 return TC_ACT_OK; 185 } 186 187 SEC("encap_ip6gre") 188 int __encap_ip6gre(struct __sk_buff *skb) 189 { 190 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6)) 191 return encap_ipv6(skb, true); 192 else 193 return TC_ACT_OK; 194 } 195 196 static int decap_internal(struct __sk_buff *skb, int off, int len, char proto) 197 { 198 char buf[sizeof(struct grev6hdr)]; 199 int olen; 200 201 switch (proto) { 202 case IPPROTO_IPIP: 203 case IPPROTO_IPV6: 204 olen = len; 205 break; 206 case IPPROTO_GRE: 207 olen = len + 4 /* gre hdr */; 208 break; 209 default: 210 return TC_ACT_OK; 211 } 212 213 if (bpf_skb_adjust_room(skb, -olen, BPF_ADJ_ROOM_MAC, 214 BPF_F_ADJ_ROOM_FIXED_GSO)) 215 return TC_ACT_SHOT; 216 217 return TC_ACT_OK; 218 } 219 220 static int decap_ipv4(struct __sk_buff *skb) 221 { 222 struct iphdr iph_outer; 223 224 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer, 225 sizeof(iph_outer)) < 0) 226 return TC_ACT_OK; 227 228 if (iph_outer.ihl != 5) 229 return TC_ACT_OK; 230 231 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer), 232 iph_outer.protocol); 233 } 234 235 static int decap_ipv6(struct __sk_buff *skb) 236 { 237 struct ipv6hdr iph_outer; 238 239 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer, 240 sizeof(iph_outer)) < 0) 241 return TC_ACT_OK; 242 243 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer), 244 iph_outer.nexthdr); 245 } 246 247 SEC("decap") 248 int decap_f(struct __sk_buff *skb) 249 { 250 switch (skb->protocol) { 251 case __bpf_constant_htons(ETH_P_IP): 252 return decap_ipv4(skb); 253 case __bpf_constant_htons(ETH_P_IPV6): 254 return decap_ipv6(skb); 255 default: 256 /* does not match, ignore */ 257 return TC_ACT_OK; 258 } 259 } 260 261 char __license[] SEC("license") = "GPL"; 262