Implement key=value parsing

4 files changed, 411 insertions, 46 deletions

diff --git a/Makefile b/Makefile
index 26f6874..d7dce6a 100644
--- a/Makefile
+++ b/Makefile
@@ -22,7 +22,7 @@ endif
 PLATFORM ?= $(shell uname -s | tr '[:upper:]' '[:lower:]')
 
 CFLAGS ?= -O3
-CFLAGS += -std=gnu99 -D_GNU_SOURCE
+CFLAGS += -std=gnu99
 CFLAGS += -Wall -Wextra
 CFLAGS += -MMD -MP
 CFLAGS += -DRUNSTATEDIR="\"$(RUNSTATEDIR)\""
@@ -45,15 +45,15 @@ endif
 
 all: wg-dynamic-server wg-dynamic-client
 
-wg-dynamic-client: wg-dynamic-client.o netlink.o
-wg-dynamic-server: wg-dynamic-server.o netlink.o
+wg-dynamic-client: wg-dynamic-client.o netlink.o common.o
+wg-dynamic-server: wg-dynamic-server.o netlink.o common.o
 
 ifneq ($(V),1)
 clean:
-	@for i in wg-dynamic *.o *.d; do echo "  RM      $$i"; $(RM) "$$i"; done
+	@for i in wg-dynamic-client wg-dynamic-server *.o *.d; do echo "  RM      $$i"; $(RM) "$$i"; done
 else
 clean:
-	$(RM) wg-dynamic *.o *.d
+	$(RM) wg-dynamic-client wg-dynamic-server *.o *.d
 endif
 
 install: wg
