fix arp tun to gre

1 files changed, 17 insertions, 14 deletions

diff --git a/gre.c b/gre.c
index 6740228..c7bc46e 100644
--- a/gre.c
+++ b/gre.c
@@ -51,7 +51,7 @@ uint8_t buf[4096];
 static void gre_cb(void);
 static int gre_ipv4(uint8_t **buf, int *n);
 static int gre_ipv6(uint8_t **buf, int *n, const struct sockaddr_in6 *src);
-static void gre_any(const uint8_t *buf, int n);
+static int gre_any(const uint8_t *buf, int n);
 static int tun_cb(void);
 static int tun_new(short type, const char *dev);
 static int setnonblock(int fd);
@@ -202,7 +202,7 @@ static void gre_cb(void)
     switch (remote.ss_family) {
         case AF_INET: res = gre_ipv4(&frame, &n); break;
         case AF_INET6: res = gre_ipv6(&frame, &n, (const struct sockaddr_in6*)&src); break;
-       default: return;
+        default: return;
     }
 
     if (res < 0)
@@ -271,21 +271,26 @@ static int gre_ipv6(uint8_t **bufp, int *np, const struct sockaddr_in6 *src)
     return 0;
 }
 
-static void gre_any(const uint8_t *buf, int n)
+static int gre_any(const uint8_t *buf, int n)
 {
-#if 0
     // parse GRE header
     if (*(uint16_t *)(buf) != 0)
     {
-        return;
+        return -1;
     }
     uint16_t protocol = ntohs(*(uint16_t *)(buf + 2));
-    if (protocol != ETHERTYPE_IP && protocol != ETHERTYPE_IPV6)
-    {
-        return;
+    switch (protocol) {
+    case ETHERTYPE_IP:
+    case ETHERTYPE_IPV6:
+        break;
+    case ETHERTYPE_ARP:
+        if (type == IFF_TAP)
+            break;
+        return -1;
+    default:
+        return -1;
     }
-#endif
-    write(tun, buf, n);
+    return write(tun, buf, n);
 }
 
 static int tun_cb(void)
@@ -308,10 +313,8 @@ static int tun_cb(void)
     if (type == IFF_TAP)
     {
         size_t ethsize = 6 + 6 + 2;
-        frame = buf + ethsize;
-        n = n - ethsize;
-        frame[2] = buf[2];
-        frame[3] = buf[3];
+        frame = buf;
+        *(uint16_t*)(frame + 2) = htons(ETH_P_TEB);
     } else {
         frame = buf;
     }