1 // SPDX-License-Identifier: GPL-2.0 2 3 /* In-place tunneling */ 4 5 #include <linux/stddef.h> 6 #include <linux/bpf.h> 7 #include <linux/if_ether.h> 8 #include <linux/in.h> 9 #include <linux/ip.h> 10 #include <linux/ipv6.h> 11 #include <linux/tcp.h> 12 #include <linux/pkt_cls.h> 13 #include <linux/types.h> 14 15 #include "bpf_endian.h" 16 #include "bpf_helpers.h" 17 18 static const int cfg_port = 8000; 19 20 static __always_inline void set_ipv4_csum(struct iphdr *iph) 21 { 22 __u16 *iph16 = (__u16 *)iph; 23 __u32 csum; 24 int i; 25 26 iph->check = 0; 27 28 #pragma clang loop unroll(full) 29 for (i = 0, csum = 0; i < sizeof(*iph) >> 1; i++) 30 csum += *iph16++; 31 32 iph->check = ~((csum & 0xffff) + (csum >> 16)); 33 } 34 35 static int encap_ipv4(struct __sk_buff *skb) 36 { 37 struct iphdr iph_outer, iph_inner; 38 struct tcphdr tcph; 39 40 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner, 41 sizeof(iph_inner)) < 0) 42 return TC_ACT_OK; 43 44 /* filter only packets we want */ 45 if (iph_inner.ihl != 5 || iph_inner.protocol != IPPROTO_TCP) 46 return TC_ACT_OK; 47 48 if (bpf_skb_load_bytes(skb, ETH_HLEN + sizeof(iph_inner), 49 &tcph, sizeof(tcph)) < 0) 50 return TC_ACT_OK; 51 52 if (tcph.dest != __bpf_constant_htons(cfg_port)) 53 return TC_ACT_OK; 54 55 /* add room between mac and network header */ 56 if (bpf_skb_adjust_room(skb, sizeof(iph_outer), BPF_ADJ_ROOM_NET, 0)) 57 return TC_ACT_SHOT; 58 59 /* prepare new outer network header */ 60 iph_outer = iph_inner; 61 iph_outer.protocol = IPPROTO_IPIP; 62 iph_outer.tot_len = bpf_htons(sizeof(iph_outer) + 63 bpf_htons(iph_outer.tot_len)); 64 set_ipv4_csum(&iph_outer); 65 66 /* store new outer network header */ 67 if (bpf_skb_store_bytes(skb, ETH_HLEN, &iph_outer, sizeof(iph_outer), 68 BPF_F_INVALIDATE_HASH) < 0) 69 return TC_ACT_SHOT; 70 71 /* bpf_skb_adjust_room has moved header to start of room: restore */ 72 if (bpf_skb_store_bytes(skb, ETH_HLEN + sizeof(iph_outer), 73 &iph_inner, sizeof(iph_inner), 74 BPF_F_INVALIDATE_HASH) < 0) 75 return TC_ACT_SHOT; 76 77 return TC_ACT_OK; 78 } 79 80 static int encap_ipv6(struct __sk_buff *skb) 81 { 82 struct ipv6hdr iph_outer, iph_inner; 83 struct tcphdr tcph; 84 85 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner, 86 sizeof(iph_inner)) < 0) 87 return TC_ACT_OK; 88 89 /* filter only packets we want */ 90 if (bpf_skb_load_bytes(skb, ETH_HLEN + sizeof(iph_inner), 91 &tcph, sizeof(tcph)) < 0) 92 return TC_ACT_OK; 93 94 if (tcph.dest != __bpf_constant_htons(cfg_port)) 95 return TC_ACT_OK; 96 97 /* add room between mac and network header */ 98 if (bpf_skb_adjust_room(skb, sizeof(iph_outer), BPF_ADJ_ROOM_NET, 0)) 99 return TC_ACT_SHOT; 100 101 /* prepare new outer network header */ 102 iph_outer = iph_inner; 103 iph_outer.nexthdr = IPPROTO_IPV6; 104 iph_outer.payload_len = bpf_htons(sizeof(iph_outer) + 105 bpf_ntohs(iph_outer.payload_len)); 106 107 /* store new outer network header */ 108 if (bpf_skb_store_bytes(skb, ETH_HLEN, &iph_outer, sizeof(iph_outer), 109 BPF_F_INVALIDATE_HASH) < 0) 110 return TC_ACT_SHOT; 111 112 /* bpf_skb_adjust_room has moved header to start of room: restore */ 113 if (bpf_skb_store_bytes(skb, ETH_HLEN + sizeof(iph_outer), 114 &iph_inner, sizeof(iph_inner), 115 BPF_F_INVALIDATE_HASH) < 0) 116 return TC_ACT_SHOT; 117 118 return TC_ACT_OK; 119 } 120 121 SEC("encap") 122 int encap_f(struct __sk_buff *skb) 123 { 124 switch (skb->protocol) { 125 case __bpf_constant_htons(ETH_P_IP): 126 return encap_ipv4(skb); 127 case __bpf_constant_htons(ETH_P_IPV6): 128 return encap_ipv6(skb); 129 default: 130 /* does not match, ignore */ 131 return TC_ACT_OK; 132 } 133 } 134 135 static int decap_internal(struct __sk_buff *skb, int off, int len) 136 { 137 char buf[sizeof(struct ipv6hdr)]; 138 139 if (bpf_skb_load_bytes(skb, off + len, &buf, len) < 0) 140 return TC_ACT_OK; 141 142 if (bpf_skb_adjust_room(skb, -len, BPF_ADJ_ROOM_NET, 0)) 143 return TC_ACT_SHOT; 144 145 /* bpf_skb_adjust_room has moved outer over inner header: restore */ 146 if (bpf_skb_store_bytes(skb, off, buf, len, BPF_F_INVALIDATE_HASH) < 0) 147 return TC_ACT_SHOT; 148 149 return TC_ACT_OK; 150 } 151 152 static int decap_ipv4(struct __sk_buff *skb) 153 { 154 struct iphdr iph_outer; 155 156 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer, 157 sizeof(iph_outer)) < 0) 158 return TC_ACT_OK; 159 160 if (iph_outer.ihl != 5 || iph_outer.protocol != IPPROTO_IPIP) 161 return TC_ACT_OK; 162 163 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer)); 164 } 165 166 static int decap_ipv6(struct __sk_buff *skb) 167 { 168 struct ipv6hdr iph_outer; 169 170 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer, 171 sizeof(iph_outer)) < 0) 172 return TC_ACT_OK; 173 174 if (iph_outer.nexthdr != IPPROTO_IPV6) 175 return TC_ACT_OK; 176 177 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer)); 178 } 179 180 SEC("decap") 181 int decap_f(struct __sk_buff *skb) 182 { 183 switch (skb->protocol) { 184 case __bpf_constant_htons(ETH_P_IP): 185 return decap_ipv4(skb); 186 case __bpf_constant_htons(ETH_P_IPV6): 187 return decap_ipv6(skb); 188 default: 189 /* does not match, ignore */ 190 return TC_ACT_OK; 191 } 192 } 193 194 char __license[] SEC("license") = "GPL"; 195