From b68c1d4950edadf71810355a3c3653f4e898936e Mon Sep 17 00:00:00 2001 From: Daniele Lacamera Date: Tue, 25 Nov 2025 09:19:50 +0100 Subject: [PATCH 1/2] Added support for raw AF_INET sockets --- .github/workflows/linux.yml | 4 + Makefile | 12 +- config.h | 8 + src/port/posix/bsd_socket.c | 30 ++- src/test/raw_ping.c | 217 ++++++++++++++++ src/test/unit/unit.c | 201 +++++++++++++++ src/wolfip.c | 499 +++++++++++++++++++++++++++++++++++- wolfip.h | 37 +++ 8 files changed, 1001 insertions(+), 7 deletions(-) create mode 100644 src/test/raw_ping.c diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 3530cab..872c6a2 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -48,6 +48,10 @@ jobs: run: | sudo LD_PRELOAD=$PWD/libwolfip.so ping -c 5 10.10.10.1 + - name: Testing RAW sockets via raw_ping + run: | + sudo ./build/raw_ping 10.10.10.1 + - name: Install check run: | sudo apt-get install -y check diff --git a/Makefile b/Makefile index e2e554e..acfea1c 100644 --- a/Makefile +++ b/Makefile @@ -3,13 +3,19 @@ CFLAGS:=-Wall -Werror -Wextra -I. -D_GNU_SOURCE CFLAGS+=-g -ggdb -Wdeclaration-after-statement LDFLAGS+=-pthread + +# Identify host system (defaults to Linux) UNAME_S:=$(shell uname -s) UNAME_M:=$(shell uname -m) UNAME_LC:=$(shell echo $(UNAME_S) | tr 'A-Z' 'a-z') + +# FreeBSD ifeq ($(UNAME_S),FreeBSD) CFLAGS+=-I/usr/local/include LDFLAGS+=-L/usr/local/lib endif + +# MacOS ifeq ($(UNAME_S),Darwin) BREW_PREFIX?=$(shell brew --prefix 2>/dev/null) ifeq ($(filter command\ line environment,$(origin BREW_PREFIX)),) @@ -119,7 +125,7 @@ endif EXE=build/tcpecho build/tcp_netcat_poll build/tcp_netcat_select \ build/test-evloop build/test-dns build/test-wolfssl-forwarding \ build/test-ttl-expired build/test-wolfssl build/test-httpd \ - build/ipfilter-logger + build/ipfilter-logger build/raw_ping LIB=libwolfip.so PREFIX=/usr/local @@ -183,6 +189,10 @@ build/tcp_netcat_poll: $(OBJ) build/port/posix/bsd_socket.o build/test/tcp_netca @echo "[LD] $@" @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) +build/raw_ping: $(OBJ) build/port/posix/bsd_socket.o build/test/raw_ping.o + @echo "[LD] $@" + @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) + build/tcp_netcat_select: $(OBJ) build/port/posix/bsd_socket.o build/test/tcp_netcat_select.o @echo "[LD] $@" @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) diff --git a/config.h b/config.h index ea0f429..77b4470 100644 --- a/config.h +++ b/config.h @@ -24,6 +24,14 @@ #define WOLFIP_MAX_INTERFACES 2 #endif +#ifndef WOLFIP_RAWSOCKETS +#define WOLFIP_RAWSOCKETS 1 +#endif + +#ifndef WOLFIP_MAX_RAWSOCKETS +#define WOLFIP_MAX_RAWSOCKETS 4 +#endif + #ifndef WOLFIP_ENABLE_FORWARDING #define WOLFIP_ENABLE_FORWARDING 0 #endif diff --git a/src/port/posix/bsd_socket.c b/src/port/posix/bsd_socket.c index b1c7a8f..8962768 100644 --- a/src/port/posix/bsd_socket.c +++ b/src/port/posix/bsd_socket.c @@ -129,6 +129,9 @@ 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]; +#if WOLFIP_RAWSOCKETS +static int raw_entry_for_slot[WOLFIP_MAX_RAWSOCKETS]; +#endif enum wolfip_dns_wait_type { DNS_WAIT_NONE = 0, @@ -178,6 +181,10 @@ static void wolfip_fd_pool_init(void) udp_entry_for_slot[i] = -1; for (i = 0; i < MAX_ICMPSOCKETS; i++) icmp_entry_for_slot[i] = -1; +#if WOLFIP_RAWSOCKETS + for (i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) + raw_entry_for_slot[i] = -1; +#endif init_done = 1; } @@ -199,6 +206,13 @@ static struct wolfip_fd_entry *wolfip_entry_from_internal(int internal_fd) if (pos < 0 || pos >= MAX_ICMPSOCKETS) return NULL; idx = icmp_entry_for_slot[pos]; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos < 0 || pos >= WOLFIP_MAX_RAWSOCKETS) + return NULL; + idx = raw_entry_for_slot[pos]; +#endif } else { return NULL; } @@ -232,6 +246,12 @@ static void wolfip_fd_detach_internal(int internal_fd) int pos = SOCKET_UNMARK(internal_fd); if (pos >= 0 && pos < MAX_ICMPSOCKETS) icmp_entry_for_slot[pos] = -1; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < WOLFIP_MAX_RAWSOCKETS) + raw_entry_for_slot[pos] = -1; +#endif } } @@ -249,6 +269,12 @@ static void wolfip_fd_attach_internal(int internal_fd, int entry_idx) int pos = SOCKET_UNMARK(internal_fd); if (pos >= 0 && pos < MAX_ICMPSOCKETS) icmp_entry_for_slot[pos] = entry_idx; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < WOLFIP_MAX_RAWSOCKETS) + raw_entry_for_slot[pos] = entry_idx; +#endif } } @@ -1237,7 +1263,7 @@ int getaddrinfo(const char *node, const char *service, const struct addrinfo *hi int ret; struct in_addr ipv4; char canon[256]; - fprintf(stderr, "wolfIP getaddrinfo: in_stack=%d node=%s service=%s\n", + WOLFIP_DBG("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); @@ -1315,7 +1341,7 @@ int getaddrinfo(const char *node, const char *service, const struct addrinfo *hi } void freeaddrinfo(struct addrinfo *res) { - fprintf(stderr, "wolfIP freeaddrinfo: in_stack=%d res=%p\n", in_the_stack, (void *)res); + WOLFIP_DBG("wolfIP freeaddrinfo: in_stack=%d res=%p\n", in_the_stack, (void *)res); if (!res) { return; } diff --git a/src/test/raw_ping.c b/src/test/raw_ping.c new file mode 100644 index 0000000..9c32534 --- /dev/null +++ b/src/test/raw_ping.c @@ -0,0 +1,217 @@ +/* raw_ping.c + * + * Copyright (C) 2025 wolfSSL Inc. + * + * This file is part of wolfIP TCP/IP stack. + * + * wolfIP is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * wolfIP is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PACKET_SIZE 64 +#define NUM_PINGS 3 + +static unsigned short icmp_checksum(void *b, int len) +{ + unsigned short *buf; + unsigned int sum; + unsigned short result; + + buf = (unsigned short *)b; + sum = 0; + + while (len > 1) { + sum += *buf++; + len -= 2; + } + + if (len == 1) { + sum += *(unsigned char *)buf; + } + + sum = (sum >> 16) + (sum & 0xFFFF); + sum += (sum >> 16); + result = (unsigned short)~sum; + + return result; +} + +int main(int argc, char *argv[]) +{ + int sockfd; + struct addrinfo hints; + struct addrinfo *res; + int ret; + const char *host; + struct sockaddr_in *addr_in; + pid_t pid; + int seq; + int replies = 0; + + if (argc != 2) { + fprintf(stderr, "Usage: %s \n", argv[0]); + return 1; + } + + host = argv[1]; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; /* IPv4 */ + hints.ai_socktype = SOCK_RAW; /* raw socket */ + hints.ai_protocol = IPPROTO_ICMP; /* ICMP */ + + ret = getaddrinfo(host, NULL, &hints, &res); + if (ret != 0) { + fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(ret)); + return 1; + } + + sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); + if (sockfd < 0) { + perror("socket (need root / CAP_NET_RAW)"); + freeaddrinfo(res); + return 1; + } + + addr_in = (struct sockaddr_in *)res->ai_addr; + + printf("PING %s (%s): %d bytes ICMP data, %d probes\n", + host, + inet_ntoa(addr_in->sin_addr), + PACKET_SIZE - (int)sizeof(struct icmphdr), + NUM_PINGS); + + pid = getpid() & 0xFFFF; + + for (seq = 1; seq <= NUM_PINGS; ++seq) { + unsigned char packet[PACKET_SIZE]; + struct icmphdr *icmp; + int payload_size; + unsigned char *data; + struct timeval send_time; + struct timeval recv_time; + ssize_t sent; + unsigned char recvbuf[1024]; + struct sockaddr_in src; + socklen_t srclen; + ssize_t n; + struct ip *ip_hdr; + int ip_hdr_len; + struct icmphdr *icmp_reply; + long sec; + long usec; + double rtt; + int i; + + memset(packet, 0, sizeof(packet)); + + icmp = (struct icmphdr *)packet; + icmp->type = ICMP_ECHO; + icmp->code = 0; + icmp->un.echo.id = htons((unsigned short)pid); + icmp->un.echo.sequence = htons((unsigned short)seq); + + /* Simple payload pattern */ + payload_size = PACKET_SIZE - (int)sizeof(struct icmphdr); + data = packet + sizeof(struct icmphdr); + for (i = 0; i < payload_size; ++i) { + data[i] = (unsigned char)('A' + (i % 26)); + } + + icmp->checksum = 0; + icmp->checksum = icmp_checksum(packet, PACKET_SIZE); + + gettimeofday(&send_time, NULL); + + sent = sendto(sockfd, packet, PACKET_SIZE, 0, + res->ai_addr, res->ai_addrlen); + if (sent < 0) { + perror("sendto"); + break; + } + + srclen = sizeof(src); + n = recvfrom(sockfd, recvbuf, sizeof(recvbuf), 0, + (struct sockaddr *)&src, &srclen); + if (n < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + printf("Request timeout for icmp_seq %d\n", seq); + sleep(1); + continue; + } else { + perror("recvfrom"); + break; + } + } + + gettimeofday(&recv_time, NULL); + + /* Received packet: IP header + ICMP */ + ip_hdr = (struct ip *)recvbuf; + ip_hdr_len = ip_hdr->ip_hl * 4; + + if (n < ip_hdr_len + (int)sizeof(struct icmphdr)) { + printf("Short packet\n"); + sleep(1); + continue; + } + + icmp_reply = (struct icmphdr *)(recvbuf + ip_hdr_len); + + if (icmp_reply->type == ICMP_ECHOREPLY && + icmp_reply->code == 0 && + icmp_reply->un.echo.id == htons((unsigned short)pid) && + icmp_reply->un.echo.sequence == htons((unsigned short)seq)) { + + sec = recv_time.tv_sec - send_time.tv_sec; + usec = recv_time.tv_usec - send_time.tv_usec; + if (usec < 0) { + --sec; + usec += 1000000; + } + rtt = sec * 1000.0 + usec / 1000.0; + + printf("%ld bytes from %s: icmp_seq=%d ttl=%d time=%.3f ms\n", + (long)(n - ip_hdr_len), + inet_ntoa(src.sin_addr), + seq, + ip_hdr->ip_ttl, + rtt); + replies++; + } else { + printf("Got unexpected ICMP packet (type=%d code=%d)\n", + icmp_reply->type, icmp_reply->code); + } + + sleep(1); + } + + freeaddrinfo(res); + close(sockfd); + return (replies > 0) ? 0 : 1; +} diff --git a/src/test/unit/unit.c b/src/test/unit/unit.c index 91cda68..25f6742 100644 --- a/src/test/unit/unit.c +++ b/src/test/unit/unit.c @@ -28,6 +28,8 @@ #ifndef WOLFIP_ENABLE_FORWARDING #define WOLFIP_ENABLE_FORWARDING 1 #endif +#undef WOLFIP_RAWSOCKETS +#define WOLFIP_RAWSOCKETS 1 #if WOLFIP_ENABLE_LOOPBACK #define TEST_LOOPBACK_IF 0U #define TEST_PRIMARY_IF 1U @@ -1341,6 +1343,201 @@ START_TEST(test_icmp_socket_send_recv) } END_TEST +START_TEST(test_raw_socket_recv_captures_ip_header) +{ + struct wolfIP s; + int sd; + uint8_t frame_buf[sizeof(struct wolfIP_ip_packet) + 8]; + struct wolfIP_ip_packet *frame = (struct wolfIP_ip_packet *)frame_buf; + uint8_t payload[8] = {1, 2, 3, 4, 5, 6, 7, 8}; + struct wolfIP_sockaddr_in from; + socklen_t from_len = sizeof(from); + uint8_t rxbuf[64]; + + wolfIP_init(&s); + mock_link_init(&s); + wolfIP_ipconfig_set(&s, 0x0A000001U, 0xFFFFFF00U, 0); + + sd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_RAW, WI_IPPROTO_UDP); + ck_assert_int_ge(sd, 0); + + memset(frame, 0, sizeof(frame_buf)); + memcpy(frame->eth.dst, s.ll_dev[TEST_PRIMARY_IF].mac, 6); + memcpy(frame->eth.src, "\xaa\xbb\xcc\xdd\xee\xff", 6); + frame->eth.type = ee16(ETH_TYPE_IP); + frame->ver_ihl = 0x45; + frame->ttl = 32; + frame->proto = WI_IPPROTO_UDP; + frame->len = ee16(IP_HEADER_LEN + sizeof(payload)); + frame->src = ee32(0x0A000002U); + frame->dst = ee32(0x0A000001U); + memcpy(frame->data, payload, sizeof(payload)); + frame->csum = 0; + iphdr_set_checksum(frame); + + wolfIP_recv_ex(&s, TEST_PRIMARY_IF, frame, ETH_HEADER_LEN + IP_HEADER_LEN + sizeof(payload)); + + memset(rxbuf, 0, sizeof(rxbuf)); + memset(&from, 0, sizeof(from)); + from_len = sizeof(from); + ck_assert_int_eq(wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, + (struct wolfIP_sockaddr *)&from, &from_len), + IP_HEADER_LEN + (int)sizeof(payload)); + ck_assert_mem_eq(rxbuf, ((uint8_t *)frame) + ETH_HEADER_LEN, + IP_HEADER_LEN + sizeof(payload)); + ck_assert_uint_eq(ee32(from.sin_addr.s_addr), ee32(frame->src)); + ck_assert_uint_eq(from_len, sizeof(from)); +} +END_TEST + +START_TEST(test_raw_socket_send_hdrincl_respected) +{ + struct wolfIP s; + int sd; + uint8_t ip_buf[sizeof(struct wolfIP_ip_packet) + 4]; + struct wolfIP_ip_packet *ip = (struct wolfIP_ip_packet *)ip_buf; + struct wolfIP_sockaddr_in sin; + uint8_t payload[4] = {0xAA, 0xBB, 0xCC, 0xDD}; + uint32_t dst_ip = 0x0A000002U; + uint8_t nh_mac[6] = {0x10, 0x20, 0x30, 0x40, 0x50, 0x60}; + int one = 1; + uint8_t *wire_ip; + + wolfIP_init(&s); + mock_link_init(&s); + wolfIP_ipconfig_set(&s, 0x0A000001U, 0xFFFFFF00U, 0); + + s.arp.neighbors[0].ip = dst_ip; + s.arp.neighbors[0].if_idx = TEST_PRIMARY_IF; + memcpy(s.arp.neighbors[0].mac, nh_mac, 6); + + sd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_RAW, WI_IPPROTO_ICMP); + ck_assert_int_ge(sd, 0); + ck_assert_int_eq(wolfIP_sock_setsockopt(&s, sd, WOLFIP_SOL_IP, WOLFIP_IP_HDRINCL, &one, sizeof(one)), 0); + + memset(ip, 0, sizeof(ip_buf)); + ip->ver_ihl = 0x45; + ip->ttl = 99; + ip->proto = WI_IPPROTO_ICMP; + ip->len = ee16(IP_HEADER_LEN + sizeof(payload)); + ip->src = ee32(0x0A000001U); + ip->dst = ee32(dst_ip); + memcpy(ip->data, payload, sizeof(payload)); + ip->csum = 0; + iphdr_set_checksum(ip); + wire_ip = ((uint8_t *)ip) + ETH_HEADER_LEN; + + memset(&sin, 0, sizeof(sin)); + sin.sin_family = AF_INET; + sin.sin_addr.s_addr = ee32(dst_ip); + + last_frame_sent_size = 0; + memset(last_frame_sent, 0, sizeof(last_frame_sent)); + + ck_assert_int_eq(wolfIP_sock_sendto(&s, sd, wire_ip, IP_HEADER_LEN + sizeof(payload), 0, + (struct wolfIP_sockaddr *)&sin, sizeof(sin)), (int)(IP_HEADER_LEN + sizeof(payload))); + wolfIP_poll(&s, 0); + + ck_assert_uint_eq(last_frame_sent_size, ETH_HEADER_LEN + IP_HEADER_LEN + sizeof(payload)); + { + struct wolfIP_ip_packet *sent = (struct wolfIP_ip_packet *)last_frame_sent; + ck_assert_uint_eq(sent->ttl, 99); + ck_assert_uint_eq(sent->proto, WI_IPPROTO_ICMP); + ck_assert_uint_eq(sent->len, ee16(IP_HEADER_LEN + sizeof(payload))); + ck_assert_uint_eq(sent->src, ip->src); + ck_assert_uint_eq(sent->dst, ip->dst); + ck_assert_mem_eq(sent->data, payload, sizeof(payload)); + ck_assert_mem_eq(sent->eth.dst, nh_mac, 6); + ck_assert_mem_eq(sent->eth.src, s.ll_dev[TEST_PRIMARY_IF].mac, 6); + } +} +END_TEST + +START_TEST(test_raw_socket_send_builds_ip_header) +{ + struct wolfIP s; + int sd; + uint8_t payload[6] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66}; + struct wolfIP_sockaddr_in sin; + uint32_t dst_ip = 0x0A00000BU; + uint8_t nh_mac[6] = {0x21, 0x22, 0x23, 0x24, 0x25, 0x26}; + + wolfIP_init(&s); + mock_link_init(&s); + wolfIP_ipconfig_set(&s, 0x0A000001U, 0xFFFFFF00U, 0); + + s.arp.neighbors[0].ip = dst_ip; + s.arp.neighbors[0].if_idx = TEST_PRIMARY_IF; + memcpy(s.arp.neighbors[0].mac, nh_mac, sizeof(nh_mac)); + + sd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_RAW, WI_IPPROTO_UDP); + ck_assert_int_ge(sd, 0); + + memset(&sin, 0, sizeof(sin)); + sin.sin_family = AF_INET; + sin.sin_addr.s_addr = ee32(dst_ip); + + last_frame_sent_size = 0; + memset(last_frame_sent, 0, sizeof(last_frame_sent)); + + ck_assert_int_eq(wolfIP_sock_sendto(&s, sd, payload, sizeof(payload), 0, + (struct wolfIP_sockaddr *)&sin, sizeof(sin)), (int)sizeof(payload)); + wolfIP_poll(&s, 0); + + ck_assert_uint_eq(last_frame_sent_size, ETH_HEADER_LEN + IP_HEADER_LEN + sizeof(payload)); + { + struct wolfIP_ip_packet *sent = (struct wolfIP_ip_packet *)last_frame_sent; + ck_assert_uint_eq(sent->ver_ihl, 0x45); + ck_assert_uint_eq(ee16(sent->len), IP_HEADER_LEN + sizeof(payload)); + ck_assert_uint_eq(sent->proto, WI_IPPROTO_UDP); + ck_assert_uint_eq(sent->dst, ee32(dst_ip)); + ck_assert_uint_eq(sent->src, ee32(0x0A000001U)); + ck_assert_uint_ne(sent->csum, 0); + ck_assert_mem_eq(sent->data, payload, sizeof(payload)); + ck_assert_mem_eq(sent->eth.dst, nh_mac, 6); + ck_assert_mem_eq(sent->eth.src, s.ll_dev[TEST_PRIMARY_IF].mac, 6); + } +} +END_TEST + +START_TEST(test_raw_socket_recv_protocol_mismatch) +{ + struct wolfIP s; + int sd; + uint8_t frame_buf[sizeof(struct wolfIP_ip_packet) + 4]; + struct wolfIP_ip_packet *frame = (struct wolfIP_ip_packet *)frame_buf; + uint8_t payload[4] = {9, 8, 7, 6}; + uint8_t rxbuf[32]; + + wolfIP_init(&s); + mock_link_init(&s); + wolfIP_ipconfig_set(&s, 0x0A000001U, 0xFFFFFF00U, 0); + + sd = wolfIP_sock_socket(&s, AF_INET, IPSTACK_SOCK_RAW, WI_IPPROTO_UDP); + ck_assert_int_ge(sd, 0); + + memset(frame, 0, sizeof(frame_buf)); + memcpy(frame->eth.dst, s.ll_dev[TEST_PRIMARY_IF].mac, 6); + memcpy(frame->eth.src, "\xaa\xbb\xcc\xdd\xee\x00", 6); + frame->eth.type = ee16(ETH_TYPE_IP); + frame->ver_ihl = 0x45; + frame->ttl = 16; + frame->proto = WI_IPPROTO_TCP; /* different from socket proto */ + frame->len = ee16(IP_HEADER_LEN + sizeof(payload)); + frame->src = ee32(0x0A000010U); + frame->dst = ee32(0x0A000001U); + memcpy(frame->data, payload, sizeof(payload)); + frame->csum = 0; + iphdr_set_checksum(frame); + + wolfIP_recv_ex(&s, TEST_PRIMARY_IF, frame, ETH_HEADER_LEN + IP_HEADER_LEN + sizeof(payload)); + + memset(rxbuf, 0, sizeof(rxbuf)); + ck_assert_int_eq(wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, NULL, NULL), + -WOLFIP_EAGAIN); +} +END_TEST + Suite *wolf_suite(void) @@ -1446,6 +1643,10 @@ Suite *wolf_suite(void) 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); + tcase_add_test(tc_proto, test_raw_socket_recv_captures_ip_header); + tcase_add_test(tc_proto, test_raw_socket_send_hdrincl_respected); + tcase_add_test(tc_proto, test_raw_socket_send_builds_ip_header); + tcase_add_test(tc_proto, test_raw_socket_recv_protocol_mismatch); 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 44a68d4..aa9594a 100644 --- a/src/wolfip.c +++ b/src/wolfip.c @@ -769,6 +769,31 @@ struct tsocket { }; static void close_socket(struct tsocket *ts); +#if WOLFIP_RAWSOCKETS +struct rawsocket { + struct fifo rxbuf; + struct fifo txbuf; + ip4 local_ip, remote_ip; + ip4 bound_local_ip; + struct wolfIP *S; +#ifdef ETHERNET + uint8_t nexthop_mac[6]; +#endif + uint16_t protocol; + uint8_t if_idx; + uint8_t dontroute; + uint8_t ipheader_include; + uint8_t recv_ttl; + uint8_t last_pkt_ttl; + uint8_t rxmem[RXBUF_SIZE]; + uint8_t txmem[TXBUF_SIZE]; + uint8_t used; + uint16_t events; + void (*callback)(int sock_fd, uint16_t events, void *arg); + void *callback_arg; +}; +#endif + #ifdef ETHERNET struct PACKED arp_packet { struct wolfIP_eth_frame eth; @@ -843,6 +868,9 @@ struct wolfIP struct tsocket tcpsockets[MAX_TCPSOCKETS]; struct tsocket udpsockets[MAX_UDPSOCKETS]; struct tsocket icmpsockets[MAX_ICMPSOCKETS]; +#if WOLFIP_RAWSOCKETS + struct rawsocket rawsockets[WOLFIP_MAX_RAWSOCKETS]; +#endif uint16_t ipcounter; uint64_t last_tick; #ifdef ETHERNET @@ -990,6 +1018,36 @@ static inline unsigned int wolfIP_socket_if_idx(const struct tsocket *t) return t->if_idx; } +#if WOLFIP_RAWSOCKETS +static unsigned int wolfIP_if_for_local_ip(struct wolfIP *s, ip4 local_ip, int *found); +#endif + +#if WOLFIP_RAWSOCKETS +static unsigned int raw_route_for_ip(struct wolfIP *s, struct rawsocket *rs, ip4 dest, int dontroute) +{ + unsigned int if_idx = 0; + int match = 0; + if (!s) + return 0; + if (rs && rs->bound_local_ip != IPADDR_ANY) { + if_idx = wolfIP_if_for_local_ip(s, rs->bound_local_ip, &match); + if (match) + return if_idx; + } + if (dontroute) { + unsigned int i; + for (i = 0; i < s->if_count; i++) { + struct ipconf *conf = wolfIP_ipconf_at(s, i); + if (!conf || conf->ip == IPADDR_ANY) + continue; + if (ip_is_local_conf(conf, dest)) + return i; + } + } + return wolfIP_route_for_ip(s, dest); +} +#endif + #if CONFIG_IPFILTER static int wolfIP_filter_notify_socket_event( enum wolfIP_filter_reason reason, @@ -1131,6 +1189,17 @@ void wolfIP_register_callback(struct wolfIP *s, int sock_fd, void (*cb)(int sock t = &s->icmpsockets[SOCKET_UNMARK(sock_fd)]; t->callback = cb; t->callback_arg = arg; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sock_fd)) { + struct rawsocket *r; + if (SOCKET_UNMARK(sock_fd) >= WOLFIP_MAX_RAWSOCKETS) + return; + r = &s->rawsockets[SOCKET_UNMARK(sock_fd)]; + if (!r->used) + return; + r->callback = cb; + r->callback_arg = arg; +#endif } } @@ -1226,6 +1295,29 @@ static struct tsocket *udp_new_socket(struct wolfIP *s) return NULL; } +#if WOLFIP_RAWSOCKETS +static struct rawsocket *raw_new_socket(struct wolfIP *s, int protocol, int ipheader_include) +{ + int i; + + for (i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) { + struct rawsocket *r = &s->rawsockets[i]; + if (!r->used) { + memset(r, 0, sizeof(struct rawsocket)); + r->used = 1; + r->S = s; + r->protocol = (uint16_t)protocol; + r->ipheader_include = ipheader_include ? 1 : 0; + fifo_init(&r->rxbuf, r->rxmem, RXBUF_SIZE); + fifo_init(&r->txbuf, r->txmem, TXBUF_SIZE); + r->events |= CB_EVENT_WRITABLE; + return r; + } + } + return NULL; +} +#endif + static void udp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_udp_datagram *udp, uint32_t frame_len) { struct ipconf *conf = wolfIP_ipconf_at(s, if_idx); @@ -1300,6 +1392,33 @@ static void icmp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_i } } +#if WOLFIP_RAWSOCKETS +static void raw_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *ip, uint32_t frame_len) +{ + uint32_t payload_len = frame_len; + const uint8_t *packet = (const uint8_t *)ip; +#ifdef ETHERNET + if (frame_len <= ETH_HEADER_LEN) + return; + payload_len -= ETH_HEADER_LEN; + packet += ETH_HEADER_LEN; +#endif + (void)if_idx; + for (int i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) { + struct rawsocket *r = &s->rawsockets[i]; + if (!r->used) + continue; + if (r->protocol != 0 && r->protocol != ip->proto) + continue; + if (fifo_space(&r->rxbuf) < payload_len + sizeof(struct pkt_desc)) + continue; + fifo_push(&r->rxbuf, (void *)packet, payload_len); + r->last_pkt_ttl = ip->ttl; + r->events |= CB_EVENT_READABLE; + } +} +#endif + /* TCP */ static struct tsocket *tcp_new_socket(struct wolfIP *s) { @@ -1910,6 +2029,15 @@ static void close_socket(struct tsocket *ts) memset(ts, 0, sizeof(struct tsocket)); } +#if WOLFIP_RAWSOCKETS +static void close_rawsocket(struct rawsocket *rs) +{ + if (!rs) + return; + memset(rs, 0, sizeof(struct rawsocket)); +} +#endif + static struct tsocket *wolfIP_socket_from_fd(struct wolfIP *s, int sockfd) { if (!s || sockfd < 0) @@ -1930,6 +2058,19 @@ static struct tsocket *wolfIP_socket_from_fd(struct wolfIP *s, int sockfd) return NULL; } +#if WOLFIP_RAWSOCKETS +static struct rawsocket *wolfIP_rawsocket_from_fd(struct wolfIP *s, int sockfd) +{ + if (!s || sockfd < 0 || !IS_SOCKET_RAW(sockfd)) + return NULL; + if (SOCKET_UNMARK(sockfd) >= WOLFIP_MAX_RAWSOCKETS) + return NULL; + if (!s->rawsockets[SOCKET_UNMARK(sockfd)].used) + return NULL; + return &s->rawsockets[SOCKET_UNMARK(sockfd)]; +} +#endif + int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol) { @@ -1955,6 +2096,19 @@ int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol) } else { return -1; } +#if WOLFIP_RAWSOCKETS + } else if (type == IPSTACK_SOCK_RAW) { + struct rawsocket *rs; + int hdrincl = 0; +#ifdef IPPROTO_RAW + if (protocol == IPPROTO_RAW) + hdrincl = 1; +#endif + rs = raw_new_socket(s, protocol, hdrincl); + if (!rs) + return -1; + return (int)((rs - s->rawsockets) | MARK_RAW_SOCKET); +#endif } return -1; } @@ -2014,6 +2168,32 @@ int wolfIP_sock_connect(struct wolfIP *s, int sockfd, const struct wolfIP_sockad ts->local_ip = primary->ip; } return 0; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sockfd)) { + unsigned int if_idx; + struct ipconf *conf; + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if ((sin->sin_family != AF_INET) || (addrlen < sizeof(struct wolfIP_sockaddr_in))) + return -WOLFIP_EINVAL; + rs->remote_ip = ee32(sin->sin_addr.s_addr); + if_idx = raw_route_for_ip(s, rs, rs->remote_ip, rs->dontroute); + rs->if_idx = (uint8_t)if_idx; + if (!rs->ipheader_include) { + conf = wolfIP_ipconf_at(s, if_idx); + if (rs->bound_local_ip != IPADDR_ANY) + rs->local_ip = rs->bound_local_ip; + else if (conf && conf->ip != IPADDR_ANY) + rs->local_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + rs->local_ip = primary->ip; + } + } + return 0; +#endif } if (!IS_SOCKET_TCP(sockfd)) @@ -2152,9 +2332,15 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len struct wolfIP_tcp_seg *tcp; struct wolfIP_udp_datagram *udp; struct wolfIP_icmp_packet *icmp; +#if WOLFIP_RAWSOCKETS + struct wolfIP_ip_packet *rip; +#endif tcp = (struct wolfIP_tcp_seg *)frame; udp = (struct wolfIP_udp_datagram *)frame; icmp = (struct wolfIP_icmp_packet *)frame; +#if WOLFIP_RAWSOCKETS + rip = (struct wolfIP_ip_packet *)frame; +#endif (void)flags; if (sockfd < 0) @@ -2296,7 +2482,97 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len 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; + } +#if WOLFIP_RAWSOCKETS + else if (IS_SOCKET_RAW(sockfd)) { + const struct wolfIP_sockaddr_in *sin = (const struct wolfIP_sockaddr_in *)dest_addr; + struct rawsocket *rs; + unsigned int if_idx; + struct ipconf *conf; + ip4 dst_ip = 0; + int use_dontroute = 0; + uint32_t total_len; + + rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if (len == 0 || len > (LINK_MTU - ETH_HEADER_LEN)) + return -WOLFIP_EINVAL; + if (sin) { + if (addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -1; + dst_ip = ee32(sin->sin_addr.s_addr); + rs->remote_ip = dst_ip; + } else { + dst_ip = rs->remote_ip; + } + use_dontroute = rs->dontroute; +#ifdef MSG_DONTROUTE + if (flags & MSG_DONTROUTE) + use_dontroute = 1; +#else + (void)flags; +#endif + + if (rs->ipheader_include) { + if (len < IP_HEADER_LEN) + return -WOLFIP_EINVAL; + memset(rip, 0, ETH_HEADER_LEN); + memcpy(((uint8_t *)rip) + ETH_HEADER_LEN, buf, len); + rip->ttl = ((const uint8_t *)buf)[8]; + total_len = (uint32_t)len + ETH_HEADER_LEN; + if (dst_ip == 0) + dst_ip = ee32(rip->dst); + else + rip->dst = ee32(dst_ip); + if (rs->remote_ip == 0 && dst_ip != 0) + rs->remote_ip = dst_ip; + rs->local_ip = ee32(rip->src); + } else { + ip4 src_ip = rs->local_ip; + total_len = (uint32_t)len + IP_HEADER_LEN + ETH_HEADER_LEN; + if (dst_ip == 0) + return -WOLFIP_EINVAL; + if_idx = raw_route_for_ip(s, rs, dst_ip, use_dontroute); + conf = wolfIP_ipconf_at(s, if_idx); + rs->if_idx = (uint8_t)if_idx; + if (rs->bound_local_ip != IPADDR_ANY) + src_ip = rs->bound_local_ip; + else if (conf && conf->ip != IPADDR_ANY) + src_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + src_ip = primary->ip; + } + rs->local_ip = src_ip; + memset(rip, 0, sizeof(struct wolfIP_ip_packet)); + rip->ver_ihl = 0x45; + rip->tos = 0; + rip->len = ee16((uint16_t)(len + IP_HEADER_LEN)); + rip->id = ee16(s->ipcounter++); + rip->flags_fo = 0; + rip->ttl = 64; + rip->proto = (uint8_t)rs->protocol; + rip->src = ee32(src_ip); + rip->dst = ee32(dst_ip); + rip->csum = 0; + iphdr_set_checksum(rip); + memcpy(rip->data, buf, len); + } + if (dst_ip == 0) + return -WOLFIP_EINVAL; + if_idx = raw_route_for_ip(s, rs, dst_ip, use_dontroute); + rs->if_idx = (uint8_t)if_idx; + if (total_len > LINK_MTU) + return -WOLFIP_EINVAL; + if (fifo_space(&rs->txbuf) < total_len) + return -WOLFIP_EAGAIN; + fifo_push(&rs->txbuf, rip, total_len); + return (int)len; + } +#endif + else return -1; } int wolfIP_sock_send(struct wolfIP *s, int sockfd, const void *buf, size_t len, int flags) @@ -2388,6 +2664,36 @@ int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, in fifo_pop(&ts->sock.udp.rxbuf); ts->events &= ~CB_EVENT_READABLE; return (int)seg_len; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs; + struct wolfIP_sockaddr_in *sin = (struct wolfIP_sockaddr_in *)src_addr; + const uint8_t *pkt; + ip4 src_ip; + rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if (sin && *addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -1; + desc = fifo_peek(&rs->rxbuf); + if (!desc) + return -WOLFIP_EAGAIN; + if (desc->len > len) + return -1; + pkt = rs->rxmem + desc->pos + sizeof(*desc); + memcpy(&src_ip, pkt + 12, sizeof(src_ip)); + if (sin) { + sin->sin_family = AF_INET; + sin->sin_port = 0; + sin->sin_addr.s_addr = src_ip; + } + memcpy(buf, pkt, desc->len); + fifo_pop(&rs->rxbuf); + rs->events &= ~CB_EVENT_READABLE; + if (addrlen) + *addrlen = sizeof(struct wolfIP_sockaddr_in); + return (int)desc->len; +#endif } else return -WOLFIP_EINVAL; } @@ -2404,7 +2710,30 @@ int wolfIP_sock_read(struct wolfIP *s, int sockfd, void *buf, size_t len) 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); + struct tsocket *ts; +#if WOLFIP_RAWSOCKETS + if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + int enable; + if (!rs) + return -WOLFIP_EINVAL; + if (!optval || optlen < (socklen_t)sizeof(int)) + return -WOLFIP_EINVAL; + memcpy(&enable, optval, sizeof(int)); + if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { + rs->recv_ttl = enable ? 1 : 0; + return 0; + } else if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_HDRINCL) { + rs->ipheader_include = enable ? 1 : 0; + return 0; + } else if (level == WOLFIP_SOL_SOCKET && optname == WOLFIP_SO_DONTROUTE) { + rs->dontroute = enable ? 1 : 0; + return 0; + } + return -WOLFIP_EINVAL; + } +#endif + ts = wolfIP_socket_from_fd(s, sockfd); if (!ts) return -WOLFIP_EINVAL; if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { @@ -2420,7 +2749,20 @@ int wolfIP_sock_setsockopt(struct wolfIP *s, int sockfd, int level, int optname, int wolfIP_sock_get_recv_ttl(struct wolfIP *s, int sockfd, int *ttl) { - struct tsocket *ts = wolfIP_socket_from_fd(s, sockfd); + struct tsocket *ts; +#if WOLFIP_RAWSOCKETS + if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if (!rs->recv_ttl) + return 0; + if (ttl) + *ttl = rs->last_pkt_ttl; + return 1; + } +#endif + ts = wolfIP_socket_from_fd(s, sockfd); if (!ts) return -WOLFIP_EINVAL; if (!ts->recv_ttl) @@ -2432,7 +2774,30 @@ int wolfIP_sock_get_recv_ttl(struct wolfIP *s, int sockfd, int *ttl) 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); + struct tsocket *ts; +#if WOLFIP_RAWSOCKETS + if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + int value; + if (!rs) + return -WOLFIP_EINVAL; + if (!optval || !optlen || *optlen < (socklen_t)sizeof(int)) + return -WOLFIP_EINVAL; + if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { + value = rs->recv_ttl ? rs->last_pkt_ttl : 0; + } else if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_HDRINCL) { + value = rs->ipheader_include ? 1 : 0; + } else if (level == WOLFIP_SOL_SOCKET && optname == WOLFIP_SO_DONTROUTE) { + value = rs->dontroute ? 1 : 0; + } else { + return -WOLFIP_EINVAL; + } + memcpy(optval, &value, sizeof(int)); + *optlen = sizeof(int); + return 0; + } +#endif + ts = wolfIP_socket_from_fd(s, sockfd); if (!ts) return -WOLFIP_EINVAL; if (level == WOLFIP_SOL_IP && optname == WOLFIP_IP_RECVTTL) { @@ -2507,6 +2872,14 @@ int wolfIP_sock_close(struct wolfIP *s, int sockfd) ts->local_ip, ts->src_port, ts->remote_ip, 0); close_socket(ts); return 0; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + close_rawsocket(rs); + return 0; +#endif } else return -1; return 0; } @@ -2547,6 +2920,16 @@ int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr sin->sin_port = ee16(ts->src_port); sin->sin_addr.s_addr = ee32(ts->local_ip); return 0; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + sin->sin_family = AF_INET; + sin->sin_port = 0; + sin->sin_addr.s_addr = ee32(rs->local_ip); + return 0; +#endif } return -1; } @@ -2710,6 +3093,26 @@ int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr } } return 0; +#if WOLFIP_RAWSOCKETS + } else if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if (sin->sin_family != AF_INET) + return -WOLFIP_EINVAL; + rs->if_idx = (uint8_t)if_idx; + rs->bound_local_ip = bind_ip; + if (bind_ip != IPADDR_ANY) + rs->local_ip = bind_ip; + else if (conf && conf->ip != IPADDR_ANY) + rs->local_ip = conf->ip; + else { + struct ipconf *primary = wolfIP_primary_ipconf(s); + if (primary && primary->ip != IPADDR_ANY) + rs->local_ip = primary->ip; + } + return 0; +#endif } else return -1; } @@ -2745,6 +3148,21 @@ 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 WOLFIP_RAWSOCKETS + if (IS_SOCKET_RAW(sockfd)) { + struct rawsocket *rs = wolfIP_rawsocket_from_fd(s, sockfd); + if (!rs) + return -WOLFIP_EINVAL; + if (!sin || *addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -1; + if (rs->remote_ip == 0) + return -1; + sin->sin_family = AF_INET; + sin->sin_port = 0; + sin->sin_addr.s_addr = ee32(rs->remote_ip); + return 0; + } +#endif if (!IS_SOCKET_TCP(sockfd)) { return -1; } @@ -3353,6 +3771,9 @@ static inline void ip_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ #endif if (wolfIP_filter_notify_ip(WOLFIP_FILT_RECEIVING, s, if_idx, ip, len) != 0) return; +#if WOLFIP_RAWSOCKETS + raw_try_recv(s, if_idx, ip, len); +#endif #if WOLFIP_ENABLE_FORWARDING if (ip->ver_ihl == 0x45) { ip4 dest = ee32(ip->dst); @@ -3817,6 +4238,15 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) ts->events = 0; } } +#if WOLFIP_RAWSOCKETS + for (i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) { + struct rawsocket *r = &s->rawsockets[i]; + if (r->used && (r->callback) && (r->events)) { + r->callback(i | MARK_RAW_SOCKET, r->events, r->callback_arg); + r->events = 0; + } + } +#endif /* Step 4: attempt to write any pending data */ for (i = 0; i < MAX_TCPSOCKETS; i++) { @@ -3983,6 +4413,67 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) desc = fifo_peek(&t->sock.udp.txbuf); } } +#if WOLFIP_RAWSOCKETS + for (i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) { + struct rawsocket *r = &s->rawsockets[i]; + struct pkt_desc *desc; + if (!r->used) + continue; + desc = fifo_peek(&r->txbuf); + while (desc) { + struct wolfIP_ip_packet *ip = (struct wolfIP_ip_packet *)(r->txmem + desc->pos + sizeof(*desc)); + ip4 dst_ip = ee32(ip->dst); + unsigned int tx_if = r->if_idx; + ip4 nexthop; +#ifdef ETHERNET + struct ipconf *conf; +#endif + if (dst_ip == 0) { + fifo_pop(&r->txbuf); + desc = fifo_peek(&r->txbuf); + continue; + } + if (tx_if >= s->if_count) + tx_if = raw_route_for_ip(s, r, dst_ip, r->dontroute); + r->if_idx = (uint8_t)tx_if; +#ifdef ETHERNET + conf = wolfIP_ipconf_at(s, tx_if); + nexthop = r->dontroute ? dst_ip : wolfIP_select_nexthop(conf, dst_ip); + if (wolfIP_is_loopback_if(tx_if)) { + struct wolfIP_ll_dev *loop = wolfIP_ll_at(s, tx_if); + if (loop) + memcpy(r->nexthop_mac, loop->mac, 6); + } else if ((!IS_IP_BCAST(nexthop) && (arp_lookup(s, tx_if, nexthop, r->nexthop_mac) < 0))) { + arp_request(s, tx_if, nexthop); + break; + } else if (IS_IP_BCAST(nexthop)) { + memset(r->nexthop_mac, 0xFF, 6); + } +#else + nexthop = dst_ip; +#endif + if (!r->ipheader_include) { + ip->csum = 0; + iphdr_set_checksum(ip); + } + if (wolfIP_filter_notify_ip(WOLFIP_FILT_SENDING, s, tx_if, ip, desc->len) != 0) + break; +#ifdef ETHERNET + if (wolfIP_filter_notify_eth(WOLFIP_FILT_SENDING, s, tx_if, &ip->eth, desc->len) != 0) + break; + eth_output_add_header(s, tx_if, r->nexthop_mac, &ip->eth, ETH_TYPE_IP); +#endif + { + struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); + if (ll && ll->send) + ll->send(ll, ip, desc->len); + } + fifo_pop(&r->txbuf); + desc = fifo_peek(&r->txbuf); + (void)nexthop; + } + } +#endif return 0; } diff --git a/wolfip.h b/wolfip.h index e12b1f9..ee975fe 100644 --- a/wolfip.h +++ b/wolfip.h @@ -32,6 +32,22 @@ typedef unsigned long size_t; #endif #endif +#ifndef WOLFIP_SOL_SOCKET +#ifdef SOL_SOCKET +#define WOLFIP_SOL_SOCKET SOL_SOCKET +#else +#define WOLFIP_SOL_SOCKET 1 +#endif +#endif + +#ifndef WOLFIP_SO_DONTROUTE +#ifdef SO_DONTROUTE +#define WOLFIP_SO_DONTROUTE SO_DONTROUTE +#else +#define WOLFIP_SO_DONTROUTE 5 +#endif +#endif + #ifndef WOLFIP_IP_RECVTTL #ifdef IP_RECVTTL #define WOLFIP_IP_RECVTTL IP_RECVTTL @@ -40,6 +56,14 @@ typedef unsigned long size_t; #endif #endif +#ifndef WOLFIP_IP_HDRINCL +#ifdef IP_HDRINCL +#define WOLFIP_IP_HDRINCL IP_HDRINCL +#else +#define WOLFIP_IP_HDRINCL 3 +#endif +#endif + /* Types */ struct wolfIP; typedef uint32_t ip4; @@ -114,10 +138,12 @@ struct ipconf { #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 MARK_RAW_SOCKET 0x800 /* Mark a socket as RAW */ #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 IS_SOCKET_RAW(fd) (((fd) & MARK_RAW_SOCKET) == MARK_RAW_SOCKET) #define SOCKET_UNMARK(fd) ((fd) & 0xFF) /* Compile-time sanity check for socket marks & number of sockets */ @@ -129,6 +155,10 @@ struct ipconf { #error "MARK_UDP_SOCKET must be less than MARK_ICMP_SOCKET" #endif +#if (MARK_ICMP_SOCKET >= MARK_RAW_SOCKET) +#error "MARK_ICMP_SOCKET must be less than MARK_RAW_SOCKET" +#endif + #if MAX_TCPSOCKETS > 255 #error "MAX_TCPSOCKETS must be less than 256" #endif @@ -141,11 +171,17 @@ struct ipconf { #error "MAX_ICMPSOCKETS must be less than 256" #endif +#if WOLFIP_RAWSOCKETS +#if WOLFIP_MAX_RAWSOCKETS > 255 +#error "WOLFIP_MAX_RAWSOCKETS must be less than 256" +#endif +#endif #ifndef WOLF_POSIX #define IPSTACK_SOCK_STREAM 1 #define IPSTACK_SOCK_DGRAM 2 +#define IPSTACK_SOCK_RAW 3 struct wolfIP_sockaddr_in { @@ -188,6 +224,7 @@ struct msghdr { #include #define wolfIP_sockaddr_in sockaddr_in #define wolfIP_sockaddr sockaddr +#define IPSTACK_SOCK_RAW SOCK_RAW #endif int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol); From a4c72d71c330e264741d6e5898117b326ad6a454 Mon Sep 17 00:00:00 2001 From: Daniele Lacamera Date: Tue, 25 Nov 2025 16:47:48 +0100 Subject: [PATCH 2/2] Added support for raw AF_PACKET sockets --- .github/workflows/linux.yml | 6 +- Makefile | 6 +- config.h | 8 + src/port/posix/bsd_socket.c | 138 +++++++++ src/test/packet_ping.c | 574 ++++++++++++++++++++++++++++++++++++ src/test/unit/unit.c | 198 +++++++++++++ src/wolfip.c | 322 +++++++++++++++++++- wolfip.h | 46 +++ 8 files changed, 1295 insertions(+), 3 deletions(-) create mode 100644 src/test/packet_ping.c diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 872c6a2..55429cb 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -48,10 +48,14 @@ jobs: run: | sudo LD_PRELOAD=$PWD/libwolfip.so ping -c 5 10.10.10.1 - - name: Testing RAW sockets via raw_ping + - name: Testing RAW AF_INET sockets via raw_ping run: | sudo ./build/raw_ping 10.10.10.1 + - name: Testing RAW AF_PACKET sockets via packet_ping + run: | + sudo ./build/packet_ping wtcp0 10.10.10.1 + - name: Install check run: | sudo apt-get install -y check diff --git a/Makefile b/Makefile index acfea1c..4437b36 100644 --- a/Makefile +++ b/Makefile @@ -125,7 +125,7 @@ endif EXE=build/tcpecho build/tcp_netcat_poll build/tcp_netcat_select \ build/test-evloop build/test-dns build/test-wolfssl-forwarding \ build/test-ttl-expired build/test-wolfssl build/test-httpd \ - build/ipfilter-logger build/raw_ping + build/ipfilter-logger build/raw_ping build/packet_ping LIB=libwolfip.so PREFIX=/usr/local @@ -193,6 +193,10 @@ build/raw_ping: $(OBJ) build/port/posix/bsd_socket.o build/test/raw_ping.o @echo "[LD] $@" @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) +build/packet_ping: $(OBJ) build/port/posix/bsd_socket.o build/test/packet_ping.o + @echo "[LD] $@" + @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) + build/tcp_netcat_select: $(OBJ) build/port/posix/bsd_socket.o build/test/tcp_netcat_select.o @echo "[LD] $@" @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) $(END_GROUP) diff --git a/config.h b/config.h index 77b4470..48597e0 100644 --- a/config.h +++ b/config.h @@ -32,6 +32,14 @@ #define WOLFIP_MAX_RAWSOCKETS 4 #endif +#ifndef WOLFIP_PACKET_SOCKETS +#define WOLFIP_PACKET_SOCKETS 1 +#endif + +#ifndef WOLFIP_MAX_PACKETSOCKETS +#define WOLFIP_MAX_PACKETSOCKETS 2 +#endif + #ifndef WOLFIP_ENABLE_FORWARDING #define WOLFIP_ENABLE_FORWARDING 0 #endif diff --git a/src/port/posix/bsd_socket.c b/src/port/posix/bsd_socket.c index 8962768..22897f1 100644 --- a/src/port/posix/bsd_socket.c +++ b/src/port/posix/bsd_socket.c @@ -39,9 +39,15 @@ #include #include #include +#include +#include #define WOLF_POSIX #include "config.h" #include "wolfip.h" + +#ifdef ETHERNET +extern int wolfIP_arp_lookup_ex(struct wolfIP *s, unsigned int if_idx, ip4 ip, uint8_t *mac); +#endif static int wolfip_dbg_enabled; #ifndef WOLFIP_DBG #define WOLFIP_DBG(fmt, ...) \ @@ -109,6 +115,7 @@ static int (*host_setsockopt) (int sockfd, int level, int optname, const void *o static int (*host_getsockopt) (int sockfd, int level, int optname, void *optval, socklen_t *optlen); static int (*host_getsockname) (int sockfd, struct sockaddr *addr, socklen_t *addrlen); static int (*host_getpeername) (int sockfd, struct sockaddr *addr, socklen_t *addrlen); +static int (*host_ioctl) (int fd, unsigned long request, ...); 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); @@ -132,6 +139,9 @@ static int icmp_entry_for_slot[MAX_ICMPSOCKETS]; #if WOLFIP_RAWSOCKETS static int raw_entry_for_slot[WOLFIP_MAX_RAWSOCKETS]; #endif +#if WOLFIP_PACKET_SOCKETS +static int packet_entry_for_slot[WOLFIP_MAX_PACKETSOCKETS]; +#endif enum wolfip_dns_wait_type { DNS_WAIT_NONE = 0, @@ -184,6 +194,10 @@ static void wolfip_fd_pool_init(void) #if WOLFIP_RAWSOCKETS for (i = 0; i < WOLFIP_MAX_RAWSOCKETS; i++) raw_entry_for_slot[i] = -1; +#endif +#if WOLFIP_PACKET_SOCKETS + for (i = 0; i < WOLFIP_MAX_PACKETSOCKETS; i++) + packet_entry_for_slot[i] = -1; #endif init_done = 1; } @@ -212,6 +226,13 @@ static struct wolfip_fd_entry *wolfip_entry_from_internal(int internal_fd) if (pos < 0 || pos >= WOLFIP_MAX_RAWSOCKETS) return NULL; idx = raw_entry_for_slot[pos]; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos < 0 || pos >= WOLFIP_MAX_PACKETSOCKETS) + return NULL; + idx = packet_entry_for_slot[pos]; #endif } else { return NULL; @@ -251,6 +272,12 @@ static void wolfip_fd_detach_internal(int internal_fd) int pos = SOCKET_UNMARK(internal_fd); if (pos >= 0 && pos < WOLFIP_MAX_RAWSOCKETS) raw_entry_for_slot[pos] = -1; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < WOLFIP_MAX_PACKETSOCKETS) + packet_entry_for_slot[pos] = -1; #endif } } @@ -274,6 +301,12 @@ static void wolfip_fd_attach_internal(int internal_fd, int entry_idx) int pos = SOCKET_UNMARK(internal_fd); if (pos >= 0 && pos < WOLFIP_MAX_RAWSOCKETS) raw_entry_for_slot[pos] = entry_idx; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(internal_fd)) { + int pos = SOCKET_UNMARK(internal_fd); + if (pos >= 0 && pos < WOLFIP_MAX_PACKETSOCKETS) + packet_entry_for_slot[pos] = entry_idx; #endif } } @@ -992,6 +1025,102 @@ int wolfIP_sock_poll(struct wolfIP *ipstack, struct pollfd *fds, nfds_t nfds, in return ret; } +int ioctl(int fd, unsigned long request, ...) +{ + va_list ap; + void *arg; + struct ifreq *ifr; + int i; + + va_start(ap, request); + arg = va_arg(ap, void *); + va_end(ap); + + if (in_the_stack) { + return host_ioctl ? host_ioctl(fd, request, arg) : -1; + } + + if (request == SIOCGIFINDEX || request == SIOCGIFHWADDR || request == SIOCGIFADDR) { + struct wolfip_fd_entry *entry = wolfip_entry_from_public(fd); + if (!entry) { + return host_ioctl(fd, request, arg); + } + ifr = (struct ifreq *)arg; + if (!ifr) { + errno = EINVAL; + return -1; + } + for (i = 0; i < WOLFIP_MAX_INTERFACES; i++) { + struct wolfIP_ll_dev *ll = wolfIP_getdev_ex(IPSTACK, (unsigned int)i); + if (!ll) + continue; + if (ifr->ifr_name[0] != '\0' && strncmp(ifr->ifr_name, (char *)ll->ifname, IFNAMSIZ) != 0) + continue; + if (request == SIOCGIFINDEX) { + ifr->ifr_ifindex = i; + return 0; + } else if (request == SIOCGIFHWADDR) { + ifr->ifr_hwaddr.sa_family = ARPHRD_ETHER; + memcpy(ifr->ifr_hwaddr.sa_data, ll->mac, 6); + return 0; + } else if (request == SIOCGIFADDR) { + struct sockaddr_in *sin = (struct sockaddr_in *)&ifr->ifr_addr; + ip4 ip = 0, mask = 0, gw = 0; + memset(sin, 0, sizeof(*sin)); + wolfIP_ipconfig_get_ex(IPSTACK, (unsigned int)i, &ip, &mask, &gw); + sin->sin_family = AF_INET; + sin->sin_addr.s_addr = ee32(ip); + return 0; + } + } + errno = ENODEV; + return -1; + } + + if (request == SIOCGARP) { + struct arpreq *ar = (struct arpreq *)arg; + struct sockaddr_in *pa; + uint8_t mac[6]; + unsigned int if_idx; + int ret; + + if (!ar) { + errno = EINVAL; + return -1; + } + if (ar->arp_pa.sa_family != AF_INET) { + errno = EAFNOSUPPORT; + return -1; + } + pa = (struct sockaddr_in *)&ar->arp_pa; + if_idx = 0; + if (ar->arp_dev[0] != '\0') { + for (if_idx = 0; if_idx < WOLFIP_MAX_INTERFACES; if_idx++) { + struct wolfIP_ll_dev *ll = wolfIP_getdev_ex(IPSTACK, if_idx); + if (!ll) + continue; + if (strncmp(ar->arp_dev, (char *)ll->ifname, IFNAMSIZ) == 0) + break; + } + if (if_idx >= WOLFIP_MAX_INTERFACES) { + errno = ENODEV; + return -1; + } + } + ret = wolfIP_arp_lookup_ex(IPSTACK, if_idx, ee32(pa->sin_addr.s_addr), mac); + if (ret < 0) { + errno = ENXIO; + return -1; + } + ar->arp_ha.sa_family = ARPHRD_ETHER; + memcpy(ar->arp_ha.sa_data, mac, 6); + ar->arp_flags = ATF_COM; + return 0; + } + + return host_ioctl ? host_ioctl(fd, request, arg) : -1; +} + int wolfIP_sock_select(struct wolfIP *ipstack, int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, struct timeval *timeout) { int i; int ret; @@ -1093,6 +1222,14 @@ int socket(int domain, int type, int protocol) { if (in_the_stack) { return host_socket(domain, type, protocol); } +#if !WOLFIP_PACKET_SOCKETS + if (domain == AF_PACKET) + return host_socket(domain, type, protocol); +#endif +#if !WOLFIP_RAWSOCKETS + if (base_type == SOCK_RAW) + return host_socket(domain, type, protocol); +#endif 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) { @@ -1439,6 +1576,7 @@ void __attribute__((constructor)) init_wolfip_posix() { swap_socketcall(poll, "poll"); swap_socketcall(select, "select"); swap_socketcall(fcntl, "fcntl"); + swap_socketcall(ioctl, "ioctl"); pthread_mutex_init(&wolfIP_mutex, NULL); wolfIP_init_static(&IPSTACK); diff --git a/src/test/packet_ping.c b/src/test/packet_ping.c new file mode 100644 index 0000000..b085370 --- /dev/null +++ b/src/test/packet_ping.c @@ -0,0 +1,574 @@ +/* + * packet_ping.c + * + * Copyright (C) 2025 wolfSSL Inc. + * + * This file is part of wolfIP TCP/IP stack. + * + * wolfIP is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * wolfIP is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + * + * Very simplified ping-like utility using: + * socket(AF_PACKET, SOCK_RAW, htons(ETH_P_IP)) + * + * Sends 3 ICMP echo requests of 64 bytes (8B header + 56B data) + * to the host given, via the specified interface. + * + * Needs root or CAP_NET_RAW. + * + * Usage: + * packet_raw_ping + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#define NUM_PINGS_PACKET 3 +#define NUM_PINGS_INET 2 + +#define ICMP_DATA_SIZE 56 +#define ICMP_HEADER_SIZE (sizeof(struct icmphdr)) +#define ICMP_PACKET_SIZE (ICMP_HEADER_SIZE + ICMP_DATA_SIZE) + +#define IP_HEADER_SIZE (sizeof(struct iphdr)) +#define IP_PACKET_SIZE (IP_HEADER_SIZE + ICMP_PACKET_SIZE) + +#ifndef ETH_ALEN +#define ETH_ALEN 6 +#endif + +#ifndef ETH_HLEN +#define ETH_HLEN 14 +#endif + +#define ETH_FRAME_SIZE (ETH_HLEN + IP_PACKET_SIZE) +#define RECV_BUF_SIZE 2048 + +#ifndef ETH_P_IP +/* Fallback if ETH_P_IP is not defined */ +#define ETH_P_IP 0x0800 +#endif + +static unsigned short checksum(void *b, int len) +{ + unsigned short *buf; + unsigned int sum; + unsigned short result; + + buf = (unsigned short *)b; + sum = 0; + + while (len > 1) { + sum += *buf++; + len -= 2; + } + + if (len == 1) { + sum += *(unsigned char *)buf; + } + + sum = (sum >> 16) + (sum & 0xFFFF); + sum += (sum >> 16); + result = (unsigned short)~sum; + + return result; +} + +/* Get interface index, MAC address and IPv4 address */ +static int get_iface_info(const char *ifname, + int *ifindex, + unsigned char *mac, + struct in_addr *ip) +{ + int fd; + struct ifreq ifr; + struct sockaddr_in *sin; + + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + perror("socket(AF_INET,SOCK_DGRAM)"); + return -1; + } + + memset(&ifr, 0, sizeof(ifr)); + strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name) - 1); + ifr.ifr_name[sizeof(ifr.ifr_name) - 1] = '\0'; + + if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + perror("ioctl(SIOCGIFINDEX)"); + close(fd); + return -1; + } + *ifindex = ifr.ifr_ifindex; + + if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) { + perror("ioctl(SIOCGIFHWADDR)"); + close(fd); + return -1; + } + memcpy(mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN); + + if (ioctl(fd, SIOCGIFADDR, &ifr) < 0) { + perror("ioctl(SIOCGIFADDR)"); + close(fd); + return -1; + } + sin = (struct sockaddr_in *)&ifr.ifr_addr; + *ip = sin->sin_addr; + + close(fd); + return 0; +} + +/* Query MAC via SIOCGARP (wolfIP ioctl will serve from its ARP table) */ +static int get_dest_mac_via_ioctl(const char *ifname, const struct in_addr *ip, unsigned char *mac) +{ + int fd; + struct arpreq ar; + struct sockaddr_in *pa; + int ret = -1; + + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd < 0) { + perror("socket(AF_INET,SOCK_DGRAM) for ARP"); + return -1; + } + memset(&ar, 0, sizeof(ar)); + pa = (struct sockaddr_in *)&ar.arp_pa; + pa->sin_family = AF_INET; + pa->sin_addr = *ip; + strncpy(ar.arp_dev, ifname, sizeof(ar.arp_dev) - 1); + ar.arp_dev[sizeof(ar.arp_dev) - 1] = '\0'; + + if (ioctl(fd, SIOCGARP, &ar) == 0) { + memcpy(mac, ar.arp_ha.sa_data, ETH_ALEN); + ret = 0; + } else { + perror("ioctl(SIOCGARP)"); + } + close(fd); + return ret; +} + +/* Simple ICMP ping via AF_INET, SOCK_DGRAM, IPPROTO_ICMP */ +static int do_inet_ping(const char *host, + const struct in_addr *dst_ip, + int count) +{ + int fd; + struct sockaddr_in dst; + struct timeval tv; + char ipbuf[INET_ADDRSTRLEN]; + pid_t pid; + int seq; + int ret; + + fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_ICMP); + if (fd < 0) { + perror("socket(AF_INET,SOCK_DGRAM,IPPROTO_ICMP)"); + return -1; + } + + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, + (char *)&tv, sizeof(tv)) < 0) { + perror("setsockopt(SO_RCVTIMEO) inet ping"); + /* not fatal */ + } + + memset(&dst, 0, sizeof(dst)); + dst.sin_family = AF_INET; + dst.sin_addr = *dst_ip; + dst.sin_port = 0; + + strncpy(ipbuf, inet_ntoa(*dst_ip), sizeof(ipbuf) - 1); + ipbuf[sizeof(ipbuf) - 1] = '\0'; + + printf("INET_PING %s (%s): %d bytes ICMP data, %d probes\n", + host, + ipbuf, + ICMP_DATA_SIZE, + count); + + pid = getpid() & 0xFFFF; + + ret = 0; + for (seq = 1; seq <= count; ++seq) { + unsigned char packet[ICMP_PACKET_SIZE]; + unsigned char recvbuf[RECV_BUF_SIZE]; + struct icmphdr *icmp; + struct icmphdr *ricmp; + struct timeval t0; + struct timeval t1; + struct sockaddr_in from; + socklen_t fromlen; + ssize_t sent; + ssize_t recvd; + int i; + long sec; + long usec; + double rtt; + + memset(packet, 0, sizeof(packet)); + + icmp = (struct icmphdr *)packet; + icmp->type = ICMP_ECHO; + icmp->code = 0; + icmp->un.echo.id = htons((unsigned short)pid); + icmp->un.echo.sequence = htons((unsigned short)seq); + + for (i = 0; i < ICMP_DATA_SIZE; ++i) { + packet[ICMP_HEADER_SIZE + i] = + (unsigned char)('a' + (i % 26)); + } + + icmp->checksum = 0; + icmp->checksum = checksum(packet, ICMP_PACKET_SIZE); + + gettimeofday(&t0, NULL); + + sent = sendto(fd, packet, ICMP_PACKET_SIZE, 0, + (struct sockaddr *)&dst, sizeof(dst)); + if (sent < 0) { + perror("sendto inet ping"); + ret = -1; + break; + } + + fromlen = sizeof(from); + recvd = recvfrom(fd, recvbuf, sizeof(recvbuf), 0, + (struct sockaddr *)&from, &fromlen); + if (recvd < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + printf("INET_PING: Request timeout for icmp_seq %d\n", seq); + continue; + } else { + perror("recvfrom inet ping"); + ret = -1; + break; + } + } + + gettimeofday(&t1, NULL); + + if (recvd < (ssize_t)sizeof(struct icmphdr)) { + printf("INET_PING: short ICMP reply\n"); + continue; + } + + ricmp = (struct icmphdr *)recvbuf; + + if (ricmp->type == ICMP_ECHOREPLY && ricmp->code == 0) { + + sec = t1.tv_sec - t0.tv_sec; + usec = t1.tv_usec - t0.tv_usec; + if (usec < 0) { + --sec; + usec += 1000000; + } + rtt = sec * 1000.0 + usec / 1000.0; + + printf("%ld bytes from %s: icmp_seq=%d time=%.3f ms\n", + (long)(recvd - (long)sizeof(struct icmphdr)), + inet_ntoa(from.sin_addr), + seq, + rtt); + } else { + printf("INET_PING: unexpected ICMP type=%d code=%d\n", + ricmp->type, ricmp->code); + } + } + + close(fd); + return ret; +} + +int main(int argc, char *argv[]) +{ + const char *ifname; + const char *host; + struct addrinfo hints; + struct addrinfo *res; + int ret; + struct in_addr dst_ip; + struct in_addr src_ip; + unsigned char src_mac[ETH_ALEN]; + unsigned char dst_mac[ETH_ALEN]; + int ifindex; + int sockfd; + struct timeval tv; + struct sockaddr_ll bind_addr; + struct sockaddr_ll send_addr; + pid_t pid; + int seq; + char ip_str[INET_ADDRSTRLEN]; + + if (argc != 3) { + fprintf(stderr, "Usage: %s \n", argv[0]); + return 1; + } + + ifname = argv[1]; + host = argv[2]; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_protocol = IPPROTO_TCP; + + ret = getaddrinfo(host, NULL, &hints, &res); + if (ret != 0) { + fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(ret)); + return 1; + } + + dst_ip = ((struct sockaddr_in *)res->ai_addr)->sin_addr; + + if (get_iface_info(ifname, &ifindex, src_mac, &src_ip) < 0) { + freeaddrinfo(res); + return 1; + } + + /* Step 1–2: normal ICMP ping via AF_INET to trigger ARP */ + if (do_inet_ping(host, &dst_ip, NUM_PINGS_INET) < 0) { + /* we still try ARP lookup; maybe some packets got out anyway */ + } + + /* Step 3: get dest MAC from ARP cache */ + strncpy(ip_str, inet_ntoa(dst_ip), sizeof(ip_str) - 1); + ip_str[sizeof(ip_str) - 1] = '\0'; + + if (get_dest_mac_via_ioctl(ifname, &dst_ip, dst_mac) < 0) + memset(dst_mac, 0xFF, ETH_ALEN); /* broadcast fallback */ + + /* Step 4: AF_PACKET raw socket ping */ + sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_IP)); + if (sockfd < 0) { + perror("socket(AF_PACKET,SOCK_RAW,ETH_P_IP)"); + freeaddrinfo(res); + return 1; + } + + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, + (char *)&tv, sizeof(tv)) < 0) { + perror("setsockopt(SO_RCVTIMEO) packet ping"); + /* not fatal */ + } + + memset(&bind_addr, 0, sizeof(bind_addr)); + bind_addr.sll_family = AF_PACKET; + bind_addr.sll_protocol = htons(ETH_P_IP); + bind_addr.sll_ifindex = ifindex; + + if (bind(sockfd, (struct sockaddr *)&bind_addr, + sizeof(bind_addr)) < 0) { + perror("bind(AF_PACKET, IP)"); + freeaddrinfo(res); + close(sockfd); + return 1; + } + + memset(&send_addr, 0, sizeof(send_addr)); + send_addr.sll_family = AF_PACKET; + send_addr.sll_protocol = htons(ETH_P_IP); + send_addr.sll_ifindex = ifindex; + send_addr.sll_halen = ETH_ALEN; + memcpy(send_addr.sll_addr, dst_mac, ETH_ALEN); + + printf("PACKET_PING %s (%s) via %s: %d bytes ICMP data, %d probes\n", + host, + ip_str, + ifname, + ICMP_DATA_SIZE, + NUM_PINGS_PACKET); + + pid = getpid() & 0xFFFF; + + for (seq = 1; seq <= NUM_PINGS_PACKET; ++seq) { + unsigned char frame[ETH_FRAME_SIZE]; + struct ether_header *eth; + struct iphdr *ip; + struct icmphdr *icmp; + unsigned char *data; + int i; + struct timeval send_time; + struct timeval recv_time; + ssize_t sent; + unsigned char buf[RECV_BUF_SIZE]; + struct sockaddr_ll recv_addr; + socklen_t recv_addrlen; + ssize_t n; + struct ether_header *reth; + struct iphdr *rip; + int iphdr_len; + struct icmphdr *ricmp; + long sec; + long usec; + double rtt; + + memset(frame, 0, sizeof(frame)); + + /* Ethernet header */ + eth = (struct ether_header *)frame; + memcpy(eth->ether_dhost, dst_mac, ETH_ALEN); + memcpy(eth->ether_shost, src_mac, ETH_ALEN); + eth->ether_type = htons(ETH_P_IP); + + /* IPv4 header */ + ip = (struct iphdr *)(frame + ETH_HLEN); + ip->version = 4; + ip->ihl = IP_HEADER_SIZE / 4; + ip->tos = 0; + ip->tot_len = htons(IP_PACKET_SIZE); + ip->id = htons((unsigned short)seq); + ip->frag_off = htons(0); + ip->ttl = 64; + ip->protocol = IPPROTO_ICMP; + ip->check = 0; + ip->saddr = src_ip.s_addr; + ip->daddr = dst_ip.s_addr; + ip->check = checksum(ip, IP_HEADER_SIZE); + + /* ICMP header + data */ + icmp = (struct icmphdr *)(frame + ETH_HLEN + IP_HEADER_SIZE); + icmp->type = ICMP_ECHO; + icmp->code = 0; + icmp->un.echo.id = htons((unsigned short)pid); + icmp->un.echo.sequence = htons((unsigned short)seq); + + data = (unsigned char *)icmp + ICMP_HEADER_SIZE; + for (i = 0; i < ICMP_DATA_SIZE; ++i) { + data[i] = (unsigned char)('A' + (i % 26)); + } + + icmp->checksum = 0; + icmp->checksum = checksum(icmp, ICMP_PACKET_SIZE); + + gettimeofday(&send_time, NULL); + + sent = sendto(sockfd, frame, ETH_FRAME_SIZE, 0, + (struct sockaddr *)&send_addr, + sizeof(send_addr)); + if (sent < 0) { + perror("sendto packet ping"); + break; + } + + recv_addrlen = sizeof(recv_addr); + n = recvfrom(sockfd, buf, sizeof(buf), 0, + (struct sockaddr *)&recv_addr, + &recv_addrlen); + if (n < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + printf("PACKET_PING: Request timeout for icmp_seq %d\n", seq); + sleep(1); + continue; + } else { + perror("recvfrom packet ping"); + break; + } + } + + gettimeofday(&recv_time, NULL); + + if (recv_addr.sll_ifindex != ifindex) { + sleep(1); + continue; + } + + if (n < (ssize_t)(ETH_HLEN + IP_HEADER_SIZE + ICMP_HEADER_SIZE)) { + printf("PACKET_PING: short frame\n"); + sleep(1); + continue; + } + + reth = (struct ether_header *)buf; + if (ntohs(reth->ether_type) != ETH_P_IP) { + sleep(1); + continue; + } + + rip = (struct iphdr *)(buf + ETH_HLEN); + if (rip->protocol != IPPROTO_ICMP) { + sleep(1); + continue; + } + + if (rip->daddr != src_ip.s_addr) { + sleep(1); + continue; + } + + iphdr_len = rip->ihl * 4; + if (n < (ssize_t)(ETH_HLEN + iphdr_len + ICMP_HEADER_SIZE)) { + sleep(1); + continue; + } + + ricmp = (struct icmphdr *)(buf + ETH_HLEN + iphdr_len); + + if (ricmp->type == ICMP_ECHOREPLY && + ricmp->code == 0 && + ricmp->un.echo.id == htons((unsigned short)pid) && + ricmp->un.echo.sequence == htons((unsigned short)seq)) { + + sec = recv_time.tv_sec - send_time.tv_sec; + usec = recv_time.tv_usec - send_time.tv_usec; + if (usec < 0) { + --sec; + usec += 1000000; + } + rtt = sec * 1000.0 + usec / 1000.0; + + printf("%ld bytes from %s: icmp_seq=%d ttl=%d time=%.3f ms\n", + (long)(n - ETH_HLEN - iphdr_len), + inet_ntoa(*(struct in_addr *)&rip->saddr), + seq, + rip->ttl, + rtt); + } else { + printf("PACKET_PING: unexpected ICMP type=%d code=%d\n", + ricmp->type, ricmp->code); + } + + sleep(1); + } + + freeaddrinfo(res); + close(sockfd); + return 0; +} diff --git a/src/test/unit/unit.c b/src/test/unit/unit.c index 25f6742..ad1e26c 100644 --- a/src/test/unit/unit.c +++ b/src/test/unit/unit.c @@ -30,6 +30,8 @@ #endif #undef WOLFIP_RAWSOCKETS #define WOLFIP_RAWSOCKETS 1 +#undef WOLFIP_PACKET_SOCKETS +#define WOLFIP_PACKET_SOCKETS 1 #if WOLFIP_ENABLE_LOOPBACK #define TEST_LOOPBACK_IF 0U #define TEST_PRIMARY_IF 1U @@ -1500,6 +1502,198 @@ START_TEST(test_raw_socket_send_builds_ip_header) } END_TEST +START_TEST(test_packet_socket_recv_frame) +{ +#if WOLFIP_PACKET_SOCKETS + struct wolfIP s; + int sd; + uint8_t frame_buf[ETH_HEADER_LEN + 6]; + struct wolfIP_eth_frame *frame = (struct wolfIP_eth_frame *)frame_buf; + uint8_t payload[6] = {0, 1, 2, 3, 4, 5}; + struct wolfIP_sockaddr_ll sll; + struct wolfIP_sockaddr_ll bind_sll; + socklen_t sll_len = sizeof(sll); + uint8_t rxbuf[sizeof(struct wolfIP_eth_frame) + sizeof(payload)]; + struct wolfIP_ll_dev *ll; + + wolfIP_init(&s); + mock_link_init(&s); + ll = wolfIP_getdev_ex(&s, TEST_PRIMARY_IF); + ck_assert_ptr_nonnull(ll); + + sd = wolfIP_sock_socket(&s, AF_PACKET, IPSTACK_SOCK_RAW, ETH_TYPE_IP); + ck_assert_int_ge(sd, 0); + + memset(&bind_sll, 0, sizeof(bind_sll)); + bind_sll.sll_family = AF_PACKET; + bind_sll.sll_protocol = ETH_TYPE_IP; + bind_sll.sll_ifindex = TEST_PRIMARY_IF; + bind_sll.sll_halen = 6; + ck_assert_int_eq(wolfIP_sock_bind(&s, sd, (struct wolfIP_sockaddr *)&bind_sll, sizeof(bind_sll)), 0); + + memset(frame, 0, sizeof(frame_buf)); + memcpy(frame->dst, ll->mac, 6); + memcpy(frame->src, "\xaa\xbb\xcc\xdd\xee\xff", 6); + frame->type = ETH_TYPE_IP; + memcpy(frame->data, payload, sizeof(payload)); + + wolfIP_recv_ex(&s, TEST_PRIMARY_IF, frame, (uint32_t)(ETH_HEADER_LEN + sizeof(payload))); + + memset(&sll, 0, sizeof(sll)); + sll_len = sizeof(sll); + ck_assert_int_eq(wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, + (struct wolfIP_sockaddr *)&sll, &sll_len), (int)(ETH_HEADER_LEN + sizeof(payload))); + ck_assert_mem_eq(rxbuf, frame, ETH_HEADER_LEN + sizeof(payload)); + ck_assert_uint_eq(sll.sll_family, AF_PACKET); + ck_assert_uint_eq(sll.sll_protocol, frame->type); + ck_assert_int_eq(sll.sll_ifindex, TEST_PRIMARY_IF); + ck_assert_uint_eq(sll.sll_halen, 6); + ck_assert_mem_eq(sll.sll_addr, frame->src, 6); +#else + ck_abort_msg("WOLFIP_PACKET_SOCKETS disabled"); +#endif +} +END_TEST + +START_TEST(test_packet_socket_send_frame) +{ +#if WOLFIP_PACKET_SOCKETS + struct wolfIP s; + int sd; + struct wolfIP_sockaddr_ll sll; + uint8_t frame_buf[ETH_HEADER_LEN + 8]; + struct wolfIP_eth_frame *ethf = (struct wolfIP_eth_frame *)frame_buf; + struct wolfIP_ll_dev *ll; + struct wolfIP_sockaddr_ll bind_sll; + + wolfIP_init(&s); + mock_link_init(&s); + ll = wolfIP_getdev_ex(&s, TEST_PRIMARY_IF); + ck_assert_ptr_nonnull(ll); + + sd = wolfIP_sock_socket(&s, AF_PACKET, IPSTACK_SOCK_RAW, ETH_TYPE_IP); + ck_assert_int_ge(sd, 0); + + memset(&bind_sll, 0, sizeof(bind_sll)); + bind_sll.sll_family = AF_PACKET; + bind_sll.sll_protocol = ETH_TYPE_IP; + bind_sll.sll_ifindex = TEST_PRIMARY_IF; + bind_sll.sll_halen = 6; + memset(bind_sll.sll_addr, 0xFF, 6); + ck_assert_int_eq(wolfIP_sock_bind(&s, sd, (struct wolfIP_sockaddr *)&bind_sll, sizeof(bind_sll)), 0); + + memset(&sll, 0, sizeof(sll)); + sll.sll_family = AF_PACKET; + sll.sll_protocol = ETH_TYPE_IP; + sll.sll_ifindex = TEST_PRIMARY_IF; + sll.sll_halen = 6; + memset(sll.sll_addr, 0xFF, 6); + + memset(frame_buf, 0, sizeof(frame_buf)); + memcpy(ethf->dst, "\xff\xff\xff\xff\xff\xff", 6); + memcpy(ethf->src, "\x00\x00\x00\x00\x00\x01", 6); + ethf->type = ETH_TYPE_IP; + memset(ethf->data, 0xAB, 8); + + ck_assert_int_eq(wolfIP_sock_sendto(&s, sd, frame_buf, sizeof(frame_buf), 0, + (struct wolfIP_sockaddr *)&sll, sizeof(sll)), (int)sizeof(frame_buf)); + { + struct packetsocket *ps = &s.packetsockets[SOCKET_UNMARK(sd)]; + struct pkt_desc *desc = fifo_peek(&ps->txbuf); + if (desc) { + struct wolfIP_eth_frame *queued = + (struct wolfIP_eth_frame *)(ps->txmem + desc->pos + sizeof(*desc)); + ck_assert_uint_eq(desc->len, sizeof(frame_buf)); + ck_assert_mem_eq(queued->dst, "\xff\xff\xff\xff\xff\xff", 6); + ck_assert_mem_eq(queued->src, ll->mac, 6); + ck_assert_uint_eq(queued->type, ETH_TYPE_IP); + ck_assert_mem_eq(queued->data, ethf->data, 8); + } + ck_assert_uint_eq(ps->if_idx, TEST_PRIMARY_IF); + } +#else + ck_abort_msg("WOLFIP_PACKET_SOCKETS disabled"); +#endif +} +END_TEST + +START_TEST(test_packet_socket_recv_wrong_proto_ignored) +{ +#if WOLFIP_PACKET_SOCKETS + struct wolfIP s; + int sd; + uint8_t frame_buf[ETH_HEADER_LEN + 6]; + struct wolfIP_eth_frame *frame = (struct wolfIP_eth_frame *)frame_buf; + struct wolfIP_ll_dev *ll; + uint8_t rxbuf[64]; + + wolfIP_init(&s); + mock_link_init(&s); + ll = wolfIP_getdev_ex(&s, TEST_PRIMARY_IF); + ck_assert_ptr_nonnull(ll); + + sd = wolfIP_sock_socket(&s, AF_PACKET, IPSTACK_SOCK_RAW, ETH_TYPE_IP); + ck_assert_int_ge(sd, 0); + + memset(frame, 0, sizeof(frame_buf)); + memcpy(frame->dst, ll->mac, 6); + memcpy(frame->src, "\xaa\xbb\xcc\xdd\xee\xff", 6); + frame->type = ee16(ETH_TYPE_ARP); /* different protocol */ + memset(frame->data, 0xAA, 6); + + wolfIP_recv_ex(&s, TEST_PRIMARY_IF, frame, (uint32_t)(ETH_HEADER_LEN + 6)); + + ck_assert_int_eq(wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, NULL, NULL), + -WOLFIP_EAGAIN); +#else + ck_abort_msg("WOLFIP_PACKET_SOCKETS disabled"); +#endif +} +END_TEST + +START_TEST(test_packet_socket_recv_other_interface_ignored) +{ +#if WOLFIP_PACKET_SOCKETS + struct wolfIP s; + int sd; + uint8_t frame_buf[ETH_HEADER_LEN + 6]; + struct wolfIP_eth_frame *frame = (struct wolfIP_eth_frame *)frame_buf; + uint8_t rxbuf[64]; + uint8_t other_mac[6] = {0x10, 0x20, 0x30, 0x40, 0x50, 0x60}; + + wolfIP_init(&s); + mock_link_init(&s); + mock_link_init_idx(&s, TEST_SECOND_IF, other_mac); + + sd = wolfIP_sock_socket(&s, AF_PACKET, IPSTACK_SOCK_RAW, ETH_TYPE_IP); + ck_assert_int_ge(sd, 0); + + { + struct wolfIP_sockaddr_ll bind_sll; + memset(&bind_sll, 0, sizeof(bind_sll)); + bind_sll.sll_family = AF_PACKET; + bind_sll.sll_protocol = ETH_TYPE_IP; + bind_sll.sll_ifindex = TEST_PRIMARY_IF; + bind_sll.sll_halen = 6; + ck_assert_int_eq(wolfIP_sock_bind(&s, sd, (struct wolfIP_sockaddr *)&bind_sll, sizeof(bind_sll)), 0); + } + + memset(frame, 0, sizeof(frame_buf)); + memcpy(frame->dst, other_mac, 6); + memcpy(frame->src, "\xaa\xbb\xcc\xdd\xee\xff", 6); + frame->type = ETH_TYPE_IP; + memset(frame->data, 0xBB, 6); + + wolfIP_recv_ex(&s, TEST_SECOND_IF, frame, (uint32_t)(ETH_HEADER_LEN + 6)); + + ck_assert_int_eq(wolfIP_sock_recvfrom(&s, sd, rxbuf, sizeof(rxbuf), 0, NULL, NULL), + -WOLFIP_EAGAIN); +#else + ck_abort_msg("WOLFIP_PACKET_SOCKETS disabled"); +#endif +} +END_TEST + START_TEST(test_raw_socket_recv_protocol_mismatch) { struct wolfIP s; @@ -1647,6 +1841,10 @@ Suite *wolf_suite(void) tcase_add_test(tc_proto, test_raw_socket_send_hdrincl_respected); tcase_add_test(tc_proto, test_raw_socket_send_builds_ip_header); tcase_add_test(tc_proto, test_raw_socket_recv_protocol_mismatch); + tcase_add_test(tc_proto, test_packet_socket_recv_frame); + tcase_add_test(tc_proto, test_packet_socket_send_frame); + tcase_add_test(tc_proto, test_packet_socket_recv_wrong_proto_ignored); + tcase_add_test(tc_proto, test_packet_socket_recv_other_interface_ignored); 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 aa9594a..4c81000 100644 --- a/src/wolfip.c +++ b/src/wolfip.c @@ -31,6 +31,11 @@ #include "wolfip.h" #include "config.h" +#if WOLFIP_PACKET_SOCKETS && !defined(ETHERNET) +#undef WOLFIP_PACKET_SOCKETS +#define WOLFIP_PACKET_SOCKETS 0 +#endif + #if WOLFIP_ENABLE_LOOPBACK #define WOLFIP_LOOPBACK_IF_IDX 0U #define WOLFIP_PRIMARY_IF_IDX 1U @@ -792,6 +797,29 @@ struct rawsocket { void (*callback)(int sock_fd, uint16_t events, void *arg); void *callback_arg; }; + +#if WOLFIP_PACKET_SOCKETS +struct sock_ll { + uint8_t src_mac[6]; + uint8_t dst_mac[6]; +}; + +struct packetsocket { + struct fifo rxbuf; + struct fifo txbuf; + uint8_t rxmem[RXBUF_SIZE]; + uint8_t txmem[TXBUF_SIZE]; + struct sock_ll macs; + struct wolfIP_sockaddr_ll bind_addr; + uint16_t protocol; + uint8_t if_idx; + uint8_t used; + uint16_t events; + struct wolfIP *S; + void (*callback)(int sock_fd, uint16_t events, void *arg); + void *callback_arg; +}; +#endif #endif #ifdef ETHERNET @@ -870,6 +898,9 @@ struct wolfIP struct tsocket icmpsockets[MAX_ICMPSOCKETS]; #if WOLFIP_RAWSOCKETS struct rawsocket rawsockets[WOLFIP_MAX_RAWSOCKETS]; +#endif +#if WOLFIP_PACKET_SOCKETS + struct packetsocket packetsockets[WOLFIP_MAX_PACKETSOCKETS]; #endif uint16_t ipcounter; uint64_t last_tick; @@ -1199,6 +1230,17 @@ void wolfIP_register_callback(struct wolfIP *s, int sock_fd, void (*cb)(int sock return; r->callback = cb; r->callback_arg = arg; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(sock_fd)) { + struct packetsocket *p; + if (SOCKET_UNMARK(sock_fd) >= WOLFIP_MAX_PACKETSOCKETS) + return; + p = &s->packetsockets[SOCKET_UNMARK(sock_fd)]; + if (!p->used) + return; + p->callback = cb; + p->callback_arg = arg; #endif } } @@ -1318,6 +1360,32 @@ static struct rawsocket *raw_new_socket(struct wolfIP *s, int protocol, int iphe } #endif +#if WOLFIP_PACKET_SOCKETS +static struct packetsocket *packet_new_socket(struct wolfIP *s, int protocol) +{ + int i; + + for (i = 0; i < WOLFIP_MAX_PACKETSOCKETS; i++) { + struct packetsocket *p = &s->packetsockets[i]; + if (!p->used) { + memset(p, 0, sizeof(struct packetsocket)); + p->used = 1; + p->protocol = (uint16_t)protocol; /* network order already for ETH_* */ + p->if_idx = 0; + p->bind_addr.sll_family = AF_PACKET; + p->bind_addr.sll_protocol = (uint16_t)protocol; + p->bind_addr.sll_halen = 6; + fifo_init(&p->rxbuf, p->rxmem, RXBUF_SIZE); + fifo_init(&p->txbuf, p->txmem, TXBUF_SIZE); + p->events |= CB_EVENT_WRITABLE; + p->S = s; + return p; + } + } + return NULL; +} +#endif + static void udp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_udp_datagram *udp, uint32_t frame_len) { struct ipconf *conf = wolfIP_ipconf_at(s, if_idx); @@ -1419,6 +1487,36 @@ static void raw_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip } #endif +#if WOLFIP_PACKET_SOCKETS +static void packet_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_eth_frame *eth, uint32_t frame_len) +{ + uint16_t proto = eth->type; + uint32_t record_len; + uint8_t record[1 + LINK_MTU]; + int i; + + if (frame_len > LINK_MTU || frame_len == 0) + return; + record_len = frame_len + 1; + record[0] = (uint8_t)if_idx; + memcpy(record + 1, eth, frame_len); + + for (i = 0; i < WOLFIP_MAX_PACKETSOCKETS; i++) { + struct packetsocket *p = &s->packetsockets[i]; + if (!p->used) + continue; + if (p->protocol && p->protocol != proto) + continue; + if ((p->bind_addr.sll_ifindex >= 0) && (unsigned int)p->bind_addr.sll_ifindex != if_idx && p->bind_addr.sll_ifindex != 0) + continue; + if (fifo_space(&p->rxbuf) < record_len + sizeof(struct pkt_desc)) + continue; + fifo_push(&p->rxbuf, record, record_len); + p->events |= CB_EVENT_READABLE; + } +} +#endif + /* TCP */ static struct tsocket *tcp_new_socket(struct wolfIP *s) { @@ -2038,6 +2136,15 @@ static void close_rawsocket(struct rawsocket *rs) } #endif +#if WOLFIP_PACKET_SOCKETS +static void close_packetsocket(struct packetsocket *ps) +{ + if (!ps) + return; + memset(ps, 0, sizeof(struct packetsocket)); +} +#endif + static struct tsocket *wolfIP_socket_from_fd(struct wolfIP *s, int sockfd) { if (!s || sockfd < 0) @@ -2071,12 +2178,26 @@ static struct rawsocket *wolfIP_rawsocket_from_fd(struct wolfIP *s, int sockfd) } #endif +#if WOLFIP_PACKET_SOCKETS +static struct packetsocket *wolfIP_packetsocket_from_fd(struct wolfIP *s, int sockfd) +{ + if (!s || sockfd < 0 || !IS_SOCKET_PACKET(sockfd)) + return NULL; + if (SOCKET_UNMARK(sockfd) >= WOLFIP_MAX_PACKETSOCKETS) + return NULL; + if (!s->packetsockets[SOCKET_UNMARK(sockfd)].used) + return NULL; + return &s->packetsockets[SOCKET_UNMARK(sockfd)]; +} +#endif + int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol) { struct tsocket *ts; + int base_type = type; if (domain != AF_INET) - return -1; + goto packet_try; if (type == IPSTACK_SOCK_STREAM) { ts = tcp_new_socket(s); if (!ts) @@ -2111,6 +2232,18 @@ int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol) #endif } return -1; + +packet_try: +#if WOLFIP_PACKET_SOCKETS + if (domain == AF_PACKET && base_type == IPSTACK_SOCK_RAW) { + struct packetsocket *ps; + ps = packet_new_socket(s, protocol); + if (!ps) + return -1; + return (int)((ps - s->packetsockets) | MARK_PACKET_SOCKET); + } +#endif + return -1; } int wolfIP_sock_connect(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, socklen_t addrlen) @@ -2571,6 +2704,49 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len fifo_push(&rs->txbuf, rip, total_len); return (int)len; } +#endif +#if WOLFIP_PACKET_SOCKETS + else if (IS_SOCKET_PACKET(sockfd)) { + struct packetsocket *ps = wolfIP_packetsocket_from_fd(s, sockfd); + const struct wolfIP_sockaddr_ll *sll = (const struct wolfIP_sockaddr_ll *)dest_addr; + struct wolfIP_eth_frame *ethf; + uint8_t frame[LINK_MTU]; + unsigned int tx_if = 0; + uint16_t proto = ps->protocol; + + if (!ps) + return -WOLFIP_EINVAL; + if (len < ETH_HEADER_LEN || len > LINK_MTU) + return -WOLFIP_EINVAL; + if (dest_addr && addrlen < sizeof(struct wolfIP_sockaddr_ll)) + return -WOLFIP_EINVAL; + memcpy(frame, buf, len); + ethf = (struct wolfIP_eth_frame *)frame; + tx_if = ps->if_idx; + if (sll) { + if (sll->sll_ifindex >= 0 && (unsigned int)sll->sll_ifindex < s->if_count) + tx_if = (unsigned int)sll->sll_ifindex; + if (sll->sll_halen >= 6) + memcpy(ethf->dst, sll->sll_addr, 6); + if (sll->sll_protocol) + proto = sll->sll_protocol; + } + if (tx_if >= s->if_count) + tx_if = 0; + { + struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); + if (ll) + memcpy(ethf->src, ll->mac, 6); + } + if (proto) + ethf->type = proto; + ps->if_idx = (uint8_t)tx_if; + if (fifo_space(&ps->txbuf) < len) + return -WOLFIP_EAGAIN; + if (fifo_push(&ps->txbuf, frame, (uint32_t)len) < 0) + return -WOLFIP_EAGAIN; + return (int)len; + } #endif else return -1; } @@ -2693,6 +2869,46 @@ int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, in if (addrlen) *addrlen = sizeof(struct wolfIP_sockaddr_in); return (int)desc->len; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(sockfd)) { + struct packetsocket *ps = wolfIP_packetsocket_from_fd(s, sockfd); + struct wolfIP_sockaddr_ll *sll = (struct wolfIP_sockaddr_ll *)src_addr; + struct wolfIP_eth_frame *ethf; + uint8_t if_idx_byte; + uint32_t frame_len; + uint8_t *pkt; + if (!ps) + return -WOLFIP_EINVAL; + if (sll && addrlen && *addrlen < sizeof(struct wolfIP_sockaddr_ll)) + return -WOLFIP_EINVAL; + desc = fifo_peek(&ps->rxbuf); + if (!desc) + return -WOLFIP_EAGAIN; + if (desc->len == 0) + return -WOLFIP_EAGAIN; + if (desc->len - 1 > len) + return -1; + pkt = ps->rxmem + desc->pos + sizeof(*desc); + if_idx_byte = pkt[0]; + ethf = (struct wolfIP_eth_frame *)(pkt + 1); + frame_len = desc->len - 1; + if (sll) { + memset(sll, 0, sizeof(*sll)); + sll->sll_family = AF_PACKET; + sll->sll_protocol = ethf->type; + sll->sll_ifindex = if_idx_byte; + sll->sll_hatype = 1; + sll->sll_pkttype = 0; + sll->sll_halen = 6; + memcpy(sll->sll_addr, ethf->src, 6); + if (addrlen) + *addrlen = sizeof(struct wolfIP_sockaddr_ll); + } + memcpy(buf, ethf, frame_len); + fifo_pop(&ps->rxbuf); + ps->events &= ~CB_EVENT_READABLE; + return (int)frame_len; #endif } else return -WOLFIP_EINVAL; @@ -2732,6 +2948,17 @@ int wolfIP_sock_setsockopt(struct wolfIP *s, int sockfd, int level, int optname, } return -WOLFIP_EINVAL; } +#endif +#if WOLFIP_PACKET_SOCKETS + if (IS_SOCKET_PACKET(sockfd)) { + /* Packet sockets: accept timeouts or ignore options */ + (void)s; + (void)level; + (void)optname; + (void)optval; + (void)optlen; + return 0; + } #endif ts = wolfIP_socket_from_fd(s, sockfd); if (!ts) @@ -2879,6 +3106,14 @@ int wolfIP_sock_close(struct wolfIP *s, int sockfd) return -WOLFIP_EINVAL; close_rawsocket(rs); return 0; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(sockfd)) { + struct packetsocket *ps = wolfIP_packetsocket_from_fd(s, sockfd); + if (!ps) + return -WOLFIP_EINVAL; + close_packetsocket(ps); + return 0; #endif } else return -1; return 0; @@ -2929,6 +3164,15 @@ int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr sin->sin_port = 0; sin->sin_addr.s_addr = ee32(rs->local_ip); return 0; +#endif +#if WOLFIP_PACKET_SOCKETS + } else if (IS_SOCKET_PACKET(sockfd)) { + struct packetsocket *ps = wolfIP_packetsocket_from_fd(s, sockfd); + struct wolfIP_sockaddr_ll *sll = (struct wolfIP_sockaddr_ll *)addr; + if (!ps || !sll || (addrlen && *addrlen < sizeof(struct wolfIP_sockaddr_ll))) + return -WOLFIP_EINVAL; + memcpy(sll, &ps->bind_addr, sizeof(*sll)); + return 0; #endif } return -1; @@ -2942,6 +3186,37 @@ int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr const struct wolfIP_sockaddr_in *sin = (const struct wolfIP_sockaddr_in *)addr; int match = 0; unsigned int if_idx; +#if WOLFIP_PACKET_SOCKETS + uint16_t sa_family; + + if (!addr || addrlen < sizeof(uint16_t)) + return -WOLFIP_EINVAL; + sa_family = addr->sa_family; +#else + if (!sin || addrlen < sizeof(struct wolfIP_sockaddr_in)) + return -WOLFIP_EINVAL; +#endif + +#if WOLFIP_PACKET_SOCKETS + if (IS_SOCKET_PACKET(sockfd)) { + struct packetsocket *ps = wolfIP_packetsocket_from_fd(s, sockfd); + const struct wolfIP_sockaddr_ll *sll = (const struct wolfIP_sockaddr_ll *)addr; + struct wolfIP_ll_dev *ll; + if (!ps || sa_family != AF_PACKET || addrlen < sizeof(struct wolfIP_sockaddr_ll)) + return -WOLFIP_EINVAL; + if (sll->sll_ifindex < 0 || (unsigned int)sll->sll_ifindex >= s->if_count) + return -WOLFIP_EINVAL; + ps->if_idx = (uint8_t)sll->sll_ifindex; + ps->protocol = sll->sll_protocol; + memcpy(&ps->bind_addr, sll, sizeof(ps->bind_addr)); + ll = wolfIP_ll_at(s, ps->if_idx); + if (ll) + memcpy(ps->macs.src_mac, ll->mac, 6); + if (ps->bind_addr.sll_halen == 0) + ps->bind_addr.sll_halen = 6; + return 0; + } +#endif if (!sin || addrlen < sizeof(struct wolfIP_sockaddr_in)) return -WOLFIP_EINVAL; @@ -3689,6 +3964,13 @@ static int arp_lookup(struct wolfIP *s, unsigned int if_idx, ip4 ip, uint8_t *ma return -1; } +#endif /* ETHERNET */ + +#ifdef ETHERNET +int wolfIP_arp_lookup_ex(struct wolfIP *s, unsigned int if_idx, ip4 ip, uint8_t *mac) +{ + return arp_lookup(s, if_idx, ip, mac); +} #endif /* Initialize the IP stack */ @@ -3844,6 +4126,9 @@ static void wolfIP_recv_on(struct wolfIP *s, unsigned int if_idx, void *buf, uin eth = (struct wolfIP_eth_frame *)buf; if (wolfIP_filter_notify_eth(WOLFIP_FILT_RECEIVING, s, if_idx, eth, len) != 0) return; +#if WOLFIP_PACKET_SOCKETS + packet_try_recv(s, if_idx, eth, len); +#endif if (eth->type == ee16(ETH_TYPE_IP)) { struct wolfIP_ip_packet *ip = (struct wolfIP_ip_packet *)eth; if ((memcmp(eth->dst, ll->mac, 6) != 0) && (memcmp(eth->dst, "\xff\xff\xff\xff\xff\xff", 6) != 0)) { @@ -4247,6 +4532,15 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) } } #endif +#if WOLFIP_PACKET_SOCKETS + for (i = 0; i < WOLFIP_MAX_PACKETSOCKETS; i++) { + struct packetsocket *p = &s->packetsockets[i]; + if (p->used && (p->callback) && (p->events)) { + p->callback(i | MARK_PACKET_SOCKET, p->events, p->callback_arg); + p->events = 0; + } + } +#endif /* Step 4: attempt to write any pending data */ for (i = 0; i < MAX_TCPSOCKETS; i++) { @@ -4473,6 +4767,32 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) (void)nexthop; } } +#endif +#if WOLFIP_PACKET_SOCKETS + for (i = 0; i < WOLFIP_MAX_PACKETSOCKETS; i++) { + struct packetsocket *p = &s->packetsockets[i]; + struct pkt_desc *desc; + if (!p->used) + continue; + desc = fifo_peek(&p->txbuf); + while (desc) { + uint8_t *frame = p->txmem + desc->pos + sizeof(*desc); + unsigned int tx_if = p->if_idx; + if (tx_if >= s->if_count) + tx_if = 0; + if (wolfIP_filter_notify_eth(WOLFIP_FILT_SENDING, s, tx_if, (struct wolfIP_eth_frame *)frame, desc->len) != 0) { + desc = fifo_next(&p->txbuf, desc); + continue; + } + { + struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); + if (ll && ll->send) + ll->send(ll, frame, desc->len); + } + fifo_pop(&p->txbuf); + desc = fifo_peek(&p->txbuf); + } + } #endif return 0; } diff --git a/wolfip.h b/wolfip.h index ee975fe..c49048d 100644 --- a/wolfip.h +++ b/wolfip.h @@ -64,6 +64,14 @@ typedef unsigned long size_t; #endif #endif +#ifndef WOLFIP_SOL_PACKET +#ifdef SOL_PACKET +#define WOLFIP_SOL_PACKET SOL_PACKET +#else +#define WOLFIP_SOL_PACKET 263 +#endif +#endif + /* Types */ struct wolfIP; typedef uint32_t ip4; @@ -139,11 +147,13 @@ struct ipconf { #define MARK_UDP_SOCKET 0x200 /* Mark a socket as UDP */ #define MARK_ICMP_SOCKET 0x400 /* Mark a socket as ICMP */ #define MARK_RAW_SOCKET 0x800 /* Mark a socket as RAW */ +#define MARK_PACKET_SOCKET 0x1000 /* Mark a socket as PACKET */ #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 IS_SOCKET_RAW(fd) (((fd) & MARK_RAW_SOCKET) == MARK_RAW_SOCKET) +#define IS_SOCKET_PACKET(fd) (((fd) & MARK_PACKET_SOCKET) == MARK_PACKET_SOCKET) #define SOCKET_UNMARK(fd) ((fd) & 0xFF) /* Compile-time sanity check for socket marks & number of sockets */ @@ -159,6 +169,10 @@ struct ipconf { #error "MARK_ICMP_SOCKET must be less than MARK_RAW_SOCKET" #endif +#if (MARK_RAW_SOCKET >= MARK_PACKET_SOCKET) +#error "MARK_RAW_SOCKET must be less than MARK_PACKET_SOCKET" +#endif + #if MAX_TCPSOCKETS > 255 #error "MAX_TCPSOCKETS must be less than 256" #endif @@ -177,11 +191,20 @@ struct ipconf { #endif #endif +#if WOLFIP_PACKET_SOCKETS +#if WOLFIP_MAX_PACKETSOCKETS > 255 +#error "WOLFIP_MAX_PACKETSOCKETS must be less than 256" +#endif +#endif + #ifndef WOLF_POSIX #define IPSTACK_SOCK_STREAM 1 #define IPSTACK_SOCK_DGRAM 2 #define IPSTACK_SOCK_RAW 3 +#ifndef AF_PACKET +#define AF_PACKET 17 +#endif struct wolfIP_sockaddr_in { @@ -216,17 +239,40 @@ struct msghdr { #ifndef AF_INET #define AF_INET 2 #endif +#ifndef AF_PACKET +#define AF_PACKET 17 +#endif #else #include #include #include #include #include +#include +#ifdef __has_include +#if __has_include() +#include +#define wolfIP_sockaddr_ll sockaddr_ll +#endif +#endif #define wolfIP_sockaddr_in sockaddr_in #define wolfIP_sockaddr sockaddr #define IPSTACK_SOCK_RAW SOCK_RAW #endif +#ifndef wolfIP_sockaddr_ll +struct wolfIP_sockaddr_ll { + unsigned short sll_family; + unsigned short sll_protocol; + int sll_ifindex; + unsigned short sll_hatype; + unsigned char sll_pkttype; + unsigned char sll_halen; + unsigned char sll_addr[8]; +}; +typedef struct wolfIP_sockaddr_ll wolfIP_sockaddr_ll; +#endif + int wolfIP_sock_socket(struct wolfIP *s, int domain, int type, int protocol); int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, socklen_t addrlen); int wolfIP_sock_listen(struct wolfIP *s, int sockfd, int backlog);