diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 1a2d4fd..3530cab 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -44,6 +44,10 @@ jobs: run: | ./build/test-ttl-expired + - name: Testing ICMP socket by stealing system calls in ping + run: | + sudo LD_PRELOAD=$PWD/libwolfip.so ping -c 5 10.10.10.1 + - name: Install check run: | sudo apt-get install -y check @@ -55,4 +59,3 @@ jobs: - name: Run unit tests run: | build/test/unit - diff --git a/README.md b/README.md index aabe0ff..349ecd2 100644 --- a/README.md +++ b/README.md @@ -27,8 +27,35 @@ A single network interface can be associated with the device. - Pre-allocated buffers for packet processing in static memory +## Functional tests with `LD_PRELOAD` + +The POSIX shim builds `libwolfip.so`, which can be injected in front of +host tools so that calls to `socket(2)` and friends are redirected to the +wolfIP stack and the TAP device (`wtcp0`). After running `make`: + +```sh +sudo LD_PRELOAD=$PWD/libwolfip.so nc 10.10.10.2 80 +``` + +The example above mirrors the existing `nc`-driven demos: any TCP sockets +opened by the intercepted process are serviced by wolfIP instead of the host +kernel. + +### Ping over the TAP device + +ICMP datagram sockets can be validated the same way. With the TAP interface +created automatically by the shim and the host endpoint configured in +`config.h` (`HOST_STACK_IP` defaults to `10.10.10.1`), run: + +```sh +sudo LD_PRELOAD=$PWD/libwolfip.so ping -I wtcp0 -c5 10.10.10.1 +``` + +The `-I wtcp0` flag pins the test to the injected interface and `-c5` +generates five echo requests. Successful replies confirm the ICMP +datagram socket support end-to-end through the tap device. + ## Copyright and License wolfIP is licensed under the GPLv3 license. See the LICENSE file for details. Copyright (c) 2025 wolfSSL Inc. - diff --git a/config.h b/config.h index bda8ff8..ea0f429 100644 --- a/config.h +++ b/config.h @@ -10,9 +10,14 @@ #define MAX_TCPSOCKETS 4 #define MAX_UDPSOCKETS 2 +#define MAX_ICMPSOCKETS 2 #define RXBUF_SIZE LINK_MTU * 16 #define TXBUF_SIZE LINK_MTU * 16 +#ifndef WOLFIP_POSIX_TCPDUMP +#define WOLFIP_POSIX_TCPDUMP 0 +#endif + #define MAX_NEIGHBORS 16 #ifndef WOLFIP_MAX_INTERFACES @@ -34,5 +39,6 @@ /* Linux test configuration */ #define WOLFIP_IP "10.10.10.2" #define HOST_STACK_IP "10.10.10.1" +#define WOLFIP_STATIC_DNS_IP "9.9.9.9" #endif diff --git a/src/port/posix/bsd_socket.c b/src/port/posix/bsd_socket.c index a8eaa32..b1c7a8f 100644 --- a/src/port/posix/bsd_socket.c +++ b/src/port/posix/bsd_socket.c @@ -29,21 +29,70 @@ #include #include #include +#include #include #include +#include +#include +#include +#include +#include +#include +#include #define WOLF_POSIX #include "config.h" #include "wolfip.h" +static int wolfip_dbg_enabled; +#ifndef WOLFIP_DBG +#define WOLFIP_DBG(fmt, ...) \ + do { if (wolfip_dbg_enabled) fprintf(stderr, "[DBG] bsd_socket: " fmt "\n", ##__VA_ARGS__); } while (0) +#endif static __thread int in_the_stack = 1; static struct wolfIP *IPSTACK = NULL; pthread_mutex_t wolfIP_mutex; +int wolfIP_sock_poll(struct wolfIP *ipstack, struct pollfd *fds, nfds_t nfds, int timeout); + +#if WOLFIP_POSIX_TCPDUMP +static pid_t tcpdump_pid = -1; + +static void wolfIP_stop_tcpdump(void); + +static void wolfIP_start_tcpdump(const char *ifname) +{ + if (tcpdump_pid > 0 || !ifname || ifname[0] == '\0') + return; + tcpdump_pid = fork(); + if (tcpdump_pid == 0) { + execlp("tcpdump", "tcpdump", "-i", ifname, + "-w", "/tmp/wolfip.pcap", "-U", "-n", NULL); + _exit(127); + } else if (tcpdump_pid < 0) { + perror("tcpdump fork"); + } +} + +static void wolfIP_stop_tcpdump(void) +{ + if (tcpdump_pid > 0) { + kill(tcpdump_pid, SIGINT); + tcpdump_pid = -1; + } +} + +static void wolfIP_stop_tcpdump_atexit(void) +{ + wolfIP_stop_tcpdump(); +} +#endif + /* host_ functions are the original functions from the libc */ static int (*host_socket ) (int domain, int type, int protocol) = NULL; static int (*host_bind ) (int sockfd, const struct sockaddr *addr, socklen_t addrlen); static int (*host_connect ) (int sockfd, const struct sockaddr *addr, socklen_t addrlen); static int (*host_accept ) (int sockfd, struct sockaddr *addr, socklen_t *addrlen); +static int (*host_accept4 ) (int sockfd, struct sockaddr *addr, socklen_t *addrlen, int flags); static int (*host_listen ) (int sockfd, int backlog); static ssize_t (*host_recvfrom) (int sockfd, void *buf, size_t len, int flags, struct sockaddr *addr, socklen_t *addrlen); static ssize_t (*host_recv ) (int sockfd, void *buf, size_t len, int flags); @@ -51,6 +100,10 @@ static ssize_t (*host_read ) (int sockfd, void *buf, size_t len); static ssize_t (*host_sendto ) (int sockfd, const void *buf, size_t len, int flags, const struct sockaddr *addr, socklen_t addrlen); static ssize_t (*host_send ) (int sockfd, const void *buf, size_t len, int flags); static ssize_t (*host_write ) (int sockfd, const void *buf, size_t len); +static ssize_t (*host_sendmsg ) (int sockfd, const struct msghdr *msg, int flags); +static ssize_t (*host_recvmsg ) (int sockfd, struct msghdr *msg, int flags); +static int (*host_getaddrinfo) (const char *node, const char *service, const struct addrinfo *hints, struct addrinfo **res); +static void (*host_freeaddrinfo) (struct addrinfo *res); static int (*host_close ) (int sockfd); static int (*host_setsockopt) (int sockfd, int level, int optname, const void *optval, socklen_t optlen); static int (*host_getsockopt) (int sockfd, int level, int optname, void *optval, socklen_t *optlen); @@ -61,6 +114,223 @@ static int (*host_poll) (struct pollfd *fds, nfds_t nfds, int timeout); static int (*host_select) (int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, struct timeval *timeout); static int (*host_fcntl) (int fd, int cmd, ...); +#define WOLFIP_MAX_PUBLIC_FDS 256 + +struct wolfip_fd_entry { + int internal_fd; /* MARK_* encoded */ + int public_fd; /* Returned to user; read end of pipe */ + int pipe_write; /* Write end used for wakeups */ + uint8_t nonblock; + uint8_t in_use; + uint16_t events; /* Events armed for current poll/select */ +}; + +static struct wolfip_fd_entry wolfip_fd_entries[WOLFIP_MAX_PUBLIC_FDS]; +static int tcp_entry_for_slot[MAX_TCPSOCKETS]; +static int udp_entry_for_slot[MAX_UDPSOCKETS]; +static int icmp_entry_for_slot[MAX_ICMPSOCKETS]; + +enum wolfip_dns_wait_type { + DNS_WAIT_NONE = 0, + DNS_WAIT_FORWARD, + DNS_WAIT_PTR +}; + +struct wolfip_dns_wait_ctx { + pthread_mutex_t mutex; + pthread_cond_t cond; + int pending; + enum wolfip_dns_wait_type type; + int status; + uint32_t ip; + char name[256]; +}; + +static struct wolfip_dns_wait_ctx dns_wait_ctx = { + PTHREAD_MUTEX_INITIALIZER, + PTHREAD_COND_INITIALIZER, + 0, + DNS_WAIT_NONE, + 0, + 0, + {0} +}; + +struct wolfip_gai_alloc { + struct addrinfo *res; + struct wolfip_gai_alloc *next; +}; + +static pthread_mutex_t wolfip_gai_alloc_mutex = PTHREAD_MUTEX_INITIALIZER; +static struct wolfip_gai_alloc *wolfip_gai_alloc_head; + +static void wolfip_fd_pool_init(void) +{ + int i; + static int init_done; + if (init_done) + return; + for (i = 0; i < WOLFIP_MAX_PUBLIC_FDS; i++) + wolfip_fd_entries[i].in_use = 0; + for (i = 0; i < MAX_TCPSOCKETS; i++) + tcp_entry_for_slot[i] = -1; + for (i = 0; i < MAX_UDPSOCKETS; i++) + udp_entry_for_slot[i] = -1; + for (i = 0; i < MAX_ICMPSOCKETS; i++) + icmp_entry_for_slot[i] = -1; + init_done = 1; +} + +static struct wolfip_fd_entry *wolfip_entry_from_internal(int internal_fd) +{ + int idx; + if (IS_SOCKET_TCP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos < 0 || pos >= MAX_TCPSOCKETS) + return NULL; + idx = tcp_entry_for_slot[pos]; + } else if (IS_SOCKET_UDP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos < 0 || pos >= MAX_UDPSOCKETS) + return NULL; + idx = udp_entry_for_slot[pos]; + } else if (IS_SOCKET_ICMP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos < 0 || pos >= MAX_ICMPSOCKETS) + return NULL; + idx = icmp_entry_for_slot[pos]; + } else { + return NULL; + } + if (idx < 0 || idx >= WOLFIP_MAX_PUBLIC_FDS) + return NULL; + if (!wolfip_fd_entries[idx].in_use) + return NULL; + return &wolfip_fd_entries[idx]; +} + +static struct wolfip_fd_entry *wolfip_entry_from_public(int public_fd) +{ + if (public_fd < 0 || public_fd >= WOLFIP_MAX_PUBLIC_FDS) + return NULL; + if (!wolfip_fd_entries[public_fd].in_use) + return NULL; + return &wolfip_fd_entries[public_fd]; +} + +static void wolfip_fd_detach_internal(int internal_fd) +{ + if (IS_SOCKET_TCP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_TCPSOCKETS) + tcp_entry_for_slot[pos] = -1; + } else if (IS_SOCKET_UDP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_UDPSOCKETS) + udp_entry_for_slot[pos] = -1; + } else if (IS_SOCKET_ICMP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_ICMPSOCKETS) + icmp_entry_for_slot[pos] = -1; + } +} + +static void wolfip_fd_attach_internal(int internal_fd, int entry_idx) +{ + if (IS_SOCKET_TCP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_TCPSOCKETS) + tcp_entry_for_slot[pos] = entry_idx; + } else if (IS_SOCKET_UDP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_UDPSOCKETS) + udp_entry_for_slot[pos] = entry_idx; + } else if (IS_SOCKET_ICMP(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < MAX_ICMPSOCKETS) + icmp_entry_for_slot[pos] = entry_idx; + } +} + +static int wolfip_fd_alloc(int internal_fd, int nonblock) +{ + int pipefds[2]; + int idx; + wolfip_fd_pool_init(); + WOLFIP_DBG("fd_alloc: enter internal=%d nonblock=%d", internal_fd, nonblock ? 1 : 0); + if (pipe(pipefds) < 0) { + WOLFIP_DBG("fd_alloc: pipe failed errno=%d", errno); + return -errno; + } + WOLFIP_DBG("fd_alloc: pipe fds r=%d w=%d", pipefds[0], pipefds[1]); + if (host_fcntl) { + host_fcntl(pipefds[0], F_SETFD, FD_CLOEXEC); + host_fcntl(pipefds[1], F_SETFD, FD_CLOEXEC); + host_fcntl(pipefds[0], F_SETFL, O_NONBLOCK); + } else { + fcntl(pipefds[0], F_SETFD, FD_CLOEXEC); + fcntl(pipefds[1], F_SETFD, FD_CLOEXEC); + fcntl(pipefds[0], F_SETFL, O_NONBLOCK); + } + if (pipefds[0] < 0 || pipefds[0] >= WOLFIP_MAX_PUBLIC_FDS || wolfip_fd_entries[pipefds[0]].in_use) { + if (host_close) { + host_close(pipefds[0]); + host_close(pipefds[1]); + } + WOLFIP_DBG("fd_alloc: pipe fd %d unusable", pipefds[0]); + return -EMFILE; + } + idx = pipefds[0]; + wolfip_fd_entries[idx].internal_fd = internal_fd; + wolfip_fd_entries[idx].public_fd = pipefds[0]; + wolfip_fd_entries[idx].pipe_write = pipefds[1]; + wolfip_fd_entries[idx].nonblock = nonblock ? 1 : 0; + wolfip_fd_entries[idx].events = 0; + wolfip_fd_entries[idx].in_use = 1; + wolfip_fd_attach_internal(internal_fd, idx); + WOLFIP_DBG("fd_alloc: internal=%d -> public=%d (nonblock=%d) attached", internal_fd, pipefds[0], nonblock ? 1 : 0); + return pipefds[0]; +} + +static void wolfip_fd_release(int public_fd) +{ + if (public_fd < 0 || public_fd >= WOLFIP_MAX_PUBLIC_FDS) + return; + if (wolfip_fd_entries[public_fd].in_use) { + if (host_close) { + host_close(wolfip_fd_entries[public_fd].public_fd); + host_close(wolfip_fd_entries[public_fd].pipe_write); + } + wolfip_fd_detach_internal(wolfip_fd_entries[public_fd].internal_fd); + WOLFIP_DBG("fd_release: public=%d internal=%d", public_fd, wolfip_fd_entries[public_fd].internal_fd); + } + wolfip_fd_entries[public_fd].in_use = 0; + wolfip_fd_entries[public_fd].events = 0; +} + +static int wolfip_fd_internal_from_public(int public_fd) +{ + struct wolfip_fd_entry *e = wolfip_entry_from_public(public_fd); + if (!e) + return -1; + return e->internal_fd; +} + +static int wolfip_fd_set_nonblock_flag(int public_fd, int nonblock) +{ + struct wolfip_fd_entry *e = wolfip_entry_from_public(public_fd); + if (!e) + return -WOLFIP_EINVAL; + e->nonblock = nonblock ? 1 : 0; + return 0; +} + +static int wolfip_fd_is_nonblock(int public_fd) +{ + struct wolfip_fd_entry *e = wolfip_entry_from_public(public_fd); + return e ? (e->nonblock != 0) : 0; +} + #define swap_socketcall(call, name) \ { \ const char *msg; \ @@ -72,37 +342,62 @@ static int (*host_fcntl) (int fd, int cmd, ...); } -#define conditional_steal_call(call, fd, ...) \ +#define conditional_steal_call(call, user_fd, ...) \ if(in_the_stack) { \ - return host_##call(fd, ## __VA_ARGS__); \ + return host_##call(user_fd, ## __VA_ARGS__); \ } else { \ + int __wolfip_internal = wolfip_fd_internal_from_public(user_fd); \ pthread_mutex_lock(&wolfIP_mutex); \ - if ((fd & (MARK_TCP_SOCKET | MARK_UDP_SOCKET)) != 0) { \ - int __wolfip_retval = wolfIP_sock_##call(IPSTACK, fd, ## __VA_ARGS__); \ + if (__wolfip_internal >= 0) { \ + int __wolfip_retval = wolfIP_sock_##call(IPSTACK, __wolfip_internal, ## __VA_ARGS__); \ if (__wolfip_retval < 0) { \ errno = __wolfip_retval; \ pthread_mutex_unlock(&wolfIP_mutex); \ return -1; \ } \ pthread_mutex_unlock(&wolfIP_mutex); \ + errno = 0; \ return __wolfip_retval; \ - }else { \ + } else { \ pthread_mutex_unlock(&wolfIP_mutex); \ - return host_##call(fd, ## __VA_ARGS__); \ + return host_##call(user_fd, ## __VA_ARGS__); \ } \ } -#define conditional_steal_blocking_call(call, fd, ...) \ +#define conditional_steal_blocking_call(call, user_fd, wait_events, ...) \ if(in_the_stack) { \ - return host_##call(fd, ## __VA_ARGS__); \ + return host_##call(user_fd, ## __VA_ARGS__); \ } else { \ + int __wolfip_internal = wolfip_fd_internal_from_public(user_fd); \ pthread_mutex_lock(&wolfIP_mutex); \ - if ((fd & (MARK_TCP_SOCKET | MARK_UDP_SOCKET)) != 0) { \ + if (__wolfip_internal >= 0) { \ int __wolfip_retval; \ + int __wolfip_nonblock = wolfip_fd_is_nonblock(user_fd); \ + struct wolfip_fd_entry *__entry = wolfip_entry_from_public(user_fd); \ + struct pollfd __pfd; \ do { \ - __wolfip_retval = wolfIP_sock_##call(IPSTACK, fd, ## __VA_ARGS__); \ + __wolfip_retval = wolfIP_sock_##call(IPSTACK, __wolfip_internal, ## __VA_ARGS__); \ if (__wolfip_retval == -EAGAIN) { \ - usleep(1000); \ + if (__wolfip_nonblock) { \ + errno = EAGAIN; \ + pthread_mutex_unlock(&wolfIP_mutex); \ + return -1; \ + } \ + if (__entry) { \ + __entry->events = (wait_events); \ + wolfIP_register_callback(IPSTACK, __entry->internal_fd, poller_callback, IPSTACK); \ + __pfd.fd = __entry->public_fd; \ + __pfd.events = POLLIN; \ + __pfd.revents = 0; \ + pthread_mutex_unlock(&wolfIP_mutex); \ + host_poll(&__pfd, 1, -1); \ + pthread_mutex_lock(&wolfIP_mutex); \ + { char __c; while (host_read(__entry->public_fd, &__c, 1) > 0) {} } \ + } else { \ + pthread_mutex_unlock(&wolfIP_mutex); \ + usleep(1000); \ + pthread_mutex_lock(&wolfIP_mutex); \ + } \ } \ } while (__wolfip_retval == -EAGAIN); \ if (__wolfip_retval < 0) { \ @@ -111,327 +406,644 @@ static int (*host_fcntl) (int fd, int cmd, ...); return -1; \ } \ pthread_mutex_unlock(&wolfIP_mutex); \ + errno = 0; \ return __wolfip_retval; \ }else { \ pthread_mutex_unlock(&wolfIP_mutex); \ - return host_##call(fd, ## __VA_ARGS__); \ + return host_##call(user_fd, ## __VA_ARGS__); \ } \ } -int wolfIP_sock_setsockopt(struct wolfIP *ipstack, int fd, int level, int optname, const void *optval, socklen_t optlen) { - printf("Intercepted setsockopt\n"); + +int wolfIP_sock_fcntl(struct wolfIP *ipstack, int fd, int cmd, int arg) { (void)ipstack; - (void)fd; - (void)level; - (void)optname; - (void)optval; - (void)optlen; + switch (cmd) { + case F_SETFL: + return wolfip_fd_set_nonblock_flag(fd, (arg & O_NONBLOCK) ? 1 : 0); + case F_GETFL: { + int flags = wolfip_fd_is_nonblock(fd) ? O_NONBLOCK : 0; + return flags; + } + default: + return -WOLFIP_EINVAL; + } +} + +#define WOLFIP_IOV_STACK_BUF 2048 + +static int wolfip_calc_msghdr_len(const struct msghdr *msg, size_t *total_len) +{ + size_t len = 0; + size_t i; + + if (!msg || msg->msg_iovlen == 0 || !msg->msg_iov) + return -WOLFIP_EINVAL; + for (i = 0; i < msg->msg_iovlen; i++) { + const struct iovec *iov = &msg->msg_iov[i]; + if (!iov) + return -WOLFIP_EINVAL; + if (!iov->iov_base && iov->iov_len != 0) + return -WOLFIP_EINVAL; + if (SIZE_MAX - len < iov->iov_len) + return -WOLFIP_EINVAL; + len += iov->iov_len; + } + if (total_len) + *total_len = len; return 0; } -int wolfIP_sock_getsockopt(struct wolfIP *ipstack, int fd, int level, int optname, void *optval, socklen_t *optlen) { - printf("Intercepted getsockopt\n"); - (void)ipstack; - (void)fd; - (void)level; - (void)optname; - (void)optval; - (void)optlen; +static void wolfip_flatten_iov(uint8_t *dst, const struct msghdr *msg) +{ + size_t offset = 0; + size_t i; + + if (!dst || !msg) + return; + for (i = 0; i < msg->msg_iovlen; i++) { + const struct iovec *iov = &msg->msg_iov[i]; + if (!iov || !iov->iov_base || iov->iov_len == 0) + continue; + memcpy(dst + offset, iov->iov_base, iov->iov_len); + offset += iov->iov_len; + } +} + +static void wolfip_scatter_iov(const struct msghdr *msg, const uint8_t *src, size_t len) +{ + size_t offset = 0; + size_t i; + + if (!msg || !msg->msg_iov || !src) + return; + for (i = 0; i < msg->msg_iovlen && offset < len; i++) { + const struct iovec *iov = &msg->msg_iov[i]; + size_t chunk; + + if (!iov || !iov->iov_base || iov->iov_len == 0) + continue; + chunk = iov->iov_len; + if (chunk > len - offset) + chunk = len - offset; + memcpy(iov->iov_base, src + offset, chunk); + offset += chunk; + } +} + +static void wolfip_fill_ttl_control(struct wolfIP *ipstack, int sockfd, struct msghdr *msg) +{ + int ttl; + int ttl_status; + struct cmsghdr *cmsg; + socklen_t ctrl_len; + + if (!msg) + return; + ctrl_len = msg->msg_controllen; + msg->msg_controllen = 0; + ttl_status = wolfIP_sock_get_recv_ttl(ipstack, sockfd, &ttl); + if (ttl_status <= 0) + return; + if (!msg->msg_control || ctrl_len < (socklen_t)CMSG_SPACE(sizeof(int))) + return; + cmsg = (struct cmsghdr *)msg->msg_control; + cmsg->cmsg_level = SOL_IP; + cmsg->cmsg_type = IP_TTL; + cmsg->cmsg_len = CMSG_LEN(sizeof(int)); + *((int *)CMSG_DATA(cmsg)) = ttl; + msg->msg_controllen = cmsg->cmsg_len; +} + +static size_t wolfip_strlcpy(char *dst, const char *src, size_t size) +{ + size_t len = 0; + if (!dst || size == 0) + return 0; + if (src) { + while (src[len] && len + 1 < size) { + dst[len] = src[len]; + len++; + } + dst[len] = '\0'; + while (src[len]) + len++; + } else { + dst[0] = '\0'; + } + return len; +} + +static int wolfip_dns_error_to_eai(int err) +{ + switch (err) { + case 0: + return 0; + case -16: + return EAI_AGAIN; + case -22: + return EAI_NONAME; + case -101: + return EAI_FAIL; + default: + return EAI_FAIL; + } +} + +static int wolfip_dns_begin_wait(enum wolfip_dns_wait_type type) +{ + pthread_mutex_lock(&dns_wait_ctx.mutex); + if (dns_wait_ctx.pending) { + pthread_mutex_unlock(&dns_wait_ctx.mutex); + return EAI_AGAIN; + } + dns_wait_ctx.pending = 1; + dns_wait_ctx.type = type; + dns_wait_ctx.status = EAI_FAIL; + dns_wait_ctx.name[0] = '\0'; + pthread_mutex_unlock(&dns_wait_ctx.mutex); return 0; } -int wolfIP_sock_fcntl(struct wolfIP *ipstack, int fd, int cmd, int arg) { - printf("Intercepted fcntl\n"); - (void)ipstack; - (void)fd; - (void)cmd; - (void)arg; +static void wolfip_dns_abort_wait(int status) +{ + pthread_mutex_lock(&dns_wait_ctx.mutex); + dns_wait_ctx.pending = 0; + dns_wait_ctx.type = DNS_WAIT_NONE; + dns_wait_ctx.status = status; + pthread_cond_signal(&dns_wait_ctx.cond); + pthread_mutex_unlock(&dns_wait_ctx.mutex); +} + +static int wolfip_dns_wait(enum wolfip_dns_wait_type type, uint32_t *ip_out, char *name_out, size_t name_len) +{ + struct timespec ts; + int status; + clock_gettime(CLOCK_REALTIME, &ts); + ts.tv_sec += 5; + pthread_mutex_lock(&dns_wait_ctx.mutex); + while (dns_wait_ctx.pending && dns_wait_ctx.type == type) { + int err = pthread_cond_timedwait(&dns_wait_ctx.cond, &dns_wait_ctx.mutex, &ts); + if (err == ETIMEDOUT) { + dns_wait_ctx.pending = 0; + dns_wait_ctx.type = DNS_WAIT_NONE; + pthread_mutex_unlock(&dns_wait_ctx.mutex); + return EAI_AGAIN; + } + } + if (dns_wait_ctx.type != type) { + int status = dns_wait_ctx.status ? dns_wait_ctx.status : EAI_FAIL; + pthread_mutex_unlock(&dns_wait_ctx.mutex); + return status; + } + status = dns_wait_ctx.status; + if (status == 0) { + if (ip_out) + *ip_out = dns_wait_ctx.ip; + if (name_out && name_len) + wolfip_strlcpy(name_out, dns_wait_ctx.name, name_len); + } + dns_wait_ctx.type = DNS_WAIT_NONE; + pthread_mutex_unlock(&dns_wait_ctx.mutex); + return status; +} + +static void wolfip_dns_forward_cb(ip4 ip) +{ + pthread_mutex_lock(&dns_wait_ctx.mutex); + if (dns_wait_ctx.pending && dns_wait_ctx.type == DNS_WAIT_FORWARD) { + dns_wait_ctx.ip = ip; + dns_wait_ctx.status = 0; + dns_wait_ctx.pending = 0; + pthread_cond_signal(&dns_wait_ctx.cond); + } + pthread_mutex_unlock(&dns_wait_ctx.mutex); +} + +static void wolfip_dns_reverse_cb(const char *name) +{ + pthread_mutex_lock(&dns_wait_ctx.mutex); + if (dns_wait_ctx.pending && dns_wait_ctx.type == DNS_WAIT_PTR) { + wolfip_strlcpy(dns_wait_ctx.name, name, sizeof(dns_wait_ctx.name)); + dns_wait_ctx.status = 0; + dns_wait_ctx.pending = 0; + pthread_cond_signal(&dns_wait_ctx.cond); + } + pthread_mutex_unlock(&dns_wait_ctx.mutex); +} + +static int wolfip_dns_forward_query(const char *node, uint32_t *ip_out) +{ + uint16_t dns_id; + int err = wolfip_dns_begin_wait(DNS_WAIT_FORWARD); + if (err != 0) + return err; + pthread_mutex_lock(&wolfIP_mutex); + err = nslookup(IPSTACK, node, &dns_id, wolfip_dns_forward_cb); + pthread_mutex_unlock(&wolfIP_mutex); + if (err < 0) { + int eai = wolfip_dns_error_to_eai(err); + wolfip_dns_abort_wait(eai); + return eai; + } + return wolfip_dns_wait(DNS_WAIT_FORWARD, ip_out, NULL, 0); +} + +static int wolfip_dns_reverse_query(uint32_t ip, char *name, size_t name_len) +{ + uint16_t dns_id; + int err = wolfip_dns_begin_wait(DNS_WAIT_PTR); + if (err != 0) + return err; + pthread_mutex_lock(&wolfIP_mutex); + err = wolfIP_dns_ptr_lookup(IPSTACK, ip, &dns_id, wolfip_dns_reverse_cb); + pthread_mutex_unlock(&wolfIP_mutex); + if (err < 0) { + int eai = wolfip_dns_error_to_eai(err); + wolfip_dns_abort_wait(eai); + return eai; + } + return wolfip_dns_wait(DNS_WAIT_PTR, NULL, name, name_len); +} + +static int wolfip_parse_service(const char *service, uint16_t *port) +{ + char *end; + long value; + if (!port) + return EAI_FAIL; + if (!service) { + *port = 0; + return 0; + } + value = strtol(service, &end, 10); + if (*end != '\0' || value < 0 || value > 65535) + return EAI_SERVICE; + *port = (uint16_t)value; + return 0; +} + +static struct addrinfo *wolfip_alloc_addrinfo(void) +{ + return (struct addrinfo *)calloc(1, sizeof(struct addrinfo)); +} + +static int wolfip_build_addrinfo(uint32_t ip, uint16_t port, const char *canon, + const struct addrinfo *hints, struct addrinfo **res) +{ + struct addrinfo *ai = wolfip_alloc_addrinfo(); + struct sockaddr_in *sa = (struct sockaddr_in *)calloc(1, sizeof(struct sockaddr_in)); + if (!ai || !sa) { + free(ai); + free(sa); + return EAI_MEMORY; + } + ai->ai_family = AF_INET; + ai->ai_socktype = hints ? hints->ai_socktype : 0; + ai->ai_protocol = hints ? hints->ai_protocol : 0; + ai->ai_addrlen = sizeof(struct sockaddr_in); + ai->ai_flags = hints ? hints->ai_flags : 0; + sa->sin_family = AF_INET; + sa->sin_port = htons(port); + sa->sin_addr.s_addr = htonl(ip); + ai->ai_addr = (struct sockaddr *)sa; + if (canon) { + ai->ai_canonname = strdup(canon); + if (!ai->ai_canonname) { + free(sa); + free(ai); + return EAI_MEMORY; + } + } + *res = ai; + return 0; +} + +static void wolfip_free_addrinfo_list(struct addrinfo *res) +{ + while (res) { + struct addrinfo *next = res->ai_next; + free(res->ai_canonname); + free(res->ai_addr); + free(res); + res = next; + } +} + +static int wolfip_register_gai_alloc(struct addrinfo *res) +{ + struct wolfip_gai_alloc *node = (struct wolfip_gai_alloc *)malloc(sizeof(struct wolfip_gai_alloc)); + if (!node) { + wolfip_free_addrinfo_list(res); + return EAI_MEMORY; + } + node->res = res; + pthread_mutex_lock(&wolfip_gai_alloc_mutex); + node->next = wolfip_gai_alloc_head; + wolfip_gai_alloc_head = node; + pthread_mutex_unlock(&wolfip_gai_alloc_mutex); return 0; } +static int wolfip_take_gai_alloc(struct addrinfo *res) +{ + struct wolfip_gai_alloc **pp; + struct wolfip_gai_alloc *cur; + pthread_mutex_lock(&wolfip_gai_alloc_mutex); + pp = &wolfip_gai_alloc_head; + while ((cur = *pp) != NULL) { + if (cur->res == res) { + *pp = cur->next; + pthread_mutex_unlock(&wolfip_gai_alloc_mutex); + free(cur); + return 1; + } + pp = &cur->next; + } + pthread_mutex_unlock(&wolfip_gai_alloc_mutex); + return 0; +} + +int wolfIP_sock_sendmsg(struct wolfIP *ipstack, int sockfd, const struct msghdr *msg, int flags) +{ + const struct wolfIP_sockaddr *dest = NULL; + socklen_t addrlen = 0; + size_t total_len = 0; + int ret; + uint8_t stack_buf[WOLFIP_IOV_STACK_BUF]; + uint8_t *heap_buf = NULL; + const void *payload = NULL; + + if (wolfip_calc_msghdr_len(msg, &total_len) < 0) + return -WOLFIP_EINVAL; + if (msg->msg_name && msg->msg_namelen > 0) { + dest = (const struct wolfIP_sockaddr *)msg->msg_name; + addrlen = msg->msg_namelen; + } + if (msg->msg_iovlen == 1) { + payload = msg->msg_iov[0].iov_base; + } else if (total_len > 0) { + uint8_t *tmp = stack_buf; + if (total_len > sizeof(stack_buf)) { + heap_buf = (uint8_t *)malloc(total_len); + if (!heap_buf) + return -WOLFIP_ENOMEM; + tmp = heap_buf; + } + wolfip_flatten_iov(tmp, msg); + payload = tmp; + } + + ret = wolfIP_sock_sendto(ipstack, sockfd, payload, total_len, flags, dest, addrlen); + if (heap_buf) + free(heap_buf); + if (ret == -WOLFIP_EAGAIN) + return -EWOULDBLOCK; + return ret; +} + +int wolfIP_sock_recvmsg(struct wolfIP *ipstack, int sockfd, struct msghdr *msg, int flags) +{ + struct wolfIP_sockaddr *src = NULL; + socklen_t addrlen = 0; + size_t total_len = 0; + int ret; + uint8_t stack_buf[WOLFIP_IOV_STACK_BUF]; + uint8_t *heap_buf = NULL; + uint8_t *buf = NULL; + struct pollfd pfd; + WOLFIP_DBG("recvmsg: fd=%d flags=0x%x iovlen=%zu", sockfd, flags, msg ? msg->msg_iovlen : 0); + + if (wolfip_calc_msghdr_len(msg, &total_len) < 0) + return -WOLFIP_EINVAL; + if (msg->msg_name && msg->msg_namelen > 0) { + src = (struct wolfIP_sockaddr *)msg->msg_name; + addrlen = msg->msg_namelen; + } + if (msg->msg_iovlen == 1) { + buf = (uint8_t *)msg->msg_iov[0].iov_base; + } else if (total_len > 0) { + if (total_len > sizeof(stack_buf)) { + heap_buf = (uint8_t *)malloc(total_len); + if (!heap_buf) + return -WOLFIP_ENOMEM; + buf = heap_buf; + } else { + buf = stack_buf; + } + } + + pfd.fd = sockfd; + pfd.events = POLLIN; + pfd.revents = 0; + while (1) { + ret = wolfIP_sock_recvfrom(ipstack, sockfd, buf ? buf : msg->msg_iov[0].iov_base, + total_len, flags, src, src ? &addrlen : NULL); + if (ret != -WOLFIP_EAGAIN) + break; + (void)wolfIP_sock_poll(ipstack, &pfd, 1, -1); + } + if (ret >= 0 && msg->msg_iovlen > 1) { + wolfip_scatter_iov(msg, buf, (size_t)ret); + } + if (heap_buf) + free(heap_buf); + if (ret >= 0) { + if (src) + msg->msg_namelen = addrlen; + msg->msg_flags = 0; + wolfip_fill_ttl_control(ipstack, sockfd, msg); + } + if (ret == -WOLFIP_EAGAIN) + return -EWOULDBLOCK; + return ret; +} + int fcntl(int fd, int cmd, ...) { va_list ap; - int arg; + int arg = 0; int ret; va_start(ap, cmd); - arg = va_arg(ap, int); + if (cmd != F_GETFD && cmd != F_GETFL) { + arg = va_arg(ap, int); + } va_end(ap); if (in_the_stack) { return host_fcntl(fd, cmd, arg); } else { pthread_mutex_lock(&wolfIP_mutex); ret = wolfIP_sock_fcntl(IPSTACK, fd, cmd, arg); + if (ret == -WOLFIP_EINVAL) { + pthread_mutex_unlock(&wolfIP_mutex); + return host_fcntl(fd, cmd, arg); + } pthread_mutex_unlock(&wolfIP_mutex); + if (ret < 0) { + errno = -ret; + return -1; + } return ret; } } -struct bsd_poll_helper { - int fd; /* Original fd */ - int events; /* Original events */ - int pipefds[2]; /* Pipe for triggering events */ -}; - -/* Static arrays for poll helpers */ -static struct bsd_poll_helper tcp_pollers[MAX_TCPSOCKETS] = {{0}}; -static struct bsd_poll_helper udp_pollers[MAX_UDPSOCKETS] = {{0}}; - void poller_callback(int fd, uint16_t event, void *arg) { - struct bsd_poll_helper *poller; + struct wolfip_fd_entry *entry; char c; (void)arg; - if ((fd & MARK_TCP_SOCKET) != 0) - poller = &tcp_pollers[fd & ~MARK_TCP_SOCKET]; - else if ((fd & MARK_UDP_SOCKET) != 0) - poller = &udp_pollers[fd & ~MARK_UDP_SOCKET]; - else - return; - if (poller->fd != fd) + entry = wolfip_entry_from_internal(fd); + if (!entry) return; - if (event & CB_EVENT_READABLE) + WOLFIP_DBG("poller_cb: internal=%d public=%d events=0x%x entry_events=0x%x", fd, entry->public_fd, event, entry->events); + if (event & CB_EVENT_READABLE) { c = 'r'; - else if (event & CB_EVENT_WRITABLE) + } else if (event & CB_EVENT_WRITABLE) { c = 'w'; - else if (event & CB_EVENT_CLOSED) + } else if (event & CB_EVENT_CLOSED) { c = 'h'; - else + } else { return; - write(poller->pipefds[1], &c, 1); + } + WOLFIP_DBG("poller_callback: internal=%d public=%d event=%c", fd, entry->public_fd, c); + write(entry->pipe_write, &c, 1); } int wolfIP_sock_poll(struct wolfIP *ipstack, struct pollfd *fds, nfds_t nfds, int timeout) { nfds_t i; - int fd; int ret; - int miss = 0; + char discard; if (in_the_stack) { return host_poll(fds, nfds, timeout); } - memset(tcp_pollers, 0, sizeof(tcp_pollers)); - memset(udp_pollers, 0, sizeof(udp_pollers)); + WOLFIP_DBG("sock_poll: nfds=%zu timeout=%d", (size_t)nfds, timeout); for (i = 0; i < nfds; i++) { - struct bsd_poll_helper *poller = NULL; - fd = fds[i].fd; - - if ((fd & MARK_TCP_SOCKET) != 0) - poller = &tcp_pollers[fd & ~MARK_TCP_SOCKET]; - else if ((fd & MARK_UDP_SOCKET) != 0) - poller = &udp_pollers[fd & ~MARK_UDP_SOCKET]; - else + struct wolfip_fd_entry *entry = wolfip_entry_from_public(fds[i].fd); + if (!entry) continue; - if (pipe(poller->pipefds) < 0) { - perror("pipe"); - return -1; + entry->events = fds[i].events; + WOLFIP_DBG("sock_poll: arm public=%d internal=%d events=0x%x", entry->public_fd, entry->internal_fd, entry->events); + /* Drain any stale notifications */ + while (host_read(entry->public_fd, &discard, 1) > 0) { } - poller->fd = fd; - poller->events = fds[i].events; - /* Replace the original fd with the read end of the pipe */ - fds[i].fd = poller->pipefds[0]; - fds[i].events = POLLIN; + wolfIP_register_callback(ipstack, entry->internal_fd, poller_callback, ipstack); fds[i].revents = 0; - /* Assign the callback */ - wolfIP_register_callback(ipstack, fd, poller_callback, ipstack); + fds[i].fd = entry->public_fd; } - /* Call the original poll */ -repeat: - miss = 0; pthread_mutex_unlock(&wolfIP_mutex); ret = host_poll(fds, nfds, timeout); pthread_mutex_lock(&wolfIP_mutex); - if (ret <= 0) - return ret; - for (i = 0; i < nfds; i++) { - struct bsd_poll_helper *poller = NULL; - int j; - char c = 0; - fd = fds[i].fd; - for (j = 0; j < MAX_TCPSOCKETS; j++) { - if (tcp_pollers[j].fd == 0) + WOLFIP_DBG("sock_poll: host_poll ret=%d", ret); + if (ret > 0) { + for (i = 0; i < nfds; i++) { + struct wolfip_fd_entry *entry = wolfip_entry_from_public(fds[i].fd); + short revents = 0; + char c; + if (!entry) continue; - if (tcp_pollers[j].pipefds[0] == fd) { - poller = &tcp_pollers[j]; - break; - } - } - if (!poller) { - for (j = 0; j < MAX_UDPSOCKETS; j++) { - if (udp_pollers[j].fd == 0) - continue; - if (udp_pollers[j].pipefds[0] == fd) { - poller = &udp_pollers[j]; - break; + if (fds[i].revents & POLLIN) { + while (host_read(entry->public_fd, &c, 1) > 0) { + if (c == 'r') + revents |= POLLIN; + else if (c == 'w') + revents |= POLLOUT; + else if (c == 'e') + revents |= POLLERR; + else if (c == 'h') + revents |= POLLHUP; } - } - } - if (poller) { - if ((fds[i].revents & POLLIN) != 0) { - fds[i].revents = 0; - host_read(poller->pipefds[0], &c, 1); - switch(c) { - case 'r': - fds[i].revents |= POLLIN; - break; - case 'w': - fds[i].revents |= POLLOUT; - break; - case 'e': - fds[i].revents |= POLLERR; - break; - case 'h': - fds[i].revents |= POLLHUP; - break; + if (revents == 0) { + wolfIP_register_callback(ipstack, entry->internal_fd, NULL, NULL); + entry->events = 0; } - if ((fds[i].revents != 0) && (fds[i].revents & (poller->events | POLLHUP | POLLERR)) == 0) { - miss++; - ret--; - continue; - } - fds[i].revents &= (POLLHUP | POLLERR | poller->events); - } else { - fds[i].revents = 0; } - fds[i].fd = poller->fd; - fds[i].events = poller->events; - host_close(poller->pipefds[0]); - host_close(poller->pipefds[1]); - poller->fd = 0; - wolfIP_register_callback(ipstack, poller->fd, NULL, NULL); + WOLFIP_DBG("sock_poll: fd=%d internal=%d host_revents=0x%x mapped=0x%x armed=0x%x", + entry->public_fd, entry->internal_fd, fds[i].revents, revents, entry->events); + fds[i].revents = revents & (entry->events | POLLERR | POLLHUP); + fds[i].events = entry->events; } } - if ((miss != 0) && (ret == 0)) - goto repeat; return ret; } int wolfIP_sock_select(struct wolfIP *ipstack, int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, struct timeval *timeout) { int i; - int maxfd; int ret; - fd_set readfds_local; - /* Assume MARK_TCP_SOCKET < MARK_UDP_SOCKET */ - if (nfds < MARK_TCP_SOCKET + 1) { + int maxfd = nfds - 1; + if (in_the_stack) { return host_select(nfds, readfds, writefds, exceptfds, timeout); } - memset(tcp_pollers, 0, sizeof(tcp_pollers)); - memset(udp_pollers, 0, sizeof(udp_pollers)); - for (i = 0; (i < MARK_TCP_SOCKET) && (i < nfds); i++) { - if ((readfds && FD_ISSET(i, readfds)) || - (writefds && FD_ISSET(i, writefds)) || - (exceptfds && FD_ISSET(i, exceptfds))) { - maxfd = i; - } - } - /* At this point, we do need a fd_set to read from pipes */ - if (!readfds) { - FD_ZERO(&readfds_local); - readfds = &readfds_local; - } - for (i = MARK_TCP_SOCKET; i < nfds && i < (MARK_TCP_SOCKET | MAX_TCPSOCKETS); i++) { - int tcp_pos = i & (~MARK_TCP_SOCKET); - if ((readfds && (FD_ISSET(i, readfds))) || (writefds && (FD_ISSET(i, writefds))) || (exceptfds && (FD_ISSET(i, exceptfds)))) { - if (pipe(tcp_pollers[tcp_pos].pipefds) < 0) - return -1; - tcp_pollers[tcp_pos].fd = i; - tcp_pollers[tcp_pos].events = 0; - wolfIP_register_callback(ipstack, i, poller_callback, ipstack); - if (readfds && (FD_ISSET(i, readfds))) { - tcp_pollers[tcp_pos].events |= POLLIN; - FD_CLR(i, readfds); - FD_SET(tcp_pollers[tcp_pos].pipefds[0], readfds); - } - if (writefds && (FD_ISSET(i, writefds))) { - tcp_pollers[tcp_pos].events |= POLLOUT; - FD_CLR(i, writefds); - FD_SET(tcp_pollers[tcp_pos].pipefds[0], writefds); - } - if (exceptfds && (FD_ISSET(i, exceptfds))) { - tcp_pollers[tcp_pos].events |= POLLERR | POLLHUP; - FD_CLR(i, exceptfds); - FD_SET(tcp_pollers[tcp_pos].pipefds[0], exceptfds); - } - if (maxfd < tcp_pollers[tcp_pos].pipefds[0]) { - maxfd = tcp_pollers[tcp_pos].pipefds[0]; - } - } else { - } - } - for (i = MARK_UDP_SOCKET; i < nfds && i < (MARK_UDP_SOCKET | MAX_UDPSOCKETS); i++) { - int udp_pos = i & (~MARK_UDP_SOCKET); - if (FD_ISSET(i, readfds) || FD_ISSET(i, writefds) || FD_ISSET(i, exceptfds)) { - pipe(udp_pollers[udp_pos].pipefds); - udp_pollers[udp_pos].fd = i; - udp_pollers[udp_pos].events = 0; - wolfIP_register_callback(ipstack, i, poller_callback, ipstack); - if (readfds && FD_ISSET(i, readfds)) { - udp_pollers[udp_pos].events |= POLLIN; - FD_CLR(i, readfds); - FD_SET(udp_pollers[udp_pos].pipefds[0], readfds); - } - if (writefds && FD_ISSET(i, writefds)) { - udp_pollers[udp_pos].events |= POLLOUT; - FD_CLR(i, writefds); - FD_SET(udp_pollers[udp_pos].pipefds[0], writefds); - } - if (exceptfds && FD_ISSET(i, exceptfds)) { - udp_pollers[udp_pos].events |= POLLERR | POLLHUP; - FD_CLR(i, exceptfds); - FD_SET(udp_pollers[udp_pos].pipefds[0], exceptfds); - } - if (maxfd < udp_pollers[udp_pos].pipefds[0]) { - maxfd = udp_pollers[udp_pos].pipefds[0]; + WOLFIP_DBG("sock_select: nfds=%d", nfds); + /* Arm callbacks for sockets present in fd_sets */ + for (i = 0; i < WOLFIP_MAX_PUBLIC_FDS; i++) { + struct wolfip_fd_entry *entry; + if (!wolfip_fd_entries[i].in_use) + continue; + entry = &wolfip_fd_entries[i]; + entry->events = 0; + if (readfds && FD_ISSET(entry->public_fd, readfds)) + entry->events |= POLLIN; + if (writefds && FD_ISSET(entry->public_fd, writefds)) + entry->events |= POLLOUT; + if (exceptfds && FD_ISSET(entry->public_fd, exceptfds)) + entry->events |= POLLERR | POLLHUP; + if (entry->events == 0) + continue; + WOLFIP_DBG("sock_select: arm public=%d internal=%d events=0x%x", entry->public_fd, entry->internal_fd, entry->events); + { + char discard; + while (host_read(entry->public_fd, &discard, 1) > 0) { } } + wolfIP_register_callback(ipstack, entry->internal_fd, poller_callback, ipstack); + if (entry->public_fd > maxfd) + maxfd = entry->public_fd; } - /* Call the original select */ pthread_mutex_unlock(&wolfIP_mutex); ret = host_select(maxfd + 1, readfds, writefds, exceptfds, timeout); pthread_mutex_lock(&wolfIP_mutex); - if (ret <= 0) { - return ret; - } - - for (i = 0; i < MAX_TCPSOCKETS; i++) { - if (tcp_pollers[i].fd == 0) { - continue; - } - if (FD_ISSET(tcp_pollers[i].pipefds[0], readfds)) { + WOLFIP_DBG("sock_select: host_select ret=%d", ret); + if (ret > 0) { + int idx; + for (idx = 0; idx < WOLFIP_MAX_PUBLIC_FDS; idx++) { + struct wolfip_fd_entry *entry; char c; - host_read(tcp_pollers[i].pipefds[0], &c, 1); - if (readfds && (c == 'r')) { - FD_SET(tcp_pollers[i].fd, readfds); - } else if (writefds && (c == 'w')) { - FD_SET(tcp_pollers[i].fd, writefds); - } else if (exceptfds && (c == 'e')) { - FD_SET(tcp_pollers[i].fd, exceptfds); - } - } - wolfIP_register_callback(ipstack, tcp_pollers[i].fd, NULL, NULL); - host_close(tcp_pollers[i].pipefds[0]); - host_close(tcp_pollers[i].pipefds[1]); - tcp_pollers[i].fd = 0; - } - for (i = 0; i < MAX_UDPSOCKETS; i++) { - if (udp_pollers[i].fd == 0) { + int saw_r = 0, saw_w = 0, saw_e = 0; + if (!wolfip_fd_entries[idx].in_use) + continue; + entry = &wolfip_fd_entries[idx]; + if (entry->events == 0) continue; - } - if (FD_ISSET(udp_pollers[i].pipefds[0], readfds)) { - char c; - read(udp_pollers[i].pipefds[0], &c, 1); - if (readfds && (c == 'r')) { - FD_SET(udp_pollers[i].fd, readfds); - } else if (writefds && (c == 'w')) { - FD_SET(udp_pollers[i].fd, writefds); - } else if (exceptfds && (c == 'e')) { - FD_SET(udp_pollers[i].fd, exceptfds); + if ((readfds && FD_ISSET(entry->public_fd, readfds)) || + (writefds && FD_ISSET(entry->public_fd, writefds)) || + (exceptfds && FD_ISSET(entry->public_fd, exceptfds))) { + while (host_read(entry->public_fd, &c, 1) > 0) { + if (c == 'r') + saw_r = 1; + else if (c == 'w') + saw_w = 1; + else if (c == 'e' || c == 'h') + saw_e = 1; + } + if (!saw_r && !saw_w && !saw_e) { + /* No payload left; clear events to avoid busy wakeups */ + wolfIP_register_callback(ipstack, entry->internal_fd, NULL, NULL); + entry->events = 0; + } + if (readfds && FD_ISSET(entry->public_fd, readfds) && !saw_r) + FD_CLR(entry->public_fd, readfds); + if (writefds && FD_ISSET(entry->public_fd, writefds) && !saw_w) + FD_CLR(entry->public_fd, writefds); + if (exceptfds && FD_ISSET(entry->public_fd, exceptfds) && !saw_e) + FD_CLR(entry->public_fd, exceptfds); + WOLFIP_DBG("sock_select: public=%d internal=%d host_r=%d host_w=%d host_e=%d saw r/w/e=%d/%d/%d armed=0x%x", + entry->public_fd, entry->internal_fd, + readfds && FD_ISSET(entry->public_fd, readfds), + writefds && FD_ISSET(entry->public_fd, writefds), + exceptfds && FD_ISSET(entry->public_fd, exceptfds), + saw_r, saw_w, saw_e, entry->events); } } - host_close(udp_pollers[i].pipefds[0]); - host_close(udp_pollers[i].pipefds[1]); - wolfIP_register_callback(ipstack, tcp_pollers[i].fd, NULL, NULL); - udp_pollers[i].fd = 0; } return ret; } @@ -449,11 +1061,27 @@ int select(int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, struc } int socket(int domain, int type, int protocol) { + int base_type = type & ~(SOCK_NONBLOCK | SOCK_CLOEXEC); + int internal_fd; + int public_fd; if (in_the_stack) { return host_socket(domain, type, protocol); - } else { - return wolfIP_sock_socket(IPSTACK, domain, type, protocol); } + WOLFIP_DBG("socket: domain=%d type=%d proto=%d", domain, type, protocol); + internal_fd = wolfIP_sock_socket(IPSTACK, domain, base_type, protocol); + if (internal_fd < 0) { + errno = -internal_fd; + WOLFIP_DBG("socket: failed errno=%d", errno); + return -1; + } + public_fd = wolfip_fd_alloc(internal_fd, (type & SOCK_NONBLOCK) ? 1 : 0); + if (public_fd < 0) { + wolfIP_sock_close(IPSTACK, internal_fd); + errno = -public_fd; + return -1; + } + WOLFIP_DBG("socket: returning public fd=%d internal=%d", public_fd, internal_fd); + return public_fd; } int listen(int sockfd, int backlog) { @@ -481,40 +1109,237 @@ int setsockopt(int sockfd, int level, int optname, const void *optval, socklen_t } int close(int sockfd) { - conditional_steal_call(close, sockfd); + int ret; + struct wolfip_fd_entry *entry; + if (in_the_stack) { + return host_close(sockfd); + } + pthread_mutex_lock(&wolfIP_mutex); + entry = wolfip_entry_from_public(sockfd); + if (entry) { + int internal_fd = entry->internal_fd; + wolfip_fd_release(sockfd); + ret = wolfIP_sock_close(IPSTACK, internal_fd); + if (ret < 0) { + errno = ret; + pthread_mutex_unlock(&wolfIP_mutex); + return -1; + } + pthread_mutex_unlock(&wolfIP_mutex); + WOLFIP_DBG("close: public=%d internal=%d", sockfd, internal_fd); + return ret; + } + pthread_mutex_unlock(&wolfIP_mutex); + return host_close(sockfd); } /* Blocking calls */ +static int wolfip_accept_common(int sockfd, struct sockaddr *addr, socklen_t *addrlen, int flags) +{ + int want_nonblock = (flags & SOCK_NONBLOCK) ? 1 : 0; + struct wolfip_fd_entry *entry; + struct pollfd pfd; + char c; + + if (in_the_stack) { + if (flags) + return host_accept4(sockfd, addr, addrlen, flags); + return host_accept(sockfd, addr, addrlen); + } + pthread_mutex_lock(&wolfIP_mutex); + WOLFIP_DBG("accept: entering public=%d want_nonblock=%d", sockfd, want_nonblock); + entry = wolfip_entry_from_public(sockfd); + if (entry) { + int internal_ret; + int public_fd; + if (!want_nonblock) + want_nonblock = wolfip_fd_is_nonblock(sockfd); + do { + internal_ret = wolfIP_sock_accept(IPSTACK, entry->internal_fd, addr, addrlen); + WOLFIP_DBG("accept: wolfIP_sock_accept internal=%d returned %d", entry->internal_fd, internal_ret); + if (internal_ret == -EAGAIN) { + WOLFIP_DBG("accept: internal=%d public=%d -> EAGAIN", entry->internal_fd, sockfd); + if (want_nonblock) { + errno = EAGAIN; + pthread_mutex_unlock(&wolfIP_mutex); + WOLFIP_DBG("accept: would block public=%d", sockfd); + return -1; + } + entry->events = POLLIN; + wolfIP_register_callback(IPSTACK, entry->internal_fd, poller_callback, IPSTACK); + pfd.fd = entry->public_fd; + pfd.events = POLLIN; + pfd.revents = 0; + pthread_mutex_unlock(&wolfIP_mutex); + host_poll(&pfd, 1, -1); + pthread_mutex_lock(&wolfIP_mutex); + while (host_read(entry->public_fd, &c, 1) > 0) { + } + } + } while (internal_ret == -EAGAIN); + if (internal_ret < 0) { + errno = internal_ret; + pthread_mutex_unlock(&wolfIP_mutex); + return -1; + } + WOLFIP_DBG("accept: allocating public fd for internal=%d nonblock=%d", internal_ret, want_nonblock); + public_fd = wolfip_fd_alloc(internal_ret, want_nonblock); + if (public_fd < 0) { + wolfIP_sock_close(IPSTACK, internal_ret); + errno = -public_fd; + pthread_mutex_unlock(&wolfIP_mutex); + WOLFIP_DBG("accept: fd_alloc failed internal=%d ret=%d errno=%d", internal_ret, public_fd, errno); + return -1; + } + WOLFIP_DBG("accept: child ready public=%d internal=%d", public_fd, internal_ret); + pthread_mutex_unlock(&wolfIP_mutex); + WOLFIP_DBG("accept: success parent=%d -> child public=%d internal=%d", sockfd, public_fd, internal_ret); + return public_fd; + } + pthread_mutex_unlock(&wolfIP_mutex); + if (flags) + return host_accept4(sockfd, addr, addrlen, flags); + return host_accept(sockfd, addr, addrlen); +} + int accept(int sockfd, struct sockaddr *addr, socklen_t *addrlen) { - conditional_steal_blocking_call(accept, sockfd, addr, addrlen); + return wolfip_accept_common(sockfd, addr, addrlen, 0); +} + +int accept4(int sockfd, struct sockaddr *addr, socklen_t *addrlen, int flags) { + return wolfip_accept_common(sockfd, addr, addrlen, flags); } int connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen) { - conditional_steal_blocking_call(connect, sockfd, addr, addrlen); + WOLFIP_DBG("connect: fd=%d", sockfd); + conditional_steal_blocking_call(connect, sockfd, POLLOUT, addr, addrlen); } ssize_t recvfrom(int sockfd, void *buf, size_t len, int flags, struct sockaddr *addr, socklen_t *addrlen) { - conditional_steal_blocking_call(recvfrom, sockfd, buf, len, flags, addr, addrlen); + conditional_steal_blocking_call(recvfrom, sockfd, POLLIN, buf, len, flags, addr, addrlen); +} + +ssize_t recvmsg(int sockfd, struct msghdr *msg, int flags) { + conditional_steal_blocking_call(recvmsg, sockfd, POLLIN, msg, flags); } ssize_t recv(int sockfd, void *buf, size_t len, int flags) { - conditional_steal_blocking_call(recv, sockfd, buf, len, flags); + conditional_steal_blocking_call(recv, sockfd, POLLIN, buf, len, flags); } ssize_t read(int sockfd, void *buf, size_t len) { - conditional_steal_blocking_call(read, sockfd, buf, len); + conditional_steal_blocking_call(read, sockfd, POLLIN, buf, len); +} + +int getaddrinfo(const char *node, const char *service, const struct addrinfo *hints, struct addrinfo **res) { + uint16_t port; + struct addrinfo *ai; + int ret; + struct in_addr ipv4; + char canon[256]; + fprintf(stderr, "wolfIP getaddrinfo: in_stack=%d node=%s service=%s\n", + in_the_stack, node ? node : "(null)", service ? service : "(null)"); + if (in_the_stack || !res) { + return host_getaddrinfo(node, service, hints, res); + } + if (!node) { + struct in_addr local_ip; + uint32_t ip_host; + if (hints && (hints->ai_family != AF_UNSPEC) && (hints->ai_family != AF_INET)) + return EAI_FAMILY; + ret = wolfip_parse_service(service, &port); + if (ret != 0) + return ret; + if (hints && (hints->ai_flags & AI_PASSIVE)) { + ip_host = 0; /* INADDR_ANY */ + canon[0] = '\0'; + } else { + inet_aton(WOLFIP_IP, &local_ip); + ip_host = ntohl(local_ip.s_addr); + wolfip_strlcpy(canon, WOLFIP_IP, sizeof(canon)); + } + ret = wolfip_build_addrinfo(ip_host, port, canon[0] ? canon : NULL, hints, &ai); + if (ret != 0) + return ret; + ret = wolfip_register_gai_alloc(ai); + if (ret != 0) + return ret; + *res = ai; + return 0; + } + ret = wolfip_parse_service(service, &port); + if (ret != 0) + return ret; + if (hints && (hints->ai_family != AF_UNSPEC) && (hints->ai_family != AF_INET)) + return EAI_FAMILY; + if (inet_pton(AF_INET, node, &ipv4) == 1) { + uint32_t ip_host = ntohl(ipv4.s_addr); + canon[0] = '\0'; + if (hints && (hints->ai_flags & AI_CANONNAME) && !(hints->ai_flags & AI_NUMERICHOST)) { + if (wolfip_dns_reverse_query(ip_host, canon, sizeof(canon)) != 0) + wolfip_strlcpy(canon, node, sizeof(canon)); + } else if (hints && (hints->ai_flags & AI_CANONNAME)) { + wolfip_strlcpy(canon, node, sizeof(canon)); + } + ret = wolfip_build_addrinfo(ip_host, port, canon[0] ? canon : NULL, hints, &ai); + if (ret != 0) + return ret; + ret = wolfip_register_gai_alloc(ai); + if (ret != 0) + return ret; + *res = ai; + return 0; + } else if (hints && (hints->ai_flags & AI_NUMERICHOST)) { + return EAI_NONAME; + } + { + uint32_t ip_host; + ret = wolfip_dns_forward_query(node, &ip_host); + if (ret != 0) + return ret; + if (hints && (hints->ai_flags & AI_CANONNAME)) + wolfip_strlcpy(canon, node, sizeof(canon)); + else + canon[0] = '\0'; + ret = wolfip_build_addrinfo(ip_host, port, + (hints && (hints->ai_flags & AI_CANONNAME)) ? canon : NULL, + hints, &ai); + if (ret != 0) + return ret; + ret = wolfip_register_gai_alloc(ai); + if (ret != 0) + return ret; + *res = ai; + return 0; + } +} + +void freeaddrinfo(struct addrinfo *res) { + fprintf(stderr, "wolfIP freeaddrinfo: in_stack=%d res=%p\n", in_the_stack, (void *)res); + if (!res) { + return; + } + if (wolfip_take_gai_alloc(res)) { + wolfip_free_addrinfo_list(res); + } else { + host_freeaddrinfo(res); + } } ssize_t sendto(int sockfd, const void *buf, size_t len, int flags, const struct sockaddr *addr, socklen_t addrlen) { - conditional_steal_blocking_call(sendto, sockfd, buf, len, flags, addr, addrlen); + conditional_steal_blocking_call(sendto, sockfd, POLLOUT, buf, len, flags, addr, addrlen); +} + +ssize_t sendmsg(int sockfd, const struct msghdr *msg, int flags) { + conditional_steal_blocking_call(sendmsg, sockfd, POLLOUT, msg, flags); } ssize_t send(int sockfd, const void *buf, size_t len, int flags) { - conditional_steal_blocking_call(send, sockfd, buf, len, flags); + conditional_steal_blocking_call(send, sockfd, POLLOUT, buf, len, flags); } ssize_t write(int sockfd, const void *buf, size_t len) { - conditional_steal_blocking_call(write, sockfd, buf, len); + conditional_steal_blocking_call(write, sockfd, POLLOUT, buf, len); } int poll(struct pollfd *fds, nfds_t nfds, int timeout) { @@ -555,16 +1380,25 @@ void __attribute__((constructor)) init_wolfip_posix() { struct in_addr host_stack_ip; struct wolfIP_ll_dev *tapdev; pthread_t wolfIP_thread; +#if WOLFIP_POSIX_TCPDUMP + static int tcpdump_atexit_registered; +#endif + const char *dbg_env = getenv("WOLFIP_DEBUG"); + wolfip_dbg_enabled = (dbg_env && dbg_env[0] && dbg_env[0] != '0') ? 1 : 0; if (IPSTACK) return; + printf("wolfIP: Serving process PID=%hu, TID=%x\n", getpid(), (unsigned short)pthread_self()); inet_aton(HOST_STACK_IP, &host_stack_ip); swap_socketcall(socket, "socket"); swap_socketcall(bind, "bind"); swap_socketcall(listen, "listen"); swap_socketcall(accept, "accept"); + swap_socketcall(accept4, "accept4"); swap_socketcall(connect, "connect"); swap_socketcall(sendto, "sendto"); + swap_socketcall(sendmsg, "sendmsg"); swap_socketcall(recvfrom, "recvfrom"); + swap_socketcall(recvmsg, "recvmsg"); swap_socketcall(recv, "recv"); swap_socketcall(send, "send"); swap_socketcall(close, "close"); @@ -572,8 +1406,10 @@ void __attribute__((constructor)) init_wolfip_posix() { swap_socketcall(read, "read"); swap_socketcall(getsockname, "getsockname"); swap_socketcall(getpeername, "getpeername"); - swap_socketcall(setsockopt, "getaddrinfo"); - swap_socketcall(getsockopt, "freeaddrinfo"); + swap_socketcall(setsockopt, "setsockopt"); + swap_socketcall(getsockopt, "getsockopt"); + swap_socketcall(getaddrinfo, "getaddrinfo"); + swap_socketcall(freeaddrinfo, "freeaddrinfo"); swap_socketcall(poll, "poll"); swap_socketcall(select, "select"); swap_socketcall(fcntl, "fcntl"); @@ -583,7 +1419,15 @@ void __attribute__((constructor)) init_wolfip_posix() { tapdev = wolfIP_getdev(IPSTACK); if (tap_init(tapdev, "wtcp0", host_stack_ip.s_addr) < 0) { perror("tap init"); + return; + } +#if WOLFIP_POSIX_TCPDUMP + if (!tcpdump_atexit_registered) { + atexit(wolfIP_stop_tcpdump_atexit); + tcpdump_atexit_registered = 1; } + wolfIP_start_tcpdump((tapdev && tapdev->ifname[0]) ? tapdev->ifname : "wtcp0"); +#endif wolfIP_ipconfig_set(IPSTACK, atoip4(WOLFIP_IP), atoip4("255.255.255.0"), atoip4(HOST_STACK_IP)); printf("IP: manually configured - %s\n", WOLFIP_IP); diff --git a/src/port/posix/tap_darwin.c b/src/port/posix/tap_darwin.c index 841dc80..968c8ee 100644 --- a/src/port/posix/tap_darwin.c +++ b/src/port/posix/tap_darwin.c @@ -43,8 +43,11 @@ #include #include +#include "config.h" +#define WOLF_POSIX #include "config.h" #include "wolfip.h" +#undef WOLF_POSIX #ifndef ETHERTYPE_IP #define ETHERTYPE_IP 0x0800 diff --git a/src/port/posix/tap_freebsd.c b/src/port/posix/tap_freebsd.c index a04ab6c..7ba9bfc 100644 --- a/src/port/posix/tap_freebsd.c +++ b/src/port/posix/tap_freebsd.c @@ -39,7 +39,10 @@ #include #include +#define WOLF_POSIX +#include "config.h" #include "wolfip.h" +#undef WOLF_POSIX static int tap_fd = -1; diff --git a/src/port/posix/tap_linux.c b/src/port/posix/tap_linux.c index 00ecb50..d16858b 100644 --- a/src/port/posix/tap_linux.c +++ b/src/port/posix/tap_linux.c @@ -28,7 +28,10 @@ #include #include #include +#define WOLF_POSIX +#include "config.h" #include "wolfip.h" +#undef WOLF_POSIX #include #include @@ -76,18 +79,31 @@ int tap_init(struct wolfIP_ll_dev *ll, const char *ifname, uint32_t host_ip) struct sockaddr_in *addr; int sock_fd; - if ((tap_fd = open("/dev/net/tun", O_RDWR)) < 0) { - perror("accessing /dev/net/tun"); - return -1; - } memset(&ifr, 0, sizeof(ifr)); ifr.ifr_flags = IFF_TAP | IFF_NO_PI; strncpy(ifr.ifr_name, ifname, IFNAMSIZ); ifr.ifr_name[IFNAMSIZ-1] = '\0'; - if (ioctl(tap_fd, TUNSETIFF, (void *)&ifr) < 0) { + + tap_fd = open("/dev/net/tun", O_RDWR); + if (tap_fd >= 0 && ioctl(tap_fd, TUNSETIFF, (void *)&ifr) == 0) { + /* created successfully */ + } else { perror("ioctl TUNSETIFF"); - close(tap_fd); - return -1; + if (tap_fd >= 0) { + close(tap_fd); + tap_fd = -1; + } + /* try to reuse existing device */ + tap_fd = open("/dev/net/tun", O_RDWR); + if (tap_fd >= 0) { + if (ioctl(tap_fd, TUNSETIFF, (void *)&ifr) != 0) { + close(tap_fd); + tap_fd = -1; + } + } + if (tap_fd < 0) { + return -1; + } } /* Get mac address */ if (ioctl(tap_fd, SIOCGIFHWADDR, &ifr) < 0) { diff --git a/src/test/unit/unit.c b/src/test/unit/unit.c index 80d3a21..91cda68 100644 --- a/src/test/unit/unit.c +++ b/src/test/unit/unit.c @@ -946,7 +946,8 @@ START_TEST(test_wolfip_forwarding_ttl_expired) ck_assert_uint_eq(ee32(icmp->ip.dst), ee32(frame.src)); ck_assert_mem_eq(icmp->orig_packet, ((uint8_t *)&frame) + ETH_HEADER_LEN, - TTL_EXCEEDED_ORIG_PACKET_SIZE); + ee16(frame.len) < TTL_EXCEEDED_ORIG_PACKET_SIZE ? + ee16(frame.len) : TTL_EXCEEDED_ORIG_PACKET_SIZE); ck_assert_uint_eq(frame.ttl, 1); /* original packet should remain unchanged */ } END_TEST @@ -998,7 +999,7 @@ START_TEST(test_tcp_listen_rejects_wrong_interface) listen_fd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_STREAM, 0); ck_assert_int_ge(listen_fd, 0); - listener = &s.tcpsockets[listen_fd & ~MARK_TCP_SOCKET]; + listener = &s.tcpsockets[SOCKET_UNMARK(listen_fd)]; memset(&addr, 0, sizeof(addr)); addr.sin_family = AF_INET; @@ -1030,7 +1031,7 @@ START_TEST(test_tcp_listen_accepts_bound_interface) listen_fd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_STREAM, 0); ck_assert_int_ge(listen_fd, 0); - listener = &s.tcpsockets[listen_fd & ~MARK_TCP_SOCKET]; + listener = &s.tcpsockets[SOCKET_UNMARK(listen_fd)]; memset(&addr, 0, sizeof(addr)); addr.sin_family = AF_INET; @@ -1047,7 +1048,7 @@ START_TEST(test_tcp_listen_accepts_bound_interface) client_fd = wolfIP_sock_accept(&s, listen_fd, NULL, NULL); ck_assert_int_ge(client_fd, 0); - client = &s.tcpsockets[client_fd & ~MARK_TCP_SOCKET]; + client = &s.tcpsockets[SOCKET_UNMARK(client_fd)]; ck_assert_uint_eq(client->local_ip, secondary_ip); ck_assert_uint_eq(client->bound_local_ip, secondary_ip); ck_assert_int_eq(client->sock.tcp.state, TCP_ESTABLISHED); @@ -1070,7 +1071,7 @@ START_TEST(test_tcp_listen_accepts_any_interface) listen_fd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_STREAM, 0); ck_assert_int_ge(listen_fd, 0); - listener = &s.tcpsockets[listen_fd & ~MARK_TCP_SOCKET]; + listener = &s.tcpsockets[SOCKET_UNMARK(listen_fd)]; memset(&addr, 0, sizeof(addr)); addr.sin_family = AF_INET; @@ -1087,7 +1088,7 @@ START_TEST(test_tcp_listen_accepts_any_interface) client_fd = wolfIP_sock_accept(&s, listen_fd, NULL, NULL); ck_assert_int_ge(client_fd, 0); - client = &s.tcpsockets[client_fd & ~MARK_TCP_SOCKET]; + client = &s.tcpsockets[SOCKET_UNMARK(client_fd)]; ck_assert_uint_eq(client->local_ip, secondary_ip); ck_assert_int_eq(client->sock.tcp.state, TCP_ESTABLISHED); } @@ -1115,7 +1116,7 @@ START_TEST(test_sock_connect_selects_local_ip_multi_if) dst.sin_port = ee16(5555); dst.sin_addr.s_addr = ee32(remote_secondary); ck_assert_int_eq(wolfIP_sock_connect(&s, udp_fd, (struct wolfIP_sockaddr *)&dst, sizeof(dst)), 0); - ts = &s.udpsockets[udp_fd & ~MARK_UDP_SOCKET]; + ts = &s.udpsockets[SOCKET_UNMARK(udp_fd)]; ck_assert_uint_eq(ts->local_ip, secondary_ip); ck_assert_uint_eq(ts->if_idx, TEST_SECOND_IF); @@ -1127,7 +1128,7 @@ START_TEST(test_sock_connect_selects_local_ip_multi_if) dst.sin_addr.s_addr = ee32(remote_primary); ret = wolfIP_sock_connect(&s, tcp_fd, (struct wolfIP_sockaddr *)&dst, sizeof(dst)); ck_assert_int_eq(ret, -WOLFIP_EAGAIN); - ts = &s.tcpsockets[tcp_fd & ~MARK_TCP_SOCKET]; + ts = &s.tcpsockets[SOCKET_UNMARK(tcp_fd)]; ck_assert_uint_eq(ts->local_ip, primary_ip); ck_assert_uint_eq(ts->if_idx, TEST_PRIMARY_IF); } @@ -1238,6 +1239,108 @@ START_TEST(test_ip_output_add_header) { } END_TEST +START_TEST(test_icmp_socket_send_recv) +{ + struct wolfIP s; + int sd; + struct wolfIP_sockaddr_in sin; + uint8_t payload[ICMP_HEADER_LEN + 4]; + uint32_t local_ip = 0x0A000001U; + uint32_t remote_ip = 0x0A000002U; + struct tsocket *ts; + struct wolfIP_icmp_packet *sent_pkt; + uint8_t reply_buf[sizeof(struct wolfIP_icmp_packet) + sizeof(payload)]; + struct wolfIP_icmp_packet *reply = (struct wolfIP_icmp_packet *)reply_buf; + uint8_t peer_mac[6] = {0x00, 0xaa, 0xbb, 0xcc, 0xdd, 0x01}; + uint8_t rxbuf[sizeof(payload)]; + struct wolfIP_sockaddr_in from; + socklen_t from_len = sizeof(from); + int ret; + + wolfIP_init(&s); + mock_link_init(&s); + wolfIP_ipconfig_set(&s, local_ip, 0xFFFFFF00U, 0); + s.arp.neighbors[0].ip = remote_ip; + s.arp.neighbors[0].if_idx = TEST_PRIMARY_IF; + memcpy(s.arp.neighbors[0].mac, peer_mac, sizeof(peer_mac)); + + sd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_DGRAM, WI_IPPROTO_ICMP); + ck_assert_int_gt(sd, 0); + ts = &s.icmpsockets[SOCKET_UNMARK(sd)]; + ck_assert_uint_gt(fifo_space(&ts->sock.udp.txbuf), (uint32_t)sizeof(payload)); + fifo_init(&ts->sock.udp.txbuf, ts->txmem, TXBUF_SIZE); + fifo_init(&ts->sock.udp.rxbuf, ts->rxmem, RXBUF_SIZE); + + memset(&sin, 0, sizeof(sin)); + sin.sin_family = AF_INET; + sin.sin_addr.s_addr = ee32(remote_ip); + + memset(payload, 0, sizeof(payload)); + payload[0] = ICMP_ECHO_REQUEST; + payload[1] = 0; + payload[ICMP_HEADER_LEN] = 'P'; + payload[ICMP_HEADER_LEN + 1] = 'I'; + payload[ICMP_HEADER_LEN + 2] = 'N'; + payload[ICMP_HEADER_LEN + 3] = 'G'; + + ret = wolfIP_sock_sendto(&s, sd, payload, sizeof(payload), 0, + (struct wolfIP_sockaddr *)&sin, sizeof(sin)); + ck_assert_int_eq(ret, (int)sizeof(payload)); + ts = &s.icmpsockets[SOCKET_UNMARK(sd)]; + ck_assert_uint_eq(ts->if_idx, TEST_PRIMARY_IF); + + { + struct pkt_desc *desc = fifo_peek(&ts->sock.udp.txbuf); + ck_assert_ptr_nonnull(desc); + sent_pkt = (struct wolfIP_icmp_packet *)(ts->txmem + desc->pos + sizeof(*desc)); + ck_assert_uint_eq(sent_pkt->type, ICMP_ECHO_REQUEST); + ck_assert_uint_eq(icmp_echo_id(sent_pkt), ts->src_port); + fifo_pop(&ts->sock.udp.txbuf); + } + + memset(reply, 0, sizeof(reply_buf)); + memcpy(reply->ip.eth.dst, s.ll_dev[TEST_PRIMARY_IF].mac, 6); + memcpy(reply->ip.eth.src, peer_mac, 6); + reply->ip.eth.type = ee16(ETH_TYPE_IP); + reply->ip.ver_ihl = 0x45; + reply->ip.ttl = 64; + reply->ip.proto = WI_IPPROTO_ICMP; + reply->ip.len = ee16(IP_HEADER_LEN + sizeof(payload)); + reply->ip.src = ee32(remote_ip); + reply->ip.dst = ee32(local_ip); + reply->type = ICMP_ECHO_REPLY; + reply->code = 0; + icmp_set_echo_id(reply, ts->src_port); + { + uint16_t seq = ee16(1); + memcpy(reply->unused + sizeof(uint16_t), &seq, sizeof(seq)); + } + memcpy(((uint8_t *)&reply->type) + ICMP_HEADER_LEN, + payload + ICMP_HEADER_LEN, + sizeof(payload) - ICMP_HEADER_LEN); + reply->csum = 0; + reply->csum = ee16(icmp_checksum(reply, sizeof(payload))); + reply->ip.csum = 0; + iphdr_set_checksum(&reply->ip); + + wolfIP_recv(&s, reply, ETH_HEADER_LEN + IP_HEADER_LEN + sizeof(payload)); + + memset(&from, 0, sizeof(from)); + ret = wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, + (struct wolfIP_sockaddr *)&from, &from_len); + ck_assert_int_eq(ret, (int)sizeof(payload)); + ck_assert_mem_eq(rxbuf, &reply->type, sizeof(payload)); + ck_assert_uint_eq(from.sin_addr.s_addr, ee32(remote_ip)); + ck_assert_uint_eq(from_len, sizeof(from)); + + /* Ensure the packet was removed from the queue */ + memset(rxbuf, 0, sizeof(rxbuf)); + ret = wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, + NULL, NULL); + ck_assert_int_eq(ret, -WOLFIP_EAGAIN); +} +END_TEST + Suite *wolf_suite(void) @@ -1342,6 +1445,7 @@ Suite *wolf_suite(void) tcase_add_test(tc_proto, test_tcp_listen_accepts_any_interface); suite_add_tcase(s, tc_proto); tcase_add_test(tc_proto, test_sock_connect_selects_local_ip_multi_if); + tcase_add_test(tc_proto, test_icmp_socket_send_recv); suite_add_tcase(s, tc_proto); tcase_add_test(tc_utils, test_transport_checksum); diff --git a/src/wolfip.c b/src/wolfip.c index e691b57..44a68d4 100644 --- a/src/wolfip.c +++ b/src/wolfip.c @@ -22,6 +22,12 @@ #include #include #include +#include +#ifdef WOLF_POSIX +#include +#include +#include +#endif #include "wolfip.h" #include "config.h" @@ -439,6 +445,20 @@ struct PACKED wolfIP_icmp_ttl_exceeded_packet { uint8_t orig_packet[TTL_EXCEEDED_ORIG_PACKET_SIZE]; }; +static uint16_t icmp_echo_id(const struct wolfIP_icmp_packet *icmp) +{ + uint16_t net = 0; + memcpy(&net, icmp->unused, sizeof(net)); + return ee16(net); +} + +static void icmp_set_echo_id(struct wolfIP_icmp_packet *icmp, uint16_t id) +{ + uint16_t net = ee16(id); + memcpy(icmp->unused, &net, sizeof(net)); +} + + #if CONFIG_IPFILTER static wolfIP_filter_cb wolfip_filter_cb; static void *wolfip_filter_arg; @@ -740,6 +760,8 @@ struct tsocket { uint8_t nexthop_mac[6]; #endif uint8_t if_idx; + uint8_t recv_ttl; + uint8_t last_pkt_ttl; uint8_t rxmem[RXBUF_SIZE]; uint8_t txmem[TXBUF_SIZE]; void (*callback)(int sock_fd, uint16_t events, void *arg); @@ -813,10 +835,14 @@ struct wolfIP ip4 dns_server; uint16_t dns_id; int dns_udp_sd; + uint8_t dns_query_type; void (*dns_lookup_cb)(ip4 ip); + void (*dns_ptr_cb)(const char *name); + char dns_ptr_name[256]; struct timers_binheap timers; struct tsocket tcpsockets[MAX_TCPSOCKETS]; struct tsocket udpsockets[MAX_UDPSOCKETS]; + struct tsocket icmpsockets[MAX_ICMPSOCKETS]; uint16_t ipcounter; uint64_t last_tick; #ifdef ETHERNET @@ -989,6 +1015,10 @@ static int wolfIP_filter_notify_socket_event( meta.ip_proto = WOLFIP_FILTER_PROTO_UDP; meta.l4.udp.src_port = ee16(local_port); meta.l4.udp.dst_port = ee16(remote_port); + } else if (ts->proto == WI_IPPROTO_ICMP) { + meta.ip_proto = WOLFIP_FILTER_PROTO_ICMP; + meta.l4.icmp.type = 0; + meta.l4.icmp.code = 0; } else meta.ip_proto = 0; } @@ -1083,16 +1113,22 @@ void wolfIP_register_callback(struct wolfIP *s, int sock_fd, void (*cb)(int sock struct tsocket *t; if (sock_fd < 0) return; - if (sock_fd & MARK_TCP_SOCKET) { - if ((sock_fd & (~MARK_TCP_SOCKET)) >= MAX_TCPSOCKETS) + if (IS_SOCKET_TCP(sock_fd)) { + if (SOCKET_UNMARK(sock_fd) >= MAX_TCPSOCKETS) return; - t = &s->tcpsockets[sock_fd & ~MARK_TCP_SOCKET]; + t = &s->tcpsockets[SOCKET_UNMARK(sock_fd)]; t->callback = cb; t->callback_arg = arg; - } else if (sock_fd & MARK_UDP_SOCKET) { - if ((sock_fd &(~MARK_UDP_SOCKET)) >= MAX_UDPSOCKETS) + } else if (IS_SOCKET_UDP(sock_fd)) { + if (SOCKET_UNMARK(sock_fd) >= MAX_UDPSOCKETS) return; - t = &s->udpsockets[sock_fd & ~MARK_UDP_SOCKET]; + t = &s->udpsockets[SOCKET_UNMARK(sock_fd)]; + t->callback = cb; + t->callback_arg = arg; + } else if (IS_SOCKET_ICMP(sock_fd)) { + if (SOCKET_UNMARK(sock_fd) >= MAX_ICMPSOCKETS) + return; + t = &s->icmpsockets[SOCKET_UNMARK(sock_fd)]; t->callback = cb; t->callback_arg = arg; } @@ -1217,6 +1253,53 @@ static void udp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ud } } +/* ICMP sockets reuse the UDP fifo bookkeeping */ +static struct tsocket *icmp_new_socket(struct wolfIP *s) +{ + struct tsocket *t; + int i; + + for (i = 0; i < MAX_ICMPSOCKETS; i++) { + t = &s->icmpsockets[i]; + if (t->proto == 0) { + t->proto = WI_IPPROTO_ICMP; + t->S = s; + t->if_idx = 0; + fifo_init(&t->sock.udp.rxbuf, t->rxmem, RXBUF_SIZE); + fifo_init(&t->sock.udp.txbuf, t->txmem, TXBUF_SIZE); + t->events |= CB_EVENT_WRITABLE; + return t; + } + } + return NULL; +} + +static void icmp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_icmp_packet *icmp, uint32_t frame_len) +{ + int i; + ip4 src_ip = ee32(icmp->ip.src); + ip4 dst_ip = ee32(icmp->ip.dst); + uint16_t echo_id = icmp_echo_id(icmp); + (void)if_idx; + + for (i = 0; i < MAX_ICMPSOCKETS; i++) { + struct tsocket *t = &s->icmpsockets[i]; + if (t->proto != WI_IPPROTO_ICMP) + continue; + if (t->local_ip != 0 && t->local_ip != dst_ip) + continue; + if (t->src_port != 0 && t->src_port != echo_id) + continue; + if (t->remote_ip != 0 && t->remote_ip != src_ip) + continue; + if ((int)frame_len != ee16(icmp->ip.len) + ETH_HEADER_LEN) + continue; + fifo_push(&t->sock.udp.rxbuf, icmp, frame_len); + t->last_pkt_ttl = icmp->ip.ttl; + t->events |= CB_EVENT_READABLE; + } +} + /* TCP */ static struct tsocket *tcp_new_socket(struct wolfIP *s) { @@ -1313,11 +1396,6 @@ static void tcp_send_syn(struct tsocket *t, uint8_t flags) TCP_OPTIONS_LEN + TCP_OPTION_MSS_LEN); } -static void tcp_send_synack(struct tsocket *t) -{ - return tcp_send_syn(t, 0x12); -} - /* Add a segment to the rx buffer for the application to consume */ static void tcp_recv(struct tsocket *t, struct wolfIP_tcp_seg *seg) { @@ -1513,6 +1591,10 @@ static int ip_output_add_header(struct tsocket *t, struct wolfIP_ip_packet *ip, struct wolfIP_udp_datagram *udp = (struct wolfIP_udp_datagram *)ip; udp->csum = 0; udp->csum = ee16(transport_checksum(&ph, &udp->src_port)); + } else if (proto == WI_IPPROTO_ICMP) { + struct wolfIP_icmp_packet *icmp = (struct wolfIP_icmp_packet *)ip; + icmp->csum = 0; + icmp->csum = ee16(icmp_checksum(icmp, ee16(ph.ph.len))); } #ifdef ETHERNET if_idx = wolfIP_socket_if_idx(t); @@ -1828,23 +1910,51 @@ static void close_socket(struct tsocket *ts) memset(ts, 0, sizeof(struct tsocket)); } +static struct tsocket *wolfIP_socket_from_fd(struct wolfIP *s, int sockfd) +{ + if (!s || sockfd < 0) + return NULL; + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) + return NULL; + return &s->tcpsockets[SOCKET_UNMARK(sockfd)]; + } else if (IS_SOCKET_UDP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) + return NULL; + return &s->udpsockets[SOCKET_UNMARK(sockfd)]; + } else if (IS_SOCKET_ICMP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return NULL; + return &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + } + return NULL; +} + int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol) { struct tsocket *ts; if (domain != AF_INET) return -1; - (void)protocol; if (type == IPSTACK_SOCK_STREAM) { ts = tcp_new_socket(s); if (!ts) return -1; return (ts - s->tcpsockets) | MARK_TCP_SOCKET; } else if (type == IPSTACK_SOCK_DGRAM) { - ts = udp_new_socket(s); - if (!ts) + if (protocol == 0 || protocol == WI_IPPROTO_UDP) { + ts = udp_new_socket(s); + if (!ts) + return -1; + return (ts - s->udpsockets) | MARK_UDP_SOCKET; + } else if (protocol == WI_IPPROTO_ICMP) { + ts = icmp_new_socket(s); + if (!ts) + return -1; + return (ts - s->icmpsockets) | MARK_ICMP_SOCKET; + } else { return -1; - return (ts - s->udpsockets) | MARK_UDP_SOCKET; + } } return -1; } @@ -1857,12 +1967,12 @@ int wolfIP_sock_connect(struct wolfIP *s, int sockfd, const struct wolfIP_sockad if ((!addr)|| (sockfd < 0)) return -WOLFIP_EINVAL; sin = (const struct wolfIP_sockaddr_in *)addr; - if (sockfd & MARK_UDP_SOCKET) { + if (IS_SOCKET_UDP(sockfd)) { struct ipconf *conf; - if ((sockfd & ~MARK_UDP_SOCKET) >= MAX_UDPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->udpsockets[sockfd & ~MARK_UDP_SOCKET]; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; ts->dst_port = ee16(sin->sin_port); ts->remote_ip = ee32(sin->sin_addr.s_addr); if (ts->bound_local_ip != IPADDR_ANY) { @@ -1884,14 +1994,34 @@ int wolfIP_sock_connect(struct wolfIP *s, int sockfd, const struct wolfIP_sockad } } return 0; + } else if (IS_SOCKET_ICMP(sockfd)) { + struct ipconf *conf; + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) + return -WOLFIP_EINVAL; + ts->remote_ip = ee32(sin->sin_addr.s_addr); + if_idx = wolfIP_route_for_ip(s, ts->remote_ip); + conf = wolfIP_ipconf_at(s, if_idx); + ts->if_idx = (uint8_t)if_idx; + if (ts->local_ip == 0 && conf && conf->ip != IPADDR_ANY) + ts->local_ip = conf->ip; + else if (ts->local_ip == 0) { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + ts->local_ip = primary->ip; + } + return 0; } - if ((sockfd & MARK_TCP_SOCKET) == 0) + if (!IS_SOCKET_TCP(sockfd)) return -WOLFIP_EINVAL; - if ((sockfd & ~MARK_TCP_SOCKET)>= MAX_TCPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (ts->sock.tcp.state == TCP_ESTABLISHED) return 0; if (ts->sock.tcp.state == TCP_SYN_SENT) @@ -1955,18 +2085,14 @@ int wolfIP_sock_accept(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *add if (addr && addrlen) *addrlen = sizeof(struct wolfIP_sockaddr_in); - if (sockfd & MARK_TCP_SOCKET) { - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if ((ts->sock.tcp.state != TCP_SYN_RCVD) && (ts->sock.tcp.state != TCP_LISTEN)) return -1; if (ts->sock.tcp.state == TCP_SYN_RCVD) { - ip4 conn_local = ts->local_ip; - uint8_t conn_if = ts->if_idx; - - tcp_send_synack(ts); newts = tcp_new_socket(s); if (!newts) return -1; @@ -1974,15 +2100,23 @@ int wolfIP_sock_accept(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *add newts->events |= CB_EVENT_WRITABLE; newts->callback = ts->callback; newts->callback_arg = ts->callback_arg; - newts->local_ip = conn_local; - newts->bound_local_ip = conn_local; - newts->if_idx = conn_if; + newts->local_ip = ts->local_ip; + newts->bound_local_ip = (ts->bound_local_ip != IPADDR_ANY) ? ts->bound_local_ip : ts->local_ip; + newts->if_idx = ts->if_idx; newts->remote_ip = ts->remote_ip; newts->src_port = ts->src_port; newts->dst_port = ts->dst_port; newts->sock.tcp.ack = ts->sock.tcp.ack; - newts->sock.tcp.seq = ts->sock.tcp.seq + 1; + newts->sock.tcp.seq = ts->sock.tcp.seq; + newts->sock.tcp.last_ts = ts->sock.tcp.last_ts; newts->sock.tcp.state = TCP_ESTABLISHED; + /* Send SYN-ACK to accept connection. + * Send the syn-ack from the newly established socket: + * the caller could still close the listening socket + * while we're still accepting. + */ + tcp_send_syn(newts, 0x12); + newts->sock.tcp.seq++; if (sin) { sin->sin_family = AF_INET; sin->sin_port = ee16(ts->dst_port); @@ -1995,9 +2129,6 @@ int wolfIP_sock_accept(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *add unsigned int bound_if = wolfIP_if_for_local_ip(s, ts->bound_local_ip, &bound_match); ts->if_idx = bound_match ? (uint8_t)bound_if : ts->if_idx; ts->local_ip = ts->bound_local_ip; - } else { - ts->local_ip = conn_local; - ts->if_idx = conn_if; } if (wolfIP_filter_notify_socket_event( WOLFIP_FILT_ACCEPTING, s, newts, @@ -2020,8 +2151,10 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len struct tsocket *ts; struct wolfIP_tcp_seg *tcp; struct wolfIP_udp_datagram *udp; + struct wolfIP_icmp_packet *icmp; tcp = (struct wolfIP_tcp_seg *)frame; udp = (struct wolfIP_udp_datagram *)frame; + icmp = (struct wolfIP_icmp_packet *)frame; (void)flags; if (sockfd < 0) @@ -2030,13 +2163,13 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len if ((!buf) || (len == 0)) return -1; - if (sockfd & MARK_TCP_SOCKET) { + if (IS_SOCKET_TCP(sockfd)) { size_t sent = 0; struct tcp_opt_ts *tsopt = (struct tcp_opt_ts *)tcp->data; - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (ts->sock.tcp.state != TCP_ESTABLISHED) return -1; @@ -2072,14 +2205,14 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len return -WOLFIP_EAGAIN; else return sent; - } else if (sockfd & MARK_UDP_SOCKET) { + } else if (IS_SOCKET_UDP(sockfd)) { const struct wolfIP_sockaddr_in *sin = (const struct wolfIP_sockaddr_in *)dest_addr; unsigned int if_idx; struct ipconf *conf; - if ((sockfd & ~MARK_UDP_SOCKET) >= MAX_UDPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->udpsockets[sockfd & ~MARK_UDP_SOCKET]; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; if ((ts->dst_port == 0) && (dest_addr == NULL)) return -1; memset(udp, 0, sizeof(struct wolfIP_udp_datagram)); @@ -2120,6 +2253,49 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len memcpy(udp->data, buf, len); fifo_push(&ts->sock.udp.txbuf, udp, sizeof(struct wolfIP_udp_datagram) + len); return len; + } else if (IS_SOCKET_ICMP(sockfd)) { + const struct wolfIP_sockaddr_in *sin = (const struct wolfIP_sockaddr_in *)dest_addr; + unsigned int if_idx; + struct ipconf *conf; + uint32_t payload_len = (uint32_t)len; + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + if (sin) { + if (addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -1; + ts->remote_ip = ee32(sin->sin_addr.s_addr); + } + if (ts->remote_ip == 0) + return -1; + if (payload_len < ICMP_HEADER_LEN || payload_len > (WI_IP_MTU - IP_HEADER_LEN)) + return -WOLFIP_EINVAL; + if (fifo_space(&ts->sock.udp.txbuf) < payload_len) + return -WOLFIP_EAGAIN; + if (ts->src_port == 0) { + ts->src_port = (uint16_t)(wolfIP_getrandom() & 0xFFFF); + if (ts->src_port == 0) + ts->src_port = 1; + } + if_idx = wolfIP_route_for_ip(s, ts->remote_ip); + conf = wolfIP_ipconf_at(s, if_idx); + ts->if_idx = (uint8_t)if_idx; + if (ts->local_ip == 0) { + if (conf && conf->ip != IPADDR_ANY) + ts->local_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + ts->local_ip = primary->ip; + } + } + memcpy(&icmp->type, buf, payload_len); + if (icmp->type == ICMP_ECHO_REQUEST) + icmp_set_echo_id(icmp, ts->src_port); + icmp->csum = 0; + icmp->csum = ee16(icmp_checksum(icmp, (uint16_t)payload_len)); + fifo_push(&ts->sock.udp.txbuf, icmp, sizeof(struct wolfIP_ip_packet) + payload_len); + return (int)payload_len; } else return -1; } @@ -2139,16 +2315,17 @@ int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, in uint32_t seg_len; struct pkt_desc *desc; struct wolfIP_udp_datagram *udp; + struct wolfIP_icmp_packet *icmp; struct tsocket *ts; (void)flags; if (sockfd < 0) return -WOLFIP_EINVAL; - if (sockfd & MARK_TCP_SOCKET) { - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (ts->sock.tcp.state == TCP_CLOSE_WAIT) { /* In close-wait, return 0 if the queue is empty */ @@ -2163,11 +2340,11 @@ int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, in } else { /* Not established */ return -1; } - } else if (sockfd & MARK_UDP_SOCKET) { + } else if (IS_SOCKET_UDP(sockfd)) { struct wolfIP_sockaddr_in *sin = (struct wolfIP_sockaddr_in *)src_addr; - if ((sockfd & ~MARK_UDP_SOCKET) >= MAX_UDPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->udpsockets[sockfd & ~MARK_UDP_SOCKET]; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; if (sin && *addrlen < sizeof(struct wolfIP_sockaddr_in)) return -1; if (addrlen) *addrlen = sizeof(struct wolfIP_sockaddr_in); @@ -2186,6 +2363,31 @@ int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, in memcpy(buf, udp->data, seg_len); fifo_pop(&ts->sock.udp.rxbuf); return seg_len; + } else if (IS_SOCKET_ICMP(sockfd)) { + struct wolfIP_sockaddr_in *sin = (struct wolfIP_sockaddr_in *)src_addr; + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + if (sin && *addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -1; + if (addrlen) + *addrlen = sizeof(struct wolfIP_sockaddr_in); + desc = fifo_peek(&ts->sock.udp.rxbuf); + if (!desc) + return -WOLFIP_EAGAIN; + icmp = (struct wolfIP_icmp_packet *)(ts->rxmem + desc->pos + sizeof(*desc)); + seg_len = ee16(icmp->ip.len) - IP_HEADER_LEN; + if (seg_len > len) + return -1; + if (sin) { + sin->sin_family = AF_INET; + sin->sin_port = 0; + sin->sin_addr.s_addr = icmp->ip.src; + } + memcpy(buf, &icmp->type, seg_len); + fifo_pop(&ts->sock.udp.rxbuf); + ts->events &= ~CB_EVENT_READABLE; + return (int)seg_len; } else return -WOLFIP_EINVAL; } @@ -2200,15 +2402,59 @@ int wolfIP_sock_read(struct wolfIP *s, int sockfd, void *buf, size_t len) return wolfIP_sock_recvfrom(s, sockfd, buf, len, 0, NULL, 0); } +int wolfIP_sock_setsockopt(struct wolfIP *s, int sockfd, int level, int optname, const void *optval, socklen_t optlen) +{ + struct tsocket *ts = wolfIP_socket_from_fd(s, sockfd); + if (!ts) + return -WOLFIP_EINVAL; + if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { + int enable; + if (!optval || optlen < (socklen_t)sizeof(int)) + return -WOLFIP_EINVAL; + memcpy(&enable, optval, sizeof(int)); + ts->recv_ttl = enable ? 1 : 0; + return 0; + } + return 0; +} + +int wolfIP_sock_get_recv_ttl(struct wolfIP *s, int sockfd, int *ttl) +{ + struct tsocket *ts = wolfIP_socket_from_fd(s, sockfd); + if (!ts) + return -WOLFIP_EINVAL; + if (!ts->recv_ttl) + return 0; + if (ttl) + *ttl = ts->last_pkt_ttl; + return 1; +} + +int wolfIP_sock_getsockopt(struct wolfIP *s, int sockfd, int level, int optname, void *optval, socklen_t *optlen) +{ + struct tsocket *ts = wolfIP_socket_from_fd(s, sockfd); + if (!ts) + return -WOLFIP_EINVAL; + if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { + int value; + if (!optval || !optlen || *optlen < (socklen_t)sizeof(int)) + return -WOLFIP_EINVAL; + value = ts->recv_ttl ? ts->last_pkt_ttl : 0; + memcpy(optval, &value, sizeof(int)); + *optlen = sizeof(int); + return 0; + } + return 0; +} int wolfIP_sock_close(struct wolfIP *s, int sockfd) { if (sockfd < 0) return -WOLFIP_EINVAL; - if (sockfd & MARK_TCP_SOCKET) { + if (IS_SOCKET_TCP(sockfd)) { struct tsocket *ts; - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (ts->sock.tcp.state == TCP_ESTABLISHED) { ts->sock.tcp.state = TCP_FIN_WAIT_1; tcp_send_finack(ts); @@ -2241,16 +2487,26 @@ int wolfIP_sock_close(struct wolfIP *s, int sockfd) close_socket(ts); return 0; } else return -1; - } else if (sockfd & MARK_UDP_SOCKET) { + } else if (IS_SOCKET_UDP(sockfd)) { struct tsocket *ts; - if ((sockfd & ~MARK_UDP_SOCKET) >= MAX_UDPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->udpsockets[sockfd & ~MARK_UDP_SOCKET]; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; (void)wolfIP_filter_notify_socket_event( WOLFIP_FILT_DISSOCIATE, s, ts, ts->local_ip, ts->src_port, ts->remote_ip, ts->dst_port); close_socket(ts); return 0; + } else if (IS_SOCKET_ICMP(sockfd)) { + struct tsocket *ts; + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + (void)wolfIP_filter_notify_socket_event( + WOLFIP_FILT_DISSOCIATE, s, ts, + ts->local_ip, ts->src_port, ts->remote_ip, 0); + close_socket(ts); + return 0; } else return -1; return 0; } @@ -2263,19 +2519,36 @@ int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr if ((!addr) || (sockfd < 0)) return -WOLFIP_EINVAL; - if ((sockfd & MARK_TCP_SOCKET) == 0) - return -1; - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) - return -WOLFIP_EINVAL; - - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; sin = (struct wolfIP_sockaddr_in *)addr; - if (!sin || *addrlen < sizeof(struct wolfIP_sockaddr_in)) + if (!sin || (addrlen && *addrlen < sizeof(struct wolfIP_sockaddr_in))) return -1; - sin->sin_family = AF_INET; - sin->sin_port = ts->src_port; - sin->sin_addr.s_addr = ts->local_ip; - return 0; + + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; + sin->sin_family = AF_INET; + sin->sin_port = ts->src_port; + sin->sin_addr.s_addr = ts->local_ip; + return 0; + } else if (IS_SOCKET_UDP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; + sin->sin_family = AF_INET; + sin->sin_port = ee16(ts->src_port); + sin->sin_addr.s_addr = ee32(ts->local_ip); + return 0; + } else if (IS_SOCKET_ICMP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + sin->sin_family = AF_INET; + sin->sin_port = ee16(ts->src_port); + sin->sin_addr.s_addr = ee32(ts->local_ip); + return 0; + } + return -1; } int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, socklen_t addrlen) @@ -2299,10 +2572,10 @@ int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr if ((bind_ip != IPADDR_ANY) && !match) return -1; - if (sockfd & MARK_TCP_SOCKET) { - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (ts->sock.tcp.state != TCP_CLOSED) return -1; if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) @@ -2339,10 +2612,10 @@ int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr ts->bound_local_ip = ts->local_ip; ts->src_port = ee16(sin->sin_port); return 0; - } else if (sockfd & MARK_UDP_SOCKET) { - if ((sockfd & ~MARK_UDP_SOCKET) >= MAX_UDPSOCKETS) + } else if (IS_SOCKET_UDP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_UDPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->udpsockets[sockfd & ~MARK_UDP_SOCKET]; + ts = &s->udpsockets[SOCKET_UNMARK(sockfd)]; if (ts->src_port != 0) return -1; if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) @@ -2373,6 +2646,70 @@ int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr ts->bound_local_ip = ts->local_ip; ts->src_port = ee16(sin->sin_port); return 0; + } else if (IS_SOCKET_ICMP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + if (ts->src_port != 0) + return -1; + if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) + return -1; + { + ip4 prev_ip = ts->local_ip; + uint16_t prev_id = ts->src_port; + uint16_t new_id = ee16(sin->sin_port); + ts->if_idx = (uint8_t)if_idx; + if (bind_ip != IPADDR_ANY) + ts->local_ip = bind_ip; + else if (conf && conf->ip != IPADDR_ANY) + ts->local_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + ts->local_ip = primary->ip; + } + ts->src_port = new_id; + if (wolfIP_filter_notify_socket_event( + WOLFIP_FILT_BINDING, s, ts, + ts->local_ip, new_id, IPADDR_ANY, 0) != 0) { + ts->local_ip = prev_ip; + ts->src_port = prev_id; + return -1; + } + } + return 0; + } else if (IS_SOCKET_ICMP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_ICMPSOCKETS) + return -WOLFIP_EINVAL; + ts = &s->icmpsockets[SOCKET_UNMARK(sockfd)]; + if (ts->src_port != 0) + return -1; + if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) + return -1; + { + ip4 prev_ip = ts->local_ip; + uint16_t prev_id = ts->src_port; + uint16_t new_id = ee16(sin->sin_port); + ts->if_idx = (uint8_t)if_idx; + if (bind_ip != IPADDR_ANY) + ts->local_ip = bind_ip; + else if (conf && conf->ip != IPADDR_ANY) + ts->local_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + ts->local_ip = primary->ip; + } + ts->src_port = new_id; + if (wolfIP_filter_notify_socket_event( + WOLFIP_FILT_BINDING, s, ts, + ts->local_ip, new_id, IPADDR_ANY, 0) != 0) { + ts->local_ip = prev_ip; + ts->src_port = prev_id; + return -1; + } + } + return 0; } else return -1; } @@ -2383,10 +2720,10 @@ int wolfIP_sock_listen(struct wolfIP *s, int sockfd, int backlog) (void)backlog; if (sockfd < 0) return -WOLFIP_EINVAL; - if (sockfd & MARK_TCP_SOCKET) { - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (IS_SOCKET_TCP(sockfd)) { + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; } else return -1; @@ -2408,13 +2745,13 @@ int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr struct wolfIP_sockaddr_in *sin = (struct wolfIP_sockaddr_in *)addr; if (sockfd < 0) return -WOLFIP_EINVAL; - if ((sockfd & MARK_TCP_SOCKET) == 0) { + if (!IS_SOCKET_TCP(sockfd)) { return -1; } - if ((sockfd & ~MARK_TCP_SOCKET) >= MAX_TCPSOCKETS) + if (SOCKET_UNMARK(sockfd) >= MAX_TCPSOCKETS) return -WOLFIP_EINVAL; - ts = &s->tcpsockets[sockfd & ~MARK_TCP_SOCKET]; + ts = &s->tcpsockets[SOCKET_UNMARK(sockfd)]; if (!sin || *addrlen < sizeof(struct wolfIP_sockaddr_in)) return -1; sin->sin_family = AF_INET; @@ -2432,6 +2769,10 @@ static void icmp_input(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_p struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, if_idx); if (wolfIP_filter_notify_icmp(WOLFIP_FILT_RECEIVING, s, if_idx, icmp, len) != 0) return; + if (icmp->type == ICMP_ECHO_REPLY) { + icmp_try_recv(s, if_idx, icmp, len); + return; + } if (!DHCP_IS_RUNNING(s) && (icmp->type == ICMP_ECHO_REQUEST)) { icmp->type = ICMP_ECHO_REPLY; icmp->csum += 8; @@ -2982,6 +3323,11 @@ void wolfIP_init_static(struct wolfIP **s) if (!s) return; wolfIP_init(&wolfIP_static); + if (wolfIP_static.dns_server == 0) { +#ifdef WOLFIP_STATIC_DNS_IP + wolfIP_static.dns_server = atoip4(WOLFIP_STATIC_DNS_IP); +#endif + } *s = &wolfIP_static; } #endif @@ -3113,7 +3459,11 @@ void wolfIP_recv_ex(struct wolfIP *s, unsigned int if_idx, void *buf, uint32_t l #define DNS_QUERY 0x00 #define DNS_RESPONSE 0x80 #define DNS_A 0x01 /* A record only */ +#define DNS_PTR 0x0C #define DNS_RD 0x0100 /* Recursion desired */ +#define DNS_QUERY_TYPE_NONE 0 +#define DNS_QUERY_TYPE_A 1 +#define DNS_QUERY_TYPE_PTR 2 struct PACKED dns_header { uint16_t id; @@ -3130,12 +3480,134 @@ struct PACKED dns_question { }; #define MAX_DNS_RESPONSE 512 +struct PACKED dns_rr { + uint16_t type; + uint16_t class; + uint32_t ttl; + uint16_t rdlength; +}; + +static size_t dns_write_u8(char *dst, uint8_t val) +{ + char tmp[3]; + size_t n = 0; + if (val >= 100) { + tmp[n++] = '0' + (val / 100); + val %= 100; + } + if (val >= 10 || n != 0) { + tmp[n++] = '0' + (val / 10); + val %= 10; + } + tmp[n++] = '0' + val; + memcpy(dst, tmp, n); + return n; +} + +static int dns_format_ptr_name(char *dst, size_t len, uint32_t ip) +{ + uint8_t octets[4] = { + (uint8_t)(ip & 0xFF), + (uint8_t)((ip >> 8) & 0xFF), + (uint8_t)((ip >> 16) & 0xFF), + (uint8_t)((ip >> 24) & 0xFF) + }; + size_t pos = 0; + size_t i; + static const char suffix[] = "in-addr.arpa"; + for (i = 0; i < 4; i++) { + uint8_t val = octets[i]; + size_t written; + if (pos + 3 >= len) + return -1; + written = dns_write_u8(dst + pos, val); + pos += written; + if (pos + 1 >= len) + return -1; + dst[pos++] = '.'; + } + { + size_t suffix_len = sizeof(suffix); + if (pos + suffix_len >= len) + return -1; + memcpy(dst + pos, suffix, suffix_len); + pos += suffix_len - 1; + dst[pos] = '\0'; + } + return 0; +} + +static int dns_skip_name(const uint8_t *buf, int len, int offset) +{ + int pos = offset; + int loop = 0; + while (pos < len && loop++ < len) { + uint8_t c = buf[pos++]; + if (c == 0) + break; + if ((c & 0xC0) == 0xC0) { + if (pos >= len) + return -1; + pos++; + break; + } + pos += c; + if (pos > len) + return -1; + } + if (loop >= len) + return -1; + return pos; +} + +static int dns_copy_name(const uint8_t *buf, int len, int offset, char *out, size_t out_len) +{ + int pos = offset; + size_t o = 0; + int loop = 0; + int jumped = 0; + while (pos < len && loop++ < len) { + uint8_t c = buf[pos]; + if (c == 0) { + if (!jumped) + pos++; + if (o >= out_len) + return -1; + out[o] = '\0'; + return 0; + } + if ((c & 0xC0) == 0xC0) { + if (pos + 1 >= len) + return -1; + { + uint16_t ptr = ((c & 0x3F) << 8) | buf[pos + 1]; + pos = ptr; + } + jumped = 1; + continue; + } + pos++; + if (pos + c > len) + return -1; + if (o != 0) { + if (o + 1 >= out_len) + return -1; + out[o++] = '.'; + } + if (o + c >= out_len) + return -1; + memcpy(out + o, buf + pos, c); + o += c; + pos += c; + } + return -1; +} + void dns_callback(int dns_sd, uint16_t ev, void *arg) { struct wolfIP *s = (struct wolfIP *)arg; char buf[MAX_DNS_RESPONSE]; struct dns_header *hdr = (struct dns_header *)buf; - struct dns_question *q; int dns_len; if (!s) return; @@ -3149,29 +3621,59 @@ void dns_callback(int dns_sd, uint16_t ev, void *arg) } /* Parse DNS response */ if ((ee16(hdr->flags) & 0x8100) == 0x8100) { - /* Skip the question */ - char *q_name = buf + sizeof(struct dns_header); - uint32_t ip; - while (*q_name) q_name++; - if (q_name - buf > dns_len) { - s->dns_id = 0; - return; + int pos = sizeof(struct dns_header); + int qcount = ee16(hdr->qdcount); + int ancount = ee16(hdr->ancount); + while (qcount-- > 0) { + pos = dns_skip_name((const uint8_t *)buf, dns_len, pos); + if (pos < 0 || pos + (int)sizeof(struct dns_question) > dns_len) { + s->dns_id = 0; + return; + } + pos += sizeof(struct dns_question); } - q_name++; /* Skip the null terminator */ - q = (struct dns_question *)q_name; - if (ee16(q->qtype) == DNS_A) { - uint8_t *ip_ptr = (uint8_t *)(buf + dns_len - 4); - ip = ip_ptr[3] | (ip_ptr[2] << 8) | (ip_ptr[1] << 16) | (ip_ptr[0] << 24); - if(s->dns_lookup_cb) - s->dns_lookup_cb(ee32(ip)); - LOG("DNS response: %u.%u.%u.%u\n", (ip >> 24) & 0xFF, (ip >> 16) & 0xFF, (ip >> 8) & 0xFF, ip & 0xFF); - s->dns_id = 0; + while (ancount-- > 0) { + struct dns_rr *rr; + uint16_t rdlen; + pos = dns_skip_name((const uint8_t *)buf, dns_len, pos); + if (pos < 0 || pos + (int)sizeof(struct dns_rr) > dns_len) { + s->dns_id = 0; + return; + } + rr = (struct dns_rr *)(buf + pos); + pos += sizeof(struct dns_rr); + rdlen = ee16(rr->rdlength); + if (pos + rdlen > dns_len) { + s->dns_id = 0; + return; + } + if (s->dns_query_type == DNS_QUERY_TYPE_A && ee16(rr->type) == DNS_A && rdlen >= 4) { + uint32_t ip = (buf[pos + 3] & 0xFF) | + ((buf[pos + 2] & 0xFF) << 8) | + ((buf[pos + 1] & 0xFF) << 16) | + ((buf[pos + 0] & 0xFF) << 24); + if (s->dns_lookup_cb) + s->dns_lookup_cb(ee32(ip)); + s->dns_id = 0; + s->dns_query_type = DNS_QUERY_TYPE_NONE; + return; + } else if (s->dns_query_type == DNS_QUERY_TYPE_PTR && ee16(rr->type) == DNS_PTR) { + if (dns_copy_name((const uint8_t *)buf, dns_len, pos, + s->dns_ptr_name, sizeof(s->dns_ptr_name)) == 0) { + if (s->dns_ptr_cb) + s->dns_ptr_cb(s->dns_ptr_name); + s->dns_id = 0; + s->dns_query_type = DNS_QUERY_TYPE_NONE; + return; + } + } + pos += rdlen; } } } } -int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb)(uint32_t ip)) +static int dns_send_query(struct wolfIP *s, const char *dname, uint16_t *id, uint16_t qtype) { uint8_t buf[512]; struct dns_header *hdr; @@ -3179,7 +3681,7 @@ int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb char *q_name, *tok_start, *tok_end; struct wolfIP_sockaddr_in dns_srv; uint32_t tok_len = 0; - if (!dname || !id || !lookup_cb) return -22; /* Invalid arguments */ + if (!dname || !id) return -22; if (strlen(dname) > 256) return -22; /* Invalid arguments */ if (s->dns_server == 0) return -101; /* Network unreachable: No DNS server configured */ if (s->dns_id != 0) return -16; /* DNS query already in progress */ @@ -3189,10 +3691,10 @@ int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb return -1; wolfIP_register_callback(s, s->dns_udp_sd, dns_callback, s); } - s->dns_lookup_cb = lookup_cb; s->dns_id = wolfIP_getrandom(); *id = s->dns_id; memset(buf, 0, 512); + s->dns_query_type = (qtype == DNS_PTR) ? DNS_QUERY_TYPE_PTR : DNS_QUERY_TYPE_A; hdr = (struct dns_header *)buf; hdr->id = ee16(s->dns_id); hdr->flags = ee16(DNS_QUERY); @@ -3218,7 +3720,7 @@ int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb *q_name = 0; tok_len++; q = (struct dns_question *)(buf + sizeof(struct dns_header) + tok_len); - q->qtype = ee16(DNS_A); + q->qtype = ee16(qtype); q->qclass = ee16(1); memset(&dns_srv, 0, sizeof(struct wolfIP_sockaddr_in)); dns_srv.sin_family = AF_INET; @@ -3228,6 +3730,32 @@ int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb return 0; } +int nslookup(struct wolfIP *s, const char *dname, uint16_t *id, void (*lookup_cb)(uint32_t ip)) +{ + if (!s || !dname || !id || !lookup_cb) + return -22; + s->dns_lookup_cb = lookup_cb; + s->dns_ptr_cb = NULL; + s->dns_query_type = DNS_QUERY_TYPE_A; + return dns_send_query(s, dname, id, DNS_A); +} + +int wolfIP_dns_ptr_lookup(struct wolfIP *s, uint32_t ip, uint16_t *id, void (*lookup_cb)(const char *name)) +{ + char ptr_name[128]; + if (dns_format_ptr_name(ptr_name, sizeof(ptr_name), ip) < 0) + return -22; + if (!s || !id || !lookup_cb) + return -22; + snprintf(ptr_name, sizeof(ptr_name), "%u.%u.%u.%u.in-addr.arpa", + ip & 0xFF, (ip >> 8) & 0xFF, (ip >> 16) & 0xFF, (ip >> 24) & 0xFF); + s->dns_ptr_cb = lookup_cb; + s->dns_lookup_cb = NULL; + s->dns_ptr_name[0] = '\0'; + s->dns_query_type = DNS_QUERY_TYPE_PTR; + return dns_send_query(s, ptr_name, id, DNS_PTR); +} + /* wolfIP_poll: poll the network stack for incoming packets * This function should be called in a loop to process incoming packets. * It will call the poll function of the device driver and process the @@ -3282,6 +3810,13 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) ts->events = 0; } } + for (i = 0; i < MAX_ICMPSOCKETS; i++) { + struct tsocket *ts = &s->icmpsockets[i]; + if ((ts->callback) && (ts->events)) { + ts->callback(i | MARK_ICMP_SOCKET, ts->events, ts->callback_arg); + ts->events = 0; + } + } /* Step 4: attempt to write any pending data */ for (i = 0; i < MAX_TCPSOCKETS; i++) { @@ -3339,8 +3874,9 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) #endif { struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); - if (ll && ll->send) + if (ll && ll->send) { ll->send(ll, tcp, desc->len); + } } desc->flags |= PKT_FLAG_SENT; desc->time_sent = now; @@ -3406,6 +3942,47 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) desc = fifo_peek(&t->sock.udp.txbuf); } } + for (i = 0; i < MAX_ICMPSOCKETS; i++) { + struct tsocket *t = &s->icmpsockets[i]; + struct pkt_desc *desc = fifo_peek(&t->sock.udp.txbuf); + while (desc) { + struct wolfIP_icmp_packet *icmp = (struct wolfIP_icmp_packet *)(t->txmem + desc->pos + sizeof(*desc)); + unsigned int tx_if = wolfIP_socket_if_idx(t); +#ifdef ETHERNET + struct ipconf *conf = wolfIP_ipconf_at(s, tx_if); + ip4 nexthop = wolfIP_select_nexthop(conf, t->remote_ip); + if (wolfIP_is_loopback_if(tx_if)) { + struct wolfIP_ll_dev *loop = wolfIP_ll_at(s, tx_if); + if (loop) + memcpy(t->nexthop_mac, loop->mac, 6); + } else { + if ((!IS_IP_BCAST(nexthop) && (arp_lookup(s, tx_if, nexthop, t->nexthop_mac) < 0))) { + arp_request(s, tx_if, nexthop); + break; + } + if (IS_IP_BCAST(nexthop)) + memset(t->nexthop_mac, 0xFF, 6); + } +#endif + len = desc->len - ETH_HEADER_LEN; + ip_output_add_header(t, (struct wolfIP_ip_packet *)icmp, WI_IPPROTO_ICMP, len); + if (wolfIP_filter_notify_icmp(WOLFIP_FILT_SENDING, t->S, tx_if, icmp, desc->len) != 0) + break; + if (wolfIP_filter_notify_ip(WOLFIP_FILT_SENDING, t->S, tx_if, &icmp->ip, desc->len) != 0) + break; +#ifdef ETHERNET + if (wolfIP_filter_notify_eth(WOLFIP_FILT_SENDING, t->S, tx_if, &icmp->ip.eth, desc->len) != 0) + break; +#endif + { + struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); + if (ll && ll->send) + ll->send(ll, icmp, desc->len); + } + fifo_pop(&t->sock.udp.txbuf); + desc = fifo_peek(&t->sock.udp.txbuf); + } + } return 0; } diff --git a/wolfip.h b/wolfip.h index f0190f9..e12b1f9 100644 --- a/wolfip.h +++ b/wolfip.h @@ -2,6 +2,44 @@ #define WOLFIP_H #include +#if !defined(_SIZE_T) && !defined(_SIZE_T_DEFINED) && !defined(_SIZE_T_DECLARED) && \ + !defined(_BSD_SIZE_T_DEFINED_) && !defined(__DEFINED_size_t) && \ + !defined(__size_t_defined) +#if defined(__SIZE_TYPE__) +typedef __SIZE_TYPE__ size_t; +#elif defined(_MSC_VER) +#ifdef _WIN64 +typedef unsigned __int64 size_t; +#else +typedef unsigned int size_t; +#endif +#else +typedef unsigned long size_t; +#endif +#define _SIZE_T +#define _SIZE_T_DEFINED +#define _SIZE_T_DECLARED +#define _BSD_SIZE_T_DEFINED_ +#define __DEFINED_size_t +#define __size_t_defined +#endif + +#ifndef WOLFIP_SOL_IP +#ifdef SOL_IP +#define WOLFIP_SOL_IP SOL_IP +#else +#define WOLFIP_SOL_IP 0 +#endif +#endif + +#ifndef WOLFIP_IP_RECVTTL +#ifdef IP_RECVTTL +#define WOLFIP_IP_RECVTTL IP_RECVTTL +#else +#define WOLFIP_IP_RECVTTL 12 +#endif +#endif + /* Types */ struct wolfIP; typedef uint32_t ip4; @@ -28,6 +66,14 @@ typedef uint32_t ip4; #endif #endif +#ifndef WOLFIP_ENOMEM +#ifdef ENOMEM +#define WOLFIP_ENOMEM ENOMEM +#else +#define WOLFIP_ENOMEM (12) +#endif +#endif + #ifndef WOLFIP_EACCES #ifdef EACCES #define WOLFIP_EACCES EACCES @@ -67,13 +113,22 @@ struct ipconf { /* Socket interface */ #define MARK_TCP_SOCKET 0x100 /* Mark a socket as TCP */ #define MARK_UDP_SOCKET 0x200 /* Mark a socket as UDP */ +#define MARK_ICMP_SOCKET 0x400 /* Mark a socket as ICMP */ +#define IS_SOCKET_TCP(fd) (((fd) & MARK_TCP_SOCKET) == MARK_TCP_SOCKET) +#define IS_SOCKET_UDP(fd) (((fd) & MARK_UDP_SOCKET) == MARK_UDP_SOCKET) +#define IS_SOCKET_ICMP(fd)(((fd) & MARK_ICMP_SOCKET) == MARK_ICMP_SOCKET) +#define SOCKET_UNMARK(fd) ((fd) & 0xFF) /* Compile-time sanity check for socket marks & number of sockets */ #if (MARK_TCP_SOCKET >= MARK_UDP_SOCKET) #error "MARK_TCP_SOCKET must be less than MARK_UDP_SOCKET" #endif +#if (MARK_UDP_SOCKET >= MARK_ICMP_SOCKET) +#error "MARK_UDP_SOCKET must be less than MARK_ICMP_SOCKET" +#endif + #if MAX_TCPSOCKETS > 255 #error "MAX_TCPSOCKETS must be less than 256" #endif @@ -82,6 +137,9 @@ struct ipconf { #error "MAX_UDPSOCKETS must be less than 256" #endif +#if MAX_ICMPSOCKETS > 255 +#error "MAX_ICMPSOCKETS must be less than 256" +#endif @@ -97,6 +155,28 @@ struct wolfIP_sockaddr_in { }; struct wolfIP_sockaddr { uint16_t sa_family; }; typedef uint32_t socklen_t; + +#if defined(__has_include) +#if __has_include() +#include +#include +#define WOLFIP_HAVE_POSIX_TYPES 1 +#endif +#endif + +#ifndef WOLFIP_HAVE_POSIX_TYPES +struct iovec { void *iov_base; size_t iov_len; }; +struct msghdr { + void *msg_name; + socklen_t msg_namelen; + struct iovec *msg_iov; + size_t msg_iovlen; + void *msg_control; + size_t msg_controllen; + int msg_flags; +}; +#endif + #ifndef AF_INET #define AF_INET 2 #endif @@ -105,6 +185,7 @@ typedef uint32_t socklen_t; #include #include #include +#include #define wolfIP_sockaddr_in sockaddr_in #define wolfIP_sockaddr sockaddr #endif @@ -119,6 +200,12 @@ int wolfIP_sock_send(struct wolfIP *s, int sockfd, const void *buf, size_t len, int wolfIP_sock_write(struct wolfIP *s, int sockfd, const void *buf, size_t len); int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, int flags, struct wolfIP_sockaddr *src_addr, socklen_t *addrlen); int wolfIP_sock_recv(struct wolfIP *s, int sockfd, void *buf, size_t len, int flags); +int wolfIP_sock_sendmsg(struct wolfIP *s, int sockfd, const struct msghdr *msg, int flags); +int wolfIP_sock_recvmsg(struct wolfIP *s, int sockfd, struct msghdr *msg, int flags); +int wolfIP_dns_ptr_lookup(struct wolfIP *s, uint32_t ip, uint16_t *id, void (*lookup_cb)(const char *name)); +int wolfIP_sock_get_recv_ttl(struct wolfIP *s, int sockfd, int *ttl); +int wolfIP_sock_setsockopt(struct wolfIP *s, int sockfd, int level, int optname, const void *optval, socklen_t optlen); +int wolfIP_sock_getsockopt(struct wolfIP *s, int sockfd, int level, int optname, void *optval, socklen_t *optlen); int wolfIP_sock_read(struct wolfIP *s, int sockfd, void *buf, size_t len); int wolfIP_sock_close(struct wolfIP *s, int sockfd); int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, const socklen_t *addrlen);