From ffd7df64c9ff36443df6fd1f2aa22282887bdd45 Mon Sep 17 00:00:00 2001
From: HendrikVE <hendrik1995@msn.com>
Date: Tue, 1 Feb 2022 13:52:47 +0100
Subject: [PATCH] Process IPv6 scope id

Send data to the same interface we originally received from. This
allows the use of TAP devices for communication.
---
 .../src/linux/udp6/SensorNetwork.cpp          | 21 +++++++++++++------
 MQTTSNGateway/src/linux/udp6/SensorNetwork.h  |  1 +
 2 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp b/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp
index 353d793..c68be32 100644
--- a/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp
+++ b/MQTTSNGateway/src/linux/udp6/SensorNetwork.cpp
@@ -34,6 +34,9 @@
 #include "SensorNetwork.h"
 #include "MQTTSNGWProcess.h"
 
+//enough space for IPv6 + % + scopeId + : + port + brackets + \0
+#define ADDRESS_SPRINT_BUFFER_SIZE (INET6_ADDRSTRLEN + 1 + 10 + 1 + 5 + 2 + 1)
+
 //using namespace std;
 using namespace MQTTSNGW;
 
@@ -59,6 +62,11 @@ uint16_t SensorNetAddress::getPortNo(void)
     return _IpAddr.sin6_port;
 }
 
+uint32_t SensorNetAddress::getScopeId(void)
+{
+    return _IpAddr.sin6_scope_id;
+}
+
 void SensorNetAddress::setAddress(struct sockaddr_in6 *IpAddr)
 {
     memcpy((void*) &_IpAddr, IpAddr, sizeof(_IpAddr));
@@ -122,6 +130,7 @@ char* SensorNetAddress::getAddress(void)
 bool SensorNetAddress::isMatch(SensorNetAddress* addr)
 {
     return (this->_IpAddr.sin6_port == addr->_IpAddr.sin6_port)
+            && (this->_IpAddr.sin6_scope_id == addr->_IpAddr.sin6_scope_id)
             && (memcmp(this->_IpAddr.sin6_addr.s6_addr, addr->_IpAddr.sin6_addr.s6_addr,
                     sizeof(this->_IpAddr.sin6_addr.s6_addr)) == 0);
 }
@@ -135,8 +144,7 @@ SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
 
 char* SensorNetAddress::sprint(char* buf)
 {
-    sprintf(buf, "[%s]:", getAddress());
-    sprintf(buf + strlen(buf), "%d", ntohs(_IpAddr.sin6_port));
+    sprintf(buf, "[%s%%%d]:%d", getAddress(), getScopeId(), ntohs(getPortNo()));
     return buf;
 }
 
@@ -397,10 +405,11 @@ int UDPPort6::unicast(const uint8_t* buf, uint32_t length, SensorNetAddress* add
     memset(&dest, 0, sizeof(dest));
     dest.sin6_family = AF_INET6;
     dest.sin6_port = addr->getPortNo();
+    dest.sin6_scope_id = addr->getScopeId();
     memcpy(dest.sin6_addr.s6_addr, (const void*) &addr->getIpAddress()->sin6_addr, sizeof(in6_addr));
 
 #ifdef  DEBUG_NW
-    char addrBuf[INET6_ADDRSTRLEN];
+    char addrBuf[ADDRESS_SPRINT_BUFFER_SIZE];
     addr->sprint(addrBuf);
     D_NWSTACK("sendto %s\n", addrBuf);
 #endif
@@ -423,7 +432,7 @@ int UDPPort6::broadcast(const uint8_t* buf, uint32_t length)
     memcpy(dest.sin6_addr.s6_addr, (const void*) &_grpAddr.getIpAddress()->sin6_addr, sizeof(in6_addr));
 
 #ifdef  DEBUG_NW
-    char addrBuf[INET6_ADDRSTRLEN];
+    char addrBuf[ADDRESS_SPRINT_BUFFER_SIZE];
     addr->sprint(addrBuf);
     D_NWSTACK("sendto %s\n", addrBuf);
 #endif
@@ -473,9 +482,9 @@ int UDPPort6::recvfrom(int sockfd, uint8_t* buf, uint16_t len, uint8_t flags, Se
     addr->setAddress(&sender);
 
 #ifdef DEBUG_NW
-    char addrBuf[INET6_ADDRSTRLEN];
+    char addrBuf[ADDRESS_SPRINT_BUFFER_SIZE];
     addr->sprint(addrBuf);
-    D_NWSTACK("sendto %s length = %d\n", addrBuf, status);
+    D_NWSTACK("recvfrom %s length = %d\n", addrBuf, status);
 #endif
     return status;
 }
diff --git a/MQTTSNGateway/src/linux/udp6/SensorNetwork.h b/MQTTSNGateway/src/linux/udp6/SensorNetwork.h
index 68830b6..a9a0eaf 100644
--- a/MQTTSNGateway/src/linux/udp6/SensorNetwork.h
+++ b/MQTTSNGateway/src/linux/udp6/SensorNetwork.h
@@ -42,6 +42,7 @@ class SensorNetAddress
     int  setAddress(const char* data);
     uint16_t getPortNo(void);
     sockaddr_in6* getIpAddress(void);
+    uint32_t getScopeId(void);
     char* getAddress(void);
     bool isMatch(SensorNetAddress* addr);
     SensorNetAddress& operator =(SensorNetAddress& addr);