#define _DEFAULT_SOURCE 1 // ensure net/if.h defines struct ifreq
#include <arpa/inet.h>
#include <errno.h>
#include <inttypes.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <net/ethernet.h>
#include <net/if.h>
#include <pthread.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>

#define BUFFER_SIZE 9000
#define CUSTOM_ETHER_TYPE 0x1234
#define SOCKET_PROTOCOL htons(ETH_P_ALL)

static void enable_promisc(int fd, unsigned int interface_index)
{
	struct packet_mreq mr = {};
	mr.mr_ifindex = interface_index;
	mr.mr_type = PACKET_MR_PROMISC;
	if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) < 0) {
		perror("setsockopt(SOL_PACKET/PACKET_ADD_MEMBERSHIP/PACKET_MR_PROMISC) failed");
		exit(1);
	}
}

static void bind_to_interface(int fd, unsigned int interface_index)
{
	struct sockaddr_ll addr = {};
	addr.sll_family = AF_PACKET;
	addr.sll_protocol = SOCKET_PROTOCOL;
	addr.sll_ifindex = interface_index;
	if (bind(fd, (const struct sockaddr *)&addr, sizeof(addr)) < 0) {
		perror("bind() failed");
		exit(1);
	}
}

static void get_interface_mac_addr(int fd, const char *interface, uint8_t *addr)
{
	struct ifreq ifreq = {};
	strncpy(ifreq.ifr_name, interface, IFNAMSIZ - 1);
	if (ioctl(fd, SIOCGIFHWADDR, &ifreq) < 0) {
		perror("ioctl(SIOCGIFHWADDR) failed");
		exit(1);
	}
	memcpy(addr, ifreq.ifr_hwaddr.sa_data, IFHWADDRLEN);
}

static int setup_socket(const char *interface, unsigned int *interface_index)
{
	*interface_index = if_nametoindex(interface);
	if (*interface_index == 0) {
		perror("if_nametoindex() failed");
		exit(1);
	}

	int fd = socket(PF_PACKET, SOCK_RAW, SOCKET_PROTOCOL);
	if (fd < 0) {
		perror("socket() failed");
		exit(1);
	}

	enable_promisc(fd, *interface_index);
	bind_to_interface(fd, *interface_index);

	return fd;
}

static void *do_send(void *arg)
{
	const char *interface = arg;

	unsigned int interface_index;
	int fd = setup_socket(interface, &interface_index);

	uint8_t srcaddr[6] = {};
	get_interface_mac_addr(fd, interface, srcaddr);

	// Destination address does not really matter, we are using promisc mode anyways for receiving
	const uint8_t dstaddr[6] = {0x02, 0x1b, 0xfa, 0, 0, 0};

	// build ethernet frame:
	// 1. ethernet header
	// 2. first 8 bytes of ethernet payload will be used for u64 counter (see below)
	// 3. fill rest of ethernet payload with certain byte values:
	//    0 or 0xFF does not trigger the "Wrong SWA type" message, but for example 1 or 0xA5 does.
	//    (but the null deref crash happens either way)
	uint8_t buffer[BUFFER_SIZE] = {};
	struct ether_header header = {};
	memcpy(header.ether_dhost, dstaddr, sizeof header.ether_dhost);
	memcpy(header.ether_shost, srcaddr, sizeof header.ether_shost);
	header.ether_type = htons(CUSTOM_ETHER_TYPE);
	memcpy(buffer, &header, sizeof(header));
	memset(buffer + sizeof(struct ether_header) + sizeof(uint64_t), 1, BUFFER_SIZE - sizeof(struct ether_header) - sizeof(uint64_t));

	uint64_t n_frames = 0;

	while (1) {
		fd_set fds;
		FD_ZERO(&fds);
		FD_SET(fd, &fds);

		const int selected = select(fd + 1, NULL, &fds, NULL, NULL);
		if (selected < 0) {
			perror("select() failed");
			return NULL;
		}

		if (FD_ISSET(fd, &fds)) {
			n_frames++;

			// put counter into frame payload
			uint8_t *payload = buffer + sizeof(struct ether_header);
			memcpy(payload, &n_frames, sizeof(n_frames));

			struct sockaddr_ll dst = {};
			dst.sll_family = AF_PACKET;
			dst.sll_protocol = CUSTOM_ETHER_TYPE;
			dst.sll_ifindex = interface_index;
			dst.sll_halen = 6;
			memcpy(dst.sll_addr, dstaddr, 6);

			/*
			printf("send frame %" PRIu64 ": size=%i"
				" dst=%02x:%02x:%02x:%02x:%02x:%02x"
				" src=%02x:%02x:%02x:%02x:%02x:%02x"
				" ethertype=0x%04X "
				"payload=[%02x %02x %02x %02x %02x %02x %02x %02x...]\n",
				n_frames,
				BUFFER_SIZE,
				header.ether_dhost[0],
				header.ether_dhost[1],
				header.ether_dhost[2],
				header.ether_dhost[3],
				header.ether_dhost[4],
				header.ether_dhost[5],
				header.ether_shost[0],
				header.ether_shost[1],
				header.ether_shost[2],
				header.ether_shost[3],
				header.ether_shost[4],
				header.ether_shost[5],
				ntohs(header.ether_type),
				payload[0],
				payload[1],
				payload[2],
				payload[3],
				payload[4],
				payload[5],
				payload[6],
				payload[7]
			);
			*/

			const ssize_t result = sendto(fd, buffer, BUFFER_SIZE, 0, (const struct sockaddr *)&dst, sizeof(dst));
			if (result < 0) {
				perror("sendto() failed");
				return NULL;
			}
			const size_t size = result;
			if (size != BUFFER_SIZE) {
				fprintf(stderr, "sendto() sent only %zu of %i bytes", size, BUFFER_SIZE);
				return NULL;
			}
		}

		//usleep(1000000);
	}

	return NULL;
}

