diff options
| -rw-r--r-- | .gitignore | 3 | ||||
| -rw-r--r-- | Makefile | 24 | ||||
| -rw-r--r-- | client.c | 30 | ||||
| -rw-r--r-- | icmp.c | 128 | ||||
| -rw-r--r-- | icmp.h | 44 | ||||
| -rw-r--r-- | server.c | 99 | ||||
| -rw-r--r-- | util.c | 30 | ||||
| -rw-r--r-- | util.h | 6 |
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); +} @@ -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; +} @@ -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); +} @@ -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; + } +} @@ -0,0 +1,6 @@ +#pragma once + +#include <stdlib.h> + + +void xxd(const void *buf, size_t length); |