WIP: parsing (2)kv_parsing

4 files changed, 194 insertions, 77 deletions

diff --git a/Makefile b/Makefile
index 5924557..d7dce6a 100644
--- a/Makefile
+++ b/Makefile
@@ -50,10 +50,10 @@ 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
index d7cdbcc..1610512 100644
--- a/common.c
+++ b/common.c
@@ -1,44 +1,77 @@
+/* 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 <arpa/inet.h>
 
 #include "common.h"
 #include "dbg.h"
 
-struct wg_dynamic_attr *parse_value(enum wg_dynamic_key cmd, char *value)
+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;
-	void *src;
-	struct in_addr v4addr;
-	struct in6_addr v6addr;
 	char *endptr;
-	uintmax_t res;
+	union {
+		uintmax_t uresult;
+		struct wg_combined_ip ip;
+	} data = { 0 };
 
-	switch (cmd) {
+	switch (key) {
 	case WGKEY_IPV4:
-		len = sizeof(struct in_addr);
-		if (inet_pton(AF_INET, value, &v4addr))
+		len = sizeof data.ip;
+		data.ip.family = AF_INET;
+		if (!parse_ip_cidr(&data.ip, value))
 			return NULL;
 
-		src = &v4addr;
 		break;
 	case WGKEY_IPV6:
-		len = sizeof(struct in6_addr);
-		if (inet_pton(AF_INET6, value, &v6addr))
+		len = sizeof data.ip;
+		data.ip.family = AF_INET6;
+		if (!parse_ip_cidr(&data.ip, value))
 			return NULL;
 
-		src = &v6addr;
 		break;
 	case WGKEY_LEASETIME:
-		len = sizeof(uint32_t);
-		res = strtoumax(value, &endptr, 10);
-		if (res > UINT32_MAX || *endptr != '\0')
+		len = sizeof data.uresult;
+		data.uresult = strtoumax(value, &endptr, 10);
+		if (data.uresult > UINT32_MAX || *endptr != '\0')
 			return NULL;
 
-		src = &res;
 		break;
 	default:
+		debug("Invalid key %d, aborting\n", key);
 		abort();
 	}
 
@@ -47,24 +80,37 @@ struct wg_dynamic_attr *parse_value(enum wg_dynamic_key cmd, char *value)
 		fatal("malloc()");
 
 	attr->len = len;
-	memcpy(&attr->value, src, len);
+	attr->key = key;
+	attr->next = NULL;
+	memcpy(&attr->value, &data, len);
 
 	return attr;
 }
 
-enum wg_dynamic_key parse_key(char *key)
+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]))
+		if (!strcmp(key, WG_DYNAMIC_KEY[e]))
 			return e;
 
 	return WGKEY_UNKNOWN;
 }
 
-/* consume N bytes (and return that amount) and turn it into a attr */
-ssize_t parse_line(unsigned char *buf, size_t len,
-		   struct wg_dynamic_attr **attr,
-		   struct wg_dynamic_request *req)
+/* 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;
@@ -75,7 +121,7 @@ ssize_t parse_line(unsigned char *buf, size_t len,
 	line_end = memchr(buf, '\n', len > MAX_LINESIZE ? MAX_LINESIZE : len);
 	if (!line_end) {
 		if (len >= MAX_LINESIZE)
-			return -1;
+			return -E2BIG;
 
 		*attr = malloc(sizeof(struct wg_dynamic_attr) + len);
 		if (!*attr)
@@ -83,55 +129,71 @@ ssize_t parse_line(unsigned char *buf, size_t len,
 
 		(*attr)->key = WGKEY_INCOMPLETE;
 		(*attr)->len = len;
+		(*attr)->next = NULL;
 		memcpy((*attr)->value, buf, len);
 
+		debug("Copied '%s' (%zu bytes)\n", (*attr)->value, len);
 		return len;
 	}
 
-	if (len == 1)
-		return -2; // TODO: \n\n
+	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 -1;
+		return -EINVAL;
 
 	*key_end = '\0';
 	key = parse_key((char *)buf);
 	if (key == WGKEY_UNKNOWN)
-		return -1;
+		return -ENOENT;
 
-	if (!req) {
+	if (req) {
 		if (key >= WGKEY_ENDCMD)
-			return -1; // TODO: unknown command, abort
+			return -ENOENT;
 
 		*attr = NULL;
 		res = strtoumax((char *)key_end + 1, &endptr, 10);
 
-		// TODO: test case where input is empty
 		if (res > UINT32_MAX || *endptr != '\0')
-			return -1;
+			return -EINVAL;
 
 		req->cmd = key;
 		req->version = (uint32_t)res;
 
 		if (req->version != 1)
-			return -1; // TODO: unknown version
+			return -EPROTONOSUPPORT;
 	} else {
 		if (key <= WGKEY_ENDCMD)
-			return -1;
+			return -ENOENT;
 
-		// TODO: empty key?
-		*attr = parse_value(req->cmd, (char *)key_end + 1);
+		*attr = parse_value(key, (char *)key_end + 1);
 		if (!*attr)
-			return -1;
+			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;
+}
+
 int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 		  size_t len)
 {
@@ -140,7 +202,7 @@ int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 	ssize_t ret;
 
 	if (memchr(buf, '\0', len))
-		return -1; /* don't allow null bytes */
+		return -EINVAL; /* don't allow null bytes */
 
 	if (req->last && req->last->key == WGKEY_INCOMPLETE) {
 		len += req->last->len;
@@ -149,9 +211,17 @@ int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 		memcpy(buf, req->last->value, req->last->len);
 		free(req->last);
 
-		req->last = req->first;
-		while (!req->last->next)
-			req->last = req->last->next;
+		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) {
@@ -160,6 +230,9 @@ int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 		if (ret < 0)
 			return ret;
 
+		if (ret == 0)
+			return 0; /* \n\n - message complete */
+
 		len -= ret;
 		offset += ret;
 		if (!attr)
@@ -173,5 +246,5 @@ int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 		req->last = attr;
 	}
 
