...

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

6 files changed, 174 insertions, 115 deletions

diff --git a/src/pdesc.c b/src/pdesc.c
index da07533..9538e58 100644
--- a/src/pdesc.c
+++ b/src/pdesc.c
@@ -22,17 +22,17 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd
         return REMOTE_PACKET_INVALID;
     }
 
-    if (sock->current.packet.icmphdr->type == ICMP_ECHO && sock->local.is_client != 0) {
+    if (sock->current.pkt_buf.icmphdr.type == ICMP_ECHO && sock->local.is_client != 0) {
         return REMOTE_ICMP_ECHO_CLIENT;
     }
 
-    if (sock->current.packet.icmphdr->type == ICMP_ECHOREPLY && sock->local.is_client == 0) {
+    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.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) {
+            sock->current.pkt_buf.icmphdr.un.echo.id == sock->remotes.descriptors[i].identifier) {
             *desc = &sock->remotes.descriptors[i];
             return REMOTE_EXISTS;
         }
@@ -41,7 +41,7 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd
         return REMOTE_MAX_DESCRIPTORS;
     }
 
-    pdesc_init(&sock->remotes.descriptors[i], &sock->current.peer, sock->current.packet.icmphdr->un.echo.id);
+    pdesc_init(&sock->remotes.descriptors[i], &sock->current.peer, sock->current.pkt_buf.icmphdr.un.echo.id);
 
     *desc = &sock->remotes.descriptors[i];
     sock->remotes.used++;
diff --git a/src/ppkt.c b/src/ppkt.c
index 8557592..dc011a4 100644
--- a/src/ppkt.c
+++ b/src/ppkt.c
@@ -6,9 +6,9 @@
 #include <netinet/ip.h>
 #include <netinet/ip_icmp.h>
 
