Initial version

Working ping back and forth with specified data, and data
arrives at the other party.

Currently, the server uses nflog to get the pings, which:
1. requires iptables settings to work;
2. buffers and collects messages for a second.
Both are suboptimal, and I believe raw sockets can improve
on this.

8 files changed, 364 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..1c102fa
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+*.o
+client
+server
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..84683dd
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,24 @@
+CC = gcc
+CFLAGS = -Wall -Wextra -O2 -g -std=c11 -fwrapv
+
+TARGETS = server client
+
+SOURCES = $(filter-out $(patsubst %,%.c,$(TARGETS)),$(wildcard *.c))
+OBJECTS = $(patsubst %.c,%.o,$(SOURCES))
+
+.PHONY: all clean
+
+all: $(TARGETS)
+
+clean:
+	rm -f $(TARGETS) *.o
+
+
+server: server.o $(OBJECTS)
+	$(CC) $(CFLAGS) $^ -o $@ -lnetfilter_log
+
+client: client.o $(OBJECTS)
+	$(CC) $(CFLAGS) $^ -o $@
+
+%.o: %.c $(wildcard *.h)
+	$(CC) $(CFLAGS) -c -o $@ $<
diff --git a/client.c b/client.c
new file mode 100644
index 0000000..527375f
--- /dev/null
+++ b/client.c
@@ -0,0 +1,30 @@
+#include <stdio.h>
+#include <stddef.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/ip_icmp.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include "icmp.h"
+#include "util.h"
+
+int main(void) {
+	int sock = icmp_open_socket();
+	if (sock < 0) {
+		perror("icmp_open_socket");
+		return 1;
+	}
+
+	const char *ip_address = "127.0.0.1";
+	// const char *ip_address = "192.168.43.220";
+
+	struct icmp_reply reply = icmp_communicate(sock, ip_address, 1234, "kaashandel", 10);
+	if (reply.data == NULL) {
+		perror("icmp_communicate");
+		return 1;
+	}
+
+	printf("Received length: %zd\nPayload:\n", reply.length);
+	xxd(reply.data, reply.length);
+}
diff --git a/icmp.c b/icmp.c
new file mode 100644
index 0000000..e64580c
--- /dev/null
+++ b/icmp.c
@@ -0,0 +1,128 @@
+#include <stdlib.h>
+#include <stddef.h>
+#include <string.h>
+#include <assert.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/ip_icmp.h>
+#include <arpa/inet.h>
+#include "icmp.h"
+#include "util.h"
+
+
+#define PROT_ICMP 1
+
+
+static void make_sockaddr(struct sockaddr_in *dst, const char *ip_address) {
+	memset(dst, 0, sizeof *dst);
+	dst->sin_family = AF_INET;
+	dst->sin_port = htons(0);
+	dst->sin_addr.s_addr = inet_addr(ip_address);
+}
+
+static uint16_t compute_checksum(const void *buf_, size_t buflen) {
+	const uint8_t *buf_bytes = (const uint8_t*)buf_;
+	const uint16_t *buf = (const uint16_t*)buf_;
+
+	uint32_t res = 0;
+	for (size_t i = 0; i < buflen / 2; i++) {
+		res += buf[i];
+		res = (res + (res >> 16)) & 0xffff;
+	}
+
+	if (buflen % 2 == 1) {
+		res += (uint16_t)buf_bytes[buflen - 1];
+		res = (res + (res >> 16)) & 0xffff;
+	}
+
+	return res;
+}
+
+int icmp_open_socket(void) {
+	return socket(PF_INET, SOCK_DGRAM, PROT_ICMP);
+}
+
+struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum, const void *data_, size_t length) {
+	static uint8_t buffer[ICMP_MAX_PAYLOAD_LENGTH];
+
+	const uint8_t *data = (const uint8_t*)data_;
+
+	assert(length <= ICMP_MAX_PAYLOAD_LENGTH);
+
+	struct icmp_reply errret = (struct icmp_reply){.data = NULL, .length = 0, .seqnum = 0};
+
+	struct sockaddr_in addr;
+	make_sockaddr(&addr, ip_address);
+
+	struct icmp_echo msg;
+	memset(&msg, 0, sizeof msg);
+	msg.type = ICMP_ECHO;
+	msg.code = 0;
+	msg.seqnum = seqnum;
+
+	memcpy(msg.payload, data, length);
+
+	if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) {
+		return errret;
+	}
+
+	struct msghdr replyhdr;
+	memset(&replyhdr, 0, sizeof replyhdr);
+
+	struct iovec iov1;
+	memset(&iov1, 0, sizeof iov1);
+	iov1.iov_base = &msg;
+	iov1.iov_len = sizeof msg;
+
+	replyhdr.msg_name = &addr;
+	replyhdr.msg_namelen = sizeof addr;
+	replyhdr.msg_iov = &iov1;
+	replyhdr.msg_iovlen = 1;
+
+	ssize_t nr = recvmsg(sock, &replyhdr, 0);
+	if (nr < 0) {
+		return errret;
+	}
+
+	size_t payloadlen = nr - ICMP_PAYLOAD_OFFSET;
+	assert(payloadlen <= MAX_DATAGRAM_SIZE);
+
+	memcpy(buffer, msg.payload, payloadlen);
+
+	return (struct icmp_reply){
+		.data = buffer,
+		.length = payloadlen,
+		.seqnum = msg.seqnum
+	};
+}
+
+int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void *data_, size_t length) {
+	const uint8_t *data = (const uint8_t*)data_;
+
+	int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
+	int zero = 0;
+	assert(setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &zero, sizeof zero) >= 0);
+
+	struct sockaddr_in addr;
+	make_sockaddr(&addr, ip_address);
+
+	struct icmp_echo msg;
+	memset(&msg, 0, sizeof msg);
+	msg.type = ICMP_ECHOREPLY;
+	msg.code = 0;
+	msg.id = id;
+	msg.seqnum = seqnum;
+	msg.checksum = 0;
+
+	memcpy(msg.payload, data, length);
+
+	size_t total_length = ICMP_PAYLOAD_OFFSET + length;
+
+	msg.checksum = ~compute_checksum(&msg, total_length);
+
+	if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) {
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/icmp.h b/icmp.h
new file mode 100644
index 0000000..3557fc3
--- /dev/null
+++ b/icmp.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <stdint.h>
+
+
+#define IP_HEADER_SIZE 40
+
+#define MIN_MTU 576
+#define MAX_IP_PACKET_SIZE 65535
+#define MAX_DATAGRAM_SIZE (MAX_IP_PACKET_SIZE - IP_HEADER_SIZE)
+
+#define ICMP_PAYLOAD_OFFSET 8
+
+struct __attribute__((packed)) icmp_echo {
+	uint8_t type;
+	uint8_t code;
+	uint16_t checksum;
+	uint16_t id;
+	uint16_t seqnum;
+	uint8_t payload[MAX_DATAGRAM_SIZE - ICMP_PAYLOAD_OFFSET];
+};
+
+#define ICMP_MAX_PAYLOAD_LENGTH (MAX_DATAGRAM_SIZE - ICMP_PAYLOAD_OFFSET)
+#define ICMP_SAFE_PAYLOAD_LENGTH (MIN_MTU - IP_HEADER_SIZE - ICMP_PAYLOAD_OFFSET)
+
+
+struct icmp_reply {
+	const uint8_t *data;  // points to internal buffer
+	size_t length;  // length of 'data'
+
+	int seqnum;
+};
+
+// Returns -1 on error with errno.
+int icmp_open_socket(void);
+
+// Only actual IPv4 addresses allowed. Sends data in 'data' with length 'length', and
+// returns pointer to internal buffer with reply data. Buffer is invalidated on next
+// call to the function.
+// Returns {.data=NULL} on error with errno.
+struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum, const void *data, size_t length);
+
+// Returns -1 on error with errno.
+int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void *data, size_t length);
diff --git a/server.c b/server.c
new file mode 100644
index 0000000..544c59b
--- /dev/null
+++ b/server.c
@@ -0,0 +1,99 @@
+#include <stdio.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <arpa/inet.h>
+#include <netinet/ip.h>
+
+typedef uint8_t u_int8_t;
+typedef uint16_t u_int16_t;
+typedef uint32_t u_int32_t;
+#include <libnetfilter_log/libnetfilter_log.h>
+
+#include "icmp.h"
+#include "util.h"
+
+
+struct state {
+	int socket;
+};
+
+static int icmp_callback(struct nflog_g_handle *gh, struct nfgenmsg *nfmsg, struct nflog_data *ldata, void *state_) {
+	(void)gh; (void)nfmsg;
+
+	struct state *state = (struct state*)state_;
+
+	uint8_t *ip_start;  // received packet, starting at the IP buffer
+	int ip_len = nflog_get_payload(ldata, (char**)&ip_start);
+
+	struct iphdr *hdr = (struct iphdr*)ip_start;
+	int hdr_len = hdr->ihl * 4;
+	uint32_t saddr = hdr->saddr;
+
+	struct icmp_echo *msg = (struct icmp_echo*)(ip_start + hdr_len);
+	int msg_len = ip_len - hdr_len;
+	printf("Received: type %u   code %u   id %hu   seqnum %hu   payload:\n",
+			(unsigned)msg->type, (unsigned)msg->code, msg->id, msg->seqnum);
+	xxd(msg->payload, msg_len - offsetof(struct icmp_echo, payload));
+
+	uint8_t *saddr_bytes = (uint8_t*)&saddr;
+	char ip_address[16];
+	sprintf(ip_address, "%u.%u.%u.%u", saddr_bytes[0], saddr_bytes[1], saddr_bytes[2], saddr_bytes[3]);
+	if (icmp_send_echo_reply(ip_address, msg->id, msg->seqnum, "dank je wel", 11) < 0) {
+		perror("icmp_send_echo_reply");
+	}
+
+	return 0;
+}
+
+
+int main(void) {
+	struct nflog_handle *h = nflog_open();
+	if (!h) {
+		perror("nflog_open");
+		return 1;
+	}
+	if (nflog_unbind_pf(h, AF_INET) < 0) {
+		perror("nflog_unbind_pf");
+		return 1;
+	}
+	if (nflog_bind_pf(h, AF_INET) < 0) {
+		perror("nflog_bind_pf");
+		return 1;
+	}
+
+	struct nflog_g_handle *qh = nflog_bind_group(h, 0);
+	if (!qh) {
+		fprintf(stderr, "nflog_bind_group: no handle for group 0\n");
+		return 1;
+	}
+
+	if (nflog_set_mode(qh, NFULNL_COPY_PACKET, 0xffff) < 0) {
+		fprintf(stderr, "nflog_set_mode: can't set packet copy mode\n");
+		return 1;
+	}
+
+	struct state state;
+	// state.socket = icmp_open_socket();
+	// if (state.socket < 0) {
+	//     perror("icmp_open_socket");
+	//     return 1;
+	// }
+
+	nflog_callback_register(qh, &icmp_callback, &state);
+
+	int fd = nflog_fd(h);
+
+	char buf[4096];
+	while (true) {
+		ssize_t nr = recv(fd, buf, sizeof buf, 0);
+		if (nr < 0) break;
+
+		nflog_handle_packet(h, buf, nr);
+	}
+
+	nflog_unbind_group(qh);
+	nflog_close(h);
+}
diff --git a/util.c b/util.c
new file mode 100644
index 0000000..be7083c
--- /dev/null
+++ b/util.c
@@ -0,0 +1,30 @@
+#include <stdio.h>
+#include <ctype.h>
+#include "util.h"
+
+
+void xxd(const void *buf_, size_t length) {
+	unsigned char *buf = (unsigned char*)buf_;
+
+	for (size_t cursor = 0; cursor < length;) {
+		printf("%08zx:", cursor);
+
+		for (int i = 0; i < 16; i++) {
+			if (i % 2 == 0) printf(" ");
+			if (i % 8 == 0) printf(" ");
+			if (cursor + i < length) printf("%02x", (unsigned)buf[cursor + i]);
+			else printf("  ");
+		}
+
+		printf("  |");
+
+		for (int i = 0; i < 16 && cursor + i < length; i++) {
+			if (isprint(buf[cursor + i])) printf("%c", buf[cursor + i]);
+			else printf(".");
+		}
+
+		printf("|\n");
+
+		cursor += 16;
+	}
+}
diff --git a/util.h b/util.h
new file mode 100644
index 0000000..ea48028
--- /dev/null
+++ b/util.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#include <stdlib.h>
+
+
+void xxd(const void *buf, size_t length);