Merge branch 'ln/send' into ln/send2

3 files changed, 212 insertions, 22 deletions

diff --git a/common.c b/common.c
index 10883fe..f96095e 100644
--- a/common.c
+++ b/common.c
@@ -9,6 +9,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <stdarg.h>
 #include <string.h>
 #include <unistd.h>
 
@@ -166,7 +167,7 @@ static ssize_t parse_line(unsigned char *buf, size_t len,
 		req->cmd = key;
 		req->version = (uint32_t)res;
 
-		if (req->version != 1)
+		if (req->version != WG_DYNAMIC_PROTOCOL_VERSION)
 			return -EPROTONOSUPPORT;
 	} else {
 		if (key <= WGKEY_ENDCMD)
@@ -192,6 +193,9 @@ void free_wg_dynamic_request(struct wg_dynamic_request *req)
 
 	req->cmd = WGKEY_UNKNOWN;
 	req->version = 0;
+	free(req->buf);
+	req->buf = NULL;
+	req->buflen = 0;
 	req->first = NULL;
 	req->last = NULL;
 }
@@ -249,8 +253,8 @@ static int parse_request(struct wg_dynamic_request *req, unsigned char *buf,
 }
 
 bool handle_request(int fd, struct wg_dynamic_request *req,
-		    void (*success)(int, struct wg_dynamic_request *req),
-		    void (*error)(int, int))
+		    bool (*success)(int, struct wg_dynamic_request *),
+		    bool (*error)(int, int))
 {
 	ssize_t bytes;
 	int ret;
@@ -264,8 +268,8 @@ bool handle_request(int fd, struct wg_dynamic_request *req,
 
 			// TODO: handle EINTR
 
-			debug("Reading from socket failed: %s\n",
-			      strerror(errno));
+			debug("Reading from socket %d failed: %s\n",
+			      fd, strerror(errno));
 			return true;
 		} else if (bytes == 0) {
 			debug("Client disconnected unexpectedly\n");
@@ -273,14 +277,51 @@ bool handle_request(int fd, struct wg_dynamic_request *req,
 		}
 
 		ret = parse_request(req, buf, bytes);
-		if (ret < 0) {
-			error(fd, -ret);
-			return true;
-		} else if (ret == 0) {
-			success(fd, req);
-			return true;
-		}
+		if (ret < 0)
+			return error(fd, -ret);
+		else if (ret == 0)
+			return success(fd, req);
 	}
 
 	return false;
 }
+
+size_t send_message(int fd, unsigned char *buf, size_t *len)
+{
+	ssize_t bytes;
+	size_t offset = 0;
+
+	while (*len) {
+		bytes = write(fd, buf + offset, *len);
+		if (bytes < 0) {
+			if (errno == EWOULDBLOCK || errno == EAGAIN)
+				break;
+
+			// TODO: handle EINTR
+
+			debug("Writing to socket %d failed: %s\n", fd,
+			      strerror(errno));
+			*len = 0;
+			return 0;
+		}
+
+		*len -= bytes;
+		offset += bytes;
+	}
+
+	return offset;
+}
+
+size_t printf_to_buf(char *buf, size_t bufsize, size_t offset,
+		     char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	int n = vsnprintf(buf + offset, bufsize - offset, fmt, ap);
+	va_end(ap);
+	if (n < 0)
+		fatal("Failed snprintf");
+	if (n + offset >= bufsize)
+		fatal("Outbuffer too small");
+	return n;
+}
diff --git a/common.h b/common.h
index ca76dc4..3a8411d 100644
--- a/common.h
+++ b/common.h
@@ -22,6 +22,9 @@
 static const char WG_DYNAMIC_ADDR[] = "fe80::";
 static const uint16_t WG_DYNAMIC_PORT = 1337;
 
+#define WG_DYNAMIC_PROTOCOL_VERSION 1
+#define WG_DYNAMIC_LEASETIME 3600
+
 #define ITEMS                                                                  \
 	E(WGKEY_UNKNOWN, "") /* must be the first entry */                     \
 	/* CMD START */                                                        \
@@ -52,6 +55,8 @@ struct wg_dynamic_request {
 	enum wg_dynamic_key cmd;
 	uint32_t version;
 	wg_key pubkey;
+	unsigned char *buf;
+	size_t buflen;
 	struct wg_dynamic_attr *first, *last;
 };
 
@@ -68,7 +73,8 @@ struct wg_combined_ip {
 
 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));
-
+		    bool (*success)(int, struct wg_dynamic_request *),
+		    bool (*error)(int, int));
+size_t send_message(int fd, unsigned char *buf, size_t *len);
+size_t printf_to_buf(char *buf, size_t bufsize, size_t len, char *fmt, ...);
 #endif
diff --git a/wg-dynamic-server.c b/wg-dynamic-server.c
index b63c72d..861c8f3 100644
--- a/wg-dynamic-server.c
+++ b/wg-dynamic-server.c
@@ -26,6 +26,8 @@
 #include "netlink.h"
 #include "radix-trie.h"
 
+#define MAX_RESPONSE_SIZE 8192
+
 static const char *progname;
 static const char *wg_interface;
 static struct in6_addr well_known;
@@ -298,25 +300,151 @@ static void accept_incoming(int sockfd, struct wg_dynamic_request *reqs)
 static void close_connection(int *fd, struct wg_dynamic_request *req)
 {
 	if (close(*fd))
-		debug("Failed to close socket");
+		debug("Failed to close socket\n");
 
 	*fd = -1;
 	free_wg_dynamic_request(req);
 }
 
