...

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

7 files changed, 173 insertions, 33 deletions

diff --git a/src/pdesc.c b/src/pdesc.c
index 4ffd7f5..da07533 100644
--- a/src/pdesc.c
+++ b/src/pdesc.c
@@ -4,10 +4,10 @@
 
 #include <netinet/ip_icmp.h>
 
-static void pdesc_init(struct psock * sock, struct pdesc * desc, uint16_t identifier)
+void pdesc_init(struct pdesc * desc, struct sockaddr_storage * sockaddr, uint16_t identifier)
 {
     desc->state = PDESC_STATE_AUTH;
-    desc->peer = sock->current.peer;
+    desc->peer = *sockaddr;
     desc->identifier = identifier;
     desc->sequence = 0;
 }
@@ -31,7 +31,8 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd
     }
 
     for (i = 0; i < sock->remotes.used; ++i) {
-        if (sock->current.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) {
+        if (sock->remotes.descriptors[i].state != PDESC_STATE_INVALID &&
+            sock->current.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) {
             *desc = &sock->remotes.descriptors[i];
             return REMOTE_EXISTS;
         }
@@ -40,8 +41,9 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd
         return REMOTE_MAX_DESCRIPTORS;
     }
 
-    pdesc_init(sock, &sock->remotes.descriptors[i], sock->current.packet.icmphdr->un.echo.id);
+    pdesc_init(&sock->remotes.descriptors[i], &sock->current.peer, sock->current.packet.icmphdr->un.echo.id);
 
     *desc = &sock->remotes.descriptors[i];
+    sock->remotes.used++;
     return REMOTE_NOT_FOUND;
 }
diff --git a/src/pdesc.h b/src/pdesc.h
index dcba303..d81798e 100644
--- a/src/pdesc.h
+++ b/src/pdesc.h
@@ -15,7 +15,7 @@ enum pdesc_remote_errno {
     REMOTE_MAX_DESCRIPTORS,
 };
 
-enum pdesc_state { PDESC_STATE_AUTH, PDESC_STATE_DATA };
+enum pdesc_state { PDESC_STATE_INVALID = 0, PDESC_STATE_AUTH, PDESC_STATE_DATA };
 
 struct pdesc {
     enum pdesc_state state;
@@ -25,6 +25,8 @@ struct pdesc {
     uint16_t sequence;
 };
 
+void pdesc_init(struct pdesc *, struct sockaddr_storage *, uint16_t identifier);
+
 enum pdesc_remote_errno pdesc_find_current_remote(struct psock *, struct pdesc ** const);
 
 #endif
diff --git a/src/ppkt.c b/src/ppkt.c
index 5121b01..8557592 100644
--- a/src/ppkt.c
+++ b/src/ppkt.c
@@ -1,6 +1,7 @@
 #include "pdesc.h"
 #include "ppkt.h"
 #include "psock.h"
+#include "putils.h"
 
 #include <netinet/ip.h>
 #include <netinet/ip_icmp.h>
@@ -68,7 +69,7 @@ int ppkt_process_ppkt(struct psock * sock)
     return 0;
 }
 