-enum ptype ppkt_type_to_enum(struct ppkt * pkt)
+enum ptype ppkt_type_to_enum(struct ppkt_header * pheader)
 {
-    enum ptype pt = (enum ptype)pkt->type;
+    enum ptype pt = (enum ptype)pheader->type;
 
     switch (pt) {
         case PTYPE_INVALID:
@@ -23,11 +23,9 @@ enum ptype ppkt_type_to_enum(struct ppkt * pkt)
 int ppkt_process_icmp(struct psock * sock)
 {
     if (sock->current.peer.ss_family == AF_INET) {
-        sock->current.packet.icmphdr = (struct icmphdr *)(sock->current.packet.buffer + sizeof(struct iphdr));
-
-        sock->current.packet.icmphdr->checksum = ntohs(sock->current.packet.icmphdr->checksum);
-        sock->current.packet.icmphdr->un.echo.id = ntohs(sock->current.packet.icmphdr->un.echo.id);
-        sock->current.packet.icmphdr->un.echo.sequence = ntohs(sock->current.packet.icmphdr->un.echo.sequence);
+        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);
     } else {
         return -1;
     }
@@ -35,109 +33,148 @@ int ppkt_process_icmp(struct psock * sock)
     return 0;
 }
 
-int ppkt_process_ppkt(struct psock * sock)
+static size_t ppkt_size(enum ptype type)
 {
-    if (sock->current.peer.ss_family == AF_INET) {
-        if (sock->current.packet.used < sizeof(struct iphdr) + sizeof(*sock->current.packet.icmphdr)) {
-            return -1;
-        }
-
-        sock->current.packet.icmphdr = (struct icmphdr *)(sock->current.packet.buffer + sizeof(struct iphdr));
-        sock->current.packet.icmphdr->un.echo.id = ntohs(sock->current.packet.icmphdr->un.echo.id);
+    size_t body_siz = 0;
 
-        sock->current.packet.pkt =
-            (struct ppkt *)(sock->current.packet.buffer + sizeof(struct iphdr) + sizeof(*sock->current.packet.icmphdr));
-    } else {
-        return -1;
+    switch (type) {
+        case PTYPE_INVALID:
+            return 0;
+        case PTYPE_AUTH_REQUEST:
+            body_siz = sizeof(struct ppkt_auth_request);
+            break;
+        case PTYPE_AUTH_RESPONSE:
+            body_siz = sizeof(struct ppkt_auth_response);
+            break;
     }
 
-    sock->current.packet.pkt->total_size = ntohs(sock->current.packet.pkt->total_size);
-    if (sock->current.packet.pkt->total_size >
-        sizeof(struct iphdr) + sizeof(*sock->current.packet.icmphdr) + sock->current.packet.used) {
-        return -1;
-    }
+    return sizeof(struct ppkt_header) + body_siz;
+}
 
-    switch (ppkt_type_to_enum(sock->current.packet.pkt)) {
+static size_t ppkt_data_size(union ppkt_body * pbody, enum ptype type)
+{
+    size_t data_siz = 0;
+
+    switch (type) {
         case PTYPE_INVALID:
-            return -1;
+            return 0;
         case PTYPE_AUTH_REQUEST:
+            data_siz = pbody->auth_request.authdata_siz;
             break;
         case PTYPE_AUTH_RESPONSE:
+            data_siz = 0;
             break;
     }
 
-    return 0;
+    return data_siz;
 }
 
-static size_t ppkt_sizeof_pkt(struct ppkt * pkt, enum ptype type, size_t additional_size)
+int ppkt_process_ppkt(struct psock * sock)
 {
-    size_t subpkt_size;
+    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);
 
-    switch (type) {
+    if (sock->current.peer.ss_family == AF_INET) {
+        if (sock->current.bytes_read < min_pkt_siz + data_siz)
+        {
+            return -1;
+        }
+    } else {
+        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)
+    {
+        return -1;
+    }
+
+    switch (ppkt_type_to_enum(&sock->current.pkt_buf.pheader)) {
         case PTYPE_INVALID:
-            subpkt_size = 0;
-            additional_size = 0;
-            break;
+            return -1;
         case PTYPE_AUTH_REQUEST:
-            subpkt_size = sizeof(struct ppkt_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.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:
-            subpkt_size = sizeof(struct ppkt_auth_response);
             break;
     }
+    if (sock->current.bytes_read != min_pkt_siz + data_siz)
+    {
+        return -1;
+    }
 
-    pkt->total_size = htons(sizeof(*pkt) + subpkt_size + additional_size);
-    pkt->type = type;
-
-    return sizeof(*pkt) + subpkt_size + additional_size;
+    return 0;
 }
 
-static size_t ppkt_init_pkt(struct pdesc const * desc,
-                            struct ppkt_buffer * pkt_buf,
-                            enum ptype type,
-                            size_t additional_size)
+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;
     switch (type) {
         case PTYPE_INVALID:
             pkt_buf->icmphdr.type = 3; // Destination Unreachable
             break;
         case PTYPE_AUTH_REQUEST:
             pkt_buf->icmphdr.type = 8; // Echo Request
+            pkt_buf->pbody.auth_request.authdata_siz = data_siz;
             break;
         case PTYPE_AUTH_RESPONSE:
             pkt_buf->icmphdr.type = 0; // Echo Reply
             break;
     }
     pkt_buf->icmphdr.code = 0;
-
-    return ppkt_sizeof_pkt(&pkt_buf->pkt, type, additional_size);
+    pkt_buf->pheader.type = 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)
+    {
+        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);
+            break;
+        case PTYPE_AUTH_RESPONSE:
+            break;
+    }
+    pkt_buf->pheader.total_size = htons(pkt_buf->pheader.total_size);
+
     pkt_buf->icmphdr.checksum = 0;
     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 * hash, size_t hash_siz)
+void ppkt_prepare_auth_request(struct pdesc * desc, struct ppkt_buffer * pkt_buf, uint8_t * authdata, size_t authdata_siz)
 {
-    size_t total_size = ppkt_init_pkt(desc, pkt_buf, PTYPE_AUTH_REQUEST, hash_siz);
-
-    pkt_buf->auth_request.magic = htonl(PTUNNEL_MAGIC);
-    pkt_buf->auth_request.hash_siz = hash_siz;
+    ppkt_init_pkt(desc, pkt_buf, PTYPE_AUTH_REQUEST, authdata_siz);
 
     pkt_buf->iovec[0].iov_base = &pkt_buf->icmphdr;
     pkt_buf->iovec[0].iov_len = sizeof(pkt_buf->icmphdr);
 
-    pkt_buf->iovec[1].iov_base = &pkt_buf->pkt;
-    pkt_buf->iovec[1].iov_len = total_size;
+    pkt_buf->iovec[1].iov_base = &pkt_buf->pheader;
+    pkt_buf->iovec[1].iov_len = sizeof(pkt_buf->pheader);
 
-    pkt_buf->iovec[2].iov_base = &pkt_buf->auth_request;
-    pkt_buf->iovec[2].iov_len = sizeof(pkt_buf->auth_request);
+    pkt_buf->iovec[2].iov_base = &pkt_buf->pbody.auth_request;
+    pkt_buf->iovec[2].iov_len = sizeof(pkt_buf->pbody.auth_request);
 
-    pkt_buf->iovec[3].iov_base = hash;
-    pkt_buf->iovec[3].iov_len = hash_siz;
+    pkt_buf->iovec[3].iov_base = authdata;
+    pkt_buf->iovec[3].iov_len = authdata_siz;
 
     pkt_buf->iovec_used = 4;
 
diff --git a/src/ppkt.h b/src/ppkt.h
index 5772fe8..029e73b 100644
--- a/src/ppkt.h
+++ b/src/ppkt.h
@@ -7,13 +7,14 @@
 
 #define PTUNNEL_MAGIC 0xdeadc0de
 
-#define U8_PTYPE_AUTH_REQUEST 0x01u
-#define U8_PTYPE_AUTH_RESPONSE 0x02u
+#define PTUNNAL_MAX_BODY_SIZE (1500 - sizeof(struct iphdr) \
+                                    - sizeof(struct icmphdr) \
+                                    - sizeof(struct ppkt_header))
 
 enum ptype {
     PTYPE_INVALID = 0,
-    PTYPE_AUTH_REQUEST = U8_PTYPE_AUTH_REQUEST,
-    PTYPE_AUTH_RESPONSE = U8_PTYPE_AUTH_RESPONSE,
+    PTYPE_AUTH_REQUEST,
+    PTYPE_AUTH_RESPONSE,
 };
 
 struct psock;
@@ -21,18 +22,23 @@ struct pdesc;
 
 struct ppkt_auth_request {
     uint32_t magic;
-    uint16_t hash_siz;
-    uint8_t hash[0];
+    uint16_t authdata_siz;
+    uint8_t authdata[0];
 } __attribute__((__packed__));
 
 struct ppkt_auth_response {
     uint8_t code;
 } __attribute__((__packed__));
 
-struct ppkt {
+struct ppkt_header {
     uint16_t total_size;
     uint8_t type;
-    uint8_t data[0];
+} __attribute__((__packed__));
+
+union ppkt_body {
+    struct ppkt_auth_request auth_request;
+    struct ppkt_auth_response auth_response;
+    uint8_t buf[PTUNNAL_MAX_BODY_SIZE];
 } __attribute__((__packed__));
 
 struct ppkt_buffer {
@@ -40,14 +46,11 @@ struct ppkt_buffer {
     size_t iovec_used;
 
     struct icmphdr icmphdr;
-    struct ppkt pkt;
-    union {
-        struct ppkt_auth_request auth_request;
-        struct ppkt_auth_response auth_response;
-    };
+    struct ppkt_header pheader;
+    union ppkt_body pbody;
 };
 
-enum ptype ppkt_type_to_enum(struct ppkt *);
+enum ptype ppkt_type_to_enum(struct ppkt_header *);
 
 int ppkt_process_icmp(struct psock *);
 
diff --git a/src/psock.c b/src/psock.c
index 35747d1..1647a12 100644
--- a/src/psock.c
+++ b/src/psock.c
@@ -14,16 +14,18 @@
 #include <sys/types.h>
 #include <unistd.h>
 
-int psock_init(struct psock * sock, size_t max_descriptors, size_t packet_buffer_size)
+int psock_init(struct psock * sock, size_t max_descriptors)
 {
     memset(sock, 0, sizeof(*sock));
 
-    sock->current.packet.max = packet_buffer_size;
-    sock->current.packet.used = 0;
-    sock->current.packet.buffer = (uint8_t *)calloc(packet_buffer_size, sizeof(*sock->current.packet.buffer));
-    if (sock->current.packet.buffer == NULL) {
-        goto error;
-    }
+    // iovec[0] is used for the IP header, but stack allocated
+    sock->current.pkt_buf.iovec[1].iov_base = &sock->current.pkt_buf.icmphdr;
+    sock->current.pkt_buf.iovec[1].iov_len = sizeof(sock->current.pkt_buf.icmphdr);
+    sock->current.pkt_buf.iovec[2].iov_base = &sock->current.pkt_buf.pheader;
+    sock->current.pkt_buf.iovec[2].iov_len = sizeof(sock->current.pkt_buf.pheader);
+    sock->current.pkt_buf.iovec[3].iov_base = &sock->current.pkt_buf.pbody;
+    sock->current.pkt_buf.iovec[3].iov_len = sizeof(sock->current.pkt_buf.pbody);
+    sock->current.pkt_buf.iovec_used = 4;
 
     sock->remotes.max = max_descriptors;
     sock->remotes.used = 0;
@@ -163,9 +165,6 @@ int psock_add_server(struct psock * sock, char const * address)
 
 void psock_free(struct psock * sock)
 {
-    free(sock->current.packet.buffer);
-    sock->current.packet.buffer = NULL;
-
     free(sock->remotes.descriptors);
     sock->remotes.descriptors = NULL;
     sock->remotes.used = 0;
@@ -187,28 +186,28 @@ static void psock_process_cmsg(struct msghdr * hdr)
 
 static int psock_recvmsg(struct psock * sock)
 {
-    struct iovec iov;
-    struct msghdr hdr = {};
+    struct iphdr iphdr;
+    struct msghdr msghdr = {};
     ssize_t nread;
 
-    iov.iov_base = (void *)sock->current.packet.buffer;
-    iov.iov_len = sock->current.packet.max;
+    sock->current.pkt_buf.iovec[0].iov_base = &iphdr;
+    sock->current.pkt_buf.iovec[0].iov_len = sizeof(iphdr);
 
-    hdr.msg_name = &sock->current.peer;
-    hdr.msg_namelen = sizeof(sock->current.peer);
-    hdr.msg_iov = &iov;
-    hdr.msg_iovlen = 1;
+    msghdr.msg_name = &sock->current.peer;
+    msghdr.msg_namelen = sizeof(sock->current.peer);
+    msghdr.msg_iov = sock->current.pkt_buf.iovec;
+    msghdr.msg_iovlen = sock->current.pkt_buf.iovec_used;
 
     do {
-        nread = recvmsg(sock->icmp_fd, &hdr, 0);
+        nread = recvmsg(sock->icmp_fd, &msghdr, 0);
     } while (nread == -1 && errno == EINTR);
 
     if (nread >= 0) {
-        sock->current.packet.used = nread;
-        psock_process_cmsg(&hdr);
+        sock->current.bytes_read = nread;
+        psock_process_cmsg(&msghdr);
         return 0;
     } else {
-        sock->current.packet.used = 0;
+        sock->current.bytes_read = 0;
         return -1;
     }
 }
@@ -228,7 +227,7 @@ static int psock_sendmsg(struct psock * sock, struct iovec * const iov, size_t i
     return nwritten;
 }
 
-static struct pdesc * psock_handle_events(struct psock * sock)
+static struct pdesc * psock_get_remote(struct psock * sock)
 {
     struct pdesc * remote = NULL;
 
@@ -244,10 +243,10 @@ static struct pdesc * psock_handle_events(struct psock * sock)
                 logger(1, "Invalid packet received.");
                 break;
             case REMOTE_ICMP_ECHO_CLIENT:
-                logger(0, "Received ICMP echo, but I am a client.");
+                logger(0, "Received ICMP echo request, but I am a client.");
                 break;
             case REMOTE_ICMP_REPLY_SERVER:
-                logger(0, "Received ICMP reply, but I am a server.");
+                logger(0, "Received ICMP echo reply, but I am a server.");
                 break;
             case REMOTE_MAX_DESCRIPTORS:
                 logger(1, "Max descriptors reached, sorry.");
@@ -258,6 +257,26 @@ static struct pdesc * psock_handle_events(struct psock * sock)
     return remote;
 }
 
+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;
+  }
+}
+
 static void psock_loop_server(struct psock * sock)
 {
     const int max_events = 32;
@@ -271,9 +290,16 @@ static void psock_loop_server(struct psock * sock)
                 break;
             case 0:
                 continue;
-            default:
-                psock_handle_events(sock);
+            default: {
+                struct pdesc * const desc = psock_get_remote(sock);
+
+                if (desc == NULL)
+                {
+                    break;
+                }
+                psock_loop_server_process_packet(sock, desc);
                 break;
+            }
         }
     }
 }
@@ -316,7 +342,7 @@ static void psock_loop_client(struct psock * sock)
                 psock_loop_client_event_timeout(sock);
                 break;
             default:
-                psock_handle_events(sock);
+                psock_get_remote(sock);
                 break;
         }
     }