-static void send_response(int fd, struct wg_dynamic_request *req)
+static int allocate_from_pool(struct wg_dynamic_request * const req,
+			      struct wg_combined_ip addrs[2], uint32_t *expires)
+{
+	struct wg_dynamic_attr *attr;
+	int i;
+
+	/* NOTE: "allocating" whatever client asks for */
+	/* TODO: choose an ip address from pool of available
+	 * addresses, together with an appropriate lease time */
+	/* NOTE: the pool is to be drawn from what routes are pointing
+	 * to the wg interface, and kept up to date as the routing
+	 * table changes */
+
+	*expires = time(NULL) + WG_DYNAMIC_LEASETIME;
+
+	i = 0;
+	attr = req->first;
+	while (attr) {
+		switch (attr->key) {
+		case WGKEY_IPV4:
+			/* fall through */
+		case WGKEY_IPV6:
+			if (i > 1)
+				break;
+			memcpy(&addrs[i++], attr->value, sizeof *addrs);
+			break;
+		case WGKEY_LEASETIME:
+			memcpy(expires, attr->value, sizeof(uint32_t));
+			*expires += time(NULL);
+			break;
+		default:
+			debug("Ignoring invalid attribute for request_ip: %d\n",
+			      attr->key);
+		}
+
+		attr = attr->next;
+	}
+
+	return 0;
+}
+
+static bool send_error(int fd, int ret)
+{
+	debug("Error: %s\n", strerror(ret));
+	return true;
+}
+
+static void send_later(struct wg_dynamic_request *req,
+		       unsigned char * const buf, size_t msglen)
+{
+	unsigned char *newbuf = malloc(msglen);
+	if (!newbuf)
+		fatal("Failed malloc()");
+	memcpy(newbuf, buf, msglen);
+
+	free(req->buf);
+	req->buf = newbuf;
+	req->buflen = msglen;
+}
+
+static size_t serialise_lease(char *buf, size_t bufsize, size_t offset,
+			      struct wg_combined_ip addrs[2], uint32_t expires)
+{
+	char addrbuf[INET6_ADDRSTRLEN];
+	char *fmt;
+	size_t w;
+
+	w = 0;
+	for (int i = 0; i < 2; i++) {
+		switch (addrs[i].family) {
+		case AF_INET:
+			fmt = "ipv4=%s\n";
+			break;
+		case AF_INET6:
+			fmt = "ipv6=%s\n";
+			break;
+		default:
+			fatal("Invalid family: %d", addrs[i].family);
+		}
+
+		if (!inet_ntop(addrs[i].family, &addrs[i].ip.ip4, addrbuf,
+			       sizeof addrbuf))
+			fatal("Failed inet_ntop");
+		w += printf_to_buf(buf, bufsize, offset, fmt, addrbuf);
+		offset += w;
+	}
+
+	if (w)
+		offset += printf_to_buf(buf, bufsize, offset, "leasetime=%u\n",
+					expires);
+
+	return offset;
+}
+
+static bool send_response(int fd, struct wg_dynamic_request *req)
 {
+	int ret;
+	unsigned char buf[MAX_RESPONSE_SIZE+1];
+	size_t msglen;
+	size_t written;
+	struct wg_combined_ip addrs[2] = { 0 };
+	uint32_t expires;
+
 	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 void send_error(int fd, int ret)
-{
-	debug("Error: %s\n", strerror(ret));
+	ret = EINVAL;
+	msglen = 0;
+	switch (req->cmd) {
+	case WGKEY_REQUEST_IP:
+		msglen = printf_to_buf((char *) buf, sizeof buf, 0, "%s=%d\n",
+					WG_DYNAMIC_KEY[req->cmd],
+					WG_DYNAMIC_PROTOCOL_VERSION);
+		ret = allocate_from_pool(req, addrs, &expires);
+		if (ret)
+			break;
+		msglen += serialise_lease((char *) buf, sizeof buf, msglen,
+				    addrs, expires);
+
+		/* TODO: inform wg about the new address (or maybe if (send_rather wait until after we've closed this tcp if (send_connection?)  */
+
+		break;
+	default:
+		debug("Unknown command: %d\n", req->cmd);
+		return true;
+	}
+
+	msglen += printf_to_buf((char *) buf, sizeof buf, msglen,
+					"errno=%d\n\n", ret);
+	written = send_message(fd, buf, &msglen);
+	if (!msglen)
+		return true;
+
+	send_later(req, buf + written, msglen);
+	return false;
 }
 
 static void setup_socket(int *fd)
@@ -408,13 +536,28 @@ int main(int argc, char *argv[])
 		}
 
 		for (int i = 1; i < MAX_CONNECTIONS + 1; ++i) {
+			if (!(pollfds[i].revents & POLLOUT))
+				continue;
+
+			pollfds[i].revents &= ~POLLOUT;
+			if (send_message(pollfds[i].fd, reqs[i - 1].buf,
+					 &reqs[i - 1].buflen)) {
+				close_connection(&pollfds[i].fd, &reqs[i - 1]);
+				pollfds[i].events &= ~POLLOUT;
+				continue;
+			}
+		}
+
+		for (int i = 1; i < MAX_CONNECTIONS + 1; ++i) {
 			if (!(pollfds[i].revents & POLLIN))
 				continue;
 
-			pollfds[i].revents = 0;
+			pollfds[i].revents &= ~POLLIN;
 			if (handle_request(pollfds[i].fd, &reqs[i - 1],
 					   send_response, send_error))
 				close_connection(&pollfds[i].fd, &reqs[i - 1]);
+			else if (reqs[i - 1].buf)
+				pollfds[i].events |= POLLOUT;
 		}
 	}