From 1b5a1c72c734d03ae45b5eae1a18978fa110d14a Mon Sep 17 00:00:00 2001
From: Thomas Gschwantner <tharre3@gmail.com>
Date: Wed, 25 Sep 2019 18:13:13 +0200
Subject: Serialize/deserialize messages into a struct

Instead of a list of attributes, parse messages into a proper struct to
avoid duplicating code in the server/client for handling this list, as
well as making parsing nicer in general.
---
 common.c            | 389 ++++++++++++++++++++++++++--------------------------
 common.h            |  39 +++---
 wg-dynamic-server.c | 204 +++++++++++++++------------
 3 files changed, 331 insertions(+), 301 deletions(-)

diff --git a/common.c b/common.c
index 3edf872..a0f1ab0 100644
--- a/common.c
+++ b/common.c
@@ -20,6 +20,60 @@
 #include "common.h"
 #include "dbg.h"
 
+union kvalues {
+	uint32_t u32;
+	struct wg_combined_ip ip;
+	char errmsg[256];
+};
+
+static void request_ip(enum wg_dynamic_key key, union kvalues kv, void **dest)
+{
+	struct wg_combined_ip *ip = &kv.ip;
+	struct wg_dynamic_request_ip *r = (struct wg_dynamic_request_ip *)*dest;
+
+	switch (key) {
+	case WGKEY_REQUEST_IP:
+		BUG_ON(*dest);
+		*dest = calloc(1, sizeof(struct wg_dynamic_request_ip));
+		if (!*dest)
+			fatal("calloc()");
+
+		break;
+	case WGKEY_IPV4:
+		memcpy(&r->ipv4, &ip->ip4, sizeof r->ipv4);
+		r->cidrv4 = ip->cidr;
+		r->has_ipv4 = true;
+		break;
+	case WGKEY_IPV6:
+		memcpy(&r->ipv6, &ip->ip6, sizeof r->ipv6);
+		r->cidrv6 = ip->cidr;
+		r->has_ipv6 = true;
+		break;
+	case WGKEY_LEASESTART:
+		r->start = kv.u32;
+		break;
+	case WGKEY_LEASETIME:
+		r->leasetime = kv.u32;
+		break;
+	case WGKEY_ERRNO:
+		r->wg_errno = kv.u32;
+		break;
+	case WGKEY_ERRMSG:
+		r->errmsg = strdup(kv.errmsg);
+		break;
+	default:
+		debug("Invalid key %d, aborting\n", key);
+		BUG();
+	}
+}
+
+static void (*const deserialize_fptr[])(enum wg_dynamic_key key,
+					union kvalues kv, void **dest) = {
+	NULL,
+	NULL,
+	request_ip,
+};
+
 static bool parse_ip_cidr(struct wg_combined_ip *ip, char *value)
 {
 	uintmax_t res;
@@ -50,281 +104,253 @@ static bool parse_ip_cidr(struct wg_combined_ip *ip, char *value)
 	return true;
 }
 
-static struct wg_dynamic_attr *parse_value(enum wg_dynamic_key key, char *value)
+static bool parse_value(enum wg_dynamic_key key, char *str, union kvalues *kv)
 {
-	struct wg_dynamic_attr *attr;
-	size_t len;
 	char *endptr;
 	uintmax_t uresult;
-	union {
-		uint32_t uint32;
-		char errmsg[72];
-		struct wg_combined_ip ip;
-	} data = { 0 };
+	struct wg_combined_ip *ip;
 
 	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;
+		ip = &kv->ip;
+		ip->family = (key == WGKEY_IPV4) ? AF_INET : AF_INET6;
+		if (!parse_ip_cidr(ip, str))
+			return false;
 
 		break;
+	case WGKEY_REQUEST_IP:
 	case WGKEY_LEASESTART:
 	case WGKEY_LEASETIME:
 	case WGKEY_ERRNO:
-		len = sizeof data.uint32;
-		uresult = strtoumax(value, &endptr, 10);
+		uresult = strtoumax(str, &endptr, 10);
 		if (uresult > UINT32_MAX || *endptr != '\0')
-			return NULL;
-		data.uint32 = (uint32_t)uresult;
+			return false;
 
+		kv->u32 = (uint32_t)uresult;
 		break;
 	case WGKEY_ERRMSG:
-		strncpy(data.errmsg, value, sizeof data.errmsg - 1);
-		data.errmsg[sizeof data.errmsg - 1] = '\0';
-		len = MIN(sizeof data.errmsg,
-			  strlen(value) + 1); /* Copying the NUL byte too. */
-
+		strncpy(kv->errmsg, str, sizeof kv->errmsg);
+		kv->errmsg[sizeof kv->errmsg - 1] = '\0';
 		break;
 	default:
 		debug("Invalid key %d, aborting\n", key);
 		BUG();
 	}
 