-static size_t ppkt_prepare_ppkt(struct ppkt * pkt, enum ptype type, size_t additional_size)
+static size_t ppkt_sizeof_pkt(struct ppkt * pkt, enum ptype type, size_t additional_size)
 {
     size_t subpkt_size;
 
@@ -91,16 +92,41 @@ static size_t ppkt_prepare_ppkt(struct ppkt * pkt, enum ptype type, size_t addit
     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)
+static size_t ppkt_init_pkt(struct pdesc const * desc,
+                            struct ppkt_buffer * pkt_buf,
+                            enum ptype type,
+                            size_t additional_size)
 {
-    size_t total_size = ppkt_prepare_ppkt(&pkt_buf->pkt, PTYPE_AUTH_REQUEST, hash_siz);
+    pkt_buf->icmphdr.un.echo.id = desc->identifier;
+    switch (type) {
+        case PTYPE_INVALID:
+            pkt_buf->icmphdr.type = 3; // Destination Unreachable
+            break;
+        case PTYPE_AUTH_REQUEST:
+            pkt_buf->icmphdr.type = 8; // Echo Request
+            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);
+}
+
+static void ppkt_finalize_pkt(struct ppkt_buffer * const pkt_buf)
+{
+    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)
+{
+    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;
 
-    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);
 
@@ -114,4 +140,6 @@ void ppkt_prepare_auth_request(struct ppkt_buffer * const pkt_buf, uint8_t * con
     pkt_buf->iovec[3].iov_len = hash_siz;
 
     pkt_buf->iovec_used = 4;
+
+    ppkt_finalize_pkt(pkt_buf);
 }
diff --git a/src/ppkt.h b/src/ppkt.h
index fcf35d3..5772fe8 100644
--- a/src/ppkt.h
+++ b/src/ppkt.h
@@ -18,7 +18,6 @@ enum ptype {
 
 struct psock;
 struct pdesc;
-struct iovec;
 
 struct ppkt_auth_request {
     uint32_t magic;
@@ -54,6 +53,6 @@ int ppkt_process_icmp(struct psock *);
 
 int ppkt_process_ppkt(struct psock *);
 
-void ppkt_prepare_auth_request(struct ppkt_buffer *, uint8_t *, size_t);
+void ppkt_prepare_auth_request(struct pdesc *, struct ppkt_buffer *, uint8_t *, size_t);
 
 #endif
diff --git a/src/psock.c b/src/psock.c
index 548be93..35747d1 100644
--- a/src/psock.c
+++ b/src/psock.c
@@ -70,7 +70,7 @@ error:
     return -1;
 }
 
-static int psock_name_to_address(char const * address, struct sockaddr_storage * const sockaddr, size_t * sockaddr_size)
+static int psock_name_to_address(char const * address, struct sockaddr_storage * sockaddr, size_t * sockaddr_size)
 {
     struct addrinfo hints = {};
     struct addrinfo *result, *rp;
@@ -120,9 +120,15 @@ int psock_add_server(struct psock * sock, char const * address)
         return -1;
     }
 
-    for (size_t addr_i = 0; sock->remotes.used < sock->remotes.max && addr_i < max_sockaddrs;
-         ++sock->remotes.used, ++addr_i) {
-        size_t desc_i = sock->remotes.used;
+    size_t desc_i = 0;
+    for (size_t addr_i = 0; desc_i < sock->remotes.max && addr_i < max_sockaddrs;) {
+        if (sock->remotes.descriptors[desc_i].state != PDESC_STATE_INVALID) {
+            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: {
@@ -146,7 +152,10 @@ int psock_add_server(struct psock * sock, char const * address)
                 break;
             }
         }
+
         logger_early(0, "Added remote: %s", sock->remotes.descriptors[desc_i].peer_str);
+        addr_i++;
+        sock->remotes.used++;
     }
 
     return 0;
@@ -154,6 +163,9 @@ 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;
@@ -216,35 +228,37 @@ static int psock_sendmsg(struct psock * sock, struct iovec * const iov, size_t i
     return nwritten;
 }
 
-static void psock_handle_events(struct psock * sock)
+static struct pdesc * psock_handle_events(struct psock * sock)
 {
-    if (psock_recvmsg(sock) == 0) {
-        struct pdesc * remote;
+    struct pdesc * remote = NULL;
 
+    if (psock_recvmsg(sock) == 0) {
         switch (pdesc_find_current_remote(sock, &remote)) {
             case REMOTE_EXISTS:
-                printf("Existing Remote with descriptor ID: %u\n", remote->identifier);
+                logger(0, "Existing Remote with descriptor ID: %u", remote->identifier);
                 break;
             case REMOTE_NOT_FOUND:
-                printf("New Remote with descriptor ID: %u\n", remote->identifier);
+                logger(1, "New Remote with descriptor ID: %u", remote->identifier);
                 break;
             case REMOTE_PACKET_INVALID:
-                fprintf(stderr, "Invalid packet received.\n");
+                logger(1, "Invalid packet received.");
                 break;
             case REMOTE_ICMP_ECHO_CLIENT:
-                fprintf(stderr, "Received ICMP echo, but I am a client.\n");
+                logger(0, "Received ICMP echo, but I am a client.");
                 break;
             case REMOTE_ICMP_REPLY_SERVER:
-                fprintf(stderr, "Received ICMP reply, but I am a server.\n");
+                logger(0, "Received ICMP reply, but I am a server.");
                 break;
             case REMOTE_MAX_DESCRIPTORS:
-                fprintf(stderr, "Max descriptors reached, sorry.\n");
+                logger(1, "Max descriptors reached, sorry.");
                 break;
         }
     }
+
+    return remote;
 }
 
-void psock_loop(struct psock * sock)
+static void psock_loop_server(struct psock * sock)
 {
     const int max_events = 32;
     struct epoll_event events[max_events];
@@ -256,12 +270,6 @@ void psock_loop(struct psock * sock)
             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(sock);
@@ -269,3 +277,56 @@ void psock_loop(struct psock * sock)
         }
     }
 }
+
+static void psock_loop_client_event_timeout(struct psock * sock)
+{
+    for (size_t i = 0; i < sock->remotes.max; ++i) {
+        struct pdesc * const desc = &sock->remotes.descriptors[i];
+
+        switch (desc->state) {
+            case PDESC_STATE_INVALID:
+                break;
+            case PDESC_STATE_AUTH: {
+                logger(0, "Sending authentication request.");
+
+                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);
+                break;
+            }
+            case PDESC_STATE_DATA:
+                break;
+        }
+    }
+}
+
+static void psock_loop_client(struct psock * sock)
+{
+    const int max_events = 16;
+    struct epoll_event events[max_events];
+
+    while (1) {
+        int nready = epoll_wait(sock->epoll_fd, events, max_events, 1000);
+
+        switch (nready) {
+            case -1:
+                break;
+            case 0:
+                psock_loop_client_event_timeout(sock);
+                break;
+            default:
+                psock_handle_events(sock);
+                break;
+        }
+    }
+}
+
+void psock_loop(struct psock * sock)
+{
+    if (sock->local.is_client == 0) {
+        psock_loop_server(sock);
+    } else {
+        psock_loop_client(sock);
+    }
+}
diff --git a/src/putils.c b/src/putils.c
index 263b3a7..3f8563d 100644
--- a/src/putils.c
+++ b/src/putils.c
@@ -11,6 +11,7 @@
 #include <syslog.h>
 #include <sys/stat.h>
 #include <sys/types.h>
+#include <time.h>
 #include <unistd.h>
 
 static char const * app_name = NULL;
@@ -151,3 +152,45 @@ int parse_address(struct sockaddr_storage * out, char const * address)
 
     return 0;
 }
