...

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

1 files changed, 93 insertions, 56 deletions

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;