-	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;
+	return true;
 }
 
 static enum wg_dynamic_key parse_key(char *key)
 {
-	for (enum wg_dynamic_key e = 1; e < ARRAY_SIZE(WG_DYNAMIC_KEY); ++e)
+	for (enum wg_dynamic_key e = 2; 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.
+/* Consumes one full line from buf, or up to MAX_LINESIZE bytes if no newline
+ * character was found. If less then MAX_LINESIZE bytes are available, a new
+ * buffer will be allocated and req->buf and req->len set accordingly.
  *
  * Return values:
  *   > 0 : Amount of bytes consumed (<= MAX_LINESIZE)
+ *   = 0 : Consumed len bytes; need more for a full line
  *   < 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)
+			  struct wg_dynamic_request *req,
+			  enum wg_dynamic_key *key, union kvalues *kv)
 {
 	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);
+	line_end = memchr(buf, '\n', MIN(len, MAX_LINESIZE));
 	if (!line_end) {
 		if (len >= MAX_LINESIZE)
 			return -E2BIG;
 
-		*attr = malloc(sizeof(struct wg_dynamic_attr) + len);
-		if (!*attr)
+		req->len = len;
+		req->buf = malloc(len);
+		if (!req->buf)
 			fatal("malloc()");
 
-		(*attr)->key = WGKEY_INCOMPLETE;
-		(*attr)->len = len;
-		(*attr)->next = NULL;
-		memcpy((*attr)->value, buf, len);
-
-		return len;
+		memcpy(req->buf, buf, len);
+		return 0;
 	}
 
-	if (line_end == buf)
-		return 0; /* \n\n - end of message */
+	if (line_end == buf) {
+		*key = WGKEY_EOMSG;
+		return 1;
+	}
 
 	*line_end = '\0';
 	line_len = line_end - buf + 1;
 
 	key_end = memchr(buf, '=', line_len - 1);
-	if (!key_end)
+	if (!key_end || key_end == buf)
 		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;
+	*key = parse_key((char *)buf);
+	if (*key == WGKEY_UNKNOWN)
+		return line_len;
 
-		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;
-	}
+	if (!parse_value(*key, (char *)key_end + 1, kv))
+		return -EINVAL;
 
 	return line_len;
 }
 
-static int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
-			 size_t len)
+static ssize_t 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;
+	ssize_t ret, offset = 0;
+	size_t addlen = 0;
+	enum wg_dynamic_key key;
+	union kvalues kv;
+	void (*deserialize)(enum wg_dynamic_key key, union kvalues kv,
+			    void **dest);
 
 	if (memchr(buf, '\0', len))
 		return -EINVAL; /* don't allow null bytes */
 
-	if (req->last && req->last->key == WGKEY_INCOMPLETE) {
-		len += req->last->len;
+	if (req->len > 0 && req->buf) {
+		len += req->len;
+
+		memmove(buf + req->len, buf, len);
+		memcpy(buf, req->buf, req->len);
+		addlen = req->len;
+		free(req->buf);
+		req->buf = NULL;
+		req->len = 0;
+	}
 
-		memmove(buf + req->last->len, buf, len);
-		memcpy(buf, req->last->value, req->last->len);
-		free(req->last);
+	if (req->cmd == WGKEY_UNKNOWN) {
+		ret = parse_line(buf, len, req, &req->cmd, &kv);
+		if (ret <= 0)
+			return ret;
 
-		if (req->first == req->last) {
-			req->first = NULL;
-			req->last = NULL;
-		} else {
-			attr = req->first;
-			while (attr->next != req->last)
-				attr = attr->next;
+		req->version = kv.u32;
+		if (req->cmd >= WGKEY_ENDCMD || req->cmd <= WGKEY_EOMSG ||
+		    req->version != 1)
+			return -EPROTONOSUPPORT;
 
-			attr->next = NULL;
-			req->last = attr;
-		}
+		len -= ret;
+		offset += ret;
+
+		deserialize = deserialize_fptr[req->cmd];
+		deserialize(req->cmd, kv, &req->result);
+	} else {
+		deserialize = deserialize_fptr[req->cmd];
 	}
 
 	while (len > 0) {
-		ret = parse_line(buf + offset, len, &attr,
-				 req->cmd == WGKEY_UNKNOWN ? req : NULL);
+		ret = parse_line(buf + offset, len, req, &key, &kv);
 		if (ret <= 0)
-			return ret; /* either error or message complete */
+			return ret;
 
 		len -= ret;
 		offset += ret;
-		if (!attr)
-			continue;
 
-		if (!req->first)
-			req->first = attr;
-		else
-			req->last->next = attr;
+		if (key == WGKEY_EOMSG)
+			return offset - addlen;
+		else if (key == WGKEY_UNKNOWN)
+			continue;
+		else if (key <= WGKEY_ENDCMD)
+			return -EINVAL;
 
-		req->last = attr;
+		deserialize(key, kv, &req->result);
 	}
 
-	return 1;
+	return 0;
 }
 
