1 files changed, 103 insertions, 0 deletions
diff --git a/src/bpfswitch/ksrc/xdp_l2fwd.c b/src/bpfswitch/ksrc/xdp_l2fwd.c
new file mode 100644
index 00000000..750b580d
--- /dev/null
+++ b/src/bpfswitch/ksrc/xdp_l2fwd.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Example of L2 forwarding via XDP. FDB is a <vlan,dmac> hash table
+ * returning device index to redirect packet.
+ *
+ * Copyright (c) 2019-2020 David Ahern <dsahern@gmail.com>
+ * The code is modified to use 2 mac addresses as key instead of
+ * mac address and vlan id as key
+ *
+ */
+#define KBUILD_MODNAME "xdp_l2fwd"
+#include <uapi/linux/bpf.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <linux/if_vlan.h>
+#include <linux/version.h>
+#include <bpf/bpf_helpers.h>
+#include <bpf/bpf_endian.h>
+
+#include "xdp_fdb.h"
+
+/* For TX-traffic redirect requires net_device ifindex to be in this devmap */
+struct bpf_map_def SEC("maps") xdp_fwd_ports = {
+	.type = BPF_MAP_TYPE_DEVMAP_HASH,
+	.key_size = sizeof(u32),
+	.value_size = sizeof(struct bpf_devmap_val),
+	.max_entries = 512,
+};
+
+/* <vlan,dmac> to device index map */
+struct bpf_map_def SEC("maps") fdb_map = {
+	.type = BPF_MAP_TYPE_HASH,
+	.key_size = sizeof(struct fdb_key),
+	.value_size = sizeof(u32),
+	.max_entries = 512,
+};
+
+SEC("xdp_l2fwd")
+int xdp_l2fwd_prog(struct xdp_md *ctx)
+{
+	void *data_end = (void *)(long)ctx->data_end;
+	void *data = (void *)(long)ctx->data;
+	struct bpf_devmap_val *entry;
+	struct vlan_hdr *vhdr = NULL;
+	struct ethhdr *eth;
+	struct fdb_key key;
+	u8 smac[ETH_ALEN];
+	u16 h_proto = 0;
+	void *nh;
+	int rc;
+
+	/* data in context points to ethernet header */
+	eth = data;
+
+	/* set pointer to header after ethernet header */
+	nh = data + sizeof(*eth);
+	if (nh > data_end)
+		return XDP_DROP; // malformed packet
+
+	__builtin_memset(&key, 0, sizeof(key));
+	__builtin_memcpy(key.dmac, eth->h_dest, ETH_ALEN);
+	__builtin_memcpy(key.smac, eth->h_source, ETH_ALEN);
+
+	if (eth->h_proto == htons(ETH_P_8021Q)) {
+		vhdr = nh;
+		if (vhdr + 1 > data_end)
+			return XDP_DROP; // malformed packet
+
+		//key.vlan = ntohs(vhdr->h_vlan_TCI) & VLAN_VID_MASK;
+	}
+
+	entry = bpf_map_lookup_elem(&fdb_map, &key);
+	if (!entry || entry->ifindex == 0)
+		return XDP_PASS;
+
+	/* Verify redirect index exists in port map */
+	if (!bpf_map_lookup_elem(&xdp_fwd_ports, &entry->ifindex))
+		return XDP_PASS;
+
+	if (vhdr) {
+		/* remove VLAN header before hand off to VM */
+		h_proto = vhdr->h_vlan_encapsulated_proto;
+		__builtin_memcpy(smac, eth->h_source, ETH_ALEN);
+
+		if (bpf_xdp_adjust_head(ctx, sizeof(*vhdr)))
+			return XDP_PASS;
+
+		/* reset data pointers after adjust */
+		data = (void *)(long)ctx->data;
+		data_end = (void *)(long)ctx->data_end;
+		eth = data;
+		if (eth + 1 > data_end)
+			return XDP_DROP;
+
+		__builtin_memcpy(eth->h_dest, key.dmac, ETH_ALEN);
+		__builtin_memcpy(eth->h_source, smac, ETH_ALEN);
+		eth->h_proto = h_proto;
+	}
+
+	return bpf_redirect_map(&xdp_fwd_ports, entry->ifindex, 0);
+}
+
+char _license[] SEC("license") = "GPL";
+int _version SEC("version") = LINUX_VERSION_CODE;