diff --git a/common.c b/common.c
new file mode 100644
index 0000000..10883fe
--- /dev/null
+++ b/common.c
@@ -0,0 +1,286 @@
+/* SPDX-License-Identifier: MIT
+ *
+ * Copyright (C) 2019 WireGuard LLC. All Rights Reserved.
+ */
+
+#define _DEFAULT_SOURCE
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <arpa/inet.h>
+
+#include "common.h"
+#include "dbg.h"
+
+static bool parse_ip_cidr(struct wg_combined_ip *ip, char *value)
+{
+	uintmax_t res;
+	char *endptr;
+	char *sep = strchr(value, '/');
+	if (!sep)
+		return false;
+
+	*sep = '\0';
+	if (inet_pton(ip->family, value, &ip->ip) != 1)
+		return false;
+
+	res = strtoumax(sep + 1, &endptr, 10);
+	if (res > UINT8_MAX || *endptr != '\0' || sep + 1 == endptr)
+		return false;
+
+	// TODO: validate cidr range depending on ip->family
+	ip->cidr = (uint8_t)res;
+
+	return true;
+}
+
+static struct wg_dynamic_attr *parse_value(enum wg_dynamic_key key, char *value)
+{
+	struct wg_dynamic_attr *attr;
+	size_t len;
+	char *endptr;
+	uintmax_t uresult;
+	union {
+		uint32_t leasetime;
+		struct wg_combined_ip ip;
+	} data = { 0 };
+
+	switch (key) {
+	case WGKEY_IPV4:
+		len = sizeof data.ip;
+		data.ip.family = AF_INET;
+		if (!parse_ip_cidr(&data.ip, value))
+			return NULL;
+
+		break;
+	case WGKEY_IPV6:
+		len = sizeof data.ip;
+		data.ip.family = AF_INET6;
+		if (!parse_ip_cidr(&data.ip, value))
+			return NULL;
+
+		break;
+	case WGKEY_LEASETIME:
+		len = sizeof data.leasetime;
+		uresult = strtoumax(value, &endptr, 10);
+		if (uresult > UINT32_MAX || *endptr != '\0')
+			return NULL;
+
+		data.leasetime = (uint32_t)uresult;
+		break;
+	default:
+		debug("Invalid key %d, aborting\n", key);
+		abort();
+	}
+
+	attr = malloc(sizeof(struct wg_dynamic_attr) + len);
+	if (!attr)
+		fatal("malloc()");
+
+	attr->len = len;
+	attr->key = key;
+	attr->next = NULL;
+	memcpy(&attr->value, &data, len);
+
+	return attr;
+}
+
+static enum wg_dynamic_key parse_key(char *key)
+{
+	for (enum wg_dynamic_key e = 1; e < ARRAY_SIZE(WG_DYNAMIC_KEY); ++e)
+		if (!strcmp(key, WG_DYNAMIC_KEY[e]))
+			return e;
+
+	return WGKEY_UNKNOWN;
+}
+
+/* Consumes one full line from buf, or up to MAX_LINESIZE-1 bytes if no newline
+ * character was found.
+ * If req != NULL then we expect to parse a command and will set cmd and version
+ * of req accordingly, while *attr will be set to NULL.
+ * Otherwise we expect to parse a normal key=value pair, that will be stored
+ * in a newly allocated wg_dynamic_attr, pointed to by *attr.
+ *
+ * Return values:
+ *   > 0 : Amount of bytes consumed (<= MAX_LINESIZE)
+ *   < 0 : Error
+ *   = 0 : End of message
+ */
+static ssize_t parse_line(unsigned char *buf, size_t len,
+			  struct wg_dynamic_attr **attr,
+			  struct wg_dynamic_request *req)
+{
+	unsigned char *line_end, *key_end;
+	enum wg_dynamic_key key;
+	ssize_t line_len;
+	char *endptr;
+	uintmax_t res;
+
+	line_end = memchr(buf, '\n', len > MAX_LINESIZE ? MAX_LINESIZE : len);
+	if (!line_end) {
+		if (len >= MAX_LINESIZE)
+			return -E2BIG;
+
+		*attr = malloc(sizeof(struct wg_dynamic_attr) + len);
+		if (!*attr)
+			fatal("malloc()");
+
+		(*attr)->key = WGKEY_INCOMPLETE;
+		(*attr)->len = len;
+		(*attr)->next = NULL;
+		memcpy((*attr)->value, buf, len);
+
+		return len;
+	}
+
+	if (line_end == buf)
+		return 0; /* \n\n - end of message */
+
+	*line_end = '\0';
+	line_len = line_end - buf + 1;
+
+	key_end = memchr(buf, '=', line_len - 1);
+	if (!key_end)
+		return -EINVAL;
+
+	*key_end = '\0';
+	key = parse_key((char *)buf);
+	if (key == WGKEY_UNKNOWN)
+		return -ENOENT;
+
+	if (req) {
+		if (key >= WGKEY_ENDCMD)
+			return -ENOENT;
+
+		*attr = NULL;
+		res = strtoumax((char *)key_end + 1, &endptr, 10);
+
+		if (res > UINT32_MAX || *endptr != '\0')
+			return -EINVAL;
+
+		req->cmd = key;
+		req->version = (uint32_t)res;
+
+		if (req->version != 1)
+			return -EPROTONOSUPPORT;
+	} else {
+		if (key <= WGKEY_ENDCMD)
+			return -ENOENT;
+
+		*attr = parse_value(key, (char *)key_end + 1);
+		if (!*attr)
+			return -EINVAL;
+	}
+
+	return line_len;
+}
+
+void free_wg_dynamic_request(struct wg_dynamic_request *req)
+{
+	struct wg_dynamic_attr *prev, *cur = req->first;
+
+	while (cur) {
+		prev = cur;
+		cur = cur->next;
+		free(prev);
+	}
+
+	req->cmd = WGKEY_UNKNOWN;
+	req->version = 0;
+	req->first = NULL;
+	req->last = NULL;
+}
+
+static int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
+			 size_t len)
+{
+	struct wg_dynamic_attr *attr;
+	size_t offset = 0;
+	ssize_t ret;
+
+	if (memchr(buf, '\0', len))
+		return -EINVAL; /* don't allow null bytes */
+
+	if (req->last && req->last->key == WGKEY_INCOMPLETE) {
+		len += req->last->len;
+
+		memmove(buf + req->last->len, buf, len);
+		memcpy(buf, req->last->value, req->last->len);
+		free(req->last);
+
+		if (req->first == req->last) {
+			req->first = NULL;
+			req->last = NULL;
+		} else {
+			attr = req->first;
+			while (attr->next != req->last)
+				attr = attr->next;
+
+			attr->next = NULL;
+			req->last = attr;
+		}
+	}
+
+	while (len > 0) {
+		ret = parse_line(buf + offset, len, &attr,
+				 req->cmd == WGKEY_UNKNOWN ? req : NULL);
+		if (ret <= 0)
+			return ret; /* either error or message complete */
+
+		len -= ret;
+		offset += ret;
+		if (!attr)
+			continue;
+
+		if (!req->first)
+			req->first = attr;
+		else
+			req->last->next = attr;
+
+		req->last = attr;
+	}
+
+	return 1;
+}
+
+bool handle_request(int fd, struct wg_dynamic_request *req,
+		    void (*success)(int, struct wg_dynamic_request *req),
+		    void (*error)(int, int))
+{
+	ssize_t bytes;
+	int ret;
+	unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE];
+
+	while (1) {
+		bytes = read(fd, buf, RECV_BUFSIZE);
+		if (bytes < 0) {
+			if (errno == EWOULDBLOCK || errno == EAGAIN)
+				break;
+
+			// TODO: handle EINTR
+
+			debug("Reading from socket failed: %s\n",
+			      strerror(errno));
+			return true;
+		} else if (bytes == 0) {
+			debug("Client disconnected unexpectedly\n");
+			return true;
+		}
+
+		ret = parse_request(req, buf, bytes);
+		if (ret < 0) {
+			error(fd, -ret);
+			return true;
+		} else if (ret == 0) {
+			success(fd, req);
+			return true;
+		}
+	}
+
+	return false;
+}
diff --git a/common.h b/common.h
index b7ea136..10ddf61 100644
--- a/common.h
+++ b/common.h
@@ -6,11 +6,67 @@
 #ifndef __COMMON_H__
 #define __COMMON_H__
 