-bool handle_request(struct wg_dynamic_request *req,
-		    bool (*success)(struct wg_dynamic_request *),
-		    bool (*error)(struct wg_dynamic_request *, int))
+int handle_request(int fd, struct wg_dynamic_request *req,
+		   unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE],
+		   size_t *remaining)
 {
-	ssize_t bytes;
-	int ret;
-	unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE];
+	ssize_t bytes, processed;
 
-	while (1) {
-		bytes = read(req->fd, buf, RECV_BUFSIZE);
-		if (bytes < 0) {
-			if (errno == EWOULDBLOCK || errno == EAGAIN)
-				break;
+	do {
+		if (*remaining > 0)
+			bytes = *remaining;
+		else
+			bytes = read(fd, buf, RECV_BUFSIZE);
 
-			// TODO: handle EINTR
+		if (bytes < 0) {
+			if (errno == EWOULDBLOCK || errno == EAGAIN ||
+			    errno == EINTR)
+				return 0;
 
-			debug("Reading from socket %d failed: %s\n", req->fd,
+			debug("Reading from socket %d failed: %s\n", fd,
 			      strerror(errno));
-			return true;
+			return -1;
 		} else if (bytes == 0) {
-			debug("Peer disconnected unexpectedly\n");
-			return true;
+			return -1;
 		}
 
-		ret = parse_request(req, buf, bytes);
-		if (ret < 0)
-			return error(req, -ret);
-		else if (ret == 0)
-			return success(req);
-	}
+		processed = parse_request(req, buf, bytes);
+		if (processed < 0)
+			return processed; /* Parsing error */
+	} while (processed == 0);
+
+	*remaining = bytes - processed;
+	memmove(buf, buf + processed, *remaining);
+
+	return 1;
+}
+
+void free_wg_dynamic_request(struct wg_dynamic_request *req)
+{
+	BUG_ON(req->buf || req->len);
 
-	return false;
+	req->cmd = WGKEY_UNKNOWN;
+	req->version = 0;
+	if (req->result) {
+		free(((struct wg_dynamic_request_ip *)req->result)->errmsg);
+		free(req->result);
+		req->result = NULL;
+	}
 }
 
-bool send_message(struct wg_dynamic_request *req, const void *buf, size_t len)
+size_t serialize_request_ip(bool send, char *buf, size_t len,
+			    struct wg_dynamic_request_ip *rip)
 {
-	size_t offset = 0;
+	size_t off = 0;
+	char addrbuf[INET6_ADDRSTRLEN];
 
-	while (1) {
-		ssize_t written = write(req->fd, buf + offset, len - offset);
-		if (written < 0) {
-			if (errno == EWOULDBLOCK || errno == EAGAIN)
-				break;
+	if (send)
+		print_to_buf(buf, len, &off, "request_ip=1\n");
 
-			// TODO: handle EINTR
+	if (rip->has_ipv4) {
+		if (!inet_ntop(AF_INET, &rip->ipv4, addrbuf, sizeof addrbuf))
+			fatal("inet_ntop()");
 
-			debug("Writing to socket %d failed: %s\n", req->fd,
-			      strerror(errno));
-			return true;
-		}
+		print_to_buf(buf, len, &off, "ipv4=%s/32\n", addrbuf);
+	}
 
-		offset += written;
-		if (offset == len)
-			return true;
+	if (rip->has_ipv6) {
+		if (!inet_ntop(AF_INET6, &rip->ipv6, addrbuf, sizeof addrbuf))
+			fatal("inet_ntop()");
+
+		print_to_buf(buf, len, &off, "ipv6=%s/128\n", addrbuf);
 	}
 
-	debug("Socket %d blocking on write with %lu bytes left, postponing\n",
-	      req->fd, len - offset);
+	if (rip->start && rip->leasetime)
+		print_to_buf(buf, len, &off, "leasestart=%u\nleasetime=%u\n",
+			     rip->start, rip->leasetime);
 
-	if (!req->buf) {
-		req->buflen = len - offset;
-		req->buf = malloc(req->buflen);
-		if (!req->buf)
-			fatal("malloc()");
+	if (rip->errmsg)
+		print_to_buf(buf, len, &off, "errmsg=%s\n", rip->errmsg);
 
-		memcpy(req->buf, buf + offset, req->buflen);
-	} else {
-		req->buflen = len - offset;
-		memmove(req->buf, buf + offset, req->buflen);
-	}
+	if (!send)
+		print_to_buf(buf, len, &off, "errno=%u\n", rip->wg_errno);
+
+	print_to_buf(buf, len, &off, "\n");
 
-	return false;
+	return off;
 }
 
 void print_to_buf(char *buf, size_t bufsize, size_t *offset, char *fmt, ...)
@@ -353,29 +379,6 @@ uint32_t current_time()
 	return tp.tv_sec;
 }
 
-void close_connection(struct wg_dynamic_request *req)
-{
-	struct wg_dynamic_attr *prev, *cur = req->first;
-
-	if (close(req->fd))
-		debug("Failed to close socket\n");
-
-	while (cur) {
-		prev = cur;
-		cur = cur->next;
-		free(prev);
-	}
-
-	req->cmd = WGKEY_UNKNOWN;
-	req->version = 0;
-	req->fd = -1;
-	free(req->buf);
-	req->buf = NULL;
-	req->buflen = 0;
-	req->first = NULL;
-	req->last = NULL;
-}
-
 bool is_link_local(unsigned char *addr)
 {
 	/* TODO: check if the remaining 54 bits are 0 */
diff --git a/common.h b/common.h
index c44e6c7..0e97f97 100644
--- a/common.h
+++ b/common.h
@@ -15,11 +15,8 @@
 #include "netlink.h"
 
 #define MAX_CONNECTIONS 16
-
 #define MAX_LINESIZE 4096
-
 #define RECV_BUFSIZE 8192
-
 #define MAX_RESPONSE_SIZE 8192
 
 static const char WG_DYNAMIC_ADDR[] = "fe80::";
@@ -27,11 +24,11 @@ static const uint16_t WG_DYNAMIC_PORT = 970; /* ASCII sum of "wireguard" */
 
 #define ITEMS                                                                  \
 	E(WGKEY_UNKNOWN, "") /* must be the first entry */                     \
+	E(WGKEY_EOMSG, "")                                                     \
 	/* 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_LEASESTART, "leasestart")                                      \
@@ -50,6 +47,7 @@ static const char *const WG_DYNAMIC_KEY[] = { ITEMS };
 #define ITEMS                                                                  \
 	E(E_NO_ERROR, "Success") /* must be the first entry */                 \
 	E(E_INVALID_REQ, "Invalid request")                                    \
+	E(E_UNSUPP_PROTO, "Unsupported protocol")                              \
 	E(E_IP_UNAVAIL, "Chosen IP unavailable")
 
 #define E(x, y) x,
@@ -60,22 +58,21 @@ static const char *const WG_DYNAMIC_ERR[] = { 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;
-	int fd;
-	wg_key pubkey;
-	struct in6_addr lladdr;
 	unsigned char *buf;
-	size_t buflen;
-	struct wg_dynamic_attr *first, *last;
+	size_t len; /* <= MAX_LINESIZE */
+	void *result;
+};
+
+struct wg_dynamic_request_ip {
+	struct in_addr ipv4;
+	struct in6_addr ipv6;
+	uint8_t cidrv4, cidrv6;
+	uint32_t leasetime, start, wg_errno;
+	bool has_ipv4, has_ipv6;
+	char *errmsg;
 };
 
 struct wg_combined_ip {
@@ -89,14 +86,14 @@ struct wg_combined_ip {
 
 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
 
+int handle_request(int fd, struct wg_dynamic_request *req,
+		   unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE],
+		   size_t *remaining);
 void free_wg_dynamic_request(struct wg_dynamic_request *req);
-bool handle_request(struct wg_dynamic_request *req,
-		    bool (*success)(struct wg_dynamic_request *),
-		    bool (*error)(struct wg_dynamic_request *, int));
-bool send_message(struct wg_dynamic_request *req, const void *buf, size_t len);
+size_t serialize_request_ip(bool include_header, char *buf, size_t len,
+			    struct wg_dynamic_request_ip *rip);
 void print_to_buf(char *buf, size_t bufsize, size_t *offset, char *fmt, ...);
 uint32_t current_time();
-void close_connection(struct wg_dynamic_request *req);
 bool is_link_local(unsigned char *addr);
 void iface_get_all_addrs(uint8_t family, mnl_cb_t data_cb, void *cb_data);
 int data_attr_cb(const struct nlattr *attr, void *data);
diff --git a/wg-dynamic-server.c b/wg-dynamic-server.c
index fa06e40..99c7559 100644
--- a/wg-dynamic-server.c
+++ b/wg-dynamic-server.c
@@ -33,7 +33,6 @@ static const char *wg_interface = NULL;
 static struct in6_addr well_known;
 
 static wg_device *device = NULL;
-static struct wg_dynamic_request requests[MAX_CONNECTIONS] = { 0 };
 static uint32_t leasetime = 3600;
 
 static int sockfd = -1;
@@ -43,6 +42,17 @@ static struct mnl_socket *nlsock = NULL;
 KHASH_MAP_INIT_INT64(allowedht, wg_key *)
 khash_t(allowedht) * allowedips_ht;
 
+struct wg_dynamic_connection {
+	struct wg_dynamic_request req;
+	int fd;
+	wg_key pubkey;
+	struct in6_addr lladdr;
+	unsigned char *outbuf;
+	size_t buflen;
+};
+
+static struct wg_dynamic_connection connections[MAX_CONNECTIONS] = { 0 };
+
 struct mnl_cb_data {
 	uint32_t ifindex;
 	bool valid_ip_found;
@@ -170,8 +180,7 @@ static wg_key *addr_to_pubkey(struct sockaddr_storage *addr)
 	return NULL;
 }
 
-static int accept_connection(int sockfd, wg_key *dest_pubkey,
-			     struct in6_addr *dest_lladdr)
+static int accept_connection(wg_key *dest_pubkey, struct in6_addr *dest_lladdr)
 {
 	int fd;
 	wg_key *pubkey;
@@ -230,101 +239,124 @@ static int accept_connection(int sockfd, wg_key *dest_pubkey,
 	return fd;
 }
 
-static bool send_error(struct wg_dynamic_request *req, int error)
+static bool send_message(struct wg_dynamic_connection *con,
+			 const unsigned char *buf, size_t len)
 {
-	char buf[MAX_RESPONSE_SIZE];
-	size_t msglen = 0;
+	size_t offset = 0;
 
-	print_to_buf(buf, sizeof buf, &msglen, "errno=%d\nerrmsg=%s\n\n", error,
-		     WG_DYNAMIC_ERR[error]);
-
-	return send_message(req, buf, msglen);
-}
+	while (1) {
+		ssize_t written = write(con->fd, buf + offset, len - offset);
+		if (written < 0) {
+			if (errno == EWOULDBLOCK || errno == EAGAIN)
+				break;
 
-static size_t serialize_lease(char *buf, size_t len,
-			      const struct wg_dynamic_lease *lease)
-{
-	char addrbuf[INET6_ADDRSTRLEN];
-	size_t off = 0;
+			if (errno == EINTR)
+				continue;
 
-	if (lease->ipv4.s_addr) {
-		if (!inet_ntop(AF_INET, &lease->ipv4, addrbuf, sizeof addrbuf))
-			fatal("inet_ntop()");
+			debug("Writing to socket %d failed: %s\n", con->fd,
+			      strerror(errno));
+			return false;
+		}
 
-		print_to_buf(buf, len, &off, "ipv4=%s/%d\n", addrbuf, 32);
+		offset += written;
+		if (offset == len)
+			return true;
 	}
 
-	if (!IN6_IS_ADDR_UNSPECIFIED(&lease->ipv6)) {
-		if (!inet_ntop(AF_INET6, &lease->ipv6, addrbuf, sizeof addrbuf))
-			fatal("inet_ntop()");
+	debug("Socket %d blocking on write with %lu bytes left, postponing\n",
+	      con->fd, len - offset);
 
-		print_to_buf(buf, len, &off, "ipv6=%s/%d\n", addrbuf, 128);
-	}
+	if (!con->outbuf) {
+		con->buflen = len - offset;
+		con->outbuf = malloc(con->buflen);
+		if (!con->outbuf)
+			fatal("malloc()");
 
-	print_to_buf(buf, len, &off, "leasestart=%u\nleasetime=%u\nerrno=0\n\n",
-		     lease->start_real, lease->leasetime);
+		memcpy(con->outbuf, buf + offset, con->buflen);
+	} else {
+		con->buflen = len - offset;
+		memmove(con->outbuf, buf + offset, con->buflen);
+	}
 
-	return off;
+	return true;
 }
 
-static int response_request_ip(struct wg_dynamic_attr *cur, wg_key pubkey,
-			       const struct in6_addr *lladdr,
-			       struct wg_dynamic_lease **lease)
+void close_connection(struct wg_dynamic_connection *con)
 {
-	struct in_addr *ipv4 = NULL;
-	struct in6_addr *ipv6 = NULL;
+	free_wg_dynamic_request(&con->req);
 
-	while (cur) {
-		switch (cur->key) {
-		case WGKEY_IPV4:
-			ipv4 = &((struct wg_combined_ip *)cur->value)->ip4;
-			break;
-		case WGKEY_IPV6:
-			ipv6 = &((struct wg_combined_ip *)cur->value)->ip6;
-			break;
-		case WGKEY_LEASETIME:
-			leasetime = *(uint32_t *)cur->value;
-			break;
-		default:
-			debug("Ignoring invalid attribute for request_ip: %d\n",
-			      cur->key);
-		}
-		cur = cur->next;
-	}
-
-	*lease = set_lease(pubkey, leasetime, lladdr, ipv4, ipv6);
-	if (!*lease)
-		return E_IP_UNAVAIL;
+	if (close(con->fd))
+		debug("Failed to close socket\n");
 
-	return E_NO_ERROR;
+	con->fd = -1;
+	memset(con->pubkey, 0, sizeof con->pubkey);
+	free(con->outbuf);
+	con->outbuf = NULL;
+	con->buflen = 0;
 }
 
-static bool send_response(struct wg_dynamic_request *req)
+static bool send_response(struct wg_dynamic_connection *con)
 {
 	char buf[MAX_RESPONSE_SIZE];
-	struct wg_dynamic_attr *cur = req->first;
-	struct wg_dynamic_lease *lease;
 	size_t msglen;
-	int ret;
 
-	switch (req->cmd) {
-	case WGKEY_REQUEST_IP:
-		ret = response_request_ip(cur, req->pubkey, &req->lladdr,
-					  &lease);
-		if (ret)
-			break;
+	switch (con->req.cmd) {
+	case WGKEY_REQUEST_IP:;
+		struct wg_dynamic_request_ip *rip = con->req.result;
+		struct in_addr *ip4 = rip->has_ipv4 ? &rip->ipv4 : NULL;
+		struct in6_addr *ip6 = rip->has_ipv6 ? &rip->ipv6 : NULL;
+		struct wg_dynamic_lease *lease;
+		struct wg_dynamic_request_ip ans = { 0 };
+
+		lease = set_lease(con->pubkey, leasetime, &con->lladdr, ip4,
+				  ip6);
+		if (lease) {
+			memcpy(&ans.ipv4, &lease->ipv4, sizeof ans.ipv4);
+			memcpy(&ans.ipv6, &lease->ipv6, sizeof ans.ipv6);
+			ans.has_ipv4 = ans.has_ipv6 = true;
+			ans.start = lease->start_real;
+			ans.leasetime = lease->leasetime;
+		} else {
+			ans.wg_errno = E_IP_UNAVAIL;
+		}
 
-		msglen = serialize_lease(buf, sizeof buf, lease);
+		msglen = serialize_request_ip(false, buf, sizeof buf, &ans);
 		break;
 	default:
-		debug("Unknown command: %d\n", req->cmd);
+		debug("Unknown command: %d\n", con->req.cmd);
 		BUG();
 	}
 
-	if (ret)
-		return send_error(req, ret);
+	return send_message(con, (unsigned char *)buf, msglen);
+}
+
+static void handle_client(struct wg_dynamic_connection *con)
+{
+	unsigned char buf[RECV_BUFSIZE + MAX_LINESIZE];
+	size_t rem = 0;
+	ssize_t ret;
+
+	while ((ret = handle_request(con->fd, &con->req, buf, &rem)) > 0) {
+		if (!send_response(con)) {
+			close_connection(con);
+			break;
+		}
+
+		free_wg_dynamic_request(&con->req);
+	}
 
-	return send_message(req, buf, msglen);
+	if (ret < 0) {
+		size_t len = 0;
+		uint32_t err = E_INVALID_REQ;
+		if (-ret == EPROTONOSUPPORT)
+			err = E_UNSUPP_PROTO;
+
+		print_to_buf((char *)buf, sizeof buf, &len,
+			     "errno=%u\nerrmsg=%s\n\n", err,
+			     WG_DYNAMIC_ERR[err]);
+		send_message(con, buf, len);
+		close_connection(con);
+	}
 }
 
 static void setup_sockets()
@@ -394,10 +426,10 @@ static void cleanup()
 		close(epollfd);
 
 	for (int i = 0; i < MAX_CONNECTIONS; ++i) {
-		if (requests[i].fd < 0)
+		if (connections[i].fd < 0)
 			continue;
 
-		close_connection(&requests[i]);
+		close_connection(&connections[i]);
 	}
 }
 
@@ -435,7 +467,7 @@ static void setup()
 	allowedips_ht = kh_init(allowedht);
 
 	for (int i = 0; i < MAX_CONNECTIONS; ++i)
-		requests[i].fd = -1;
+		connections[i].fd = -1;
 
 	if (atexit(cleanup))
 		die("Failed to set exit function\n");
@@ -462,20 +494,19 @@ static int get_avail_request()
 		if (nfds >= MAX_CONNECTIONS)
 			return -1;
 
-		if (requests[nfds].fd < 0)
+		if (connections[nfds].fd < 0)
 			return nfds;
 	}
 }
 
-static void accept_incoming(int sockfd, int epollfd,
-			    struct wg_dynamic_request *requests)
+static void accept_incoming()
 {
 	int n, fd;
 	struct epoll_event ev;
 
 	while ((n = get_avail_request()) >= 0) {
-		fd = accept_connection(sockfd, &requests[n].pubkey,
-				       &requests[n].lladdr);
+		fd = accept_connection(&connections[n].pubkey,
+				       &connections[n].lladdr);
 		if (fd < 0) {
 			if (fd == -ENOENT) {
 				debug("Failed to match IP to pubkey\n");
@@ -490,20 +521,20 @@ static void accept_incoming(int sockfd, int epollfd,
 		}
 
 		ev.events = EPOLLIN | EPOLLET;
-		ev.data.ptr = &requests[n];
+		ev.data.ptr = &connections[n];
 		if (epoll_ctl(epollfd, EPOLL_CTL_ADD, fd, &ev) == -1)
 			fatal("epoll_ctl()");
 
-		requests[n].fd = fd;
+		connections[n].fd = fd;
 	}
 }
 
 static void handle_event(void *ptr, uint32_t events)
 {
-	struct wg_dynamic_request *req;
+	struct wg_dynamic_connection *con;
 
 	if (ptr == &sockfd) {
-		accept_incoming(sockfd, epollfd, requests);
+		accept_incoming();
 		return;
 	}
 
@@ -512,15 +543,14 @@ static void handle_event(void *ptr, uint32_t events)
 		return;
 	}
 
-	req = (struct wg_dynamic_request *)ptr;
+	con = (struct wg_dynamic_connection *)ptr;
 	if (events & EPOLLIN) {
-		if (handle_request(req, send_response, send_error))
-			close_connection(req);
+		handle_client(con);
 	}
 
 	if (events & EPOLLOUT) {
-		if (send_message(req, req->buf, req->buflen))
-			close_connection(req);
+		if (!send_message(con, con->outbuf, con->buflen))
+			close_connection(con);
 	}
 }
 
-- 
cgit v1.2.3-59-g8ed1b