-	return 0;
+	return 1;
 }
diff --git a/common.h b/common.h
index fbb0c95..91dbf2a 100644
--- a/common.h
+++ b/common.h
@@ -9,6 +9,8 @@
 #include <stdint.h>
 #include <stdlib.h>
 
+#include <netinet/in.h>
+
 #define MAX_CONNECTIONS 16
 
 #define MAX_LINESIZE 4096
@@ -50,8 +52,18 @@ struct wg_dynamic_request {
 	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);
 int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 		  size_t len);
 
diff --git a/wg-dynamic-server.c b/wg-dynamic-server.c
index b3cd417..25660db 100644
--- a/wg-dynamic-server.c
+++ b/wg-dynamic-server.c
@@ -189,50 +189,80 @@ static int get_avail_pollfds()
 	}
 }
 
-static void accept_connection(int fd, struct pollfd *pfd)
+static int accept_connection(int sockfd)
 {
-	// struct sockaddr_storage;
-
+	int fd;
 #ifdef __linux__
-	pfd->fd = accept4(fd, NULL, NULL, SOCK_NONBLOCK);
+	fd = accept4(sockfd, NULL, NULL, SOCK_NONBLOCK);
+	if (fd < 0)
+		fatal("Failed to accept connection");
 #else
-	pfd->fd = accept(fd, NULL, NULL);
-	int res = fcntl(pfd->fd, F_GETFL, 0);
-	if (res < 0 || fcntl(pfd->fd, F_SETFL, res | O_NONBLOCK) < 0)
+	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;
+}
 
-	if (pfd->fd < 0)
-		fatal("Failed to accept connection");
+static void close_connection(int *fd, struct wg_dynamic_request *req)
+{
+	if (close(*fd))
+		debug("Failed to close socket");
+
+	*fd = -1;
+	free_wg_dynamic_request(req);
 }
 
-static int handle_request(int fd)
+static void response(struct wg_dynamic_request *req)
 {
-	ssize_t read;
+	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;
+	}
+}
+
+static bool handle_request(int fd, struct wg_dynamic_request *req)
+{
+	ssize_t bytes;
+	int ret;
 	unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE];
-	struct wg_dynamic_request req = {
-		.cmd = WGKEY_UNKNOWN,
-		.version = 0,
-		.first = NULL,
-		.last = NULL,
-	};
 
 	while (1) {
-		read = recv(fd, buf, RECV_BUFSIZE, 0);
-		if (read >= 0) {
-			parse_request(&req, buf, read);
-		} else {
+		/* read = recv(fd, buf, RECV_BUFSIZE, 0); */
+		bytes = read(fd, buf, RECV_BUFSIZE);
+		if (bytes < 0) {
 			if (errno == EWOULDBLOCK || errno == EAGAIN)
 				break;
 
-			fatal("recv()");
+			// 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;
 		}
-	}
 
-	if (close(fd))
-		debug("failed to close accept() socket");
+		ret = parse_request(req, buf, bytes);
+		if (ret < 0) {
+			// TODO: send error message back
+			debug("Error: %s\n", strerror(-ret));
+			return true;
+		} else if (ret == 0) {
+			// TODO: complete message, validate? and answer
+			response(req);
+			return true;
+		}
+	}
 
-	return 1;
+	return false;
 }
 
 static void setup_socket(int *fd)
@@ -274,8 +304,9 @@ 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;
-	int n;
 
 	progname = argv[0];
 	inet_pton(AF_INET6, WG_DYNAMIC_ADDR, &well_known);
@@ -304,17 +335,18 @@ 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) {
-			pollfds[0].revents = 0;
 			n = get_avail_pollfds();
-			if (n >= 0)
-				accept_connection(pollfds[0].fd, &pollfds[n]);
+			if (n >= 0) {
+				pollfds[0].revents = 0;
+				pollfds[n].fd = accept_connection(*sockfd);
+			}
 		}
 
 		for (int i = 1; i < MAX_CONNECTIONS + 1; ++i) {
@@ -322,8 +354,8 @@ int main(int argc, char *argv[])
 				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]))
+				close_connection(&pollfds[i].fd, &reqs[i - 1]);
 		}
 	}