...ptunnel-ng/2.0.0

Signed-off-by: lns <matzeton@googlemail.com>

7 files changed, 163 insertions, 107 deletions

diff --git a/src/pdesc.c b/src/pdesc.c
index 9538e58..3ceed72 100644
--- a/src/pdesc.c
+++ b/src/pdesc.c
@@ -2,16 +2,29 @@
 #include "ppkt.h"
 #include "psock.h"
 
+#include <arpa/inet.h>
 #include <netinet/ip_icmp.h>
 
-void pdesc_init(struct pdesc * desc, struct sockaddr_storage * sockaddr, uint16_t identifier)
+void pdesc_init(struct pdesc * desc, uint16_t identifier)
 {
     desc->state = PDESC_STATE_AUTH;
-    desc->peer = *sockaddr;
     desc->identifier = identifier;
     desc->sequence = 0;
 }
 
+static enum pdesc_remote_errno pdesc_check_icmp_type(struct psock * sock, struct pdesc * const desc)
+{
+    if (sock->current.pkt_buf.icmphdr.type == ICMP_ECHO && sock->local.is_client != 0) {
+        return REMOTE_ICMP_ECHO_CLIENT;
+    }
+
+    if (sock->current.pkt_buf.icmphdr.type == ICMP_ECHOREPLY && sock->local.is_client == 0) {
+        return REMOTE_ICMP_REPLY_SERVER;
+    }
+
+    return REMOTE_EXISTS;
+}
+
 enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pdesc ** const desc)
 {
     size_t i;
@@ -22,28 +35,50 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd
         return REMOTE_PACKET_INVALID;
     }
 
-    if (sock->current.pkt_buf.icmphdr.type == ICMP_ECHO && sock->local.is_client != 0) {
-        return REMOTE_ICMP_ECHO_CLIENT;
-    }
-
-    if (sock->current.pkt_buf.icmphdr.type == ICMP_ECHOREPLY && sock->local.is_client == 0) {
-        return REMOTE_ICMP_REPLY_SERVER;
-    }
-
     for (i = 0; i < sock->remotes.used; ++i) {
         if (sock->remotes.descriptors[i].state != PDESC_STATE_INVALID &&
             sock->current.pkt_buf.icmphdr.un.echo.id == sock->remotes.descriptors[i].identifier) {
             *desc = &sock->remotes.descriptors[i];
-            return REMOTE_EXISTS;
+            return pdesc_check_icmp_type(sock, *desc);
         }
     }
     if (i == sock->remotes.max) {
         return REMOTE_MAX_DESCRIPTORS;
     }
 
-    pdesc_init(&sock->remotes.descriptors[i], &sock->current.peer, sock->current.pkt_buf.icmphdr.un.echo.id);
+    pdesc_init(&sock->remotes.descriptors[i], sock->current.pkt_buf.icmphdr.un.echo.id);
+    if (pdesc_set_addr(&sock->remotes.descriptors[i].peer, &sock->current.peer_sockaddr) != 0) {
+        return REMOTE_ADDR_INVALID;
+    }
 
     *desc = &sock->remotes.descriptors[i];
     sock->remotes.used++;
+
     return REMOTE_NOT_FOUND;
 }
