269 lines · c
1// SPDX-License-Identifier: GPL-2.02 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 24struct net_device;25struct bpf_prog;26 27struct xdp_cpumap_stats {28 unsigned int redirect;29 unsigned int pass;30 unsigned int drop;31};32 33struct {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 40struct {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 47struct {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 54struct {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 61const volatile struct in6_addr tester_addr;62const volatile struct in6_addr dut_addr;63 64static __always_inline int65xdp_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 74 if (eh + 1 > (struct ethhdr *)data_end)75 return -EINVAL;76 77 if (eh->h_proto == bpf_htons(ETH_P_IP)) {78 struct iphdr *ih = (struct iphdr *)(eh + 1);79 __be32 saddr = dut ? tester_addr.s6_addr32[3]80 : dut_addr.s6_addr32[3];81 __be32 daddr = dut ? dut_addr.s6_addr32[3]82 : tester_addr.s6_addr32[3];83 84 ih = (struct iphdr *)(eh + 1);85 if (ih + 1 > (struct iphdr *)data_end)86 return -EINVAL;87 88 if (saddr != ih->saddr)89 return -EINVAL;90 91 if (daddr != ih->daddr)92 return -EINVAL;93 94 if (ih->protocol != IPPROTO_UDP)95 return -EINVAL;96 97 uh = (struct udphdr *)(ih + 1);98 } else if (eh->h_proto == bpf_htons(ETH_P_IPV6)) {99 struct in6_addr saddr = dut ? tester_addr : dut_addr;100 struct in6_addr daddr = dut ? dut_addr : tester_addr;101 struct ipv6hdr *ih6 = (struct ipv6hdr *)(eh + 1);102 103 if (ih6 + 1 > (struct ipv6hdr *)data_end)104 return -EINVAL;105 106 if (!ipv6_addr_equal(saddr, ih6->saddr))107 return -EINVAL;108 109 if (!ipv6_addr_equal(daddr, ih6->daddr))110 return -EINVAL;111 112 if (ih6->nexthdr != IPPROTO_UDP)113 return -EINVAL;114 115 uh = (struct udphdr *)(ih6 + 1);116 } else {117 return -EINVAL;118 }119 120 if (uh + 1 > (struct udphdr *)data_end)121 return -EINVAL;122 123 port = dut ? uh->dest : uh->source;124 if (port != bpf_htons(DUT_ECHO_PORT))125 return -EINVAL;126 127 tlv = (struct tlv_hdr *)(uh + 1);128 if (tlv + 1 > data_end)129 return -EINVAL;130 131 return bpf_htons(tlv->type) == CMD_ECHO ? 0 : -EINVAL;132}133 134static __always_inline int135xdp_update_stats(struct xdp_md *xdp, bool tx, bool dut)136{137 __u32 *val, key = 0;138 139 if (xdp_process_echo_packet(xdp, tx))140 return -EINVAL;141 142 if (dut)143 val = bpf_map_lookup_elem(&dut_stats, &key);144 else145 val = bpf_map_lookup_elem(&stats, &key);146 147 if (val)148 __sync_add_and_fetch(val, 1);149 150 return 0;151}152 153/* Tester */154 155SEC("xdp")156int xdp_tester_check_tx(struct xdp_md *xdp)157{158 xdp_update_stats(xdp, true, false);159 160 return XDP_PASS;161}162 163SEC("xdp")164int xdp_tester_check_rx(struct xdp_md *xdp)165{166 xdp_update_stats(xdp, false, false);167 168 return XDP_PASS;169}170 171/* DUT */172 173SEC("xdp")174int xdp_do_pass(struct xdp_md *xdp)175{176 xdp_update_stats(xdp, true, true);177 178 return XDP_PASS;179}180 181SEC("xdp")182int xdp_do_drop(struct xdp_md *xdp)183{184 if (xdp_update_stats(xdp, true, true))185 return XDP_PASS;186 187 return XDP_DROP;188}189 190SEC("xdp")191int xdp_do_aborted(struct xdp_md *xdp)192{193 if (xdp_process_echo_packet(xdp, true))194 return XDP_PASS;195 196 return XDP_ABORTED;197}198 199SEC("xdp")200int xdp_do_tx(struct xdp_md *xdp)201{202 void *data = (void *)(long)xdp->data;203 struct ethhdr *eh = data;204 __u8 tmp_mac[ETH_ALEN];205 206 if (xdp_update_stats(xdp, true, true))207 return XDP_PASS;208 209 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN);210 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN);211 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN);212 213 return XDP_TX;214}215 216SEC("xdp")217int xdp_do_redirect(struct xdp_md *xdp)218{219 if (xdp_process_echo_packet(xdp, true))220 return XDP_PASS;221 222 return bpf_redirect_map(&cpu_map, 0, 0);223}224 225SEC("tp_btf/xdp_exception")226int BPF_PROG(xdp_exception, const struct net_device *dev,227 const struct bpf_prog *xdp, __u32 act)228{229 __u32 *val, key = 0;230 231 val = bpf_map_lookup_elem(&dut_stats, &key);232 if (val)233 __sync_add_and_fetch(val, 1);234 235 return 0;236}237 238SEC("tp_btf/xdp_cpumap_kthread")239int BPF_PROG(tp_xdp_cpumap_kthread, int map_id, unsigned int processed,240 unsigned int drops, int sched, struct xdp_cpumap_stats *xdp_stats)241{242 __u32 *val, key = 0;243 244 val = bpf_map_lookup_elem(&dut_stats, &key);245 if (val)246 __sync_add_and_fetch(val, 1);247 248 return 0;249}250 251SEC("xdp/cpumap")252int xdp_do_redirect_cpumap(struct xdp_md *xdp)253{254 void *data = (void *)(long)xdp->data;255 struct ethhdr *eh = data;256 __u8 tmp_mac[ETH_ALEN];257 258 if (xdp_process_echo_packet(xdp, true))259 return XDP_PASS;260 261 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN);262 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN);263 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN);264 265 return bpf_redirect_map(&dev_map, 0, 0);266}267 268char _license[] SEC("license") = "GPL";269