// SPDX-License-Identifier: (LGPL-2.1 OR BSD-2-Clause)
#include <linux/bpf.h>
#include <linux/if_ether.h>
#include <linux/in.h>

#include <bpf/bpf_helpers.h>
#include <bpf/bpf_endian.h>

#ifndef AF_INET
#define AF_INET 2
#endif

/* NOTICE: Re-defining VLAN header levels to parse */
#define VLAN_MAX_DEPTH 10
#include "../common/parsing_helpers.h"
#include "../common/rewrite_helpers.h"

#define unlikely(e) __builtin_expect((e), 0)

struct ethhdr_dsa {
	unsigned char	h_dest[ETH_ALEN];
	unsigned char	h_source[ETH_ALEN];
	struct dsa_tag {
		__u8	dev : 5;
		__u8	tagged : 1;
		__u8	cmd : 2;

		__u8	cfi : 1;
		__u8	res1 : 1;
		__u8	is_trunk : 1;
		__u8	port : 5;

		__u8	vid_h : 4;
		__u8	res2 : 1;
		__u8	pri : 3;

		__u8	vid_l;
	} __attribute__((packed)) dsa;
	__be16		h_proto;
} __attribute__((packed));

enum dsa_cmd {
	DSA_CMD_TO_CPU     = 0,
	DSA_CMD_FROM_CPU   = 1,
	DSA_CMD_TO_SNIFFER = 2,
	DSA_CMD_FORWARD    = 3
};

static __always_inline int ifindex_to_mv88e6xxx_port(int ifindex)
{
	switch (ifindex) {
	case 14:
	case 24:
	case 25:
		return 1;
	case 16:
	case 26:
	case 27:
		return 3;
	case 18:
	case 28:
	case 29:
		return 5;
	case 19:
	case 34:
	case 35:
		return 6;
	case 20:
	case 30:
	case 31:
		return 7;
	case 21:
	case 32:
	case 33:
		return 8;
	case 22:
	case 36:
	case 37:
	case 42:
	case 43:
		return 9;
	default:
		return -1;
	}
}

static __always_inline int ifindex_to_vid(int ifindex)
{
	switch (ifindex) {
	case 24:
		return 10;
	case 25:
		return 11;
	case 26:
		return 12;
	case 27:
		return 13;
	case 28:
		return 14;
	case 29:
		return 15;
	case 30:
		return 16;
	case 31:
		return 17;
	case 32:
		return 18;
	case 33:
		return 19;
	case 34:
		return 20;
	case 35:
		return 21;
	case 36:
		return 22;
	case 37:
		return 23;
	case 42:
		return 100;
	case 43:
		return 101;
	default:
		return -1;
	}
}

static __always_inline int ifindex_to_dsa_tag(int ifindex, struct dsa_tag *dsa)
{
	int port, vid;

	port = ifindex_to_mv88e6xxx_port(ifindex);
	if (port < 0)
		return -1;

	dsa->port = port;
	vid = ifindex_to_vid(ifindex);

	if (vid >= 0) {
		dsa->tagged = 1;
		dsa->vid_h = (vid >> 8) & 0xf;
		dsa->vid_l = vid & 0xff;
	} else {
		dsa->tagged = dsa->vid_h = dsa->vid_l = 0;
	}

	return 0;
}

static __always_inline int dsa_tag_to_ifindex(struct dsa_tag *dsa)
{
	int vid;

	if (dsa->tagged)
		vid = ((int)dsa->vid_h << 8) | dsa->vid_l;
	else
		vid = -1;

	switch (vid) {
	case 10:
		return 24;
	case 11:
		return 25;
	case 12:
		return 26;
	case 13:
		return 27;
	case 14:
		return 28;
	case 15:
		return 29;
	case 16:
		return 30;
	case 17:
		return 31;
	case 18:
		return 32;
	case 19:
		return 33;
	case 20:
		return 34;
	case 21:
		return 35;
	case 22:
		return 36;
	case 23:
		return 37;
	case 100:
		return 42;
	case 101:
		return 43;
	case -1:
		switch (dsa->port) {
		case 1:
			return 14;
		case 3:
			return 16;
		case 5:
			return 18;
		case 6:
			return 19;
		case 7:
			return 20;
		case 8:
			return 21;
		case 9:
			return 22;
		default:
			return -1;
		}
	default:
		return -1;
	}
}