+
+static inline uint16_t get_n16bit(uint8_t const * cbuf)
+{
+    uint16_t r = ((uint16_t)cbuf[0]) | (((uint16_t)cbuf[1]) << 8);
+    return r;
+}
+
+uint16_t icmp_checksum_iovec(struct iovec const * iovec, size_t iovec_size)
+{
+    uint32_t checksum = 0;
+
+    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;
+
+        for (; len > 1; len -= 2) {
+            checksum += get_n16bit(buf);
+            buf += 2;
+        }
+
+        if (len == 1) {
+            checksum += *buf;
+        }
+    }
+
+    checksum = (checksum >> 16) + (checksum & 0xFFFF);
+    checksum += (checksum >> 16);
+
+    return ~checksum;
+}
+
+uint16_t icmp_generate_identifier(void)
+{
+    uint64_t current_time = time(NULL);
+    uint16_t identifier = 0;
+
+    for (size_t i = 0; i < sizeof(current_time) / 2; ++i) {
+        identifier += ((uint16_t *)&current_time)[i];
+    }
+
+    return identifier;
+}
diff --git a/src/putils.h b/src/putils.h
index 895318a..06d034f 100644
--- a/src/putils.h
+++ b/src/putils.h
@@ -2,6 +2,8 @@
 #define PUTILS_H 1
 
 #include <stdarg.h>
+#include <stdint.h>
+#include <sys/socket.h>
 
 struct sockaddr_storage;
 
@@ -25,4 +27,7 @@ __attribute__((format(printf, 2, 3))) void logger_early(int, char const *, ...);
 
 int parse_address(struct sockaddr_storage *, char const *);
 
+uint16_t icmp_checksum_iovec(struct iovec const *, size_t);
+
+uint16_t icmp_generate_identifier(void);
 #endif