From 5722c47aa3402f1458da9eec332153a454a540b7 Mon Sep 17 00:00:00 2001
From: Tom Smeding <tom.smeding@gmail.com>
Date: Mon, 23 Jul 2018 15:48:44 +0200
Subject: 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.
---
 icmp.c | 128 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 128 insertions(+)
 create mode 100644 icmp.c

(limited to 'icmp.c')

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;
+}
-- 
cgit v1.2.3-70-g09d2