Add ARP responder to tap_darwin, improve evloop test

Author: danielinux

src/port/posix/tap_darwin.c

@@ -52,6 +52,9 @@
 #ifndef ETH_P_IP
 #define ETH_P_IP ETHERTYPE_IP
 #endif
+#ifndef ETHERTYPE_ARP
+#define ETHERTYPE_ARP 0x0806
+#endif
 
 #define PACKED __attribute__((packed))
 #define AF_HDR_SIZE 4
@@ -65,6 +68,34 @@ struct eth_hdr {
 static int utun_fd = -1;
 static uint8_t peer_mac[6] = {0x06, 0x00, 0x00, 0x00, 0x00, 0x01};
 static pthread_mutex_t utun_lock = PTHREAD_MUTEX_INITIALIZER;
+static uint8_t pending_frame[LINK_MTU + sizeof(struct eth_hdr)];
+static size_t pending_len = 0;
+static pthread_mutex_t pending_lock = PTHREAD_MUTEX_INITIALIZER;
+static uint32_t host_ip_be;
+static uint32_t peer_ip_be;
+
+struct arp_packet {
+    uint16_t htype;
+    uint16_t ptype;
+    uint8_t hlen;
+    uint8_t plen;
+    uint16_t opcode;
+    uint8_t sha[6];
+    uint32_t spa;
+    uint8_t tha[6];
+    uint32_t tpa;
+} PACKED;
+
+static int enqueue_frame(const void *frame, size_t len)
+{
+    if (len > sizeof(pending_frame))
+        return -1;
+    pthread_mutex_lock(&pending_lock);
+    memcpy(pending_frame, frame, len);
+    pending_len = len;
+    pthread_mutex_unlock(&pending_lock);
+    return 0;
+}
 
 static int tap_poll(struct wolfIP_ll_dev *ll, void *buf, uint32_t len)
 {
@@ -77,6 +108,18 @@ static int tap_poll(struct wolfIP_ll_dev *ll, void *buf, uint32_t len)
     if (utun_fd < 0)
         return -1;
 
+    pthread_mutex_lock(&pending_lock);
+    if (pending_len > 0) {
+        size_t to_copy = pending_len;
+        if (to_copy > len)
+            to_copy = len;
+        memcpy(buf, pending_frame, to_copy);
+        pending_len = 0;
+        pthread_mutex_unlock(&pending_lock);
+        return (int)to_copy;
+    }
+    pthread_mutex_unlock(&pending_lock);
+
     pfd.fd = utun_fd;
     pfd.events = POLLIN;
 
@@ -109,6 +152,7 @@ static int tap_send(struct wolfIP_ll_dev *ll, void *buf, uint32_t len)
     uint8_t tmp[LINK_MTU + AF_HDR_SIZE];
     struct eth_hdr *eth;
     size_t ip_len;
+    struct arp_packet *arp;
     uint32_t af;
     (void)ll;
 
@@ -117,6 +161,35 @@ static int tap_send(struct wolfIP_ll_dev *ll, void *buf, uint32_t len)
     if (len < sizeof(struct eth_hdr))
         return 0;
     eth = (struct eth_hdr *)buf;
+    if (ntohs(eth->type) == ETHERTYPE_ARP) {
+        if (len < sizeof(struct eth_hdr) + sizeof(struct arp_packet))
+            return (int)len;
+        arp = (struct arp_packet *)(eth + 1);
+        if (ntohs(arp->opcode) == 1 /* request */ &&
+            arp->tpa == host_ip_be) {
+            struct {
+                struct eth_hdr eth;
+                struct arp_packet arp;
+            } PACKED reply;
+            memset(&reply, 0, sizeof(reply));
+            memcpy(reply.eth.dst, eth->src, sizeof(reply.eth.dst));
+            memcpy(reply.eth.src, peer_mac, sizeof(reply.eth.src));
+            reply.eth.type = htons(ETHERTYPE_ARP);
+            reply.arp.htype = htons(1);
+            reply.arp.ptype = htons(ETHERTYPE_IP);
+            reply.arp.hlen = 6;
+            reply.arp.plen = 4;
+            reply.arp.opcode = htons(2);
+            memcpy(reply.arp.sha, peer_mac, sizeof(reply.arp.sha));
+            reply.arp.spa = host_ip_be;
+            memcpy(reply.arp.tha, arp->sha, sizeof(reply.arp.tha));
+            reply.arp.tpa = arp->spa;
+            printf("tap_darwin: answering ARP request locally\n");
+            enqueue_frame(&reply, sizeof(reply));
+            return (int)len;
+        }
+        return (int)len;
+    }
     if (ntohs(eth->type) != ETH_P_IP)
         return (int)len;
 
@@ -155,6 +228,8 @@ static int tap_setup_ipv4(const char *ifname, uint32_t host_ip, uint32_t peer_ip
     if (!inet_ntop(AF_INET, &netmask, netmask_str, sizeof(netmask_str)))
         return -1;
 
+    printf("tap_setup_ipv4: ifname=%s local=%s peer=%s\n", ifname, local_str, peer_str);
+
     snprintf(cmd, sizeof(cmd), "/sbin/ifconfig %s inet %s %s netmask %s up",
             ifname, local_str, peer_str, netmask_str);
     if (system(cmd) != 0)
@@ -173,7 +248,10 @@ int tap_init(struct wolfIP_ll_dev *ll, const char *requested_ifname, uint32_t ho
     struct sockaddr_ctl sc;
     char ifname_buf[IFNAMSIZ];
     socklen_t optlen;
-    uint32_t peer_ip = atoip4(WOLFIP_IP);
+    uint32_t peer_ip = htonl(atoip4(WOLFIP_IP));
+    host_ip_be = host_ip;
+    peer_ip_be = peer_ip;
+    printf("tap_init: host_ip=0x%08x peer_ip=0x%08x\n", host_ip_be, peer_ip_be);
     (void)requested_ifname;
 
     utun_fd = socket(PF_SYSTEM, SOCK_DGRAM, SYSPROTO_CONTROL);

src/test/test_eventloop.c

@@ -25,8 +25,10 @@
 #include <stdlib.h>
 #include <sys/time.h>
 #include <unistd.h>
+#include <fcntl.h>
 #include <string.h>
 #include <errno.h>
+#include <sys/select.h>
 #include <sys/socket.h>
 #include <sys/types.h>
 #include "config.h"
@@ -209,6 +211,11 @@ void *pt_echoclient(void *arg)
     unsigned i;
     uint8_t local_buf[BUFFER_SIZE];
     uint32_t *srv_addr = (uint32_t *)arg;
+    int old_flags = -1;
+    fd_set wfds, rfds;
+    struct timeval tv;
+    socklen_t errlen;
+    int err;
     struct sockaddr_in remote_sock = {
         .sin_family = AF_INET,
         .sin_port = ntohs(8), /* Echo */
@@ -222,20 +229,80 @@ void *pt_echoclient(void *arg)
     sleep(1);
     setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &(int){1}, sizeof(int));
     printf("Connecting to echo server\n");
+    old_flags = fcntl(fd, F_GETFL, 0);
+    if (old_flags < 0) {
+        perror("fcntl(F_GETFL)");
+        close(fd);
+        return (void *)-1;
+    }
+    if (fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) {
+        perror("fcntl(F_SETFL)");
+        close(fd);
+        return (void *)-1;
+    }
     ret = connect(fd, (struct sockaddr *)&remote_sock, sizeof(remote_sock));
     if (ret < 0) {
-        printf("test client connect: %d\n", ret);
-        perror("connect");
-        return (void *)-1;
+        err = errno;
+        printf("test client connect returned %d, errno=%d (%s)\n", ret, err, strerror(err));
+        if (err != EINPROGRESS) {
+            perror("connect");
+            close(fd);
+            return (void *)-1;
+        }
+        printf("Waiting for connect to complete...\n");
+        while (1) {
+            tv.tv_sec = 5;
+            tv.tv_usec = 0;
+            FD_ZERO(&rfds);
+            FD_ZERO(&wfds);
+            FD_SET(fd, &rfds);
+            FD_SET(fd, &wfds);
+            ret = select(fd + 1, &rfds, &wfds, NULL, &tv);
+            if (ret <= 0) {
+                printf("select returned %d (timeout or error)\n", ret);
+                if (ret < 0) {
+                    perror("select");
+                    close(fd);
+                    return (void *)-1;
+                }
+            }
+            errlen = sizeof(err);
+            if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen) < 0) {
+                perror("getsockopt(SO_ERROR)");
+                close(fd);
+                return (void *)-1;
+            }
+            if (err == 0) {
+                printf("connect completed after select()\n");
+                break;
+            }
+            if (ret == 0) {
+                printf("connect still in progress after timeout\n");
+                continue;
+            }
+            if (err != EINPROGRESS && err != EALREADY && err != EWOULDBLOCK && err != EAGAIN) {
+                printf("connect completed with error: %d (%s)\n", err, strerror(err));
+                close(fd);
+                return (void *)-1;
+            }
+        }
+    } else {
+        printf("connect returned immediately\n");
     }
+    if (fcntl(fd, F_SETFL, old_flags) < 0)
+        perror("fcntl(restore)");
+    printf("test client: connect succeeded\n");
     for (i = 0; i < sizeof(local_buf); i += sizeof(test_pattern)) {
         memcpy(local_buf + i, test_pattern, sizeof(test_pattern));
     }
     ret = write(fd, local_buf, sizeof(local_buf));
     if (ret < 0) {
-        printf("test client write: %d\n", ret);
+        int werr = errno;
+        printf("test client write: %d (errno=%d: %s)\n", ret, werr, strerror(werr));
+        perror("write");
         return (void *)-1;
     }
+    printf("test client: wrote %d bytes\n", ret);
     while (total_r < sizeof(local_buf)) {
         ret = read(fd, local_buf + total_r, sizeof(local_buf) - total_r);
         if (ret < 0) {
@@ -250,6 +317,7 @@ void *pt_echoclient(void *arg)
                 return (void *)-1;
         }
         total_r += ret;
+        printf("test client: read %d bytes (total %u)\n", ret, total_r);
     }
     for (i = 0; i < sizeof(local_buf); i += sizeof(test_pattern)) {
         if (memcmp(local_buf + i, test_pattern, sizeof(test_pattern))) {

src/wolfip.c

@@ -2507,9 +2507,9 @@ static void arp_recv(struct wolfIP *s, unsigned int if_idx, void *buf, int len)
         arp->sip = ee32(conf->ip);
         arp_store_neighbor(s, if_idx, ee32(arp->sip), arp->sma);
         eth_output_add_header(s, if_idx, arp->tma, &arp->eth, ETH_TYPE_ARP);
-        if (ll->send)
-            ll->send(ll, buf, len);
-    }
+    if (ll->send)
+        ll->send(ll, buf, len);
+}
     if (arp->opcode == ee16(ARP_REPLY)) {
         arp_store_neighbor(s, if_idx, ee32(arp->sip), arp->sma);
     }