diff --git a/src/psock.h b/src/psock.h
index eb00070..427e75c 100644
--- a/src/psock.h
+++ b/src/psock.h
@@ -1,13 +1,12 @@
 #ifndef PSOCK_H
 #define PSOCK_H 1
 
-#include <netinet/in.h>
+#include "ppkt.h"
+
 #include <stdint.h>
 #include <stdlib.h>
 
-struct icmphdr;
 struct pdesc;
-struct ppkt;
 
 struct psock {
     int epoll_fd;
@@ -19,14 +18,8 @@ struct psock {
 
     struct {
         struct sockaddr_storage peer;
-
-        struct {
-            size_t used;
-            size_t max;
-            uint8_t * buffer;
-            struct icmphdr * icmphdr;
-            struct ppkt * pkt;
-        } packet;
+        size_t bytes_read;
+        struct ppkt_buffer pkt_buf;
     } current;
 
     struct {
@@ -36,7 +29,7 @@ struct psock {
     } remotes;
 };
 
-int psock_init(struct psock *, size_t, size_t);
+int psock_init(struct psock *, size_t);
 
 int psock_setup_fds(struct psock *, int);
 
diff --git a/src/ptunnel.c b/src/ptunnel.c
index 6b2b815..b8a3df2 100644
--- a/src/ptunnel.c
+++ b/src/ptunnel.c
@@ -56,7 +56,7 @@ int main(int argc, char ** argv)
 
     init_logging("ptunnel-ng");
 
-    if (psock_init(&sock, 16, 1500) != 0) {
+    if (psock_init(&sock, 16) != 0) {
         logger(1, "%s", "Socket initialization failed");
         ret++;
         goto failure;