From 56a3a751db276993f7eb6902efb38d983b2201a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Adrien=20Gallou=C3=ABt?= <adrien@gallouet.fr>
Date: Sat, 29 Sep 2018 16:13:01 +0000
Subject: [PATCH] Use siphash to select path
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Adrien Gallouët <adrien@gallouet.fr>
---
 src/bind.c | 24 +++++++++++++++---
 src/ip.h   | 72 +++++++++++++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 86 insertions(+), 10 deletions(-)

diff --git a/src/bind.c b/src/bind.c
index c353d9c..28c462b 100644
--- a/src/bind.c
+++ b/src/bind.c
@@ -12,6 +12,8 @@
 #include "../argz/argz.h"
 #include "../mud/mud.h"
 
+#include <sodium.h>
+
 #ifndef O_CLOEXEC
 #define O_CLOEXEC 0
 #endif
@@ -151,6 +153,14 @@ gt_bind(int argc, char **argv)
     int chacha = argz_is_set(bindz, "chacha");
     int persist = argz_is_set(bindz, "persist");
 
+    if (sodium_init() == -1) {
+        gt_log("couldn't init sodium\n");
+        return 1;
+    }
+
+    unsigned char hashkey[crypto_shorthash_KEYBYTES];
+    crypto_shorthash_keygen(hashkey);
+
     struct mud *mud = mud_create((struct sockaddr *)&bind_addr);
 
     if (!mud) {
@@ -314,15 +324,21 @@ gt_bind(int argc, char **argv)
             struct ip_common ic;
             const int r = tun_read(tun_fd, buf, bufsize);
 
-            if (!ip_get_common(&ic, buf, r))
-                mud_send(mud, buf, r, ic.tc);
+            if (!ip_get_common(&ic, buf, r)) {
+                unsigned char hash[crypto_shorthash_BYTES];
+                crypto_shorthash(hash, (const unsigned char *)&ic, sizeof(ic), hashkey);
+
+                unsigned h;
+                memcpy(&h, hash, sizeof(h));
+
+                mud_send(mud, buf, r, (h << 8) | ic.tc);
+            }
         }
 
         if (FD_ISSET(mud_fd, &rfds)) {
-            struct ip_common ic;
             const int r = mud_recv(mud, buf, bufsize);
 
-            if (!ip_get_common(&ic, buf, r))
+            if (ip_is_valid(buf, r))
                 tun_write(tun_fd, buf, r);
         }
 
diff --git a/src/ip.h b/src/ip.h
index 64fa41c..a1cf83f 100644
--- a/src/ip.h
+++ b/src/ip.h
@@ -5,8 +5,27 @@
 struct ip_common {
     uint8_t tc;
     uint8_t proto;
+    struct { // data are not reordered
+        union {
+            unsigned char v6[16];
+            struct {
+                unsigned char zero[10];
+                unsigned char ff[2];
+                unsigned char v4[4];
+            };
+        };
+        unsigned char port[2];
+    } src, dst;
 };
 
+static inline int
+ip_read16(const uint8_t *src)
+{
+    uint16_t ret = src[1];
+    ret |= ((uint16_t)src[0]) << 8;
+    return (int)ret;
+}
+
 static inline uint8_t
 ip_get_version(const uint8_t *data)
 {
@@ -14,11 +33,17 @@ ip_get_version(const uint8_t *data)
 }
 
 static inline int
-ip_read16(const uint8_t *src)
+ip_is_valid(const uint8_t *data, int size)
 {
-    uint16_t ret = src[1];
-    ret |= ((uint16_t)src[0]) << 8;
-    return (int)ret;
+    if (size < 20)
+        return 0;
+
+    switch (ip_get_version(data)) {
+        case 4: return size == ip_read16(&data[2]);
+        case 6: return size == ip_read16(&data[4]) + 40;
+    }
+
+    return 0;
 }
 
 static inline int
@@ -31,11 +56,46 @@ ip_get_common(struct ip_common *ic, const uint8_t *data, int size)
     case 4:
         ic->tc = data[1];
         ic->proto = data[9];
-        return size != ip_read16(&data[2]);
+        if (size == ip_read16(&data[2])) {
+            const int hdrsize = (data[0] & 0xF) << 2;
+            memset(ic->src.zero, 0, sizeof(ic->src.zero));
+            memset(ic->src.ff, 0xff, sizeof(ic->src.ff));
+            memcpy(ic->src.v4, &data[12], sizeof(ic->src.v4));
+            memset(ic->dst.zero, 0, sizeof(ic->dst.zero));
+            memset(ic->dst.ff, 0xff, sizeof(ic->dst.ff));
+            memcpy(ic->dst.v4, &data[16], sizeof(ic->dst.v4));
+            switch (ic->proto) {
+            case 6:  // tcp
+            case 17: // udp
+                memcpy(ic->src.port, &data[hdrsize], sizeof(ic->src.port));
+                memcpy(ic->dst.port, &data[hdrsize + 2], sizeof(ic->dst.port));
+                break;
+            default:
+                memset(ic->src.port, 0, sizeof(ic->src.port));
+                memset(ic->dst.port, 0, sizeof(ic->dst.port));
+            }
+            return 0;
+        }
+        break;
     case 6:
         ic->tc = ((data[0] & 0xF) << 4) | (data[1] >> 4);
         ic->proto = data[6];
-        return size != ip_read16(&data[4]) + 40;
+        if (size == ip_read16(&data[4]) + 40) {
+            memcpy(ic->src.v6, &data[8], sizeof(ic->src.v6));
+            memcpy(ic->dst.v6, &data[24], sizeof(ic->dst.v6));
+            switch (ic->proto) {
+            case 6:  // tcp
+            case 17: // udp
+                memcpy(ic->src.port, &data[40], sizeof(ic->src.port));
+                memcpy(ic->dst.port, &data[42], sizeof(ic->dst.port));
+                break;
+            default:
+                memset(ic->src.port, 0, sizeof(ic->src.port));
+                memset(ic->dst.port, 0, sizeof(ic->dst.port));
+            }
+            return 0;
+        }
+        break;
     }
 
     return 1;