+
+int pdesc_set_addr(struct paddr * addr, struct sockaddr_storage const * sockaddr)
+{
+    addr->sockaddr = *sockaddr;
+
+    switch (sockaddr->ss_family) {
+        case AF_INET:
+            struct in_addr in_addr = ((struct sockaddr_in *)sockaddr)->sin_addr;
+            if (inet_ntop(AF_INET, &in_addr, addr->str, sizeof(struct sockaddr_in)) == NULL) {
+                return -1;
+            }
+            break;
+
+        case AF_INET6:
+            struct in6_addr in6_addr = ((struct sockaddr_in6 *)sockaddr)->sin6_addr;
+            if (inet_ntop(AF_INET, &in6_addr, addr->str, sizeof(struct sockaddr_in6)) == NULL) {
+                return -1;
+            }
+            break;
+
+        default:
+            return -1;
+    }
+
+    return 0;
+}
diff --git a/src/pdesc.h b/src/pdesc.h
index d81798e..af79364 100644
--- a/src/pdesc.h
+++ b/src/pdesc.h
@@ -10,6 +10,7 @@ enum pdesc_remote_errno {
     REMOTE_EXISTS,
     REMOTE_NOT_FOUND,
     REMOTE_PACKET_INVALID,
+    REMOTE_ADDR_INVALID,
     REMOTE_ICMP_ECHO_CLIENT,
     REMOTE_ICMP_REPLY_SERVER,
     REMOTE_MAX_DESCRIPTORS,
@@ -17,16 +18,22 @@ enum pdesc_remote_errno {
 
 enum pdesc_state { PDESC_STATE_INVALID = 0, PDESC_STATE_AUTH, PDESC_STATE_DATA };
 
+struct paddr {
+    struct sockaddr_storage sockaddr;
+    char str[INET6_ADDRSTRLEN];
+};
+
 struct pdesc {
     enum pdesc_state state;
-    struct sockaddr_storage peer;
-    char peer_str[INET6_ADDRSTRLEN];
+    struct paddr peer;
     uint16_t identifier;
     uint16_t sequence;
 };
 
-void pdesc_init(struct pdesc *, struct sockaddr_storage *, uint16_t identifier);
+void pdesc_init(struct pdesc *, uint16_t identifier);
 
 enum pdesc_remote_errno pdesc_find_current_remote(struct psock *, struct pdesc ** const);
 
+int pdesc_set_addr(struct paddr *, struct sockaddr_storage const *);
+
 #endif
diff --git a/src/ppkt.c b/src/ppkt.c
index dc011a4..7c06f27 100644
--- a/src/ppkt.c
+++ b/src/ppkt.c
@@ -6,7 +6,7 @@
 #include <netinet/ip.h>
 #include <netinet/ip_icmp.h>
 
-enum ptype ppkt_type_to_enum(struct ppkt_header * pheader)
+enum ptype ppkt_type_to_enum(struct ppkt_header const * pheader)
 {
     enum ptype pt = (enum ptype)pheader->type;
 
@@ -22,7 +22,7 @@ enum ptype ppkt_type_to_enum(struct ppkt_header * pheader)
 
 int ppkt_process_icmp(struct psock * sock)
 {
-    if (sock->current.peer.ss_family == AF_INET) {
+    if (sock->current.peer_sockaddr.ss_family == AF_INET) {
         sock->current.pkt_buf.icmphdr.checksum = ntohs(sock->current.pkt_buf.icmphdr.checksum);
         sock->current.pkt_buf.icmphdr.un.echo.id = ntohs(sock->current.pkt_buf.icmphdr.un.echo.id);
         sock->current.pkt_buf.icmphdr.un.echo.sequence = ntohs(sock->current.pkt_buf.icmphdr.un.echo.sequence);
@@ -71,52 +71,49 @@ static size_t ppkt_data_size(union ppkt_body * pbody, enum ptype type)
 
 int ppkt_process_ppkt(struct psock * sock)
 {
-    size_t const min_pkt_siz = sizeof(struct iphdr) +
-                               sizeof(sock->current.pkt_buf.icmphdr);
-    size_t data_siz = sizeof(sock->current.pkt_buf.pheader);
+    size_t const min_pkt_siz = sizeof(struct iphdr) + sizeof(sock->current.pkt_buf.icmphdr);
 
-    if (sock->current.peer.ss_family == AF_INET) {
-        if (sock->current.bytes_read < min_pkt_siz + data_siz)
-        {
-            return -1;
-        }
-    } else {
+    if (sock->current.peer_sockaddr.ss_family != AF_INET) {
+        return -1;
+    }
+
+    if (sock->current.bytes_read < min_pkt_siz + sizeof(sock->current.pkt_buf.pheader)) {
         return -1;
     }
 
     sock->current.pkt_buf.pheader.total_size = ntohs(sock->current.pkt_buf.pheader.total_size);
-    if (sock->current.pkt_buf.pheader.total_size != sock->current.bytes_read - min_pkt_siz)
-    {
+    if (sock->current.pkt_buf.pheader.total_size != sock->current.bytes_read - min_pkt_siz) {
         return -1;
     }
 
-    switch (ppkt_type_to_enum(&sock->current.pkt_buf.pheader)) {
+    enum ptype packet_type = ppkt_type_to_enum(&sock->current.pkt_buf.pheader);
+    size_t packet_body_size = ppkt_size(packet_type);
+    if (packet_body_size == 0 ||
+        packet_body_size > sock->current.bytes_read - (min_pkt_siz + sizeof(sock->current.pkt_buf.pheader))) {
+        return -1;
+    }
+
+    switch (packet_type) {
         case PTYPE_INVALID:
             return -1;
         case PTYPE_AUTH_REQUEST:
-            sock->current.pkt_buf.pbody.auth_request.magic =
-                ntohl(sock->current.pkt_buf.pbody.auth_request.magic);
+            sock->current.pkt_buf.pbody.auth_request.magic = ntohl(sock->current.pkt_buf.pbody.auth_request.magic);
             sock->current.pkt_buf.pbody.auth_request.authdata_siz =
                 ntohs(sock->current.pkt_buf.pbody.auth_request.authdata_siz);
-
-            data_siz += sizeof(sock->current.pkt_buf.pbody.auth_request);
-            data_siz += sock->current.pkt_buf.pbody.auth_request.authdata_siz;
             break;
         case PTYPE_AUTH_RESPONSE:
             break;
     }
-    if (sock->current.bytes_read != min_pkt_siz + data_siz)
-    {
+
+    size_t packet_data_size = ppkt_data_size(&sock->current.pkt_buf.pbody, packet_type);
+    if (sock->current.bytes_read != min_pkt_siz + packet_body_size + packet_data_size) {
         return -1;
     }
 
     return 0;
 }
 
-static void ppkt_init_pkt(struct pdesc * desc,
-                          struct ppkt_buffer * pkt_buf,
-                          enum ptype type,
-                          size_t data_siz)
+static void ppkt_init_pkt(struct pdesc * desc, struct ppkt_buffer * pkt_buf, enum ptype type, size_t data_siz)
 {
     pkt_buf->icmphdr.un.echo.id = desc->identifier;
     pkt_buf->icmphdr.un.echo.sequence = ++desc->sequence;
@@ -134,22 +131,19 @@ static void ppkt_init_pkt(struct pdesc * desc,
     }
     pkt_buf->icmphdr.code = 0;
     pkt_buf->pheader.type = type;
-    pkt_buf->pheader.total_size = ppkt_size(type) +
-                                  ppkt_data_size(&pkt_buf->pbody, type);
+    pkt_buf->pheader.total_size = ppkt_size(type) + ppkt_data_size(&pkt_buf->pbody, type);
 }
 
 static void ppkt_finalize_pkt(struct ppkt_buffer * const pkt_buf)
 {
     pkt_buf->icmphdr.un.echo.id = htons(pkt_buf->icmphdr.un.echo.id);
     pkt_buf->icmphdr.un.echo.sequence = htons(pkt_buf->icmphdr.un.echo.sequence);
-    switch (pkt_buf->pheader.type)
-    {
+    switch (pkt_buf->pheader.type) {
         case PTYPE_INVALID:
             break;
         case PTYPE_AUTH_REQUEST:
             pkt_buf->pbody.auth_request.magic = htonl(PTUNNEL_MAGIC);
-            pkt_buf->pbody.auth_request.authdata_siz =
-                htons(pkt_buf->pbody.auth_request.authdata_siz);
+            pkt_buf->pbody.auth_request.authdata_siz = htons(pkt_buf->pbody.auth_request.authdata_siz);
             break;
         case PTYPE_AUTH_RESPONSE:
             break;
@@ -160,7 +154,10 @@ static void ppkt_finalize_pkt(struct ppkt_buffer * const pkt_buf)
     pkt_buf->icmphdr.checksum = icmp_checksum_iovec(pkt_buf->iovec, pkt_buf->iovec_used);
 }
 
-void ppkt_prepare_auth_request(struct pdesc * desc, struct ppkt_buffer * pkt_buf, uint8_t * authdata, size_t authdata_siz)
+void ppkt_prepare_auth_request(struct pdesc * desc,
+                               struct ppkt_buffer * pkt_buf,
+                               uint8_t * authdata,
+                               size_t authdata_siz)
 {
     ppkt_init_pkt(desc, pkt_buf, PTYPE_AUTH_REQUEST, authdata_siz);
 
diff --git a/src/ppkt.h b/src/ppkt.h
index 029e73b..bdfb57d 100644
--- a/src/ppkt.h
+++ b/src/ppkt.h
@@ -7,9 +7,7 @@
 
 #define PTUNNEL_MAGIC 0xdeadc0de
 
-#define PTUNNAL_MAX_BODY_SIZE (1500 - sizeof(struct iphdr) \
-                                    - sizeof(struct icmphdr) \
-                                    - sizeof(struct ppkt_header))
+#define PTUNNAL_MAX_BODY_SIZE (1500 - sizeof(struct iphdr) - sizeof(struct icmphdr) - sizeof(struct ppkt_header))
 
 enum ptype {
     PTYPE_INVALID = 0,
@@ -31,6 +29,7 @@ struct ppkt_auth_response {
 } __attribute__((__packed__));
 
 struct ppkt_header {
+    // sizeof(header) + sizeof(body) + sizeof(data)
     uint16_t total_size;
     uint8_t type;
 } __attribute__((__packed__));
@@ -50,7 +49,7 @@ struct ppkt_buffer {
     union ppkt_body pbody;
 };
 
-enum ptype ppkt_type_to_enum(struct ppkt_header *);
+enum ptype ppkt_type_to_enum(struct ppkt_header const *);
 
 int ppkt_process_icmp(struct psock *);
 
diff --git a/src/psock.c b/src/psock.c
index 1647a12..8e202a8 100644
--- a/src/psock.c
+++ b/src/psock.c
@@ -3,9 +3,9 @@
 #include "ppkt.h"
 #include "putils.h"
 
-#include <arpa/inet.h>
 #include <errno.h>
 #include <netdb.h>
+#include <net/if.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -43,6 +43,9 @@ error:
 int psock_setup_fds(struct psock * sock, int is_client)
 {
     struct epoll_event ev;
+    socklen_t addrlen = sizeof(sock->local.addr);
+    char const ifname[IFNAMSIZ] = "wifi0";
+    const socklen_t ifname_len = strnlen(ifname, IFNAMSIZ);
 
     errno = 0;
 
@@ -51,6 +54,20 @@ int psock_setup_fds(struct psock * sock, int is_client)
         goto error;
     }
 
+    if (ifname_len == IFNAMSIZ) {
+        goto error;
+    }
+#if 0
+    if (setsockopt(sock->icmp_fd, SOL_SOCKET, SO_BINDTODEVICE, ifname, ifname_len) != 0) {
+        goto error;
+    }
+#endif
+
+    if (getsockname(sock->icmp_fd, (struct sockaddr *)&sock->local.addr, &addrlen) != 0 ||
+        addrlen > sizeof(sock->local.addr)) {
+        goto error;
+    }
+
     sock->epoll_fd = epoll_create1(EPOLL_CLOEXEC);
     if (sock->epoll_fd < 0) {
         goto error;
@@ -128,34 +145,14 @@ int psock_add_server(struct psock * sock, char const * address)
             desc_i++;
             continue;
         } else {
-            pdesc_init(&sock->remotes.descriptors[desc_i], &sockaddrs[addr_i], icmp_generate_identifier());
-            sock->remotes.descriptors[desc_i].state = PDESC_STATE_AUTH;
-        }
-
-        switch (sockaddrs[addr_i].ss_family) {
-            case AF_INET: {
-                struct in_addr addr = ((struct sockaddr_in *)&sockaddrs[addr_i])->sin_addr;
-                if (inet_ntop(AF_INET, &addr, sock->remotes.descriptors[desc_i].peer_str, sizeof(struct sockaddr_in)) ==
-                    NULL) {
-                    logger_early(1, "inet_ntop() conversion failed: %s", strerror(errno));
-                    return -1;
-                }
-                break;
-            }
-            case AF_INET6: {
-                struct in6_addr addr = ((struct sockaddr_in6 *)&sockaddrs[addr_i])->sin6_addr;
-                if (inet_ntop(AF_INET6,
-                              &addr,
-                              sock->remotes.descriptors[desc_i].peer_str,
-                              sizeof(struct sockaddr_in6)) == NULL) {
-                    logger_early(1, "inet_ntop() conversion failed: %s", strerror(errno));
-                    return -1;
-                }
-                break;
+            pdesc_init(&sock->remotes.descriptors[desc_i], icmp_generate_identifier());
+            if (pdesc_set_addr(&sock->remotes.descriptors[desc_i].peer, &sockaddrs[addr_i]) != 0) {
+                return -1;
             }
+            sock->remotes.descriptors[desc_i].state = PDESC_STATE_AUTH;
         }
 
-        logger_early(0, "Added remote: %s", sock->remotes.descriptors[desc_i].peer_str);
+        logger_early(0, "Added remote: %s", sock->remotes.descriptors[desc_i].peer.str);
         addr_i++;
         sock->remotes.used++;
     }
@@ -180,7 +177,7 @@ void psock_free(struct psock * sock)
 static void psock_process_cmsg(struct msghdr * hdr)
 {
     for (struct cmsghdr * cmsg = CMSG_FIRSTHDR(hdr); cmsg != NULL; cmsg = CMSG_NXTHDR(hdr, cmsg)) {
-        printf("CMSG TYPE/LEVEL/LEN: %d / %d / %zu\n", cmsg->cmsg_type, cmsg->cmsg_level, cmsg->cmsg_len);
+        logger(1, "CMSG TYPE/LEVEL/LEN: %d / %d / %zu\n", cmsg->cmsg_type, cmsg->cmsg_level, cmsg->cmsg_len);
     }
 }
 
@@ -193,8 +190,8 @@ static int psock_recvmsg(struct psock * sock)
     sock->current.pkt_buf.iovec[0].iov_base = &iphdr;
     sock->current.pkt_buf.iovec[0].iov_len = sizeof(iphdr);
 
-    msghdr.msg_name = &sock->current.peer;
-    msghdr.msg_namelen = sizeof(sock->current.peer);
+    msghdr.msg_name = &sock->current.peer_sockaddr;
+    msghdr.msg_namelen = sizeof(sock->current.peer_sockaddr);
     msghdr.msg_iov = sock->current.pkt_buf.iovec;
     msghdr.msg_iovlen = sock->current.pkt_buf.iovec_used;
 
@@ -212,13 +209,13 @@ static int psock_recvmsg(struct psock * sock)
     }
 }
 
-static int psock_sendmsg(struct psock * sock, struct iovec * const iov, size_t iovlen)
+static int psock_sendmsg(struct psock const * sock, struct sockaddr_storage * dest, struct iovec * iov, size_t iovlen)
 {
     struct msghdr hdr = {};
     ssize_t nwritten;
 
-    hdr.msg_name = &sock->current.peer;
-    hdr.msg_namelen = sizeof(sock->current.peer);
+    hdr.msg_name = dest;
+    hdr.msg_namelen = sizeof(*dest);
     hdr.msg_iov = iov;
     hdr.msg_iovlen = iovlen;
 
@@ -242,6 +239,9 @@ static struct pdesc * psock_get_remote(struct psock * sock)
             case REMOTE_PACKET_INVALID:
                 logger(1, "Invalid packet received.");
                 break;
+            case REMOTE_ADDR_INVALID:
+                logger(1, "Remote address invalid.");
+                break;
             case REMOTE_ICMP_ECHO_CLIENT:
                 logger(0, "Received ICMP echo request, but I am a client.");
                 break;
@@ -259,22 +259,21 @@ static struct pdesc * psock_get_remote(struct psock * sock)
 
 static void psock_loop_server_process_packet(struct psock * sock, struct pdesc * desc)
 {
-  (void)sock;
-
-  switch (desc->state) {
-    case PDESC_STATE_INVALID:
-      break;
-    case PDESC_STATE_AUTH:
-      if (ppkt_type_to_enum(&sock->current.pkt_buf.pheader) != PTYPE_AUTH_REQUEST)
-      {
-        logger(1, "Expected authentication request from client.");
-        return;
-      }
-      logger(0, "Received authentication request from client.");
-      break;
-    case PDESC_STATE_DATA:
-      break;
-  }
+    (void)sock;
+
+    switch (desc->state) {
+        case PDESC_STATE_INVALID:
+            break;
+        case PDESC_STATE_AUTH:
+            if (ppkt_type_to_enum(&sock->current.pkt_buf.pheader) != PTYPE_AUTH_REQUEST) {
+                logger(1, "Expected authentication request from client.");
+                return;
+            }
+            logger(0, "Received authentication request from client.");
+            break;
+        case PDESC_STATE_DATA:
+            break;
+    }
 }
 
 static void psock_loop_server(struct psock * sock)
@@ -293,8 +292,7 @@ static void psock_loop_server(struct psock * sock)
             default: {
                 struct pdesc * const desc = psock_get_remote(sock);
 
-                if (desc == NULL)
-                {
+                if (desc == NULL) {
                     break;
                 }
                 psock_loop_server_process_packet(sock, desc);
@@ -318,7 +316,7 @@ static void psock_loop_client_event_timeout(struct psock * sock)
                 uint8_t b[3] = {0x41, 0x42, 0x43};
                 struct ppkt_buffer pb;
                 ppkt_prepare_auth_request(desc, &pb, b, 3);
-                psock_sendmsg(sock, pb.iovec, pb.iovec_used);
+                psock_sendmsg(sock, &desc->peer.sockaddr, pb.iovec, pb.iovec_used);
                 break;
             }
             case PDESC_STATE_DATA:
diff --git a/src/psock.h b/src/psock.h
index 427e75c..80a29e2 100644
--- a/src/psock.h
+++ b/src/psock.h
@@ -14,10 +14,11 @@ struct psock {
 
     struct {
         int is_client;
+        struct paddr addr;
     } local;
 
     struct {
-        struct sockaddr_storage peer;
+        struct sockaddr_storage peer_sockaddr;
         size_t bytes_read;
         struct ppkt_buffer pkt_buf;
     } current;
diff --git a/src/putils.c b/src/putils.c
index 3f8563d..3ae1baf 100644
--- a/src/putils.c
+++ b/src/putils.c
@@ -162,25 +162,44 @@ static inline uint16_t get_n16bit(uint8_t const * cbuf)
 uint16_t icmp_checksum_iovec(struct iovec const * iovec, size_t iovec_size)
 {
     uint32_t checksum = 0;
+    uint16_t result;
+    uint8_t is_overlapping = 0;
+    uint8_t overlap[2] = {0x00, 0x00};
 
     for (size_t iov_i = 0; iov_i < iovec_size; ++iov_i) {
         uint8_t const * buf = iovec[iov_i].iov_base;
         size_t len = iovec[iov_i].iov_len;
 
+        if (is_overlapping != 0 && len > 0) {
+            overlap[1] = buf[0];
+            checksum += get_n16bit(overlap);
+            buf++;
+            len--;
+            is_overlapping = 0;
+        }
+
         for (; len > 1; len -= 2) {
             checksum += get_n16bit(buf);
             buf += 2;
         }
 
         if (len == 1) {
-            checksum += *buf;
+            overlap[0] = *buf;
+            is_overlapping = 1;
         }
     }
 
-    checksum = (checksum >> 16) + (checksum & 0xFFFF);
-    checksum += (checksum >> 16);
+    if (is_overlapping != 0) {
+        checksum += overlap[0];
+    }
+
+    while (checksum >> 16 > 0) {
+        checksum = (checksum >> 16) + (checksum & 0xFFFF);
+    }
+
+    result = ~checksum;
 
-    return ~checksum;
+    return result;
 }
 
 uint16_t icmp_generate_identifier(void)