1 files changed, 61 insertions, 14 deletions

diff --git a/gre.c b/gre.c
index dff0a99..6740228 100644
--- a/gre.c
+++ b/gre.c
@@ -44,12 +44,13 @@ static int sock;
 static struct sockaddr_storage remote;
 static size_t remote_len;
 static short type = IFF_TUN;
+static uint8_t mac[6];
 
 uint8_t buf[4096];
 
 static void gre_cb(void);
-static void gre_ipv4(const uint8_t *buf, int n);
-static void gre_ipv6(const uint8_t *buf, int n, const struct sockaddr_in6 *src);
+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 tun_cb(void);
 static int tun_new(short type, const char *dev);
@@ -182,62 +183,97 @@ int main(int argc, char **argv)
 
 static void gre_cb(void)
 {
+    int res;
     int n;
     struct sockaddr_storage src;
     socklen_t src_len = sizeof(src);
+    uint8_t offset = type == IFF_TAP ? 6 + 6 + 2 : 0;
 
     memset(&src, 0, src_len);
-    n = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr*)&src, &src_len);
+    n = recvfrom(sock, buf + offset, sizeof(buf)-offset, 0, (struct sockaddr*)&src, &src_len);
     if (n < 0)
     {
         perror("recv");
         return;
     }
 
+    uint8_t *frame = buf + offset;
+
     switch (remote.ss_family) {
-        case AF_INET: gre_ipv4(buf, n); break;
-        case AF_INET6: gre_ipv6(buf, n, (const struct sockaddr_in6*)&src); break;
+        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;
+    }
+
+    if (res < 0)
+        return;
+
+    if ( type == IFF_TAP )
+    {
+        /* Set tuntap header */
+        memset(frame - offset, 0, 2);
+        memcpy(frame - offset + 2, frame + 2, 2);
+        /* Set MAC destination */
+        memcpy(frame - offset + 4, mac, sizeof(mac));
+        /* Set MAC source */
+        memset(frame - offset + 4 + 6, 2, 6);
+        /* Protocol reused from tuntap header. */
+        memcpy(frame - offset + 4 + 6 + 6, frame - offset + 2, 2);
+        gre_any(frame - offset, n + offset);
+    } else {
+        gre_any(frame, n);
     }
 }
 
-static void gre_ipv4(const uint8_t *buf, int n)
+static int gre_ipv4(uint8_t **bufp, int *np)
 {
+    uint8_t *buf = *bufp;
+    int n = *np;
     int ihl;    // IP header length
 
     ihl = 4 * (buf[0] & 0x0f);
     if (ihl > 60 || ihl < 20)
     {
         printf("IPv4 header too long\n");
-        return;
+        return -1;
     }
     // check source IPv4 address
     const struct sockaddr_in *remote_in = (const struct sockaddr_in *)&remote;
     if (*(uint32_t *)(buf + 12) != remote_in->sin_addr.s_addr)
     {
-        return;
+        return -1;
     }
 
-    gre_any(buf + ihl, n - ihl);
+    *bufp = buf + ihl;
+    *np = n - ihl;
+    return 0;
 }
 
-static void gre_ipv6(const uint8_t *buf, int n, const struct sockaddr_in6 *src)
+static int gre_ipv6(uint8_t **bufp, int *np, const struct sockaddr_in6 *src)
 {
+    /* uint8_t *buf = *bufp; */
+    int n = *np;
+
     if (n < 40)
     {
-        return;
+        return -1;
     }
     // check source IPv6 address
     const struct sockaddr_in6 *remote_in6 = (const struct sockaddr_in6 *)&remote;
     if (memcmp(src->sin6_addr.s6_addr, remote_in6->sin6_addr.s6_addr, 16) != 0)
     {
-        return;
+        return -1;
     }
 
-    gre_any(buf, n);
+    /* Unchanged */
+    /* *bufp = buf; */
+    /* *np = n; */
+    return 0;
 }
 
 static void gre_any(const uint8_t *buf, int n)
 {
+#if 0
     // parse GRE header
     if (*(uint16_t *)(buf) != 0)
     {
@@ -248,7 +284,7 @@ static void gre_any(const uint8_t *buf, int n)
     {
         return;
     }
-
+#endif
     write(tun, buf, n);
 }
 
@@ -321,6 +357,17 @@ static int tun_new(short type, const char *dev)
     {
         return err;
     }
+
+    bzero(&ifr.ifr_hwaddr, sizeof(struct sockaddr));
+
+    err = ioctl(fd, SIOCGIFHWADDR, (void *)&ifr);
+    if (err < 0)
+    {
+        return err;
+    }
+
+    memcpy(mac, ifr.ifr_hwaddr.sa_data, 6);
+
     return fd;
 }