7 files changed, 250 insertions, 106 deletions

diff --git a/src/pdesc.c b/src/pdesc.c
index 77f511e..84c6f70 100644
--- a/src/pdesc.c
+++ b/src/pdesc.c
@@ -4,30 +4,44 @@
 
 #include <netinet/ip_icmp.h>
 
-static void pdesc_init(struct psock * psock, struct pdesc * pdesc, uint16_t identifier)
+static void pdesc_init(struct psock * sock, struct pdesc * desc, uint16_t identifier)
 {
-    pdesc->state = PDESC_STATE_AUTH;
-    pdesc->peer = psock->current.peer;
-    pdesc->identifier = identifier;
-    pdesc->sequence = 0;
+    desc->state = PDESC_STATE_AUTH;
+    desc->peer = sock->current.peer;
+    desc->identifier = identifier;
+    desc->sequence = 0;
 }
 
-struct pdesc * pdesc_find_remote(struct psock * psock)
+enum pdesc_remote_errno pdesc_find_remote(struct psock * sock, struct pdesc ** const desc)
 {
     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;
+    *desc = NULL;
+
+    if (ppkt_process_icmp(sock) != 0 || ppkt_process_ppkt(sock) != 0) {
+        return REMOTE_PACKET_INVALID;
+    }
+
+    if (sock->current.packet.icmphdr->type == ICMP_ECHO && sock->local.is_client != 0) {
+        return REMOTE_ICMP_ECHO_CLIENT;
     }
 
-    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];
+    if (sock->current.packet.icmphdr->type == ICMP_ECHOREPLY && sock->local.is_client == 0) {
+        return REMOTE_ICMP_REPLY_SERVER;
+    }
+
+    for (i = 0; i < sock->remotes.used; ++i) {
+        if (sock->current.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) {
+            *desc = &sock->remotes.descriptors[i];
+            return REMOTE_FOUND;
         }
     }
+    if (i == sock->remotes.max) {
+        return REMOTE_MAX_DESCRIPTORS;
+    }
 
-    pdesc_init(psock, &psock->remotes.descriptors[i], psock->current.packet.icmphdr->un.echo.id);
+    pdesc_init(sock, &sock->remotes.descriptors[i], sock->current.packet.icmphdr->un.echo.id);
 
-    return &psock->remotes.descriptors[i];
+    *desc = &sock->remotes.descriptors[i];
+    return REMOTE_FOUND;
 }
diff --git a/src/pdesc.h b/src/pdesc.h
index 54fad4a..b0355b4 100644
--- a/src/pdesc.h
+++ b/src/pdesc.h
@@ -6,7 +6,15 @@
 
 struct psock;
 
-enum pdesc_state { PDESC_STATE_AUTH };
+enum pdesc_remote_errno {
+    REMOTE_FOUND,
+    REMOTE_PACKET_INVALID,
+    REMOTE_ICMP_ECHO_CLIENT,
+    REMOTE_ICMP_REPLY_SERVER,
+    REMOTE_MAX_DESCRIPTORS,
+};
+
+enum pdesc_state { PDESC_STATE_AUTH, PDESC_STATE_DATA };
 
 struct pdesc {
     enum pdesc_state state;
@@ -15,6 +23,6 @@ struct pdesc {
     uint16_t sequence;
 };
 
-struct pdesc * pdesc_find_remote(struct psock *);
+enum pdesc_remote_errno pdesc_find_remote(struct psock *, struct pdesc ** const);
 
 #endif
diff --git a/src/ppkt.c b/src/ppkt.c
index 9496187..5121b01 100644
--- a/src/ppkt.c
+++ b/src/ppkt.c
@@ -1,17 +1,32 @@
+#include "pdesc.h"
 #include "ppkt.h"
 #include "psock.h"
 
 #include <netinet/ip.h>
 #include <netinet/ip_icmp.h>
 
-int ppkt_process_icmp(struct psock * psock)
+enum ptype ppkt_type_to_enum(struct ppkt * pkt)
 {
-    if (psock->current.peer.ss_family == AF_INET) {
-        psock->current.packet.icmphdr = (struct icmphdr *)(psock->current.packet.buffer + sizeof(struct iphdr));
+    enum ptype pt = (enum ptype)pkt->type;
 
-        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);
+    switch (pt) {
+        case PTYPE_INVALID:
+        case PTYPE_AUTH_REQUEST:
+        case PTYPE_AUTH_RESPONSE:
+            return pt;
+    }
+
+    return PTYPE_INVALID;
+}
+
+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);
     } else {
         return -1;
     }
@@ -19,28 +34,84 @@ int ppkt_process_icmp(struct psock * psock)
     return 0;
 }
 
