From ee02dea462326879e9cf3293cd31172c25335be3 Mon Sep 17 00:00:00 2001
From: Martin Szulecki
Date: Sun, 7 Jun 2020 02:21:01 +0200
Subject: socket: Fix socket_connect_addr() not connecting using IPv6 in some
 cases

This extends the socket helper with functions to determine the "scope" and a
suitable "scope id" of an IPv6 address. While socket_connect_addr() prefers
any initially supplied "scope id" to maintain routing information if possible,
it will attempt to determine the best suitable route with the new helpers.
This became a requirement during testing with remote usbmux connections that
provide a different "scope id" and thus might cause IPv6 routing to not work
at all. Thus the "scope id" is only valid per host.
---
 common/socket.c | 125 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 125 insertions(+)

diff --git a/common/socket.c b/common/socket.c
index 4d3956c..86cbf48 100644
--- a/common/socket.c
+++ b/common/socket.c
@@ -43,6 +43,10 @@ static int wsa_init = 0;
 #include <netdb.h>
 #include <arpa/inet.h>
 #include <fcntl.h>
+#ifdef AF_INET6
+#include <net/if.h>
+#include <ifaddrs.h>
+#endif
 #endif
 #include "socket.h"
 
@@ -342,6 +346,115 @@ int socket_create(const char* addr, uint16_t port)
 	return sfd;
 }
 
+#ifdef AF_INET6
+static uint32_t _in6_addr_scope(struct in6_addr* addr)
+{
+	uint32_t scope = 0;
+
+	if (IN6_IS_ADDR_MULTICAST(addr)) {
+		if (IN6_IS_ADDR_MC_NODELOCAL(addr)) {
+			scope = 1;
+		} else if (IN6_IS_ADDR_MC_LINKLOCAL(addr)) {
+			scope = 2;
+		} else if (IN6_IS_ADDR_MC_SITELOCAL(addr)) {
+			scope = 5;
+		}
+
+		return scope;
+	}
+
+	if (IN6_IS_ADDR_LINKLOCAL(addr)) {
+		scope = 2;
+	} else if (IN6_IS_ADDR_LOOPBACK(addr)) {
+		scope = 2;
+	} else if (IN6_IS_ADDR_SITELOCAL(addr)) {
+		scope = 5;
+	} else if (IN6_IS_ADDR_UNSPECIFIED(addr)) {
+		scope = 0;
+	}
+
+	return scope;
+}
+
+static int32_t _sockaddr_in6_scope_id(struct sockaddr_in6* addr)
+{
+	int32_t res = -1;
+	struct ifaddrs *ifaddr, *ifa;
+	uint32_t addr_scope;
+
+	/* get scope for requested address */
+	addr_scope = _in6_addr_scope(&addr->sin6_addr);
+	if (addr_scope == 0) {
+		/* global scope doesn't need a specific scope id */
+		return addr_scope;
+	}
+
+	/* get interfaces */
+	if (getifaddrs(&ifaddr) == -1) {
+		perror("getifaddrs");
+		return res;
+	}
+
+	/* loop over interfaces */
+	for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
+		/* skip if no address is available */
+		if (ifa->ifa_addr == NULL) {
+			continue;
+		}
+
+		/* skip if wrong family */
+		if (ifa->ifa_addr->sa_family != AF_INET6) {
+			continue;
+		}
+
+		/* skip if not up */
+		if ((ifa->ifa_flags & IFF_UP) == 0) {
+			continue;
+		}
+
+		/* skip if not running */
+		if ((ifa->ifa_flags & IFF_RUNNING) == 0) {
+			continue;
+		}
+
+		struct sockaddr_in6* addr_in = (struct sockaddr_in6*)ifa->ifa_addr;
+
+		/* skip if scopes do not match */
+		if (_in6_addr_scope(&addr_in->sin6_addr) != addr_scope) {
+			continue;
+		}
+
+		/* use if address is equal */
+		if (memcmp(&addr->sin6_addr.s6_addr, &addr_in->sin6_addr.s6_addr, sizeof(addr_in->sin6_addr.s6_addr)) == 0) {
+			res = addr_in->sin6_scope_id;
+			/* if scope id equals the requested one then assume it was valid */
+			if (addr->sin6_scope_id == addr_in->sin6_scope_id) {
+				break;
+			} else {
+				continue;
+			}
+		}
+
+		/* skip loopback interface if not already matched exactly above */
+		if ((ifa->ifa_flags & IFF_LOOPBACK) != 0) {
+			continue;
+		}
+
+		/* set the scope id of this interface as most likely candidate */
+		res = addr_in->sin6_scope_id;
+
+		/* if scope id equals the requested one then assume it was valid */
+		if (addr->sin6_scope_id == addr_in->sin6_scope_id) {
+			break;
+		}
+	}
+
+	freeifaddrs(ifaddr);
+
+	return res;
+}
+#endif
+
 int socket_connect_addr(struct sockaddr* addr, uint16_t port)
 {
 	int sfd = -1;
@@ -369,6 +482,18 @@ int socket_connect_addr(struct sockaddr* addr, uint16_t port)
 	else if (addr->sa_family == AF_INET6) {
 		struct sockaddr_in6* addr_in = (struct sockaddr_in6*)addr;
 		addr_in->sin6_port = htons(port);
+
+		/*
+		 * IPv6 Routing Magic:
+		 *
+		 * If the scope of the address is a link-local one, IPv6 requires the
+		 * scope id set to an interface number to allow proper routing. However,
+		 * as the provided sockaddr might contain a wrong scope id, we must find
+		 * a scope id from a suitable interface on this system or routing might
+		 * fail. An IPv6 guru should have another look though...
+		 */
+		addr_in->sin6_scope_id = _sockaddr_in6_scope_id(addr_in);
+
 		addrlen = sizeof(struct sockaddr_in6);
 	}
 #endif
-- 
cgit v1.1-32-gdbae