1 // SPDX-License-Identifier: GPL-2.0 2 3 #include <stdbool.h> 4 #include <linux/bpf.h> 5 #include <linux/netdev.h> 6 #include <bpf/bpf_helpers.h> 7 #include <bpf/bpf_endian.h> 8 #include <bpf/bpf_tracing.h> 9 #include <linux/if_ether.h> 10 #include <linux/ip.h> 11 #include <linux/ipv6.h> 12 #include <linux/in.h> 13 #include <linux/in6.h> 14 #include <linux/udp.h> 15 #include <asm-generic/errno-base.h> 16 17 #include "xdp_features.h" 18 19 #define ipv6_addr_equal(a, b) ((a).s6_addr32[0] == (b).s6_addr32[0] && \ 20 (a).s6_addr32[1] == (b).s6_addr32[1] && \ 21 (a).s6_addr32[2] == (b).s6_addr32[2] && \ 22 (a).s6_addr32[3] == (b).s6_addr32[3]) 23 24 struct net_device; 25 struct bpf_prog; 26 27 struct xdp_cpumap_stats { 28 unsigned int redirect; 29 unsigned int pass; 30 unsigned int drop; 31 }; 32 33 struct { 34 __uint(type, BPF_MAP_TYPE_ARRAY); 35 __type(key, __u32); 36 __type(value, __u32); 37 __uint(max_entries, 1); 38 } stats SEC(".maps"); 39 40 struct { 41 __uint(type, BPF_MAP_TYPE_ARRAY); 42 __type(key, __u32); 43 __type(value, __u32); 44 __uint(max_entries, 1); 45 } dut_stats SEC(".maps"); 46 47 struct { 48 __uint(type, BPF_MAP_TYPE_CPUMAP); 49 __uint(key_size, sizeof(__u32)); 50 __uint(value_size, sizeof(struct bpf_cpumap_val)); 51 __uint(max_entries, 1); 52 } cpu_map SEC(".maps"); 53 54 struct { 55 __uint(type, BPF_MAP_TYPE_DEVMAP); 56 __uint(key_size, sizeof(__u32)); 57 __uint(value_size, sizeof(struct bpf_devmap_val)); 58 __uint(max_entries, 1); 59 } dev_map SEC(".maps"); 60 61 const volatile struct in6_addr tester_addr; 62 const volatile struct in6_addr dut_addr; 63 64 static __always_inline int 65 xdp_process_echo_packet(struct xdp_md *xdp, bool dut) 66 { 67 void *data_end = (void *)(long)xdp->data_end; 68 void *data = (void *)(long)xdp->data; 69 struct ethhdr *eh = data; 70 struct tlv_hdr *tlv; 71 struct udphdr *uh; 72 __be16 port; 73 __u8 *cmd; 74 75 if (eh + 1 > (struct ethhdr *)data_end) 76 return -EINVAL; 77 78 if (eh->h_proto == bpf_htons(ETH_P_IP)) { 79 struct iphdr *ih = (struct iphdr *)(eh + 1); 80 __be32 saddr = dut ? tester_addr.s6_addr32[3] 81 : dut_addr.s6_addr32[3]; 82 __be32 daddr = dut ? dut_addr.s6_addr32[3] 83 : tester_addr.s6_addr32[3]; 84 85 ih = (struct iphdr *)(eh + 1); 86 if (ih + 1 > (struct iphdr *)data_end) 87 return -EINVAL; 88 89 if (saddr != ih->saddr) 90 return -EINVAL; 91 92 if (daddr != ih->daddr) 93 return -EINVAL; 94 95 if (ih->protocol != IPPROTO_UDP) 96 return -EINVAL; 97 98 uh = (struct udphdr *)(ih + 1); 99 } else if (eh->h_proto == bpf_htons(ETH_P_IPV6)) { 100 struct in6_addr saddr = dut ? tester_addr : dut_addr; 101 struct in6_addr daddr = dut ? dut_addr : tester_addr; 102 struct ipv6hdr *ih6 = (struct ipv6hdr *)(eh + 1); 103 104 if (ih6 + 1 > (struct ipv6hdr *)data_end) 105 return -EINVAL; 106 107 if (!ipv6_addr_equal(saddr, ih6->saddr)) 108 return -EINVAL; 109 110 if (!ipv6_addr_equal(daddr, ih6->daddr)) 111 return -EINVAL; 112 113 if (ih6->nexthdr != IPPROTO_UDP) 114 return -EINVAL; 115 116 uh = (struct udphdr *)(ih6 + 1); 117 } else { 118 return -EINVAL; 119 } 120 121 if (uh + 1 > (struct udphdr *)data_end) 122 return -EINVAL; 123 124 port = dut ? uh->dest : uh->source; 125 if (port != bpf_htons(DUT_ECHO_PORT)) 126 return -EINVAL; 127 128 tlv = (struct tlv_hdr *)(uh + 1); 129 if (tlv + 1 > data_end) 130 return -EINVAL; 131 132 return bpf_htons(tlv->type) == CMD_ECHO ? 0 : -EINVAL; 133 } 134 135 static __always_inline int 136 xdp_update_stats(struct xdp_md *xdp, bool tx, bool dut) 137 { 138 __u32 *val, key = 0; 139 140 if (xdp_process_echo_packet(xdp, tx)) 141 return -EINVAL; 142 143 if (dut) 144 val = bpf_map_lookup_elem(&dut_stats, &key); 145 else 146 val = bpf_map_lookup_elem(&stats, &key); 147 148 if (val) 149 __sync_add_and_fetch(val, 1); 150 151 return 0; 152 } 153 154 /* Tester */ 155 156 SEC("xdp") 157 int xdp_tester_check_tx(struct xdp_md *xdp) 158 { 159 xdp_update_stats(xdp, true, false); 160 161 return XDP_PASS; 162 } 163 164 SEC("xdp") 165 int xdp_tester_check_rx(struct xdp_md *xdp) 166 { 167 xdp_update_stats(xdp, false, false); 168 169 return XDP_PASS; 170 } 171 172 /* DUT */ 173 174 SEC("xdp") 175 int xdp_do_pass(struct xdp_md *xdp) 176 { 177 xdp_update_stats(xdp, true, true); 178 179 return XDP_PASS; 180 } 181 182 SEC("xdp") 183 int xdp_do_drop(struct xdp_md *xdp) 184 { 185 if (xdp_update_stats(xdp, true, true)) 186 return XDP_PASS; 187 188 return XDP_DROP; 189 } 190 191 SEC("xdp") 192 int xdp_do_aborted(struct xdp_md *xdp) 193 { 194 if (xdp_process_echo_packet(xdp, true)) 195 return XDP_PASS; 196 197 return XDP_ABORTED; 198 } 199 200 SEC("xdp") 201 int xdp_do_tx(struct xdp_md *xdp) 202 { 203 void *data = (void *)(long)xdp->data; 204 struct ethhdr *eh = data; 205 __u8 tmp_mac[ETH_ALEN]; 206 207 if (xdp_update_stats(xdp, true, true)) 208 return XDP_PASS; 209 210 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN); 211 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN); 212 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN); 213 214 return XDP_TX; 215 } 216 217 SEC("xdp") 218 int xdp_do_redirect(struct xdp_md *xdp) 219 { 220 if (xdp_process_echo_packet(xdp, true)) 221 return XDP_PASS; 222 223 return bpf_redirect_map(&cpu_map, 0, 0); 224 } 225 226 SEC("tp_btf/xdp_exception") 227 int BPF_PROG(xdp_exception, const struct net_device *dev, 228 const struct bpf_prog *xdp, __u32 act) 229 { 230 __u32 *val, key = 0; 231 232 val = bpf_map_lookup_elem(&dut_stats, &key); 233 if (val) 234 __sync_add_and_fetch(val, 1); 235 236 return 0; 237 } 238 239 SEC("tp_btf/xdp_cpumap_kthread") 240 int BPF_PROG(tp_xdp_cpumap_kthread, int map_id, unsigned int processed, 241 unsigned int drops, int sched, struct xdp_cpumap_stats *xdp_stats) 242 { 243 __u32 *val, key = 0; 244 245 val = bpf_map_lookup_elem(&dut_stats, &key); 246 if (val) 247 __sync_add_and_fetch(val, 1); 248 249 return 0; 250 } 251 252 SEC("xdp/cpumap") 253 int xdp_do_redirect_cpumap(struct xdp_md *xdp) 254 { 255 void *data = (void *)(long)xdp->data; 256 struct ethhdr *eh = data; 257 __u8 tmp_mac[ETH_ALEN]; 258 259 if (xdp_process_echo_packet(xdp, true)) 260 return XDP_PASS; 261 262 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN); 263 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN); 264 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN); 265 266 return bpf_redirect_map(&dev_map, 0, 0); 267 } 268 269 char _license[] SEC("license") = "GPL"; 270