From 5722c47aa3402f1458da9eec332153a454a540b7 Mon Sep 17 00:00:00 2001 From: Tom Smeding 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. --- .gitignore | 3 ++ Makefile | 24 ++++++++++++ client.c | 30 +++++++++++++++ icmp.c | 128 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ icmp.h | 44 +++++++++++++++++++++ server.c | 99 +++++++++++++++++++++++++++++++++++++++++++++++ util.c | 30 +++++++++++++++ util.h | 6 +++ 8 files changed, 364 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 client.c create mode 100644 icmp.c create mode 100644 icmp.h create mode 100644 server.c create mode 100644 util.c create mode 100644 util.h 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 +#include +#include +#include +#include +#include +#include +#include +#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 +#include +#include +#include +#include +#include +#include +#include +#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 + + +#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 +#include +#include +#include +#include +#include +#include +#include + +typedef uint8_t u_int8_t; +typedef uint16_t u_int16_t; +typedef uint32_t u_int32_t; +#include + +#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 +#include +#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 + + +void xxd(const void *buf, size_t length); -- cgit v1.2.3-70-g09d2