-int ppkt_process_ppkt(struct psock * psock)
+int ppkt_process_ppkt(struct psock * sock)
 {
-    if (psock->current.peer.ss_family == AF_INET) {
-        if (psock->current.packet.used < sizeof(struct iphdr) + sizeof(*psock->current.packet.icmphdr)) {
+    if (sock->current.peer.ss_family == AF_INET) {
+        if (sock->current.packet.used < sizeof(struct iphdr) + sizeof(*sock->current.packet.icmphdr)) {
             return -1;
         }
 
-        psock->current.packet.ppkt = (struct ppkt *)(psock->current.packet.buffer + sizeof(struct iphdr) +
-                                                     sizeof(*psock->current.packet.icmphdr));
+        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);
+
+        sock->current.packet.pkt =
+            (struct ppkt *)(sock->current.packet.buffer + sizeof(struct iphdr) + sizeof(*sock->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) {
+    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;
     }
 
-    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;
+    switch (ppkt_type_to_enum(sock->current.packet.pkt)) {
+        case PTYPE_INVALID:
+            return -1;
+        case PTYPE_AUTH_REQUEST:
+            break;
+        case PTYPE_AUTH_RESPONSE:
+            break;
     }
 
     return 0;
 }
+
+static size_t ppkt_prepare_ppkt(struct ppkt * pkt, enum ptype type, size_t additional_size)
+{
+    size_t subpkt_size;
+
+    switch (type) {
+        case PTYPE_INVALID:
+            subpkt_size = 0;
+            additional_size = 0;
+            break;
+        case PTYPE_AUTH_REQUEST:
+            subpkt_size = sizeof(struct ppkt_auth_request);
+            break;
+        case PTYPE_AUTH_RESPONSE:
+            subpkt_size = sizeof(struct ppkt_auth_response);
+            break;
+    }
+
+    pkt->total_size = htons(sizeof(*pkt) + subpkt_size + additional_size);
+    pkt->type = type;
+
+    return sizeof(*pkt) + subpkt_size + additional_size;
+}
+
+void ppkt_prepare_auth_request(struct ppkt_buffer * const pkt_buf, uint8_t * const hash, size_t hash_siz)
+{
+    size_t total_size = ppkt_prepare_ppkt(&pkt_buf->pkt, PTYPE_AUTH_REQUEST, hash_siz);
+
+    pkt_buf->auth_request.magic = htonl(PTUNNEL_MAGIC);
+    pkt_buf->auth_request.hash_siz = hash_siz;
+
+    pkt_buf->icmphdr.type = 8;
+    pkt_buf->icmphdr.code = 0;
+
+    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[2].iov_base = &pkt_buf->auth_request;
+    pkt_buf->iovec[2].iov_len = sizeof(pkt_buf->auth_request);
+
+    pkt_buf->iovec[3].iov_base = hash;
+    pkt_buf->iovec[3].iov_len = hash_siz;
+
+    pkt_buf->iovec_used = 4;
+}
diff --git a/src/ppkt.h b/src/ppkt.h
index a4068ec..fcf35d3 100644
--- a/src/ppkt.h
+++ b/src/ppkt.h
@@ -1,35 +1,59 @@
 #ifndef PPKT_H
 #define PPKT_H 1
 
+#include <netinet/ip_icmp.h>
 #include <stdint.h>
+#include <sys/socket.h>
 
-#define PTUNNEL_IDENT 0xdeadc0de
+#define PTUNNEL_MAGIC 0xdeadc0de
 
-struct psock;
+#define U8_PTYPE_AUTH_REQUEST 0x01u
+#define U8_PTYPE_AUTH_RESPONSE 0x02u
 
-struct ppkt_option_header {
-    uint16_t type;
-    uint16_t size;
-} __attribute__((__packed__));
+enum ptype {
+    PTYPE_INVALID = 0,
+    PTYPE_AUTH_REQUEST = U8_PTYPE_AUTH_REQUEST,
+    PTYPE_AUTH_RESPONSE = U8_PTYPE_AUTH_RESPONSE,
+};
 
-struct ppkt_option_auth {
-    struct ppkt_option_header option;
-    uint8_t data[0];
+struct psock;
+struct pdesc;
+struct iovec;
+
+struct ppkt_auth_request {
+    uint32_t magic;
+    uint16_t hash_siz;
+    uint8_t hash[0];
 } __attribute__((__packed__));
 
-union ppkt_option {
-    struct ppkt_option_auth auth;
+struct ppkt_auth_response {
+    uint8_t code;
 } __attribute__((__packed__));
-;
 
 struct ppkt {
-    uint32_t ident;
     uint16_t total_size;
-    union ppkt_option current;
+    uint8_t type;
+    uint8_t data[0];
 } __attribute__((__packed__));
 
+struct ppkt_buffer {
+    struct iovec iovec[4];
+    size_t iovec_used;
+
+    struct icmphdr icmphdr;
+    struct ppkt pkt;
+    union {
+        struct ppkt_auth_request auth_request;
+        struct ppkt_auth_response auth_response;
+    };
+};
+
+enum ptype ppkt_type_to_enum(struct ppkt *);
+
 int ppkt_process_icmp(struct psock *);
 
 int ppkt_process_ppkt(struct psock *);
 
+void ppkt_prepare_auth_request(struct ppkt_buffer *, uint8_t *, size_t);
+
 #endif
diff --git a/src/psock.c b/src/psock.c
index f5575ab..c88ec56 100644
--- a/src/psock.c
+++ b/src/psock.c
@@ -1,5 +1,6 @@
 #include "pdesc.h"
 #include "psock.h"
+#include "ppkt.h"
 
 #include <errno.h>
 #include <stdio.h>
@@ -9,39 +10,41 @@
 #include <sys/socket.h>
 #include <unistd.h>
 
-int psock_init(struct psock * psock, size_t max_descriptors, size_t packet_buffer_size)
+int psock_init(struct psock * sock, int is_client, size_t max_descriptors, size_t packet_buffer_size)
 {
     struct epoll_event ev;
 
-    memset(psock, 0, sizeof(*psock));
+    memset(sock, 0, sizeof(*sock));
 
-    psock->icmp_fd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
-    if (psock->icmp_fd < 0) {
+    sock->icmp_fd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
+    if (sock->icmp_fd < 0) {
         goto error;
     }
 
-    psock->epoll_fd = epoll_create1(EPOLL_CLOEXEC);
-    if (psock->epoll_fd < 0) {
+    sock->epoll_fd = epoll_create1(EPOLL_CLOEXEC);
+    if (sock->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) {
+    ev.data.fd = sock->icmp_fd;
+    if (epoll_ctl(sock->epoll_fd, EPOLL_CTL_ADD, sock->icmp_fd, &ev) != 0) {
         goto error;
     }
 
-    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) {
+    sock->local.is_client = is_client;
+
+    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;
     }
 
-    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) {
+    sock->remotes.max = max_descriptors;
+    sock->remotes.used = 0;
+    sock->remotes.descriptors = (struct pdesc *)calloc(max_descriptors, sizeof(*sock->remotes.descriptors));
+    if (sock->remotes.descriptors == NULL) {
         goto error;
     }
 
@@ -50,22 +53,26 @@ error:
     if (errno != 0) {
         perror("[FATAL] psock_init failed");
     }
-    psock_free(psock);
+    psock_free(sock);
     return -1;
 }
 
-void psock_free(struct psock * psock)
+int psock_add_server(char const * const address)
+{
+}
+
+void psock_free(struct psock * sock)
 {
-    free(psock->remotes.descriptors);
-    psock->remotes.descriptors = NULL;
-    psock->remotes.used = 0;
-    psock->remotes.max = 0;
+    free(sock->remotes.descriptors);
+    sock->remotes.descriptors = NULL;
+    sock->remotes.used = 0;
+    sock->remotes.max = 0;
 
-    close(psock->icmp_fd);
-    psock->icmp_fd = -1;
+    close(sock->icmp_fd);
+    sock->icmp_fd = -1;
 
-    close(psock->epoll_fd);
-    psock->epoll_fd = -1;
+    close(sock->epoll_fd);
+    sock->epoll_fd = -1;
 }
 
 static void psock_process_cmsg(struct msghdr * hdr)
@@ -75,80 +82,95 @@ static void psock_process_cmsg(struct msghdr * hdr)
     }
 }
 
-static int psock_recvmsg(struct psock * psock)
+static int psock_recvmsg(struct psock * sock)
 {
     struct iovec iov;
     struct msghdr hdr = {};
     ssize_t nread;
 
-    iov.iov_base = (void *)psock->current.packet.buffer;
-    iov.iov_len = psock->current.packet.max;
+    iov.iov_base = (void *)sock->current.packet.buffer;
+    iov.iov_len = sock->current.packet.max;
 
-    hdr.msg_name = &psock->current.peer;
-    hdr.msg_namelen = sizeof(psock->current.peer);
+    hdr.msg_name = &sock->current.peer;
+    hdr.msg_namelen = sizeof(sock->current.peer);
     hdr.msg_iov = &iov;
     hdr.msg_iovlen = 1;
 
     do {
-        nread = recvmsg(psock->icmp_fd, &hdr, 0);
+        nread = recvmsg(sock->icmp_fd, &hdr, 0);
     } while (nread == -1 && errno == EINTR);
 
     if (nread >= 0) {
-        psock->current.packet.used = nread;
+        sock->current.packet.used = nread;
         psock_process_cmsg(&hdr);
         return 0;
     } else {
-        psock->current.packet.used = 0;
+        sock->current.packet.used = 0;
         return -1;
     }
 }
 
-static int psock_sendmsg(struct psock * psock, uint8_t * buf, size_t siz)
+static int psock_sendmsg(struct psock * sock, struct iovec * const iov, size_t iovlen)
 {
-    struct iovec iov;
     struct msghdr hdr = {};
-    ssize_t n = siz, nwritten;
-
-    iov.iov_base = buf;
-    iov.iov_len = siz;
+    ssize_t nwritten;
 
-    hdr.msg_name = &psock->current.peer;
-    hdr.msg_namelen = sizeof(psock->current.peer);
-    hdr.msg_iov = &iov;
-    hdr.msg_iovlen = 1;
+    hdr.msg_name = &sock->current.peer;
+    hdr.msg_namelen = sizeof(sock->current.peer);
+    hdr.msg_iov = iov;
+    hdr.msg_iovlen = iovlen;
 
-    nwritten = sendmsg(psock->icmp_fd, &hdr, 0);
+    nwritten = sendmsg(sock->icmp_fd, &hdr, 0);
 
-    return (nwritten == n ? 0 : nwritten);
+    return nwritten;
 }
 
-static void psock_handle_events(struct psock * psock)
+static void psock_handle_events(struct psock * sock)
 {
-    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");
+    if (psock_recvmsg(sock) == 0) {
+        struct pdesc * remote;
+
+        switch (pdesc_find_remote(sock, &remote)) {
+            case REMOTE_FOUND:
+                printf("Remote descriptor ID: %u\n", remote->identifier);
+                break;
+            case REMOTE_PACKET_INVALID:
+                fprintf(stderr, "Invalid packet received.\n");
+                break;
+            case REMOTE_ICMP_ECHO_CLIENT:
+                fprintf(stderr, "Received ICMP echo, but I am a client.\n");
+                break;
+            case REMOTE_ICMP_REPLY_SERVER:
+                fprintf(stderr, "Received ICMP reply, but I am a server.\n");
+                break;
+            case REMOTE_MAX_DESCRIPTORS:
+                fprintf(stderr, "Max descriptors reached, sorry.\n");
+                break;
         }
     }
 }
 
-void psock_loop(struct psock * psock)
+void psock_loop(struct psock * sock)
 {
     const int max_events = 32;
     struct epoll_event events[max_events];
 
     while (1) {
-        int nready = epoll_wait(psock->epoll_fd, events, max_events, -1);
+        int nready = epoll_wait(sock->epoll_fd, events, max_events, 1000);
 
         switch (nready) {
             case -1:
                 break;
             case 0:
+                if (sock->local.is_client != 0) {
+                    uint8_t b[3] = {0x41, 0x42, 0x43};
+                    struct ppkt_buffer pb;
+                    ppkt_prepare_auth_request(&pb, b, 3);
+                    psock_sendmsg(sock, pb.iovec, pb.iovec_used);
+                }
                 continue;
             default:
-                psock_handle_events(psock);
+                psock_handle_events(sock);
                 break;
         }
     }
diff --git a/src/psock.h b/src/psock.h
index da2be7b..ab51bc5 100644
--- a/src/psock.h
+++ b/src/psock.h
@@ -14,6 +14,10 @@ struct psock {
     int icmp_fd;
 
     struct {
+        int is_client;
+    } local;
+
+    struct {
         struct sockaddr_storage peer;
 
         struct {
@@ -21,7 +25,7 @@ struct psock {
             size_t max;
             uint8_t * buffer;
             struct icmphdr * icmphdr;
-            struct ppkt * ppkt;
+            struct ppkt * pkt;
         } packet;
     } current;
 
@@ -32,7 +36,9 @@ struct psock {
     } remotes;
 };
 
-int psock_init(struct psock *, size_t, size_t);
+int psock_init(struct psock *, int, size_t, size_t);
+
+int psock_add_server(char const *);
 
 void psock_free(struct psock *);
 
diff --git a/src/ptunnel.c b/src/ptunnel.c
index 3f0b116..dce1a7f 100644
--- a/src/ptunnel.c
+++ b/src/ptunnel.c
@@ -11,7 +11,6 @@ static struct {
     int is_client;
     struct {
         char * str;
-        struct sockaddr_storage sockaddr;
     } address;
 } ptunnel_options = {.is_client = 0};
 
@@ -61,7 +60,7 @@ int main(int argc, char ** argv)
         return 1;
     }
 
-    if (psock_init(&psock, 16, 1500) != 0) {
+    if (psock_init(&psock, ptunnel_options.is_client, 16, 1500) != 0) {
         return 1;
     }