diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 3530cab..5556dc0 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -31,6 +31,13 @@ jobs: sudo ./build/test-evloop sudo killall tcpdump || true + - name: Run standalone "IPsec esp" test + run: | + sudo ./scripts/ip-xfrm/rfc4106 128 + sudo ./build/test-esp + sudo killall tcpdump || true + sudo ./scripts/ip-xfrm/delete_all + - name: Run standalone wolfssl test run: | sudo ./build/test-wolfssl diff --git a/.gitignore b/.gitignore index 3e93391..67816ac 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,7 @@ *.dis *.uf2 *.bin +*.swp CMakeCache.txt CMakeFiles CMakeScripts diff --git a/Makefile b/Makefile index 9ba83b1..57fcd2a 100644 --- a/Makefile +++ b/Makefile @@ -3,6 +3,24 @@ CFLAGS:=-Wall -Werror -Wextra -I. -D_GNU_SOURCE CFLAGS+=-g -ggdb -Wdeclaration-after-statement LDFLAGS+=-pthread +# +# Debug flags: +# tap debug: +# CFLAGS+=-DDEBUG_TAP +# +# print ethernet headers: +# CFLAGS+=-DDEBUG_ETH +# +# print ip headers: +# CFLAGS+=-DDEBUG_IP +# +# print tcp headers: +# CFLAGS+=-DDEBUG_TCP +# +# print esp header data: +# CFLAGS+=-DWOLFIP_DEBUG_ESP +# + UNAME_S:=$(shell uname -s) UNAME_M:=$(shell uname -m) UNAME_LC:=$(shell echo $(UNAME_S) | tr 'A-Z' 'a-z') @@ -102,6 +120,9 @@ OBJ=build/wolfip.o \ IPFILTER_OBJ=build/ipfilter/wolfip.o \ $(TAP_OBJ) +ESP_OBJ=build/esp/wolfip.o \ + $(TAP_OBJ) + HAVE_WOLFSSL:=$(shell printf "#include \nint main(void){return 0;}\n" | $(CC) $(CFLAGS) -x c - -c -o /dev/null 2>/dev/null && echo 1) # Require wolfSSL unless the requested goals are wolfSSL-independent (unit/cppcheck/clean). @@ -121,7 +142,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/test-esp LIB=libwolfip.so PREFIX=/usr/local @@ -154,6 +175,11 @@ asan: $(EXE) $(LIB) asan:CFLAGS+=-fsanitize=address asan:LDFLAGS+=-static-libasan +ESP_CFLAGS = \ + -DWOLFIP_ESP \ + -DWOLFSSL_WOLFIP \ + -DDEBUG_IP \ + -DWOLFIP_DEBUG_ESP # Test @@ -209,6 +235,20 @@ build/ipfilter/wolfip.o: src/wolfip.c build/test/ipfilter_logger.o: CFLAGS+=-DCONFIG_IPFILTER=1 +# ipsec esp +build/esp/wolfip.o: src/wolfip.c + @mkdir -p `dirname $@` || true + @echo "[CC] $< (esp)" + @$(CC) $(CFLAGS) $(ESP_CFLAGS) -c $< -o $@ + +build/test/test_esp.o: src/test/test_esp.c + @echo "[CC] $@" + @$(CC) $(CFLAGS) $(ESP_CFLAGS) -c $< -o $@ + +build/test-esp: $(ESP_OBJ) build/test/test_esp.o + @echo "[LD] $@" + @$(CC) $(CFLAGS) $(ESP_CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) -lwolfssl $(END_GROUP) + build/test-wolfssl-forwarding: build/test/test_wolfssl_forwarding.o build/test/wolfip_forwarding.o $(TAP_OBJ) build/port/wolfssl_io.o build/certs/server_key.o build/certs/ca_cert.o build/certs/server_cert.o @echo "[LD] $@" @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(BEGIN_GROUP) $(^) -lwolfssl $(END_GROUP) diff --git a/README.md b/README.md index 349ecd2..c48f60c 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,10 @@ ## Description and project goals -wolfIP is a TCP/IP stack with no dynamic memory allocations, designed to be +wolfIP is a TCP/IP stack with no dynamic memory allocations, designed to be used in resource-constrained embedded systems. -Endpoint only mode is supported, which means that wolfip can be used to +Endpoint only mode is supported, which means that wolfip can be used to establish network connections but it does not route traffic between different network interfaces. @@ -19,7 +19,7 @@ A single network interface can be associated with the device. - DHCP (RFC 2131): client only - DNS (RFC 1035): client only - UDP (RFC 768): unicast only -- TCP (RFC 793) +- TCP (RFC 793) - TCP options supported: Timestamps, Maximum Segment Size - BSD-like, non blocking socket API, with custom callbacks - No dynamic memory allocation diff --git a/core.md b/core.md index c8fb7b0..d95e7e8 100644 --- a/core.md +++ b/core.md @@ -24,7 +24,7 @@ ``` +---------------------------------------------------------------------------------------------------------------------------+ -| +-----+---+----+-----+------------------+-----+---+----+-----+------------------+ | +| +-----+---+----+-----+------------------+-----+---+----+-----+------------------+ | | | De | E | IP | TCP | Payload | De | E | IP | TCP | Payload | | | | sc | T | | | | sc | T | | | | | |* FREE SPACE * | ri | H | | | | ri | H | | | | * FREE SPACE* | @@ -32,11 +32,11 @@ | | or | | | | | or | | | | | | | +-----+---+----+-----+------------------+-----+---+----+-----+------------------+ | +---------------------------------------------------------------------------------------------------------------------------+ - ^ ^ - | | - | | - | | - |Tail Head| + ^ ^ + | | + | | + | | + |Tail Head| ``` @@ -54,11 +54,11 @@ | || || | | |*------------------------------------------*| | +--------------+--------------------------------------------+---------------------------------------------------------------+ - ^ ^ - | | - | | - | | - |Tail Head| + ^ ^ + | | + | | + | | + |Tail Head| ``` @@ -71,37 +71,32 @@ +-------------+ |Main loop TX | +-------------+ - ^ -+----------------------------------+ | -| | +------+ -| TCP Socket | | -| | | -| | | -| | | -| +-----------------------+ -| +---------------+ | | ->DATA OUT==>>|socket send() |-->| TX buffer (fifo) | -| +---------------+ | | -| +-----------------------+ -| | -| | -| | -| +-----------------------+ -| +-------------+ | | -DATA OUT==>>|socket send() |-->| TX buffer (fifo) | +| +---------------+ | | +| +-----------------------+ +| | +| | +| | +| +-----------------------+ +| +-------------+ | | + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "config.h" +#include "wolfip.h" +#include "wolfesp.h" + +#include "esp_sa_list.c" + +#define TEST_SIZE (4 * 1024) + +#define BUFFER_SIZE TEST_SIZE + +static int listen_fd = -1, client_fd = -1; +static int exit_ok = 0, exit_count = 0; +static uint8_t buf[TEST_SIZE]; +static int tot_sent = 0; +static int tot_recv = 0; +static int wolfIP_closing = 0; +static int closed = 0; +static int conn_fd = -1; +static int client_connected = 0; +/* "Test pattern - -" 16 chars without trailing null. */ +static const uint8_t test_pattern[16] = {0x54, 0x65, 0x73, 0x74, 0x20, 0x70, + 0x61, 0x74, 0x74, 0x65, 0x72, 0x6e, + 0x20, 0x2d, 0x20, 0x2d}; + + +/* wolfIP: server side callback. */ +static void server_cb(int fd, uint16_t event, void *arg) +{ + int ret = 0; + if ((fd == listen_fd) && (event & CB_EVENT_READABLE) && (client_fd == -1)) { + client_fd = wolfIP_sock_accept((struct wolfIP *)arg, listen_fd, NULL, + NULL); + if (client_fd > 0) { + printf("accept: %04x\n", client_fd); + } + } + else if ((fd == client_fd) && (event & CB_EVENT_READABLE )) { + ret = wolfIP_sock_recvfrom((struct wolfIP *)arg, client_fd, buf, + sizeof(buf), 0, NULL, NULL); + if (ret != -EAGAIN) { + if (ret < 0) { + printf("Recv error: %d\n", ret); + wolfIP_sock_close((struct wolfIP *)arg, client_fd); + } + else if (ret == 0) { + printf("Client side closed the connection.\n"); + wolfIP_sock_close((struct wolfIP *)arg, client_fd); + printf("Server: Exiting.\n"); + exit_ok = 1; + } + else if (ret > 0) { + printf("recv: %d, echoing back\n", ret); + tot_recv += ret; + } + } + } + if ((event & CB_EVENT_WRITABLE) || ((ret > 0) && !closed)) { + int snd_ret; + if ((tot_sent >= 4096) && wolfIP_closing) { + wolfIP_sock_close((struct wolfIP *)arg, client_fd); + printf("Server: I closed the connection.\n"); + closed = 1; + exit_ok = 1; + } + if ((!closed) && (tot_sent < tot_recv)) { + snd_ret = wolfIP_sock_sendto((struct wolfIP *)arg, client_fd, + buf + tot_sent, tot_recv - tot_sent, + 0, NULL, 0); + if (snd_ret != -EAGAIN) { + if (snd_ret < 0) { + printf("Send error: %d\n", snd_ret); + wolfIP_sock_close((struct wolfIP *)arg, client_fd); + } + else { + tot_sent += snd_ret; + printf("sent %d bytes\n", snd_ret); + if (tot_recv == tot_sent) { + tot_sent = 0; + tot_recv = 0; + } + } + } + } + } + if (event & CB_EVENT_CLOSED) { + printf("Closing %d, client fd: %d\n", fd, client_fd); + } + if ((fd == client_fd) && (event & CB_EVENT_CLOSED)) { + printf("Client side closed the connection (EVENT_CLOSED)\n"); + wolfIP_sock_close((struct wolfIP *)arg, client_fd); + client_fd = -1; + printf("Server: Exiting.\n"); + exit_ok = 1; + } + (void)arg; +} + +/* Client-side callback. */ +static void client_cb(int fd, uint16_t event, void *arg) +{ + struct wolfIP *s = (struct wolfIP *)arg; + uint32_t i; + int ret; + static unsigned int total_r = 0, total_w = 0; + if (fd == conn_fd) { + if ((event & CB_EVENT_WRITABLE) && (client_connected == 0)) { + printf("Client: connected\n"); + client_connected = 1; + } + } + if (total_w == 0) { + for (i = 0; i < sizeof(buf); i += sizeof(test_pattern)) { + memcpy(buf + i, test_pattern, sizeof(test_pattern)); + } + } + if (client_connected && (event & CB_EVENT_WRITABLE) && + (total_w < sizeof(buf))) { + ret = wolfIP_sock_sendto(s, fd, buf + total_w, sizeof(buf) - total_w, + 0, NULL, 0); + if (ret <= 0) { + printf("Test client write: %d\n", ret); + return; + } + total_w += ret; + } + + while ((total_r < total_w) && (event & CB_EVENT_READABLE)) { + ret = wolfIP_sock_recvfrom(s, fd, buf + total_r, sizeof(buf) - total_r, + 0, NULL, NULL); + if (ret < 0){ + if (ret != -EAGAIN) { + printf("Client read: %d\n", ret); + } + return; + } + if (ret == 0) { + printf("Client read: server has closed the connection.\n"); + return; + } + total_r += ret; + printf("Client RX total: %u\n", total_r); + } + if (total_r == sizeof(buf)) { + exit_ok = 1; + for (i = 0; i < sizeof(buf); i += sizeof(test_pattern)) { + if (memcmp(buf + i, test_pattern, sizeof(test_pattern))) { + printf("test client: pattern mismatch\n"); + printf("at position %u\n", i); + buf[i + 16] = 0; + printf("%s\n", &buf[i]); + return; + } + } + if (wolfIP_closing) { + wolfIP_sock_close(s, fd); + conn_fd = -1; + } + printf("Test client: success\n"); + } +} + +/* wolfIP side: main loop of the stack under test. */ +static int test_loop(struct wolfIP *s, int active_close) +{ + exit_ok = 0; + exit_count = 0; + tot_sent = 0; + wolfIP_closing = active_close; + closed = 0; + + while(1) { + uint32_t ms_next; + struct timeval tv; + gettimeofday(&tv, NULL); + ms_next = wolfIP_poll(s, tv.tv_sec * 1000 + tv.tv_usec / 1000); + usleep(ms_next * 1000); + if (exit_ok > 0) { + if (exit_count++ < 1) + continue; + else break; + } + } + return 0; +} + +/* Test code (host side). + * Thread with client to test the echoserver. + */ +static void *pt_echoclient(void *arg) +{ + int fd, ret; + unsigned total_r = 0; + unsigned i; + uint8_t local_buf[BUFFER_SIZE]; + uint32_t *srv_addr = (uint32_t *)arg; + int old_flags = -1; + fd_set wfds, rfds; + struct timeval tv; + socklen_t errlen; + int err; + struct sockaddr_in remote_sock = { + .sin_family = AF_INET, + .sin_port = ntohs(8), /* Echo */ + }; + remote_sock.sin_addr.s_addr = *srv_addr; + fd = socket(AF_INET, IPSTACK_SOCK_STREAM, 0); + if (fd < 0) { + printf("test client socket: %d\n", fd); + return (void *)-1; + } + sleep(1); + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &(int){1}, sizeof(int)); + printf("Connecting to echo server\n"); + old_flags = fcntl(fd, F_GETFL, 0); + if (old_flags < 0) { + perror("fcntl(F_GETFL)"); + close(fd); + return (void *)-1; + } + if (fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) { + perror("fcntl(F_SETFL)"); + close(fd); + return (void *)-1; + } + ret = connect(fd, (struct sockaddr *)&remote_sock, sizeof(remote_sock)); + if (ret < 0) { + err = errno; + printf("test client connect returned %d, errno=%d (%s)\n", ret, err, + strerror(err)); + if (err != EINPROGRESS) { + perror("connect"); + close(fd); + return (void *)-1; + } + printf("Waiting for connect to complete...\n"); + while (1) { + tv.tv_sec = 5; + tv.tv_usec = 0; + FD_ZERO(&rfds); + FD_ZERO(&wfds); + FD_SET(fd, &rfds); + FD_SET(fd, &wfds); + ret = select(fd + 1, &rfds, &wfds, NULL, &tv); + if (ret <= 0) { + printf("select returned %d (timeout or error)\n", ret); + if (ret < 0) { + perror("select"); + close(fd); + return (void *)-1; + } + } + errlen = sizeof(err); + if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen) < 0) { + perror("getsockopt(SO_ERROR)"); + close(fd); + return (void *)-1; + } + if (err == 0) { + printf("connect completed after select()\n"); + break; + } + if (ret == 0) { + printf("connect still in progress after timeout\n"); + continue; + } + if (err != EINPROGRESS && err != EALREADY && err != EWOULDBLOCK && + err != EAGAIN) { + printf("connect completed with error: %d (%s)\n", err, + strerror(err)); + close(fd); + return (void *)-1; + } + } + } + else { + printf("connect returned immediately\n"); + } + if (fcntl(fd, F_SETFL, old_flags) < 0) + perror("fcntl(restore)"); + printf("test client: connect succeeded\n"); + for (i = 0; i < sizeof(local_buf); i += sizeof(test_pattern)) { + memcpy(local_buf + i, test_pattern, sizeof(test_pattern)); + } + ret = write(fd, local_buf, sizeof(local_buf)); + if (ret < 0) { + int werr = errno; + printf("test client write: %d (errno=%d: %s)\n", ret, werr, + strerror(werr)); + perror("write"); + return (void *)-1; + } + printf("test client: wrote %d bytes\n", ret); + while (total_r < sizeof(local_buf)) { + ret = read(fd, local_buf + total_r, sizeof(local_buf) - total_r); + if (ret < 0) { + printf("failed test client read: %d\n", ret); + return (void *)-1; + } + if (ret == 0) { + printf("test client read: server has closed the connection.\n"); + if (wolfIP_closing) + return (void *)0; + else + return (void *)-1; + } + total_r += ret; + printf("test client: read %d bytes (total %u)\n", ret, total_r); + } + for (i = 0; i < sizeof(local_buf); i += sizeof(test_pattern)) { + if (memcmp(local_buf + i, test_pattern, sizeof(test_pattern))) { + printf("test client: pattern mismatch\n"); + printf("at position %u\n", i); + local_buf[i + 16] = 0; + printf("%s\n", &local_buf[i]); + return (void *)-1; + } + } + close(fd); + printf("Test client: success\n"); + return (void *)0; +} + +/* Test code (host side). + * Thread with echo server to test the client. + */ +static void *pt_echoserver(void *arg) +{ + int fd, ret; + unsigned total_r = 0; + uint8_t local_buf[BUFFER_SIZE]; + struct sockaddr_in local_sock = { + .sin_family = AF_INET, + .sin_port = ntohs(8), /* Echo */ + .sin_addr.s_addr = 0 + }; + wolfIP_closing = (uintptr_t)arg; + fd = socket(AF_INET, IPSTACK_SOCK_STREAM, 0); + if (fd < 0) { + printf("test server socket: %d\n", fd); + return (void *)-1; + } + local_sock.sin_addr.s_addr = inet_addr(HOST_STACK_IP); + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &(int){1}, sizeof(int)); + ret = bind(fd, (struct sockaddr *)&local_sock, sizeof(local_sock)); + if (ret < 0) { + printf("test server bind: %d (%s)\n", ret, strerror(errno)); + return (void *)-1; + } + ret = listen(fd, 1); + if (ret < 0) { + printf("test server listen: %d\n", ret); + return (void *)-1; + } + printf("Waiting for client\n"); + ret = accept(fd, NULL, NULL); + if (ret < 0) { + printf("test server accept: %d\n", ret); + return (void *)-1; + } + printf("test server: client %d connected\n", ret); + fd = ret; + while (1) { + ret = read(fd, local_buf + total_r, sizeof(local_buf) - total_r); + if (ret < 0) { + printf("failed test server read: %d (%s) \n", ret, strerror(errno)); + return (void *)-1; + } + if (ret == 0) { + printf("test server read: client has closed the connection.\n"); + if (wolfIP_closing) + return (void *)0; + else + return (void *)-1; + } + total_r += ret; + write(fd, local_buf + total_r - ret, ret); + } +} + +/* Catch-all function to initialize a new tap device as the network interface. + * This is defined in port/posix/bsd_socket.c + * */ +extern int tap_init(struct wolfIP_ll_dev *dev, const char *name, + uint32_t host_ip); + +/* Test cases */ + +static void test_wolfip_echoserver(struct wolfIP *s, uint32_t srv_ip) +{ + int ret, test_ret = 0; + pthread_t pt; + struct wolfIP_sockaddr_in local_sock = { + .sin_family = AF_INET, + .sin_port = ee16(8), /* Echo */ + .sin_addr.s_addr = 0 + }; + printf("TCP server tests\n"); + + listen_fd = wolfIP_sock_socket(s, AF_INET, IPSTACK_SOCK_STREAM, 0); + printf("socket: %04x\n", listen_fd); + wolfIP_register_callback(s, listen_fd, server_cb, s); + + pthread_create(&pt, NULL, pt_echoclient, &srv_ip); + printf("Starting test: echo server close-wait\n"); + ret = wolfIP_sock_bind(s, listen_fd, (struct wolfIP_sockaddr *)&local_sock, + sizeof(local_sock)); + printf("bind: %d\n", ret); + ret = wolfIP_sock_listen(s, listen_fd, 1); + printf("listen: %d\n", ret); + ret = test_loop(s, 0); + pthread_join(pt, (void **)&test_ret); + printf("Test echo server close-wait: %d\n", ret); + printf("Test host client: %d\n", test_ret); + sleep(1); + + pthread_create(&pt, NULL, pt_echoclient, &srv_ip); + printf("Starting test: echo server active close\n"); + ret = test_loop(s, 1); + printf("Test echo server close-wait: %d\n", ret); + pthread_join(pt, (void **)&test_ret); + printf("Test host client: %d\n", test_ret); + sleep(1); + + wolfIP_sock_close(s, listen_fd); +} + +static void test_wolfip_echoclient(struct wolfIP *s) +{ + int ret, test_ret = 0; + pthread_t pt; + struct wolfIP_sockaddr_in remote_sock; + /* Client side test: client is closing the connection */ + remote_sock.sin_family = AF_INET; + remote_sock.sin_port = ee16(8); + remote_sock.sin_addr.s_addr = inet_addr(HOST_STACK_IP); + printf("TCP client tests\n"); + conn_fd = wolfIP_sock_socket(s, AF_INET, IPSTACK_SOCK_STREAM, 0); + printf("client socket: %04x\n", conn_fd); + wolfIP_register_callback(s, conn_fd, client_cb, s); + printf("Connecting to %s:8\n", HOST_STACK_IP); + wolfIP_sock_connect(s, conn_fd, (struct wolfIP_sockaddr *)&remote_sock, + sizeof(remote_sock)); + pthread_create(&pt, NULL, pt_echoserver, (void*)1); + printf("Starting test: echo client active close\n"); + ret = test_loop(s, 1); + printf("Test echo client active close: %d\n", ret); + pthread_join(pt, (void **)&test_ret); + printf("Test host server: %d\n", test_ret); + + if (conn_fd >= 0) { + wolfIP_sock_close(s, conn_fd); + conn_fd = -1; + } +} + +/* Main test function. */ +int main(int argc, char **argv) +{ + struct wolfIP_ll_dev * tapdev = NULL; + struct wolfIP * s = NULL; + struct in_addr host_stack_ip; + uint32_t srv_ip = 0; + int err = 0; + + (void)argc; + (void)argv; + + err = wolfIP_esp_init(); + if (err) { + perror("esp_init"); + return 2; + } + + wolfIP_init_static(&s); + tapdev = wolfIP_getdev(s); + if (!tapdev) { + perror("wolfIP_getdev"); + return 1; + } + + inet_aton(HOST_STACK_IP, &host_stack_ip); + if (tap_init(tapdev, "wtcp0", host_stack_ip.s_addr) < 0) { + perror("tap init"); + return 2; + } + { +#if !defined(__FreeBSD__) && !defined(__APPLE__) + char cmd[128]; + snprintf(cmd, sizeof(cmd), "tcpdump -i %s -w test.pcap &", + tapdev->ifname); + system(cmd); +#else + (void)tapdev; +#endif + } + + wolfIP_ipconfig_set(s, atoip4(WOLFIP_IP), atoip4("255.255.255.0"), + atoip4(HOST_STACK_IP)); + printf("IP: manually configured\n"); + inet_pton(AF_INET, WOLFIP_IP, &srv_ip); + + wolfIP_esp_load_sa_list(test_in_sa_list, WOLFIP_ESP_NUM_SA, 1); + wolfIP_esp_load_sa_list(test_out_sa_list, WOLFIP_ESP_NUM_SA, 0); + + /* Server side test */ + test_wolfip_echoserver(s, srv_ip); + + /* Client side test */ + test_wolfip_echoclient(s); + +#if !defined(__FreeBSD__) && !defined(__APPLE__) + system("killall tcpdump"); +#endif + return 0; +} diff --git a/src/test/test_eventloop.c b/src/test/test_eventloop.c index d3eef06..dde8eb6 100644 --- a/src/test/test_eventloop.c +++ b/src/test/test_eventloop.c @@ -47,8 +47,10 @@ static int wolfIP_closing = 0; static int closed = 0; static int conn_fd = -1; static int client_connected = 0; -static const uint8_t test_pattern[16] = "Test pattern - -"; - +/* "Test pattern - -" 16 chars without trailing null. */ +static const uint8_t test_pattern[16] = {0x54, 0x65, 0x73, 0x74, 0x20, 0x70, + 0x61, 0x74, 0x74, 0x65, 0x72, 0x6e, + 0x20, 0x2d, 0x20, 0x2d}; /* wolfIP: server side callback. */ @@ -176,7 +178,6 @@ static void client_cb(int fd, uint16_t event, void *arg) } } - /* wolfIP side: main loop of the stack under test. */ static int test_loop(struct wolfIP *s, int active_close) { diff --git a/src/test/test_httpd.c b/src/test/test_httpd.c index fc8058f..bae5e1b 100644 --- a/src/test/test_httpd.c +++ b/src/test/test_httpd.c @@ -41,8 +41,6 @@ static int wolfIP_closing = 0; static int closed = 0; - - /* wolfIP side: main loop of the stack under test. */ static int test_loop(struct wolfIP *s, int active_close) { diff --git a/src/test/test_native_wolfssl.c b/src/test/test_native_wolfssl.c index 7c5e993..41b8504 100644 --- a/src/test/test_native_wolfssl.c +++ b/src/test/test_native_wolfssl.c @@ -44,7 +44,10 @@ static int tot_sent = 0; static int tot_recv = 0; static int wolfIP_closing = 0; static int closed = 0; -static const uint8_t test_pattern[16] = "Test pattern - -"; +/* "Test pattern - -" 16 chars without trailing null. */ +static const uint8_t test_pattern[16] = {0x54, 0x65, 0x73, 0x74, 0x20, 0x70, + 0x61, 0x74, 0x74, 0x65, 0x72, 0x6e, + 0x20, 0x2d, 0x20, 0x2d}; static WOLFSSL_CTX *server_ctx = NULL; /* Used by wolfIP */ static WOLFSSL_CTX *client_ctx = NULL; /* Used by Linux */ diff --git a/src/wolfesp.c b/src/wolfesp.c new file mode 100644 index 0000000..cb04d32 --- /dev/null +++ b/src/wolfesp.c @@ -0,0 +1,1107 @@ +#if defined(WOLFIP_ESP) && !defined(WOLFESP_SRC) +#define WOLFESP_SRC + +#include "wolfesp.h" + +static WC_RNG wc_rng; +static volatile int rng_inited = 0; + +int +wolfIP_esp_init(void) +{ + int ret = 0; + + if (rng_inited == 0) { + ret = wc_InitRng_ex(&wc_rng, NULL, INVALID_DEVID); + + if (ret) { + printf("error: wc_InitRng_ex returned: %d\n", ret); + } + else { + rng_inited = 1; + } + } + + return ret; +} + +static struct wolfIP_esp_sa * in_sa_list = NULL; +static struct wolfIP_esp_sa * out_sa_list = NULL; +static uint16_t in_sa_num = 0; +static uint16_t out_sa_num = 0; + +void +wolfIP_esp_load_sa_list(struct wolfIP_esp_sa * sa_list, uint16_t num, int in) +{ + #ifdef WOLFIP_DEBUG_ESP + printf("info: esp_load_sa_list: %p, %d, %d\n", sa_list, num, in); + #endif /* WOLFIP_DEBUG_ESP */ + + if (in == 1) { + in_sa_list = sa_list; + in_sa_num = num; + } + else { + out_sa_list = sa_list; + out_sa_num = num; + } + + return; +} + +#ifdef WOLFIP_DEBUG_ESP + #ifdef WOLFIP_DEBUG_ESP_VERBOSE + static void + esp_dump_data_verbose(const char * what, const uint8_t * data, + size_t data_len) + { + printf("info: %s: \n", what); + + for (size_t i = 0; i < data_len; ++i) { + printf("%02x", data[i]); + if (i && ((i + 1) % 8) == 0) { + printf("\n"); + } + } + + printf("\n"); + return; + } + #endif /* WOLFIP_DEBUG_ESP_VERBOSE */ + +static void +esp_dump_data(const char * what, const uint8_t * data, size_t data_len) +{ + printf("info: %s: 0x", what); + + for (size_t i = 0; i < data_len; ++i) { + printf("%02x", data[i]); + } + + printf("\n"); + return; +} + +#define esp_print_sep \ + printf("+------------------+\n") +#define esp_str_4hex \ + "| %02x %02x %02x %02x |" +#define esp_str_skip \ + "| .. .. .. .. |" +#define esp_pad_fld \ + "| %02x%02x | %02d | 0x%02x |" + +static inline void +esp_print_field(const char * fld, const uint8_t * val, + uint32_t val_len) +{ + esp_print_sep; + printf(esp_str_4hex " (%s, %d bytes)\n", + val[0], val[1], val[2], val[3], fld, val_len); + if (val_len > 4) { + for (size_t i = 4; i < val_len; i += 4) { + if (i > 16 || (i + 4) > val_len) { + printf(esp_str_skip "\n"); + break; + } + + printf(esp_str_4hex"\n", + val[0 + i], val[1 + i], val[2 + i], val[3 + i]); + } + } + return; +} + +/** + * Print an ESP packet. + * _______________________________________________ + * |orig IP hdr | ESP | UDP | | ESP | ESP | + * |(PROTO=50) | hdr | hdr | Data | Trailer | ICV | + * ----------------------------------------------- + * |<---- encrypted ----->| + * |<--- integrity checked ---->| + * */ +static void wolfIP_print_esp(const struct wolfIP_esp_sa * esp_sa, + const uint8_t * esp_data, uint32_t esp_len, + uint8_t pad_len, uint8_t nxt_hdr) +{ + const uint8_t * spi = esp_data; + const uint8_t * seq = esp_data + ESP_SPI_LEN; + const uint8_t * payload = esp_data + ESP_SPI_LEN + ESP_SEQ_LEN; + const uint8_t * iv = NULL; + const uint8_t * icv = NULL; + const uint8_t * padding = NULL; + uint32_t payload_len = esp_len - ESP_SPI_LEN - ESP_SEQ_LEN + - pad_len - ESP_PADDING_LEN + - ESP_NEXT_HEADER_LEN ; + + if (esp_sa->iv_len) { + iv = payload; + payload += esp_sa->iv_len; + payload_len -= esp_sa->iv_len; + } + + if (esp_sa->icv_len) { + icv = esp_data + esp_len - esp_sa->icv_len; + } + + /* last 2 bytes of padding */ + padding = esp_data + esp_len - esp_sa->icv_len - 4; + + printf("esp packet: (%d bytes)\n", esp_len); + + /** ESP header + * ______________ + * | SPI | Seq | + * | | Number | + * -------------- */ + esp_print_field("spi", spi, ESP_SPI_LEN); + esp_print_field("seq", seq, ESP_SEQ_LEN); + + /** + * ESP payload (includes IV). + * */ + if (iv) { + esp_print_field("iv", iv, esp_sa->iv_len); + } + + esp_print_field("payload", payload, payload_len); + + /** ESP trailer + * _____________________________________ + * | Padding | Pad | Next | + * | (variable length) | Length | Header | + * ------------------------------------- */ + esp_print_sep; + printf(esp_pad_fld " (padding last 2 bytes, pad len, nxt hdr)\n", + padding[0], padding[1], pad_len, nxt_hdr); + + if (icv) { + esp_print_field("icv", icv, esp_sa->icv_len); + } + + esp_print_sep; + + return; +} +#endif /* WOLFIP_DEBUG_ESP */ + +static uint8_t +esp_block_len_from_enc(esp_enc_t enc) +{ + uint8_t block_len = 0; + + switch (enc) { + case ESP_ENC_NONE: + block_len = 0; + break; + case ESP_ENC_CBC_AES: + block_len = AES_BLOCK_SIZE; + break; + #ifndef NO_DES3 + case ESP_ENC_CBC_DES3: + block_len = DES_BLOCK_SIZE; + break; + #endif /* !NO_DES3 */ + case ESP_ENC_GCM_RFC4106: + case ESP_ENC_GCM_RFC4543: + default: + block_len = 0; + break; + } + + return block_len; +} + +/* + * esp_data covers from start of ESP header to end of ESP trailer, but does not + * include the ESP ICV after trailer. + * */ +static int +esp_calc_icv_hmac(uint8_t * hash, const struct wolfIP_esp_sa * esp_sa, + const uint8_t * esp_data, uint32_t esp_len) +{ + /* SHA1 and MD5 have these digest sizes: + * - WC_SHA_DIGEST_SIZE 20 bytes + * - WC_MD5_DIGEST_SIZE 16 bytes + * */ + Hmac hmac; + int wolf_ret = 0; + int type = 0; + uint32_t auth_len = esp_len; + + switch (esp_sa->auth) { + case ESP_AUTH_MD5_RFC2403: + type = WC_MD5; + break; + case ESP_AUTH_SHA1_RFC2404: + type = WC_SHA; + break; + case ESP_AUTH_SHA256_RFC4868: + type = WC_SHA256; + break; + case ESP_AUTH_NONE: + default: + printf("error: esp_calc_icv_hmac: invalid auth: %d\n", + esp_sa->auth); + return -1; + } + + /* the icv is not included in icv calculation. */ + auth_len = esp_len - esp_sa->icv_len; + + wolf_ret = wc_HmacInit(&hmac, NULL, INVALID_DEVID); + + if (wolf_ret) { + printf("error: wc_HmacSetKey returned %d\n", wolf_ret); + goto calc_icv_hmac_end; + } + + wolf_ret = wc_HmacSetKey(&hmac, type, esp_sa->auth_key, + esp_sa->auth_key_len); + if (wolf_ret) { + printf("error: wc_HmacSetKey returned %d\n", wolf_ret); + goto calc_icv_hmac_end; + } + + /* Now calculate the ICV. The ICV covers from SPI to Next Header, + * inclusive. */ + wolf_ret = wc_HmacUpdate(&hmac, (const byte *)esp_data, auth_len); + if (wolf_ret) { + printf("error: wc_HmacUpdate returned %d\n", wolf_ret); + goto calc_icv_hmac_end; + } + + wolf_ret = wc_HmacFinal(&hmac, hash); + if (wolf_ret) { + printf("error: wc_HmacFinal returned %d\n", wolf_ret); + goto calc_icv_hmac_end; + } + +calc_icv_hmac_end: + wc_HmacFree(&hmac); + + return wolf_ret; +} + +/* From wolfcrypt misc.c */ +static int +esp_const_memcmp(const uint8_t * vec_a, const uint8_t * vec_b, uint32_t len) +{ + uint32_t i = 0; + int sum = 0; + + for (i = 0; i < len; i++) { + sum |= vec_a[i] ^ vec_b[i]; + } + + return sum; +} + +/** + * Get the encryption length for an ESP payload. + * */ +#define esp_enc_len(esp_len, iv_len, icv_len) \ + (esp_len) - ESP_SPI_LEN - ESP_SEQ_LEN \ + - (iv_len) - (icv_len) + +/** + * Get pointer to raw encryption ESP IV, skipping ESP header. + * */ +#define esp_enc_iv(data, iv_len) \ + (data) + ESP_SPI_LEN + ESP_SEQ_LEN + +/** + * Get pointer to raw encryption ESP ICV. + * */ +#define esp_enc_icv(data, esp_len, icv_len) \ + (data) + (esp_len) - (icv_len) + +/** + * Get pointer to raw encryption ESP payload, skipping ESP header and IV. + * */ +#define esp_enc_payload(data, iv_len) \ + (data) + ESP_SPI_LEN + ESP_SEQ_LEN + (iv_len) + +static int +esp_aes_rfc3602_dec(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data, + uint32_t esp_len) +{ + Aes cbc_dec; + int ret = -1; + uint8_t icv_len = esp_sa->icv_len; + uint8_t iv_len = esp_sa->iv_len; + uint8_t * enc_payload = NULL; + uint8_t * iv = NULL; + uint16_t enc_len = 0; + uint8_t inited = 0; + + #ifdef WOLFIP_DEBUG_ESP + printf("info: aes cbc dec: %d\n", esp_len); + #endif /* WOLFIP_DEBUG_ESP */ + + enc_len = esp_enc_len(esp_len, iv_len, icv_len); + enc_payload = esp_enc_payload(esp_data, iv_len); + iv = esp_enc_iv(esp_data, iv_len); + + ret = wc_AesInit(&cbc_dec, NULL, INVALID_DEVID); + + if (ret != 0) { + printf("error: wc_AesInit returned: %d\n", ret); + goto aes_dec_out; + } + + inited = 1; + ret = wc_AesSetKey(&cbc_dec, esp_sa->enc_key, esp_sa->enc_key_len, + iv, AES_DECRYPTION); + + if (ret != 0) { + printf("error: wc_AesSetKey returned: %d\n", ret); + goto aes_dec_out; + } + + /* decrypt in place. */ + ret = wc_AesCbcDecrypt(&cbc_dec, enc_payload, enc_payload, enc_len); + + if (ret != 0) { + printf("error: wc_AesCbcDecrypt returned: %d\n", ret); + goto aes_dec_out; + } + +aes_dec_out: + if (inited) { + wc_AesFree(&cbc_dec); + inited = 0; + } + + return ret; +} + +static int +esp_aes_rfc3602_enc(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data, + uint32_t esp_len) +{ + Aes cbc_enc; + int ret = -1; + uint8_t icv_len = esp_sa->icv_len; + uint8_t iv_len = esp_sa->iv_len; + uint8_t * enc_payload = NULL; + uint8_t * iv = NULL; + uint16_t enc_len = 0; + uint8_t inited = 0; + + #ifdef WOLFIP_DEBUG_ESP + printf("info: aes cbc enc: %d\n", esp_len); + #endif /* WOLFIP_DEBUG_ESP */ + + enc_len = esp_enc_len(esp_len, iv_len, icv_len); + enc_payload = esp_enc_payload(esp_data, iv_len); + iv = esp_enc_iv(esp_data, iv_len); + + /* Generate random iv block for cbc method. */ + ret = wc_RNG_GenerateBlock(&wc_rng, iv, iv_len); + + if (ret) { + printf("error: wc_RNG_GenerateBlock returned: %d\n", ret); + goto aes_enc_out; + } + + ret = wc_AesInit(&cbc_enc, NULL, INVALID_DEVID); + + if (ret != 0) { + printf("error: wc_AesInit returned: %d\n", ret); + goto aes_enc_out; + } + + inited = 1; + ret = wc_AesSetKey(&cbc_enc, esp_sa->enc_key, AES_BLOCK_SIZE, + iv, AES_ENCRYPTION); + + if (ret != 0) { + printf("error: wc_AesSetKey returned: %d\n", ret); + goto aes_enc_out; + } + + ret = wc_AesCbcEncrypt(&cbc_enc, enc_payload, enc_payload, enc_len); + + if (ret != 0) { + printf("error: wc_AesCbcEncrypt returned: %d\n", ret); + goto aes_enc_out; + } + +aes_enc_out: + if (inited) { + wc_AesFree(&cbc_enc); + inited = 0; + } + + return ret; +} + +/** + * AES-GCM-ESP + * The KEYMAT requested for each AES-GCM key is N + 4 octets. The first + * N octets are the AES key, and the remaining four octets are used as the + * salt value in the nonce. + * */ +#define esp_rfc4106_salt(esp_sa) (esp_sa)->enc_key \ + + (esp_sa)->enc_key_len \ + - ESP_GCM_RFC4106_SALT_LEN + +static int +esp_aes_rfc4106_dec(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data, + uint32_t esp_len) +{ + Aes gcm_dec; + int err = -1; + uint8_t * icv = NULL; + uint8_t icv_len = esp_sa->icv_len; + uint8_t iv_len = esp_sa->iv_len; + uint8_t * enc_payload = NULL; + uint8_t * iv = NULL; + uint16_t enc_len = 0; + uint8_t inited = 0; + uint8_t aad[ESP_SPI_LEN + ESP_SEQ_LEN]; + uint16_t aad_len = sizeof(aad); + const uint8_t * salt = NULL; + uint8_t salt_len = ESP_GCM_RFC4106_SALT_LEN; + uint8_t nonce[ESP_GCM_RFC4106_NONCE_LEN]; /* 4 salt + 8 iv */ + + #ifdef WOLFIP_DEBUG_ESP + printf("info: aes gcm dec: %d\n", esp_len); + #endif /* WOLFIP_DEBUG_ESP */ + + /* get enc payload, iv, and icv pointers. */ + enc_len = esp_enc_len(esp_len, iv_len, icv_len); + enc_payload = esp_enc_payload(esp_data, iv_len); + iv = esp_enc_iv(esp_data, iv_len); + icv = esp_enc_icv(esp_data, esp_len, esp_sa->icv_len); + + /* Get the salt, aad, and construct nonce. */ + salt = esp_rfc4106_salt(esp_sa); + memcpy(aad, esp_data, sizeof(aad)); + memcpy(nonce, salt, salt_len); + memcpy(nonce + salt_len, iv, iv_len); + + err = wc_AesInit(&gcm_dec, NULL, INVALID_DEVID); + + if (err != 0) { + printf("error: wc_AesInit: %d\n", err); + goto rfc4106_dec_out; + } + + inited = 1; + + /* subtract 4 byte salt from enc_key_len */ + err = wc_AesGcmInit(&gcm_dec, esp_sa->enc_key, esp_sa->enc_key_len - 4, + nonce, sizeof(nonce)); + + if (err != 0) { + printf("error: wc_AesGcmInit: %d\n", err); + goto rfc4106_dec_out; + } + + err = wc_AesGcmSetKey(&gcm_dec, esp_sa->enc_key, esp_sa->enc_key_len - 4); + + if (err != 0) { + printf("error: wc_AesGcmSetKey: %d\n", err); + goto rfc4106_dec_out; + } + + err = wc_AesGcmDecrypt(&gcm_dec, enc_payload, enc_payload, enc_len, + nonce, sizeof(nonce), icv, icv_len, aad, aad_len); + + if (err != 0) { + printf("error: wc_AesGcmDecrypt: %d\n", err); + goto rfc4106_dec_out; + } + +rfc4106_dec_out: + if (inited) { + wc_AesFree(&gcm_dec); + inited = 0; + } + + return err; +} + +static int +esp_aes_rfc4106_enc(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data, + uint32_t esp_len) +{ + Aes gcm_enc; + int err = -1; + uint8_t * icv = NULL; + uint8_t icv_len = esp_sa->icv_len; + uint8_t iv_len = esp_sa->iv_len; + uint8_t * enc_payload = NULL; + uint8_t * iv = NULL; + uint16_t enc_len = 0; + uint8_t inited = 0; + uint8_t aad[ESP_SPI_LEN + ESP_SEQ_LEN]; + uint16_t aad_len = sizeof(aad); + const uint8_t * salt = NULL; + uint8_t salt_len = ESP_GCM_RFC4106_SALT_LEN; + uint8_t nonce[ESP_GCM_RFC4106_NONCE_LEN]; /* 4 salt + 8 iv */ + + #ifdef WOLFIP_DEBUG_ESP + printf("info: aes gcm enc: %d\n", esp_len); + #endif /* WOLFIP_DEBUG_ESP */ + + /* get enc payload, iv, and icv pointers. */ + enc_len = esp_enc_len(esp_len, iv_len, icv_len); + enc_payload = esp_enc_payload(esp_data, iv_len); + iv = esp_enc_iv(esp_data, iv_len); + icv = esp_enc_icv(esp_data, esp_len, esp_sa->icv_len); + + /* Get the salt, aad. */ + salt = esp_rfc4106_salt(esp_sa); + memcpy(aad, esp_data, sizeof(aad)); + + { + /* Deterministic iv construction using pre-iv salt and sequence number. + * NIST SP 800-38D, section 8.2.1 Deterministic Construction, using + * an integer counter. The sequence number is used as a counter, and + * xor'ed with pre-iv salt. Based on linux kernel crypto/seqiv.c. + * */ + uint32_t seq_num = 0; + uint8_t * seq_num_u8 = (uint8_t *) &seq_num; + + seq_num = ee32(esp_sa->oseq); + + /* copy in the pre_iv. */ + memcpy(iv, esp_sa->pre_iv, sizeof(esp_sa->pre_iv)); + + /* xor pre-iv salt with current sequence number. */ + for (size_t i = 0; i < sizeof(uint32_t); ++i) { + iv[i + sizeof(uint32_t)] ^= seq_num_u8[i]; + } + } + + memcpy(nonce, salt, salt_len); + memcpy(nonce + salt_len, iv, iv_len); + + err = wc_AesInit(&gcm_enc, NULL, INVALID_DEVID); + + if (err != 0) { + printf("error: wc_AesInit: %d\n", err); + goto rfc4106_enc_out; + } + + inited = 1; + + /* subtract 4 byte salt from enc_key_len */ + err = wc_AesGcmInit(&gcm_enc, esp_sa->enc_key, esp_sa->enc_key_len - 4, + nonce, sizeof(nonce)); + + if (err != 0) { + printf("error: wc_AesGcmInit: %d\n", err); + goto rfc4106_enc_out; + } + + err = wc_AesGcmSetKey(&gcm_enc, esp_sa->enc_key, esp_sa->enc_key_len - 4); + + if (err != 0) { + printf("error: wc_AesGcmSetKey: %d\n", err); + goto rfc4106_enc_out; + } + + err = wc_AesGcmEncrypt(&gcm_enc, enc_payload, enc_payload, enc_len, + nonce, sizeof(nonce), icv, icv_len, aad, aad_len); + + if (err != 0) { + printf("error: wc_AesGcmDecrypt: %d\n", err); + goto rfc4106_enc_out; + } + +rfc4106_enc_out: + if (inited) { + wc_AesFree(&gcm_enc); + inited = 0; + } + + return err; +} + +/** + * esp_data covers from start of ESP header to end of ESP trailer, but does not + * include the ESP ICV after trailer. + * */ +static int +esp_check_icv_hmac(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data, + uint32_t esp_len) +{ + /* SHA and MD5 have these digest sizes: + * - WC_MD5_DIGEST_SIZE 16 bytes + * - WC_SHA_DIGEST_SIZE 20 bytes + * - WC_SHA256_DIGEST_SIZE 32 bytes + * */ + int rc = 0; + const uint8_t * icv = NULL; + byte hash[WC_SHA256_DIGEST_SIZE]; + + rc = esp_calc_icv_hmac(hash, esp_sa, esp_data, esp_len); + if (rc) { + return rc; + } + + icv = esp_data + esp_len - esp_sa->icv_len; + + /* compare the first N bits depending on truncation type. */ + rc = esp_const_memcmp(icv, hash, esp_sa->icv_len); + if (rc) { + #ifdef WOLFIP_DEBUG_ESP + esp_dump_data("icv not matched", hash, esp_sa->icv_len); + #endif /* WOLFIP_DEBUG_ESP */ + } + + return rc; +} + +/** + * Decapsulate an ipv4 ESP packet. The packet is + * unwrapped in-place without extra copying. + * + * The ip.proto, ip.len, and frame_len are updated + * after unwrap. + * + * Transport Mode: + * before: + * _______________________________________________ + * |orig IP hdr | ESP | UDP | | ESP | ESP | + * |(PROTO=50) | hdr | hdr | Data | Trailer | ICV | + * ----------------------------------------------- + * |<---- encrypted ----->| + * |<--- integrity checked ---->| + * + * after: + * _________________________ + * |orig IP hdr | UDP | | + * |(PROTO=17) | hdr | Data | + * ------------------------- + * + * Returns 0 on success. + * Returns -1 on error. + * */ +static int esp_unwrap(struct wolfIP *s, struct wolfIP_ip_packet *ip, + uint32_t * frame_len) +{ + uint8_t spi[ESP_SPI_LEN]; + uint32_t seq = 0; + struct wolfIP_esp_sa * esp_sa = NULL; + uint32_t esp_len = 0; + uint8_t pad_len = 0; + uint8_t nxt_hdr = 0; + + memset(spi, 0, sizeof(spi)); + + if (*frame_len <= (ETH_HEADER_LEN + IP_HEADER_LEN)) { + #ifdef WOLFIP_DEBUG_ESP + printf("error: esp: malformed frame: %d\n", *frame_len); + #endif /* WOLFIP_DEBUG_ESP */ + return -1; + } + + esp_len = *frame_len - ETH_HEADER_LEN - IP_HEADER_LEN; + + /* If not at least SPI and sequence, something wrong. */ + if (esp_len < (ESP_SPI_LEN + ESP_SEQ_LEN)) { + #ifdef WOLFIP_DEBUG_ESP + printf("error: esp: malformed packet: %d\n", esp_len); + #endif /* WOLFIP_DEBUG_ESP */ + return -1; + } + + /* First 4 bytes are the spi (Security Parameters Index). */ + memcpy(spi, ip->data, sizeof(spi)); + /* Next 4 bytes are the seq (Sequence Number).*/ + memcpy(&seq, ip->data + ESP_SPI_LEN, sizeof(seq)); + seq = ee32(seq); + + for (size_t i = 0; i < in_sa_num; ++i) { + if (memcmp(spi, in_sa_list[i].spi, sizeof(spi)) == 0) { + #ifdef WOLFIP_DEBUG_ESP + printf("info: found sa: 0x%02x%02x%02x%02x\n", + spi[0], spi[1], spi[2], spi[3]); + #endif /* WOLFIP_DEBUG_ESP */ + esp_sa = &in_sa_list[i]; + break; + } + } + + if (esp_sa == NULL) { + /** + * RFC4303: + * If no valid Security Association exists for this packet, the + * receiver MUST discard the packet; this is an auditable event. + * */ + printf("error: unknown spi: 0x%02x%02x%02x%02x\n", + spi[0], spi[1], spi[2], spi[3]); + return -1; + } + + { + /* calculate min expected length based on the security association. */ + uint32_t min_len = 0; + + min_len = (ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len + + ESP_PADDING_LEN + ESP_NEXT_HEADER_LEN + esp_sa->icv_len); + + if (esp_len < min_len) { + printf("error: esp: got %d, expected >= %d frame len", esp_len, + min_len); + return -1; + } + } + + if (esp_sa->icv_len) { + int err = 0; + switch (esp_sa->auth) { + case ESP_AUTH_MD5_RFC2403: + case ESP_AUTH_SHA1_RFC2404: + case ESP_AUTH_SHA256_RFC4868: + err = esp_check_icv_hmac(esp_sa, ip->data, esp_len); + break; + case ESP_AUTH_GCM_RFC4106: + case ESP_AUTH_GCM_RFC4543: + /* icv calculated during decrypt */ + err = 0; + break; + case ESP_AUTH_NONE: + default: + err = -1; + break; + } + + if (err) { + printf("error: icv check failed\n"); + return -1; + } + } + + if (esp_sa->iv_len != 0) { + /* Decrypt the payload in place. */ + int err = -1; + + switch(esp_sa->enc) { + case ESP_ENC_CBC_AES: + err = esp_aes_rfc3602_dec(esp_sa, ip->data, esp_len); + break; + + case ESP_ENC_GCM_RFC4106: + err = esp_aes_rfc4106_dec(esp_sa, ip->data, esp_len); + break; + + case ESP_ENC_NONE: + default: + printf("error: decrypt: invalid enc: %d\n", esp_sa->enc); + err = -1; + break; + } + + if (err) { + printf("error: esp_decrypt(%02x) returned: %d\n", esp_sa->enc, err); + return -1; + } + + /* Payload is now decrypted. We can now parse + * the ESP trailer for next header and padding. */ + } + + /* icv check good, now finish unwrapping esp packet. */ + pad_len = *(ip->data + esp_len - esp_sa->icv_len - ESP_NEXT_HEADER_LEN + - ESP_PADDING_LEN); + nxt_hdr = *(ip->data + esp_len - esp_sa->icv_len - ESP_NEXT_HEADER_LEN); + + #ifdef WOLFIP_DEBUG_ESP + wolfIP_print_esp(esp_sa, ip->data, esp_len, pad_len, nxt_hdr); + #endif /* WOLFIP_DEBUG_ESP */ + + #ifdef WOLFIP_DEBUG_ESP_VERBOSE + esp_dump_data_verbose("esp_packet before unwrap", ip->data, esp_len); + #endif /* WOLFIP_DEBUG_ESP_VERBOSE */ + + /* move ip payload forward to hide ESP header (SPI, SEQ, IV). */ + memmove(ip->data, ip->data + ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len, + esp_len - (ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len)); + + /* subtract ESP header from frame_len and ip.len. */ + *frame_len = *frame_len - (esp_sa->iv_len + ESP_SPI_LEN + ESP_SEQ_LEN); + ip->len = ee16(ip->len) - (esp_sa->iv_len + ESP_SPI_LEN + ESP_SEQ_LEN); + + /* subtract ESP trailer from frame_len and ip.len. */ + *frame_len = *frame_len - (pad_len + ESP_PADDING_LEN + + ESP_NEXT_HEADER_LEN + esp_sa->icv_len); + ip->len = ip->len - (pad_len + ESP_PADDING_LEN + + ESP_NEXT_HEADER_LEN + esp_sa->icv_len); + + /* update len, set proto to next header, recalculate iphdr checksum. */ + ip->len = ee16(ip->len); + ip->proto = nxt_hdr; + ip->csum = 0; + iphdr_set_checksum(ip); + + #ifdef WOLFIP_DEBUG_ESP_VERBOSE + esp_dump_data_verbose("esp_packet after unwrap", ip->data, + *frame_len - ETH_HEADER_LEN - IP_HEADER_LEN); + #endif /* WOLFIP_DEBUG_ESP_VERBOSE */ + + (void)s; + return 0; +} + +/** + * Encapsulate an ipv4 packet with ESP. + * + * Transport Mode: + * before: + * _________________________ + * |orig IP hdr | | | + * |(PROTO=17) | UDP | Data | + * ------------------------- + * + * after: + * _______________________________________________ + * |orig IP hdr | ESP | | | ESP | ESP | + * |(PROTO=50) | hdr | UDP | Data | Trailer | ICV | + * ----------------------------------------------- + * |<---- encrypted ----->| + * |<--- integrity checked ---->| + * + * Returns 0 on success. + * Returns -1 on error. + * */ +static int esp_wrap(struct wolfIP_ip_packet *ip, uint16_t * ip_len) +{ + uint8_t block_len = 0; + uint16_t orig_ip_len = *ip_len; + uint16_t orig_payload_len = orig_ip_len - IP_HEADER_LEN; + uint16_t payload_len = 0; + uint8_t * payload = ip->data; + uint8_t pad_len = 0; + uint32_t seq_n = 0; /* sequence num in network order */ + uint16_t icv_offset = 0; + struct wolfIP_esp_sa * esp_sa = NULL; + + /* TODO: priority, tcp/udp port-filtering? currently this grabs + * the first dst match. */ + for (size_t i = 0; i < out_sa_num; ++i) { + if (ip->dst == out_sa_list[i].dst) { + esp_sa = &out_sa_list[i]; + #ifdef WOLFIP_DEBUG_ESP + printf("info: found out sa: 0x%02x%02x%02x%02x\n", + esp_sa->spi[0], esp_sa->spi[1], esp_sa->spi[2], esp_sa->spi[3]); + #endif /* WOLFIP_DEBUG_ESP */ + break; + } + } + + if (esp_sa == NULL) { + /* nothing to do */ + #ifdef WOLFIP_DEBUG_ESP + char ip_str[32]; + memset(ip_str, '\0', sizeof(ip_str)); + iptoa(ip->dst, ip_str); + printf("info: ip dst not found: %s\n", ip_str); + #endif /* WOLFIP_DEBUG_ESP */ + return 0; + } + + #ifdef WOLFIP_DEBUG_ESP_VERBOSE + esp_dump_data_verbose("ip packet before wrap", ip->data, orig_payload_len); + #endif /* WOLFIP_DEBUG_ESP_VERBOSE */ + + #if 0 + /* return early, do nothing. */ + return 0; + #endif + + /* move ip payload back to make room for ESP header (SPI, SEQ) + IV. */ + memmove(ip->data + ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len, + ip->data, orig_payload_len); + + /* Copy in SPI and sequence number fields. */ + memcpy(payload, esp_sa->spi, sizeof(esp_sa->spi)); + payload += ESP_SPI_LEN; + + esp_sa->oseq++; + seq_n = ee32(esp_sa->oseq); + memcpy(payload, &seq_n, sizeof(seq_n)); + payload += ESP_SEQ_LEN; + + if (esp_sa->iv_len) { + /* skip iv field, will generate later. */ + payload += esp_sa->iv_len; + } + + block_len = esp_block_len_from_enc(esp_sa->enc); + + if (block_len) { + /* Block cipher. Calculate padding and encrypted length, then + * icv_offset. */ + uint32_t enc_len = 0; + enc_len = esp_sa->iv_len + orig_payload_len + pad_len + + ESP_PADDING_LEN + ESP_NEXT_HEADER_LEN; + + /* Determine padding. This needs to be flexible for + * des3 (8 byte) or aes (16 byte) block sizes.*/ + if (enc_len % block_len) { + pad_len = block_len - (enc_len % block_len); + } + + icv_offset = ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len + + orig_payload_len + pad_len + ESP_PADDING_LEN + + ESP_NEXT_HEADER_LEN; + } + else { + /* Stream cipher or auth-only. Calculate the icv offset directly. */ + icv_offset = ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len + + orig_payload_len + pad_len + ESP_PADDING_LEN + + ESP_NEXT_HEADER_LEN; + + /* Determine padding. */ + if (icv_offset % ESP_ICV_ALIGNMENT) { + pad_len = ESP_ICV_ALIGNMENT - (icv_offset % ESP_ICV_ALIGNMENT); + icv_offset += pad_len; + } + } + + /* Skip past the original payload, add padding. */ + payload += orig_payload_len; + + if (pad_len) { + uint8_t i = 0; + for (i = 0; i < pad_len; ++i) { + payload[i] = (i + 1); + } + + payload += pad_len; + } + + /* ESP trailer. Copy in padding len and next header fields. */ + memcpy(payload, &pad_len, ESP_PADDING_LEN); + payload += ESP_PADDING_LEN; + + memcpy(payload, &ip->proto, ESP_NEXT_HEADER_LEN); + payload += ESP_NEXT_HEADER_LEN; + + /* calculate final esp payload length. */ + payload_len = orig_ip_len - IP_HEADER_LEN; + payload_len += ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len + + pad_len + ESP_PADDING_LEN + ESP_NEXT_HEADER_LEN + + esp_sa->icv_len; + + /* encrypt from payload to end of ESP trailer. */ + if (esp_sa->iv_len) { + int err = -1; + + switch(esp_sa->enc) { + case ESP_ENC_CBC_AES: + err = esp_aes_rfc3602_enc(esp_sa, ip->data, payload_len); + break; + + case ESP_ENC_GCM_RFC4106: + err = esp_aes_rfc4106_enc(esp_sa, ip->data, payload_len); + break; + + case ESP_ENC_NONE: + default: + printf("error: encrypt: invalid enc: %d\n", esp_sa->enc); + err = -1; + break; + } + + if (err) { + printf("error: esp_encrypt(%02x) returned: %d\n", esp_sa->enc, err); + return -1; + } + + /* Payload is now encrypted. Now calculate ICV. */ + } + + if (esp_sa->icv_len) { + uint8_t * icv = NULL; + int err = 0; + + switch (esp_sa->auth) { + case ESP_AUTH_MD5_RFC2403: + case ESP_AUTH_SHA1_RFC2404: + case ESP_AUTH_SHA256_RFC4868: + icv = ip->data + icv_offset; + err = esp_calc_icv_hmac(icv, esp_sa, ip->data, payload_len); + break; + case ESP_AUTH_GCM_RFC4106: + case ESP_AUTH_GCM_RFC4543: + /* icv already calculated during encrypt */ + err = 0; + break; + case ESP_AUTH_NONE: + default: + err = -1; + break; + } + + if (err) { + printf("error: icv check failed\n"); + return -1; + } + } + + *ip_len = payload_len + IP_HEADER_LEN; + + #ifdef WOLFIP_DEBUG_ESP + wolfIP_print_esp(esp_sa, ip->data, payload_len, pad_len, ip->proto); + #endif /* WOLFIP_DEBUG_ESP */ + + #ifdef WOLFIP_DEBUG_ESP_VERBOSE + esp_dump_data_verbose("ip packet after wrap", ip->data, payload_len); + #endif /* WOLFIP_DEBUG_ESP_VERBOSE */ + + return 0; +} + +/** + * Copy frame to new packet so we can expand and wrap in place + * without stepping on the fifo tcp circular buffer. + * */ +static int esp_output(struct wolfIP_ll_dev * ll_dev, + const struct wolfIP_ip_packet *ip, + uint16_t len) +{ + /** + * 60 is reasonable max ESP overhead (for now), rounded up to 4 bytes. + * 8 bytes (esp header) + * + 16 bytes (iv, prepended to payload) + * + 15 bytes (max padding with block cipher) + * + 2 bytes (pad_len + nxt_hdr fields) + * + 16 bytes (icv) + * may need to increase depending on algs supported. + * */ + struct wolfIP_ip_packet * esp; + uint8_t frame[LINK_MTU + 60]; + uint16_t ip_final_len = len; + int esp_rc = 0; + + esp = (struct wolfIP_ip_packet *) frame; + memcpy(esp, ip, sizeof(struct wolfIP_ip_packet) + len); + + esp_rc = esp_wrap(esp, &ip_final_len); + + if (esp_rc) { + #ifdef WOLFIP_DEBUG_ESP + printf("error: esp_wrap returned: %d\n", esp_rc); + #endif /* WOLFIP_DEBUG_ESP */ + return esp_rc; + } + + /* update len, set proto to ESP 0x32 (50), recalculate iphdr checksum. */ + esp->len = ee16(ip_final_len); + esp->proto = 0x32; + esp->csum = 0; + iphdr_set_checksum(esp); + + ll_dev->send(ll_dev, esp, ip_final_len + ETH_HEADER_LEN); + + return 0; +} +#endif /* WOLFIP_ESP && !WOLFESP_SRC */ diff --git a/src/wolfip.c b/src/wolfip.c index 45db54b..d995123 100644 --- a/src/wolfip.c +++ b/src/wolfip.c @@ -534,7 +534,10 @@ static uint32_t wolfIP_filter_mask_for_proto(uint16_t proto) } } -static int wolfIP_filter_dispatch(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const void *buffer, uint32_t length, const struct wolfIP_filter_metadata *meta) +static int wolfIP_filter_dispatch(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const void *buffer, uint32_t length, + const struct wolfIP_filter_metadata *meta) { struct wolfIP_filter_event event; int ret; @@ -569,7 +572,9 @@ static int wolfIP_filter_dispatch(enum wolfIP_filter_reason reason, struct wolfI } #ifdef ETHERNET -static int wolfIP_filter_notify_eth(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const struct wolfIP_eth_frame *eth, uint32_t len) +static int wolfIP_filter_notify_eth(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const struct wolfIP_eth_frame *eth, uint32_t len) { struct wolfIP_filter_metadata meta; @@ -585,7 +590,8 @@ static int wolfIP_filter_notify_eth(enum wolfIP_filter_reason reason, struct wol #define wolfIP_filter_notify_eth(...) (0) #endif -static void wolfIP_filter_fill_ip_metadata(struct wolfIP_filter_metadata *meta, const struct wolfIP_ip_packet *ip) +static void wolfIP_filter_fill_ip_metadata(struct wolfIP_filter_metadata *meta, + const struct wolfIP_ip_packet *ip) { meta->src_ip = ip->src; meta->dst_ip = ip->dst; @@ -600,7 +606,9 @@ static void wolfIP_filter_fill_ip_metadata(struct wolfIP_filter_metadata *meta, #endif } -static int wolfIP_filter_notify_ip(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const struct wolfIP_ip_packet *ip, uint32_t len) +static int wolfIP_filter_notify_ip(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const struct wolfIP_ip_packet *ip, uint32_t len) { struct wolfIP_filter_metadata meta; @@ -614,7 +622,9 @@ static int wolfIP_filter_notify_ip(enum wolfIP_filter_reason reason, struct wolf return wolfIP_filter_dispatch(reason, s, if_idx, ip, len, &meta); } -static int wolfIP_filter_notify_tcp(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const struct wolfIP_tcp_seg *tcp, uint32_t len) +static int wolfIP_filter_notify_tcp(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const struct wolfIP_tcp_seg *tcp, uint32_t len) { struct wolfIP_filter_metadata meta; @@ -628,7 +638,9 @@ static int wolfIP_filter_notify_tcp(enum wolfIP_filter_reason reason, struct wol return wolfIP_filter_dispatch(reason, s, if_idx, tcp, len, &meta); } -static int wolfIP_filter_notify_udp(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const struct wolfIP_udp_datagram *udp, uint32_t len) +static int wolfIP_filter_notify_udp(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const struct wolfIP_udp_datagram *udp, uint32_t len) { struct wolfIP_filter_metadata meta; @@ -641,7 +653,9 @@ static int wolfIP_filter_notify_udp(enum wolfIP_filter_reason reason, struct wol return wolfIP_filter_dispatch(reason, s, if_idx, udp, len, &meta); } -static int wolfIP_filter_notify_icmp(enum wolfIP_filter_reason reason, struct wolfIP *s, unsigned int if_idx, const struct wolfIP_icmp_packet *icmp, uint32_t len) +static int wolfIP_filter_notify_icmp(enum wolfIP_filter_reason reason, + struct wolfIP *s, unsigned int if_idx, + const struct wolfIP_icmp_packet *icmp, uint32_t len) { struct wolfIP_filter_metadata meta; @@ -799,8 +813,9 @@ struct arp_pending_entry { static int arp_lookup(struct wolfIP *s, unsigned int if_idx, ip4 ip, uint8_t *mac); #if WOLFIP_ENABLE_FORWARDING -static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, struct wolfIP_ip_packet *ip, - uint32_t len, const uint8_t *mac, int broadcast); +static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, + struct wolfIP_ip_packet *ip, uint32_t len, + const uint8_t *mac, int broadcast); #endif #endif @@ -820,8 +835,8 @@ struct timers_binheap { uint32_t size; }; -struct wolfIP -{ +/* The main wolfip stack context structure. */ +struct wolfIP { struct wolfIP_ll_dev ll_dev[WOLFIP_MAX_INTERFACES]; struct ipconf ipconf[WOLFIP_MAX_INTERFACES]; unsigned int if_count; @@ -1055,8 +1070,9 @@ static unsigned int wolfIP_if_for_local_ip(struct wolfIP *s, ip4 local_ip, int * #ifdef ETHERNET static uint16_t icmp_checksum(struct wolfIP_icmp_packet *icmp, uint16_t len); static void iphdr_set_checksum(struct wolfIP_ip_packet *ip); -static int eth_output_add_header(struct wolfIP *S, unsigned int if_idx, const uint8_t *dst, struct wolfIP_eth_frame *eth, - uint16_t type); +static int eth_output_add_header(struct wolfIP *S, unsigned int if_idx, + const uint8_t *dst, struct wolfIP_eth_frame *eth, + uint16_t type); #endif #if WOLFIP_ENABLE_FORWARDING && defined(ETHERNET) static void arp_request(struct wolfIP *s, unsigned int if_idx, ip4 tip); @@ -1064,7 +1080,8 @@ static int arp_lookup(struct wolfIP *s, unsigned int if_idx, ip4 ip, uint8_t *ma #endif #ifdef ETHERNET -static void wolfIP_send_ttl_exceeded(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *orig) +static void wolfIP_send_ttl_exceeded(struct wolfIP *s, unsigned int if_idx, + struct wolfIP_ip_packet *orig) { struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, if_idx); struct wolfIP_icmp_ttl_exceeded_packet icmp = {0}; @@ -1098,7 +1115,8 @@ static void wolfIP_send_ttl_exceeded(struct wolfIP *s, unsigned int if_idx, stru ll->send(ll, &icmp, sizeof(icmp)); } #else -static void wolfIP_send_ttl_exceeded(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *orig) +static void wolfIP_send_ttl_exceeded(struct wolfIP *s, unsigned int if_idx, + struct wolfIP_ip_packet *orig) { (void)s; (void)if_idx; @@ -1225,7 +1243,8 @@ static struct tsocket *udp_new_socket(struct wolfIP *s) return NULL; } -static void udp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_udp_datagram *udp, uint32_t frame_len) +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); int i; @@ -1276,7 +1295,8 @@ static struct tsocket *icmp_new_socket(struct wolfIP *s) return NULL; } -static void icmp_try_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_icmp_packet *icmp, uint32_t frame_len) +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); @@ -1476,7 +1496,8 @@ static void iphdr_set_checksum(struct wolfIP_ip_packet *ip) } #ifdef ETHERNET -static int eth_output_add_header(struct wolfIP *S, unsigned int if_idx, const uint8_t *dst, struct wolfIP_eth_frame *eth, +static int eth_output_add_header(struct wolfIP *S, unsigned int if_idx, + const uint8_t *dst, struct wolfIP_eth_frame *eth, uint16_t type) { struct wolfIP_ll_dev *ll = wolfIP_ll_at(S, if_idx); @@ -1495,8 +1516,13 @@ static int eth_output_add_header(struct wolfIP *S, unsigned int if_idx, const ui } #endif +#ifdef WOLFIP_ESP +#include "src/wolfesp.c" +#endif /* WOLFIP_ESP */ + #if WOLFIP_ENABLE_FORWARDING -static int wolfIP_forward_prepare(struct wolfIP *s, unsigned int out_if, ip4 dest, uint8_t *mac, int *broadcast) +static int wolfIP_forward_prepare(struct wolfIP *s, unsigned int out_if, + ip4 dest, uint8_t *mac, int *broadcast) { #ifdef ETHERNET if (!broadcast || !mac) @@ -1527,7 +1553,9 @@ static int wolfIP_forward_prepare(struct wolfIP *s, unsigned int out_if, ip4 des #endif } -static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, struct wolfIP_ip_packet *ip, uint32_t len, const uint8_t *mac, int broadcast) +static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, + struct wolfIP_ip_packet *ip, uint32_t len, + const uint8_t *mac, int broadcast) { #ifdef ETHERNET struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, out_if); @@ -1539,11 +1567,14 @@ static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, struct else eth_output_add_header(s, out_if, mac, &ip->eth, ETH_TYPE_IP); if (ip->proto == WI_IPPROTO_TCP) - drop = wolfIP_filter_notify_tcp(WOLFIP_FILT_SENDING, s, out_if, (struct wolfIP_tcp_seg *)ip, len); + drop = wolfIP_filter_notify_tcp(WOLFIP_FILT_SENDING, s, out_if, + (struct wolfIP_tcp_seg *)ip, len); else if (ip->proto == WI_IPPROTO_UDP) - drop = wolfIP_filter_notify_udp(WOLFIP_FILT_SENDING, s, out_if, (struct wolfIP_udp_datagram *)ip, len); + drop = wolfIP_filter_notify_udp(WOLFIP_FILT_SENDING, s, out_if, + (struct wolfIP_udp_datagram *)ip, len); else if (ip->proto == WI_IPPROTO_ICMP) - drop = wolfIP_filter_notify_icmp(WOLFIP_FILT_SENDING, s, out_if, (struct wolfIP_icmp_packet *)ip, len); + drop = wolfIP_filter_notify_icmp(WOLFIP_FILT_SENDING, s, out_if, + (struct wolfIP_icmp_packet *)ip, len); if (drop != 0) return; if (wolfIP_filter_notify_ip(WOLFIP_FILT_SENDING, s, out_if, ip, len) != 0) @@ -1562,7 +1593,8 @@ static void wolfIP_forward_packet(struct wolfIP *s, unsigned int out_if, struct } #endif -static int ip_output_add_header(struct tsocket *t, struct wolfIP_ip_packet *ip, uint8_t proto, uint16_t len) +static int ip_output_add_header(struct tsocket *t, struct wolfIP_ip_packet *ip, + uint8_t proto, uint16_t len) { union transport_pseudo_header ph; unsigned int if_idx; @@ -1735,7 +1767,8 @@ static void tcp_ack(struct tsocket *t, const struct wolfIP_tcp_seg *tcp) } /* Preselect socket, parse options, manage handshakes, pass to application */ -static void tcp_input(struct wolfIP *S, unsigned int if_idx, struct wolfIP_tcp_seg *tcp, uint32_t frame_len) +static void tcp_input(struct wolfIP *S, unsigned int if_idx, + struct wolfIP_tcp_seg *tcp, uint32_t frame_len) { int i; if (wolfIP_filter_notify_tcp(WOLFIP_FILT_RECEIVING, S, if_idx, tcp, frame_len) != 0) @@ -2404,7 +2437,8 @@ 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) +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) @@ -2432,7 +2466,8 @@ int wolfIP_sock_get_recv_ttl(struct wolfIP *s, int sockfd, int *ttl) return 1; } -int wolfIP_sock_getsockopt(struct wolfIP *s, int sockfd, int level, int optname, void *optval, socklen_t *optlen) +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) @@ -2513,7 +2548,8 @@ int wolfIP_sock_close(struct wolfIP *s, int sockfd) return 0; } -int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, const socklen_t *addrlen) +int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, + const socklen_t *addrlen) { struct tsocket *ts; struct wolfIP_sockaddr_in *sin; @@ -2553,7 +2589,8 @@ int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr return -1; } -int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, socklen_t addrlen) +int wolfIP_sock_bind(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, + socklen_t addrlen) { struct tsocket *ts; ip4 bind_ip; @@ -2741,7 +2778,8 @@ int wolfIP_sock_listen(struct wolfIP *s, int sockfd, int backlog) return 0; } -int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, const socklen_t *addrlen) +int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, + const socklen_t *addrlen) { struct tsocket *ts; struct wolfIP_sockaddr_in *sin = (struct wolfIP_sockaddr_in *)addr; @@ -2764,7 +2802,8 @@ int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr /* Reply to ICecho requests */ -static void icmp_input(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *ip, uint32_t len) +static void icmp_input(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *ip, + uint32_t len) { struct wolfIP_icmp_packet *icmp = (struct wolfIP_icmp_packet *)ip; uint32_t tmp; @@ -3170,7 +3209,8 @@ static void arp_flush_pending(struct wolfIP *s, unsigned int if_idx, ip4 ip) } #endif /* WOLFIP_ENABLE_FORWARDING */ -static void arp_store_neighbor(struct wolfIP *s, unsigned int if_idx, ip4 ip, const uint8_t *mac) +static void arp_store_neighbor(struct wolfIP *s, unsigned int if_idx, ip4 ip, + const uint8_t *mac) { int i; int stored = 0; @@ -3339,12 +3379,42 @@ size_t wolfIP_instance_size(void) return sizeof(struct wolfIP); } -static inline void ip_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ip_packet *ip, - uint32_t len) +#ifdef DEBUG_IP +static void wolfIP_print_ip(struct wolfIP_ip_packet * ip) +{ + char src[32]; + char dst[32]; + memset(src, 0, sizeof(src)); + memset(dst, 0, sizeof(dst)); + iptoa(ee32(ip->src), src); + iptoa(ee32(ip->dst), dst); + + printf("ip hdr:\n"); + printf("+-----------------------------+\n"); + printf("| 0x%02x | 0x%02x | 0x%02x | %4d | (ipv, hdr_len, tos, ip_len)\n", + 0x04, ip->ver_ihl, ip->tos, ee16(ip->len)); + printf("+-----------------------------+\n"); + printf("| 0x%04x | 0x%04x | (id, flags_fo)\n", + ee16(ip->id), ee16(ip->flags_fo)); + printf("+-----------------------------+\n"); + printf("| %3d | 0x%02x | 0x%04x | (ttl, proto, chksum)\n", + ip->ttl, ip->proto, ee16(ip->csum)); + printf("+-----------------------------+\n"); + printf("| %15s | (src)\n", src); + printf("+-----------------------------+\n"); + printf("| %15s | (dst)\n", dst); + printf("+-----------------------------+\n"); + printf("\n"); +} +#endif /* DEBUG_IP*/ + +static inline void ip_recv(struct wolfIP *s, unsigned int if_idx, + struct wolfIP_ip_packet *ip, uint32_t len) { #if WOLFIP_ENABLE_FORWARDING unsigned int i; #endif + #if WOLFIP_ENABLE_LOOPBACK if (!wolfIP_is_loopback_if(if_idx)) { ip4 dest = ee32(ip->dst); @@ -3395,6 +3465,22 @@ static inline void ip_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ } } #endif + #ifdef DEBUG_IP + wolfIP_print_ip(ip); + #endif /* DEBUG_IP*/ + + #ifdef WOLFIP_ESP + if (ip->proto == 0x32) { + /* proto is ESP 0x32 (50), try to unwrap. */ + int esp_rc = 0; + esp_rc = esp_unwrap(s, ip, &len); + if (esp_rc) { + printf("info: failed to unwrap esp packet, dropping.\n"); + return; + } + } + #endif /* WOLFIP_ESP */ + if (ip->ver_ihl == 0x45 && ip->proto == 0x06) { struct wolfIP_tcp_seg *tcp = (struct wolfIP_tcp_seg *)ip; tcp_input(s, if_idx, tcp, len); @@ -3402,10 +3488,36 @@ static inline void ip_recv(struct wolfIP *s, unsigned int if_idx, struct wolfIP_ else if (ip->ver_ihl == 0x45 && ip->proto == 0x11) { struct wolfIP_udp_datagram *udp = (struct wolfIP_udp_datagram *)ip; udp_try_recv(s, if_idx, udp, len); - } else if (ip->ver_ihl == 0x45 && ip->proto == 0x01) { + } + else if (ip->ver_ihl == 0x45 && ip->proto == 0x01) { icmp_input(s, if_idx, ip, len); } + #ifdef DEBUG_IP + else { + printf("info: dropping ip packet: 0x%02x\n", ip->proto); + } + #endif +} + +#ifdef DEBUG_ETH +static void wolfIP_print_eth(struct wolfIP_eth_frame * eth, uint32_t len) +{ + uint8_t * dst = eth->dst; + uint8_t * src = eth->src; + uint8_t * type = (uint8_t *) ð->type; + printf("eth hdr:\n"); + printf("+---------------------------------------+\n"); + printf("| %02x:%02x:%02x:%02x:%02x:%02x " + "| %02x:%02x:%02x:%02x:%02x:%02x | (src, dst) \n", + src[0], src[1], src[2], src[3], src[4], src[5], + dst[0], dst[1], dst[2], dst[3], dst[4], dst[5]); + printf("+---------------------------------------+\n"); + printf("| 0x%02x%02x | %5lu bytes data | (eth type, payload) \n", + type[0], type[1], (unsigned long)len); + printf("+---------------------------------------+\n"); + printf("\n"); } +#endif /* DEBUG_ETH */ static void wolfIP_recv_on(struct wolfIP *s, unsigned int if_idx, void *buf, uint32_t len) { @@ -3562,7 +3674,8 @@ static int dns_skip_name(const uint8_t *buf, int len, int offset) return pos; } -static int dns_copy_name(const uint8_t *buf, int len, int offset, char *out, size_t out_len) +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; @@ -3675,7 +3788,8 @@ void dns_callback(int dns_sd, uint16_t ev, void *arg) } } -static int dns_send_query(struct wolfIP *s, const char *dname, uint16_t *id, uint16_t qtype) +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; @@ -3877,7 +3991,11 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) { struct wolfIP_ll_dev *ll = wolfIP_ll_at(s, tx_if); if (ll && ll->send) { + #ifdef WOLFIP_ESP + esp_output(ll, (struct wolfIP_ip_packet *)tcp, size); + #else ll->send(ll, tcp, desc->len); + #endif /* WOLFIP_ESP */ } } desc->flags |= PKT_FLAG_SENT; @@ -3998,7 +4116,8 @@ void wolfIP_ipconfig_get(struct wolfIP *s, ip4 *ip, ip4 *mask, ip4 *gw) wolfIP_ipconfig_get_ex(s, WOLFIP_PRIMARY_IF_IDX, ip, mask, gw); } -void wolfIP_ipconfig_set_ex(struct wolfIP *s, unsigned int if_idx, ip4 ip, ip4 mask, ip4 gw) +void wolfIP_ipconfig_set_ex(struct wolfIP *s, unsigned int if_idx, ip4 ip, + ip4 mask, ip4 gw) { struct ipconf *conf = wolfIP_ipconf_at(s, if_idx); if (!conf) @@ -4008,7 +4127,8 @@ void wolfIP_ipconfig_set_ex(struct wolfIP *s, unsigned int if_idx, ip4 ip, ip4 m conf->gw = gw; } -void wolfIP_ipconfig_get_ex(struct wolfIP *s, unsigned int if_idx, ip4 *ip, ip4 *mask, ip4 *gw) +void wolfIP_ipconfig_get_ex(struct wolfIP *s, unsigned int if_idx, ip4 *ip, + ip4 *mask, ip4 *gw) { struct ipconf *conf = wolfIP_ipconf_at(s, if_idx); if (!conf) diff --git a/wolfesp.h b/wolfesp.h new file mode 100644 index 0000000..08b7f16 --- /dev/null +++ b/wolfesp.h @@ -0,0 +1,67 @@ +#ifndef WOLFESP_H +#define WOLFESP_H + +#define ESP_SPI_LEN 4 +#define ESP_SEQ_LEN 4 +#define ESP_PADDING_LEN 1 +#define ESP_NEXT_HEADER_LEN 1 +#define ESP_ICV_ALIGNMENT 4 +/* hmac-[sha256, sha1, md5]-96*/ +#define ESP_ICVLEN_HMAC_96 12 +#define ESP_ICVLEN_HMAC_128 16 +#define WOLFIP_ESP_NUM_SA 3 + +/* aes-128 */ +#define ESP_128_KEY_LEN 16 +#define ESP_128_IV_LEN 16 + +/* gcm */ +#define ESP_GCM_RFC4106_ICV_LEN 16 +#define ESP_GCM_RFC4106_SALT_LEN 4 +#define ESP_GCM_RFC4106_IV_LEN 8 +#define ESP_GCM_RFC4106_NONCE_LEN (ESP_GCM_RFC4106_SALT_LEN \ + + ESP_GCM_RFC4106_IV_LEN) + +typedef enum { + ESP_ENC_NONE = 0, + ESP_ENC_CBC_AES, + ESP_ENC_CBC_DES3, + ESP_ENC_GCM_RFC4106, + ESP_ENC_GCM_RFC4543, /* placeholder to indicate gmac auth. */ +} esp_enc_t; + +typedef enum { + ESP_AUTH_NONE = 0, + ESP_AUTH_MD5_RFC2403, /* hmac(md5)-96 */ + ESP_AUTH_SHA1_RFC2404, /* hmac(sha1)-96 */ + ESP_AUTH_SHA256_RFC4868, /* hmac(sha256)-N, N=96,128 */ + ESP_AUTH_GCM_RFC4106, /* placeholder to indicate gcm auth. */ + ESP_AUTH_GCM_RFC4543 /* rfc4543 gmac */ +} esp_auth_t; + +/* Minimal ESP Security Association structure. + * Supports only transport mode. + * */ +struct wolfIP_esp_sa { + uint8_t spi[ESP_SPI_LEN]; /* security parameter index */ + ip4 src; /* ip src and dst in network byte order */ + ip4 dst; + uint32_t oseq; /* outbound sequence number */ + uint32_t seq; /* inbound sequence number */ + uint8_t iv_len; + esp_enc_t enc; + uint8_t enc_key[32]; + uint8_t enc_key_len; + esp_auth_t auth; + uint8_t auth_key[32]; + uint8_t auth_key_len; + uint8_t icv_len; + uint8_t pre_iv[ESP_GCM_RFC4106_IV_LEN]; /* unique salt that is xor'ed + * with oseq to generate iv. */ +}; + +int wolfIP_esp_init(void); +void wolfIP_esp_load_sa_list(struct wolfIP_esp_sa * sa_list, uint16_t num, + int in); + +#endif /* !WOLFESP_H */ diff --git a/wolfip.h b/wolfip.h index e12b1f9..407ace5 100644 --- a/wolfip.h +++ b/wolfip.h @@ -287,15 +287,21 @@ static inline void iptoa(ip4 ip, char *buf) } #ifdef WOLFSSL_WOLFIP -#ifdef WOLFSSL_USER_SETTINGS -#include "user_settings.h" -#else -#include -#endif -#include -#include -int wolfSSL_SetIO_wolfIP(WOLFSSL* ssl, int fd); -int wolfSSL_SetIO_wolfIP_CTX(WOLFSSL_CTX *ctx, struct wolfIP *s); -#endif - -#endif + #ifdef WOLFSSL_USER_SETTINGS + #include "user_settings.h" + #else + #include + #endif /* WOLFSSL_USER_SETTINGS */ + #include + #include + int wolfSSL_SetIO_wolfIP(WOLFSSL* ssl, int fd); + int wolfSSL_SetIO_wolfIP_CTX(WOLFSSL_CTX *ctx, struct wolfIP *s); + + #ifdef WOLFIP_ESP + #include + #include + #include + #endif /* WOLFIP_ESP */ +#endif /* WOLFSSL_WOLFIP */ + +#endif /* !WOLFIP_H */