static int do_recv(const char *interface)
{
	unsigned int interface_index;
	int fd = setup_socket(interface, &interface_index);

	uint8_t buffer[BUFFER_SIZE] = {};
	uint64_t n_frames = 0;

	while (1) {
		fd_set fds;
		FD_ZERO(&fds);
		FD_SET(fd, &fds);

		const int selected = select(fd + 1, &fds, NULL, NULL, NULL);
		if (selected < 0) {
			perror("select() failed");
			return 1;
		}

		if (FD_ISSET(fd, &fds)) {
			n_frames++;

			struct sockaddr_ll srcaddr = {};
			socklen_t srcaddrlen = sizeof(srcaddr);
			const ssize_t result = recvfrom(fd, buffer, BUFFER_SIZE, 0, (struct sockaddr *)&srcaddr, &srcaddrlen);
			if (result < 0) {
				perror("recvfrom() failed");
				return 1;
			}

			if (srcaddrlen != sizeof(srcaddr)) {
				fprintf(stderr, "unexpected src sockaddr from recvfrom(): expected %zu bytes, actual %u\n", sizeof(srcaddr), srcaddrlen);
				return 1;
			}

			const size_t size = result;
			if (size < sizeof(struct ether_header)) {
				fprintf(stderr, "packet too small for Ethernet header: %zu bytes", size);
				return 1;
			}
			struct ether_header header;
			memcpy(&header, buffer, sizeof(header));
			const uint8_t *payload = buffer + sizeof(struct ether_header);

			printf("recv frame %" PRIu64 ": ifindex=%i pkttype=%i size=%zu"
				" dst=%02x:%02x:%02x:%02x:%02x:%02x"
				" src=%02x:%02x:%02x:%02x:%02x:%02x"
				" ethertype=0x%04X "
				"payload=[%02x %02x %02x %02x %02x %02x %02x %02x...]\n",
				n_frames,
				srcaddr.sll_ifindex,
				srcaddr.sll_pkttype,
				size,
				header.ether_dhost[0],
				header.ether_dhost[1],
				header.ether_dhost[2],
				header.ether_dhost[3],
				header.ether_dhost[4],
				header.ether_dhost[5],
				header.ether_shost[0],
				header.ether_shost[1],
				header.ether_shost[2],
				header.ether_shost[3],
				header.ether_shost[4],
				header.ether_shost[5],
				ntohs(header.ether_type),
				payload[0],
				payload[1],
				payload[2],
				payload[3],
				payload[4],
				payload[5],
				payload[6],
				payload[7]
			);
		}
	}

	return 0;
}

int main(int argc, char **argv)
{
	if (argc < 2) {
		fprintf(stderr, "usage: %s <interface>\n", argv[0]);
		return 1;
	}

	char *interface = argv[1];

	pthread_t thread;
	int err = pthread_create(&thread, NULL, do_send, interface);
	if (err != 0) {
		errno = err;
		perror("pthread_create() failed");
		return 1;
	}

	do_recv(interface);
	return 0;
}