+#include <stdbool.h>
 #include <stdint.h>
+#include <stdlib.h>
+
+#include <netinet/in.h>
 
 #define MAX_CONNECTIONS 16
 
+#define MAX_LINESIZE 4096
+
+#define RECV_BUFSIZE 8192
+
 static const char WG_DYNAMIC_ADDR[] = "fe80::";
 static const uint16_t WG_DYNAMIC_PORT = 1337;
 
+#define ITEMS                                                                  \
+	E(WGKEY_UNKNOWN, "") /* must be the first entry */                     \
+	/* CMD START */                                                        \
+	E(WGKEY_REQUEST_IP, "request_ip")                                      \
+	E(WGKEY_ENDCMD, "")                                                    \
+	/* CMD END */                                                          \
+	E(WGKEY_INCOMPLETE, "")                                                \
+	E(WGKEY_IPV4, "ipv4")                                                  \
+	E(WGKEY_IPV6, "ipv6")                                                  \
+	E(WGKEY_LEASETIME, "leasetime")
+
+#define E(x, y) x,
+enum wg_dynamic_key { ITEMS };
+#undef E
+#define E(x, y) y,
+static const char *const WG_DYNAMIC_KEY[] = { ITEMS };
+#undef E
+#undef ITEMS
+
+struct wg_dynamic_attr {
+	enum wg_dynamic_key key;
+	size_t len;
+	struct wg_dynamic_attr *next;
+	unsigned char value[];
+};
+
+struct wg_dynamic_request {
+	enum wg_dynamic_key cmd;
+	uint32_t version;
+	struct wg_dynamic_attr *first, *last;
+};
+
+struct wg_combined_ip {
+	uint16_t family;
+	union {
+		struct in_addr ip4;
+		struct in6_addr ip6;
+	} ip;
+	uint8_t cidr;
+};
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+
+void free_wg_dynamic_request(struct wg_dynamic_request *req);
+bool handle_request(int fd, struct wg_dynamic_request *req,
+		    void (*success)(int, struct wg_dynamic_request *req),
+		    void (*error)(int, int));
+
 #endif
diff --git a/wg-dynamic-server.c b/wg-dynamic-server.c
index 8f27acf..81e53f6 100644
--- a/wg-dynamic-server.c
+++ b/wg-dynamic-server.c
@@ -3,15 +3,16 @@
  * Copyright (C) 2015-2019 WireGuard LLC. All Rights Reserved.
  */
 
+#define _GNU_SOURCE
 #define _POSIX_C_SOURCE 200112L
 
-#include <arpa/inet.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
 #include <time.h>
 
+#include <arpa/inet.h>
+#include <fcntl.h>
 #include <poll.h>
 #include <sys/socket.h>
 #include <sys/types.h>
@@ -175,46 +176,63 @@ static bool valid_peer_found(wg_device *device)
 	return false;
 }
 
