Better ICMP code encapsulation

7 files changed, 184 insertions, 141 deletions

diff --git a/client.c b/client.c
index 1f8663f..4c1a32f 100644
--- a/client.c
+++ b/client.c
@@ -6,22 +6,24 @@
 #include <netinet/ip_icmp.h>
 #include <arpa/inet.h>
 #include <unistd.h>
-#include "icmp.h"
+#include "icmp_client.h"
 #include "util.h"
 
 int main(void) {
-	int sock = icmp_open_socket();
+	int sock = icmp_client_open_socket();
 	if (sock < 0) {
-		perror("icmp_open_socket");
+		perror("icmp_client_open_socket");
 		return 1;
 	}
 
 	const char *ip_address = "127.0.0.1";
 	// const char *ip_address = "192.168.43.220";
+	// const char *ip_address = "198.211.118.67";  // tomsmeding.com
 
-	struct icmp_reply reply = icmp_communicate(sock, ip_address, 1234, "kaashandel", 10);
+	struct icmp_client_incoming reply =
+			icmp_client_communicate(sock, ip_address, 42, 1234, "kaashandel", 10);
 	if (reply.data == NULL) {
-		perror("icmp_communicate");
+		perror("icmp_client_communicate");
 		return 1;
 	}
 
diff --git a/icmp.c b/icmp.c
index 63436ac..bef1fd6 100644
--- a/icmp.c
+++ b/icmp.c
@@ -1,24 +1,28 @@
 #include <stdlib.h>
+#include <stdbool.h>
 #include <stddef.h>
 #include <string.h>
 #include <assert.h>
 #include <unistd.h>
 #include <sys/socket.h>
 #include <netinet/in.h>
+#include <netinet/ip.h>
 #include <netinet/ip_icmp.h>
 #include <arpa/inet.h>
-#include "icmp.h"
+#include "icmp_client.h"
+#include "icmp_server.h"
 #include "util.h"
 
 
-#define PROT_ICMP 1
-
-
-static void make_sockaddr(struct sockaddr_in *dst, const char *ip_address) {
+static void make_sockaddr_u32(struct sockaddr_in *dst, uint32_t addr) {
 	memset(dst, 0, sizeof *dst);
 	dst->sin_family = AF_INET;
 	dst->sin_port = htons(0);
-	dst->sin_addr.s_addr = inet_addr(ip_address);
+	dst->sin_addr.s_addr = addr;
+}
+
+static void make_sockaddr(struct sockaddr_in *dst, const char *ip_address) {
+	make_sockaddr_u32(dst, inet_addr(ip_address));
 }
 
 static uint16_t compute_checksum(const void *buf_, size_t buflen) {
@@ -39,18 +43,27 @@ static uint16_t compute_checksum(const void *buf_, size_t buflen) {
 	return res;
 }
 
-int icmp_open_socket(void) {
-	return socket(PF_INET, SOCK_DGRAM, PROT_ICMP);
+int icmp_client_open_socket(void) {
+	return socket(PF_INET, SOCK_DGRAM, IPPROTO_ICMP);
+}
+
+int icmp_server_open_socket(void) {
+	return socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
 }
 
-struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum, const void *data_, size_t length) {
+struct icmp_client_incoming icmp_client_communicate(
+		int sock, const char *ip_address,
+		int id, 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 icmp_client_incoming errret =
+			(struct icmp_client_incoming){.data = NULL, .length = 0, .id = 0, .seqnum = 0};
 
 	struct sockaddr_in addr;
 	make_sockaddr(&addr, ip_address);
@@ -59,7 +72,8 @@ struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum,
 	memset(&msg, 0, sizeof msg);
 	msg.type = ICMP_ECHO;
 	msg.code = 0;
-	msg.seqnum = seqnum;
+	msg.id = htons(id);
+	msg.seqnum = htons(seqnum);
 
 	memcpy(msg.payload, data, length);
 
@@ -90,25 +104,90 @@ struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum,
 
 	memcpy(buffer, msg.payload, payloadlen);
 
-	return (struct icmp_reply){
+	return (struct icmp_client_incoming){
 		.data = buffer,
 		.length = payloadlen,
-		.seqnum = msg.seqnum
+		.id = ntohs(msg.id),
+		.seqnum = ntohs(msg.seqnum)
 	};
 }
 
-int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void *data_, size_t length) {
+struct icmp_server_incoming icmp_server_receive(int sock) {
+	static uint8_t buffer[ICMP_MAX_PAYLOAD_LENGTH];
+
+	struct icmp_server_incoming errret =
+			(struct icmp_server_incoming){.data = NULL, .length = 0, .id = 0, .seqnum = 0, .source_addr = 0};
+
+	char buf[MAX_IP_PACKET_SIZE];
+
+	struct iovec iov1;
+	memset(&iov1, 0, sizeof iov1);
+	iov1.iov_base = buf;
+	iov1.iov_len = sizeof buf;
+
+	struct msghdr recv_msghdr;
+	memset(&recv_msghdr, 0, sizeof recv_msghdr);
+	recv_msghdr.msg_name = NULL;
+	recv_msghdr.msg_iov = &iov1;
+	recv_msghdr.msg_iovlen = 1;
+
+	// TODO: check whether the above stuff needs re-initialisation
+	while (true) {
+		recv_msghdr.msg_flags = 0;
+
+		ssize_t nr = recvmsg(sock, &recv_msghdr, 0);
+		if (nr < 0) return errret;
+
+		// buf now contains received data starting at the IP header
+
+		// printf("Full packet received:\n");
+		// xxd(buf, nr);
+
+		struct iphdr *hdr = (struct iphdr*)buf;
+		int hdr_len = hdr->ihl * 4;
+		uint32_t saddr = hdr->saddr;
+
+		struct icmp_echo *msg = (struct icmp_echo*)(buf + hdr_len);
+		int msg_len = nr - hdr_len;
+		int payloadlen = msg_len - offsetof(struct icmp_echo, payload);
+		// 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, payloadlen);
+
+		if (msg->type != ICMP_ECHO && msg->type != ICMP_ECHOREPLY) {
+			// printf("Not an ICMP_ECHO or ICMP_ECHOREPLY, ignoring\n");
+			continue;
+		} else {
+			memcpy(buffer, msg->payload, payloadlen);
+
+			return (struct icmp_server_incoming){
+				.data = buffer,
+				.length = payloadlen,
+				.type = msg->type,
+				.id = ntohs(msg->id),
+				.seqnum = ntohs(msg->seqnum),
+				.source_addr = saddr
+			};
+		}
+	}
+}
+
+int icmp_server_send_reply(
+			int sock, uint32_t addr_u32,
+			int id, int seqnum,
+			const void *data_, size_t length) {
+
 	const uint8_t *data = (const uint8_t*)data_;
 
 	struct sockaddr_in addr;
-	make_sockaddr(&addr, ip_address);
+	make_sockaddr_u32(&addr, addr_u32);
 
 	struct icmp_echo msg;
 	memset(&msg, 0, sizeof msg);
 	msg.type = ICMP_ECHOREPLY;
 	msg.code = 0;
-	msg.id = id;
-	msg.seqnum = seqnum;
+	msg.id = htons(id);
+	msg.seqnum = htons(seqnum);
 	msg.checksum = 0;
 
 	memcpy(msg.payload, data, length);
@@ -117,13 +196,7 @@ int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void
 
 	msg.checksum = ~compute_checksum(&msg, total_length);
 
-	int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
-	// The below is only necessary for sending on IPPROTO_RAW sockets
-	// int zero = 0;
-	// assert(setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &zero, sizeof zero) >= 0);
-
 	int ret = sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr);
-	close(sock);
 	if (ret < 0) return -1;
 
 	return 0;
diff --git a/icmp.h b/icmp.h
index 3557fc3..4cccb95 100644
--- a/icmp.h
+++ b/icmp.h
@@ -22,23 +22,3 @@ struct __attribute__((packed)) icmp_echo {
 
 #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/icmp_client.h b/icmp_client.h
new file mode 100644
index 0000000..78cfe7a
--- /dev/null
+++ b/icmp_client.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "icmp.h"
+
+
+struct icmp_client_incoming {
+	const uint8_t *data;  // points to internal buffer
+	size_t length;  // length of 'data'
+
+	int id, seqnum;
+};
+
+// Returns -1 on error with errno.
+int icmp_client_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_client_incoming icmp_client_communicate(
+		int sock, const char *ip_address,
+		int id, int seqnum,
+		const void *data, size_t length);
diff --git a/icmp_server.h b/icmp_server.h
new file mode 100644
index 0000000..fbf4158
--- /dev/null
+++ b/icmp_server.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "icmp.h"
+
+#ifndef ICMP_ECHO
+#define ICMP_ECHO 8
+#endif
+#ifndef ICMP_ECHOREPLY
+#define ICMP_ECHOREPLY 0
+#endif
+
+
+struct icmp_server_incoming {
+	const uint8_t *data;  // points to internal buffer
+	size_t length;  // length of 'data'
+
+	int type;  // ICMP_ECHO or ICMP_ECHOREPLY
+	int id, seqnum;
+	uint32_t source_addr;
+};
+
+// Returns -1 on error with errno.
+int icmp_server_open_socket(void);
+
+// Returns {.data=NULL} on error with errno.
+struct icmp_server_incoming icmp_server_receive(int sock);
+
+// Returns -1 on error with errno.
+int icmp_server_send_reply(
+		int sock, uint32_t addr,
+		int id, int seqnum,
+		const void *data, size_t length);
diff --git a/run_server.sh b/run_server.sh
index 50764b1..42b6aef 100755
--- a/run_server.sh
+++ b/run_server.sh
@@ -1,5 +1,9 @@
 #!/usr/bin/env bash
-sudo -v
-echo 1 | sudo tee /proc/sys/net/ipv4/icmp_echo_ignore_all
-sudo ./server
-echo 0 | sudo tee /proc/sys/net/ipv4/icmp_echo_ignore_all
+if [[ "$(id -u)" -ne 0 ]]; then
+	echo >&2 "Run with sudo"
+	exit 1
+fi
+
+echo 1 >/proc/sys/net/ipv4/icmp_echo_ignore_all
+trap "echo 0 >/proc/sys/net/ipv4/icmp_echo_ignore_all" EXIT
+./server
diff --git a/server.c b/server.c
index db269e5..9b16173 100644
--- a/server.c
+++ b/server.c
@@ -1,108 +1,37 @@
 #include <stdio.h>
 #include <stdbool.h>
-#include <stddef.h>
 #include <stdlib.h>
-#include <string.h>
 #include <unistd.h>
-#include <stdint.h>
-#include <arpa/inet.h>
-#include <netinet/ip.h>
-#include <netinet/ip_icmp.h>
-#include "icmp.h"
+#include "icmp_server.h"
 #include "util.h"
 
 
-struct state {
-	int socket;
-};
-
-#if 0
-static int icmp_callback(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;
-}
-#endif
-
-
 int main(void) {
-	// struct state state;
-	// state.socket = icmp_open_socket();
-	// if (state.socket < 0) {
-	//     perror("icmp_open_socket");
-	//     return 1;
-	// }
-
-	int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
-
-	struct icmp_echo msg;
+	int sock = icmp_server_open_socket();
+	if (sock < 0) {
+		perror("icmp_server_open_socket");
+		return 1;
+	}
 
-	char buf[MAX_IP_PACKET_SIZE];
 	while (true) {
-		struct iovec iov1;
-		memset(&iov1, 0, sizeof iov1);
-		iov1.iov_base = buf;
-		iov1.iov_len = sizeof buf;
-
-		struct sockaddr_in addr;
-
-		struct msghdr recv_msghdr;
-		memset(&recv_msghdr, 0, sizeof recv_msghdr);
-		recv_msghdr.msg_name = &addr;
-		recv_msghdr.msg_namelen = sizeof addr;
-		recv_msghdr.msg_iov = &iov1;
-		recv_msghdr.msg_iovlen = 1;
-
-		ssize_t nr = recvmsg(sock, &recv_msghdr, 0);
-		if (nr < 0) break;
-
-		// buf now contains received data starting at the IP header
-
-		printf("Full packet received:\n");
-		xxd(buf, nr);
-
-		struct iphdr *hdr = (struct iphdr*)buf;
-		int hdr_len = hdr->ihl * 4;
-		uint32_t saddr = hdr->saddr;
+		struct icmp_server_incoming msg = icmp_server_receive(sock);
+		if (msg.data == NULL) {
+			perror("icmp_server_communicate");
+			return 1;
+		}
 
-		struct icmp_echo *msg = (struct icmp_echo*)(buf + hdr_len);
-		int msg_len = nr - 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));
+		printf("Received: type %d   id %d   seqnum %d   data:\n", msg.type, msg.id, msg.seqnum);
+		xxd(msg.data, msg.length);
 
-		if (msg->type != ICMP_ECHO) {
+		if (msg.type != ICMP_ECHO) {
 			printf("Not an ICMP_ECHO, ignoring\n");
 			continue;
 		}
 
-		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");
+		if (icmp_server_send_reply(sock, msg.source_addr, msg.id, msg.seqnum, "dank je wel", 11) < 0) {
+			perror("icmp_server_send_reply");
 		}
 	}
+
+	close(sock);
 }