SEC("xdp_dsa")
int xdp_dsa_func(struct xdp_md *ctx)
{
	void *data_end = (void *)(long)ctx->data_end;
	void *data = (void *)(long)ctx->data;
	struct bpf_fib_lookup fib_params;
	struct ethhdr_dsa *eth = data;
	struct iphdr *iphdr;
	struct dsa_tag dsa;
	int source_ifindex;
	__u64 nh_off;
	int pop_vlan = 0;
	__u16 h_proto;
	int rc;

	nh_off = sizeof(*eth);
	if (data + nh_off > data_end)
		return XDP_DROP;

	__builtin_memcpy(&dsa, &eth->dsa, sizeof(dsa));

	if (dsa.cmd != DSA_CMD_FORWARD)
		return XDP_PASS;

	h_proto = eth->h_proto;

	if (!dsa.tagged && h_proto == bpf_htons(ETH_P_8021Q)) {
		struct vlan_hdr *vhdr;
		int vid;

		vhdr = data + nh_off;
		nh_off += sizeof(struct vlan_hdr);
		if (data + nh_off > data_end)
			return XDP_DROP;

		dsa.tagged = 1;
		dsa.pri = bpf_ntohs(vhdr->h_vlan_TCI) >> 13;
		vid = bpf_ntohs(vhdr->h_vlan_TCI) & VLAN_VID_MASK;
		dsa.vid_h = (vid >> 8) & 0xf;
		dsa.vid_l = vid & 0xff;

		h_proto = vhdr->h_vlan_encapsulated_proto;
		pop_vlan = 1;
	}

	if (h_proto != bpf_htons(ETH_P_IP))
		return XDP_PASS;

	iphdr = data + nh_off;
	nh_off += sizeof(struct iphdr);
	if (data + nh_off > data_end)
		return XDP_DROP;

	if (iphdr->ttl <= 1)
		return XDP_PASS;

	source_ifindex = dsa_tag_to_ifindex(&dsa);
	if (source_ifindex < 0)
		return XDP_PASS;

	__builtin_memset(&fib_params, 0, sizeof(fib_params));

	fib_params.family = AF_INET;
	fib_params.tos = iphdr->tos;
	fib_params.l4_protocol = iphdr->protocol;
	fib_params.sport = 0;
	fib_params.dport = 0;
	fib_params.tot_len = bpf_ntohs(iphdr->tot_len);
	fib_params.ipv4_src = iphdr->saddr;
	fib_params.ipv4_dst = iphdr->daddr;
	fib_params.ifindex = source_ifindex;

	rc = bpf_fib_lookup(ctx, &fib_params, sizeof(fib_params), 0);

	if (rc == BPF_FIB_LKUP_RET_SUCCESS) {
		__builtin_memcpy(eth->h_source, fib_params.smac, ETH_ALEN);
		__builtin_memcpy(eth->h_dest, fib_params.dmac, ETH_ALEN);

		if (ifindex_to_dsa_tag(fib_params.ifindex, &dsa) < 0) {
			/* tu by sa mohlo robit bpf_redirect_map */
			return XDP_PASS;
		}
	} else {
		return XDP_PASS;
	}

	dsa.cmd = DSA_CMD_FROM_CPU;
	dsa.res1 = 0;
	dsa.res2 = 0;
	dsa.pri = 0;
	__builtin_memcpy(&eth->dsa, &dsa, sizeof(dsa));

	if (pop_vlan) {
		eth->h_proto = h_proto;

		__builtin_memmove(data + sizeof(struct vlan_hdr), data, sizeof(*eth));
		if (bpf_xdp_adjust_head(ctx, (int)sizeof(struct vlan_hdr)))
			return XDP_DROP;

		data = (void *)(long)ctx->data;
		data_end = (void *)(long)ctx->data_end;
		eth = data;
		nh_off = sizeof(*eth);

		if (data + nh_off > data_end)
			return XDP_DROP;
	}

	return XDP_TX;
}

char _license[] SEC("license") = "GPL";