-static void accept_connection(struct pollfd pollfds[])
+static int get_avail_pollfds()
 {
-	int nfds;
-
-	pollfds[0].revents = 0;
-	for (nfds = 1;; ++nfds) {
+	for (int nfds = 1;; ++nfds) {
 		if (nfds >= MAX_CONNECTIONS + 1)
-			return;
+			return -1;
 
 		if (pollfds[nfds].fd < 0)
-			break;
+			return nfds;
 	}
-
-	pollfds[nfds].fd = accept(pollfds[0].fd, NULL, NULL);
-	if (pollfds[nfds].fd < 0)
-		fatal("failed to accept connection");
 }
 
-static int handle_request(int fd)
+static int accept_connection(int sockfd)
 {
-	ssize_t read;
-	uint8_t buf[8192];
+	int fd;
+#ifdef __linux__
+	fd = accept4(sockfd, NULL, NULL, SOCK_NONBLOCK);
+	if (fd < 0)
+		fatal("Failed to accept connection");
+#else
+	fd = accept(sockfd, NULL, NULL);
+	if (fd < 0)
+		fatal("Failed to accept connection");
+
+	int res = fcntl(fd, F_GETFL, 0);
+	if (res < 0 || fcntl(fd, F_SETFL, res | O_NONBLOCK) < 0)
+		fatal("Setting socket to nonblocking failed");
+#endif
+	return fd;
+}
 
-	read = recv(fd, buf, sizeof buf, 0);
-	if (read == -1)
-		fatal("recv()");
+static void close_connection(int *fd, struct wg_dynamic_request *req)
+{
+	if (close(*fd))
+		debug("Failed to close socket");
 
-	buf[read] = '\0';
-	debug("%s", buf);
-	// TODO: do some actual parsing
+	*fd = -1;
+	free_wg_dynamic_request(req);
+}
 
-	if (close(fd))
-		debug("failed to close accept() socket");
+static void send_response(int fd, struct wg_dynamic_request *req)
+{
+	printf("Recieved request of type %s.\n", WG_DYNAMIC_KEY[req->cmd]);
+	struct wg_dynamic_attr *cur = req->first;
+	while (cur) {
+		printf("  with attr %s.\n", WG_DYNAMIC_KEY[cur->key]);
+		cur = cur->next;
+	}
+}
 
-	return 1;
+static void send_error(int fd, int ret)
+{
+	debug("Error: %s\n", strerror(ret));
 }
 
 static void setup_socket(int *fd)
 {
-	int res, val = 1;
+	int val = 1;
 	struct sockaddr_in6 addr = {
 		.sin6_family = AF_INET6,
 		.sin6_port = htons(WG_DYNAMIC_PORT),
@@ -224,19 +242,16 @@ static void setup_socket(int *fd)
 
 	*fd = socket(AF_INET6, SOCK_STREAM, 0);
 	if (*fd < 0)
-		fatal("creating a socket failed");
+		fatal("Creating a socket failed");
 
-	res = setsockopt(*fd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof val);
-	if (res == -1)
-		fatal("setting socket option failed");
+	if (setsockopt(*fd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof val) == -1)
+		fatal("Setting socket option failed");
 
-	res = bind(*fd, (struct sockaddr *)&addr, sizeof(addr));
-	if (res == -1)
-		fatal("binding socket failed");
+	if (bind(*fd, (struct sockaddr *)&addr, sizeof(addr)) == -1)
+		fatal("Binding socket failed");
 
-	res = listen(*fd, SOMAXCONN);
-	if (res == -1)
-		fatal("listening to socket failed");
+	if (listen(*fd, SOMAXCONN) == -1)
+		fatal("Listening to socket failed");
 }
 
 static void cleanup()
@@ -254,6 +269,8 @@ static void cleanup()
 
 int main(int argc, char *argv[])
 {
+	struct wg_dynamic_request reqs[MAX_CONNECTIONS] = { 0 };
+	int *sockfd = &pollfds[0].fd, n;
 	const char *iface;
 
 	progname = argv[0];
@@ -283,22 +300,28 @@ int main(int argc, char *argv[])
 	if (!valid_peer_found(device))
 		die("%s has no peers with link-local allowedips\n", iface);
 
-	setup_socket(&pollfds[0].fd);
+	setup_socket(sockfd);
 
 	while (1) {
 		if (poll(pollfds, MAX_CONNECTIONS + 1, -1) == -1)
 			fatal("Failed to poll() fds");
 
-		if (pollfds[0].revents & POLLIN)
-			accept_connection(pollfds);
+		if (pollfds[0].revents & POLLIN) {
+			n = get_avail_pollfds();
+			if (n >= 0) {
+				pollfds[0].revents = 0;
+				pollfds[n].fd = accept_connection(*sockfd);
+			}
+		}
 
 		for (int i = 1; i < MAX_CONNECTIONS + 1; ++i) {
 			if (!(pollfds[i].revents & POLLIN))
 				continue;
 
 			pollfds[i].revents = 0;
-			if (handle_request(pollfds[i].fd) > 0)
-				pollfds[i].fd = -1;
+			if (handle_request(pollfds[i].fd, &reqs[i - 1],
+					   send_response, send_error))
+				close_connection(&pollfds[i].fd, &reqs[i - 1]);
 		}
 	}