fourth smth

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

7 files changed, 147 insertions, 82 deletions

diff --git a/src/pdesc.c b/src/pdesc.c
index 82cc600..77f511e 100644
--- a/src/pdesc.c
+++ b/src/pdesc.c
@@ -1,12 +1,33 @@
 #include "pdesc.h"
+#include "ppkt.h"
 #include "psock.h"
 
+#include <netinet/ip_icmp.h>
 
-enum pdesc_retval pdesc_find_remote(struct psock * psock)
+static void pdesc_init(struct psock * psock, struct pdesc * pdesc, uint16_t identifier)
 {
-    for (size_t i = 0; i < psock->remotes.used; ++i)
-    {
+    pdesc->state = PDESC_STATE_AUTH;
+    pdesc->peer = psock->current.peer;
+    pdesc->identifier = identifier;
+    pdesc->sequence = 0;
+}
+
+struct pdesc * pdesc_find_remote(struct psock * psock)
+{
+    size_t i;
+
+    if (psock->remotes.used == psock->remotes.max || ppkt_process_icmp(psock) != 0 ||
+        psock->current.packet.icmphdr->type != ICMP_ECHOREPLY || ppkt_process_ppkt(psock) != 0) {
+        return NULL;
     }
 
-    return PDESC_REMOTE_FOUND;
+    for (i = 0; i < psock->remotes.used; ++i) {
+        if (psock->current.packet.icmphdr->un.echo.id == psock->remotes.descriptors[i].identifier) {
+            return &psock->remotes.descriptors[i];
+        }
+    }
+
+    pdesc_init(psock, &psock->remotes.descriptors[i], psock->current.packet.icmphdr->un.echo.id);
+
+    return &psock->remotes.descriptors[i];
 }
diff --git a/src/pdesc.h b/src/pdesc.h
index 8dde3cf..54fad4a 100644
--- a/src/pdesc.h
+++ b/src/pdesc.h
@@ -1,25 +1,20 @@
 #ifndef PDESC_H
 #define PDESC_H
 
+#include <netinet/in.h>
 #include <stdint.h>
 
-
 struct psock;
 
-struct pdesc
-{
+enum pdesc_state { PDESC_STATE_AUTH };
+
+struct pdesc {
+    enum pdesc_state state;
+    struct sockaddr_storage peer;
     uint16_t identifier;
     uint16_t sequence;
 };
 
-enum pdesc_retval
-{
-    PDESC_REMOTE_NEW,
-    PDESC_REMOTE_INVALID,
-    PDESC_REMOTE_FOUND
-};
-
-
-enum pdesc_retval pdesc_find_remote(struct psock *);
+struct pdesc * pdesc_find_remote(struct psock *);
 
 #endif
diff --git a/src/ppkt.c b/src/ppkt.c
index cb01b79..9496187 100644
--- a/src/ppkt.c
+++ b/src/ppkt.c
@@ -1,6 +1,46 @@
 #include "ppkt.h"
+#include "psock.h"
 
+#include <netinet/ip.h>
+#include <netinet/ip_icmp.h>
 
-void ppkt_process_icmp(struct psock * psock)
+int ppkt_process_icmp(struct psock * psock)
 {
+    if (psock->current.peer.ss_family == AF_INET) {
+        psock->current.packet.icmphdr = (struct icmphdr *)(psock->current.packet.buffer + sizeof(struct iphdr));
+
+        psock->current.packet.icmphdr->checksum = ntohs(psock->current.packet.icmphdr->checksum);
+        psock->current.packet.icmphdr->un.echo.id = ntohs(psock->current.packet.icmphdr->un.echo.id);
+        psock->current.packet.icmphdr->un.echo.sequence = ntohs(psock->current.packet.icmphdr->un.echo.sequence);
+    } else {
+        return -1;
+    }
+
+    return 0;
+}
+
+int ppkt_process_ppkt(struct psock * psock)
+{
+    if (psock->current.peer.ss_family == AF_INET) {
+        if (psock->current.packet.used < sizeof(struct iphdr) + sizeof(*psock->current.packet.icmphdr)) {
+            return -1;
+        }
+
+        psock->current.packet.ppkt = (struct ppkt *)(psock->current.packet.buffer + sizeof(struct iphdr) +
+                                                     sizeof(*psock->current.packet.icmphdr));
+    } else {
+        return -1;
+    }
+
+    psock->current.packet.ppkt->ident = ntohl(psock->current.packet.ppkt->ident);
+    if (psock->current.packet.ppkt->ident != PTUNNEL_IDENT) {
+        return -1;
+    }
+
+    psock->current.packet.ppkt->total_size = ntohs(psock->current.packet.ppkt->total_size);
+    if (psock->current.packet.ppkt->total_size > psock->current.packet.used) {
+        return -1;
+    }
+
+    return 0;
 }
diff --git a/src/ppkt.h b/src/ppkt.h
index 34c3075..a4068ec 100644
--- a/src/ppkt.h
+++ b/src/ppkt.h
@@ -3,22 +3,33 @@
 
 #include <stdint.h>
 
-
 #define PTUNNEL_IDENT 0xdeadc0de
 
 struct psock;
 
-struct ppkt
-{
-    uint32_t ident;
+struct ppkt_option_header {
     uint16_t type;
-    uint16_t data_size;
+    uint16_t size;
+} __attribute__((__packed__));
+
+struct ppkt_option_auth {
+    struct ppkt_option_header option;
     uint8_t data[0];
 } __attribute__((__packed__));
 
+union ppkt_option {
+    struct ppkt_option_auth auth;
+} __attribute__((__packed__));
+;
+
+struct ppkt {
+    uint32_t ident;
+    uint16_t total_size;
+    union ppkt_option current;
+} __attribute__((__packed__));
 
-void ppkt_process_icmp(struct psock *);
+int ppkt_process_icmp(struct psock *);
 
-void ppkt_process_body(struct psock *);
+int ppkt_process_ppkt(struct psock *);
 
 #endif
diff --git a/src/psock.c b/src/psock.c
index 78ecbff..f5575ab 100644
--- a/src/psock.c
+++ b/src/psock.c
@@ -2,7 +2,6 @@
 #include "psock.h"
 
 #include <errno.h>
-#include <netinet/in.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -10,7 +9,6 @@
 #include <sys/socket.h>
 #include <unistd.h>
 
-
 int psock_init(struct psock * psock, size_t max_descriptors, size_t packet_buffer_size)
 {
     struct epoll_event ev;
@@ -18,44 +16,38 @@ int psock_init(struct psock * psock, size_t max_descriptors, size_t packet_buffe
     memset(psock, 0, sizeof(*psock));
 
     psock->icmp_fd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
-    if (psock->icmp_fd < 0)
-    {
+    if (psock->icmp_fd < 0) {
         goto error;
     }
 
     psock->epoll_fd = epoll_create1(EPOLL_CLOEXEC);
-    if (psock->epoll_fd < 0)
-    {
+    if (psock->epoll_fd < 0) {
         goto error;
     }
 
     ev.events = EPOLLIN;
     ev.data.fd = psock->icmp_fd;
-    if (epoll_ctl(psock->epoll_fd, EPOLL_CTL_ADD, psock->icmp_fd, &ev) != 0)
-    {
+    if (epoll_ctl(psock->epoll_fd, EPOLL_CTL_ADD, psock->icmp_fd, &ev) != 0) {
         goto error;
     }
 
-    psock->packet.max = packet_buffer_size;
-    psock->packet.used = 0;
-    psock->packet.buffer = (uint8_t *)calloc(packet_buffer_size, sizeof(*psock->packet.buffer));
-    if (psock->packet.buffer == NULL)
-    {
+    psock->current.packet.max = packet_buffer_size;
+    psock->current.packet.used = 0;
+    psock->current.packet.buffer = (uint8_t *)calloc(packet_buffer_size, sizeof(*psock->current.packet.buffer));
+    if (psock->current.packet.buffer == NULL) {
         goto error;
     }
 
     psock->remotes.max = max_descriptors;
     psock->remotes.used = 0;
     psock->remotes.descriptors = (struct pdesc *)calloc(max_descriptors, sizeof(*psock->remotes.descriptors));
-    if (psock->remotes.descriptors == NULL)
-    {
+    if (psock->remotes.descriptors == NULL) {
         goto error;
     }
 
     return 0;
 error:
-    if (errno != 0)
-    {
+    if (errno != 0) {
         perror("[FATAL] psock_init failed");
     }
     psock_free(psock);
@@ -78,24 +70,22 @@ void psock_free(struct psock * psock)
 
 static void psock_process_cmsg(struct msghdr * hdr)
 {
-    for (struct cmsghdr * cmsg = CMSG_FIRSTHDR(hdr); cmsg != NULL; cmsg = CMSG_NXTHDR(hdr, cmsg))
-    {
+    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);
     }
 }
 
 static int psock_recvmsg(struct psock * psock)
 {
-    struct sockaddr_storage peer;
     struct iovec iov;
     struct msghdr hdr = {};
     ssize_t nread;
 
-    iov.iov_base = (void *)psock->packet.buffer;
-    iov.iov_len = psock->packet.max;
+    iov.iov_base = (void *)psock->current.packet.buffer;
+    iov.iov_len = psock->current.packet.max;
 
-    hdr.msg_name = &peer;
-    hdr.msg_namelen = sizeof(peer);
+    hdr.msg_name = &psock->current.peer;
+    hdr.msg_namelen = sizeof(psock->current.peer);
     hdr.msg_iov = &iov;
     hdr.msg_iovlen = 1;
 
@@ -103,37 +93,43 @@ static int psock_recvmsg(struct psock * psock)
         nread = recvmsg(psock->icmp_fd, &hdr, 0);
     } while (nread == -1 && errno == EINTR);
 
-    if (nread >= 0)
-    {
-        psock->packet.used = nread;
+    if (nread >= 0) {
+        psock->current.packet.used = nread;
         psock_process_cmsg(&hdr);
         return 0;
     } else {
-        psock->packet.used = 0;
+        psock->current.packet.used = 0;
         return -1;
     }
 }
 
-static int psock_sendmsg(struct psock * psock)
+static int psock_sendmsg(struct psock * psock, uint8_t * buf, size_t siz)
 {
-    return -1;
+    struct iovec iov;
+    struct msghdr hdr = {};
+    ssize_t n = siz, nwritten;
+
+    iov.iov_base = buf;
+    iov.iov_len = siz;
+
+    hdr.msg_name = &psock->current.peer;
+    hdr.msg_namelen = sizeof(psock->current.peer);
+    hdr.msg_iov = &iov;
+    hdr.msg_iovlen = 1;
+
+    nwritten = sendmsg(psock->icmp_fd, &hdr, 0);
+
+    return (nwritten == n ? 0 : nwritten);
 }
 
 static void psock_handle_events(struct psock * psock)
 {
-    if (psock_recvmsg(psock) == 0)
-    {
-        switch (pdesc_find_remote(psock))
-        {
-            case PDESC_REMOTE_NEW:
-                fprintf(stderr, "Remote NEW.\n");
-                break;
-            case PDESC_REMOTE_INVALID:
-                fprintf(stderr, "Remote INVALID\n");
-                break;
-            case PDESC_REMOTE_FOUND:
-                fprintf(stderr, "Remote FOUND\n");
-                break;
+    if (psock_recvmsg(psock) == 0) {
+        struct pdesc * remote = pdesc_find_remote(psock);
+        if (remote != NULL) {
+            printf("Remote descriptor ID: %u\n", remote->identifier);
+        } else {
+            fprintf(stderr, "Could not find/alloc a remote descriptor.\n");
         }
     }
 }
@@ -143,12 +139,10 @@ void psock_loop(struct psock * psock)
     const int max_events = 32;
     struct epoll_event events[max_events];
 
-    while (1)
-    {
+    while (1) {
         int nready = epoll_wait(psock->epoll_fd, events, max_events, -1);
 
-        switch (nready)
-        {
+        switch (nready) {
             case -1:
                 break;
             case 0:
diff --git a/src/psock.h b/src/psock.h
index 5faf24b..da2be7b 100644
--- a/src/psock.h
+++ b/src/psock.h
@@ -1,22 +1,29 @@
 #ifndef PSOCK_H
 #define PSOCK_H 1
 
+#include <netinet/in.h>
 #include <stdint.h>
 #include <stdlib.h>
 
-
+struct icmphdr;
 struct pdesc;
+struct ppkt;
 
-struct psock
-{
+struct psock {
     int epoll_fd;
     int icmp_fd;
 
     struct {
-        size_t used;
-        size_t max;
-        uint8_t * buffer;
-    } packet;
+        struct sockaddr_storage peer;
+
+        struct {
+            size_t used;
+            size_t max;
+            uint8_t * buffer;
+            struct icmphdr * icmphdr;
+            struct ppkt * ppkt;
+        } packet;
+    } current;
 
     struct {
         size_t used;
@@ -25,7 +32,6 @@ struct psock
     } remotes;
 };
 
-
 int psock_init(struct psock *, size_t, size_t);
 
 void psock_free(struct psock *);
diff --git a/src/ptunnel.c b/src/ptunnel.c
index df10db9..acf87d2 100644
--- a/src/ptunnel.c
+++ b/src/ptunnel.c
@@ -4,13 +4,11 @@
 #include <stdio.h>
 #include <stdlib.h>
 
-
 int main(void)
 {
     struct psock psock = {};
 
-    if (psock_init(&psock, 16, 2048) != 0)
-    {
+    if (psock_init(&psock, 16, 1500) != 0) {
         return 1;
     }