diff --git a/.gitignore b/.gitignore index 01d773e..be3095e 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,7 @@ *.dis *.uf2 *.bin +*.swp CMakeCache.txt CMakeFiles CMakeScripts diff --git a/Makefile b/Makefile index 5077b10..39d1ea6 100644 --- a/Makefile +++ b/Makefile @@ -3,6 +3,23 @@ CFLAGS:=-Wall -Werror -Wextra -I. -D_GNU_SOURCE CFLAGS+=-g -ggdb LDFLAGS+=-pthread +# Debug flags: +# 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 +#CFLAGS+=-DWOLFIP_DEBUG_ESP_VERBOSE + +# ESP support + CFLAGS+=-DWOLFIP_ESP + CFLAGS+=-DWOLFSSL_WOLFIP + LDFLAGS+=-lwolfssl + CPPCHECK=cppcheck CPPCHECK_FLAGS=--enable=all --suppress=missingIncludeSystem \ --suppress=unusedFunction --suppress=unusedVariable \ diff --git a/README.md b/README.md index aabe0ff..4beb325 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) | +| +---------------+ | | +| +-----------------------+ +| | +| | +| | +| +-----------------------+ +| +-------------+ | | +httpd, req.path); if (!url) goto not_found; - + if ((url->handler == NULL) && (url->static_content == NULL)) goto service_unavailable; if (url->handler == NULL) { diff --git a/src/port/posix/bsd_socket.c b/src/port/posix/bsd_socket.c index b2c8f94..e825a99 100644 --- a/src/port/posix/bsd_socket.c +++ b/src/port/posix/bsd_socket.c @@ -552,7 +552,7 @@ void *wolfIP_sock_posix_ip_loop(void *arg) { } void __attribute__((constructor)) init_wolfip_posix() { - struct in_addr linux_ip; + struct in_addr linux_ip; struct ll *tapdev; pthread_t wolfIP_thread; if (IPSTACK) diff --git a/src/port/posix/linux_tap.c b/src/port/posix/linux_tap.c index 65cc824..bdf727d 100644 --- a/src/port/posix/linux_tap.c +++ b/src/port/posix/linux_tap.c @@ -34,9 +34,11 @@ static int tap_fd; -void print_buffer(uint8_t *buf, int len) +#ifdef DEBUG_TAP +void print_buffer(const char * what, uint8_t *buf, int len) { int i; + printf("%s (%d): ", what, len); for (i = 0; i < len; i++) { if (i % 16 == 0) printf("\n"); @@ -44,12 +46,14 @@ void print_buffer(uint8_t *buf, int len) } printf("\n"); } +#endif /* DEBUG_TAP */ static int tap_poll(struct ll *ll, void *buf, uint32_t len) { struct pollfd pfd; (void)ll; int ret; + ssize_t n_read = 0; pfd.fd = tap_fd; pfd.events = POLLIN; ret = poll(&pfd, 1, 2); @@ -60,13 +64,24 @@ static int tap_poll(struct ll *ll, void *buf, uint32_t len) if (ret == 0) { return 0; } - return read(tap_fd, buf, len); + + n_read = read(tap_fd, buf, len); + + #ifdef DEBUG_TAP + if (n_read > 0) { + print_buffer("tap_poll", buf, n_read); + } + #endif /* DEBUG_TAP */ + + return n_read; } static int tap_send(struct ll *ll, void *buf, uint32_t len) { (void)ll; - //print_buffer(buf, len); + #ifdef DEBUG_TAP + print_buffer("tap_send", buf, len); + #endif /* DEBUG_TAP */ return write(tap_fd, buf, len); } @@ -88,6 +103,7 @@ int tap_init(struct ll *ll, const char *ifname, uint32_t host_ip) close(tap_fd); return -1; } + /* Get mac address */ if (ioctl(tap_fd, SIOCGIFHWADDR, &ifr) < 0) { perror("ioctl SIOCGIFHWADDR"); @@ -100,7 +116,6 @@ int tap_init(struct ll *ll, const char *ifname, uint32_t host_ip) ll->poll = tap_poll; ll->send = tap_send; - /* Set up network side */ sock_fd = socket(AF_INET, SOCK_DGRAM, 0); if (sock_fd < 0) { diff --git a/src/port/raspberry-pico-usb-server/README.md b/src/port/raspberry-pico-usb-server/README.md index e913c34..538b804 100644 --- a/src/port/raspberry-pico-usb-server/README.md +++ b/src/port/raspberry-pico-usb-server/README.md @@ -8,7 +8,7 @@ - run CMake from this directory, specitying the FAMILY and PICO_COMPILER variables. Also specify the path where you cloned the pico-sdk in the previous step. -cmake . -DPICO_SDK_PATH=/path/to/src/pico-sdk -DFAMILY=rp2040 -DPICO_COMPILER=arm-none-eabi-gcc +cmake . -DPICO_SDK_PATH=/path/to/src/pico-sdk -DFAMILY=rp2040 -DPICO_COMPILER=arm-none-eabi-gcc - run make diff --git a/src/port/wolfssl_io.c b/src/port/wolfssl_io.c index 83c50c5..a104cd7 100644 --- a/src/port/wolfssl_io.c +++ b/src/port/wolfssl_io.c @@ -22,7 +22,7 @@ #include #include -static struct wolfIP *ref_ipstack = NULL; +static struct wolfIP *ref_ipstack = NULL; static int wolfIP_io_recv(WOLFSSL* ssl, char* buf, int sz, void* ctx) { diff --git a/src/test/esp_sa_list.c b/src/test/esp_sa_list.c new file mode 100644 index 0000000..0a76350 --- /dev/null +++ b/src/test/esp_sa_list.c @@ -0,0 +1,70 @@ +/* Static pre-defined SA lists for testing.*/ + +static struct wolfIP_esp_sa test_in_sa_list[WOLFIP_ESP_NUM_SA] = +{ + { + {0x2f, 0xa9, 0xd8, 0xc8}, /* spi */ + 0x010A0A0A, /* src */ + 0x020A0A0A, /* dst */ + 0,0, /* oseq, seq */ + 0, /* iv len */ + 0, {0}, 0, /* null enc */ + ESP_AUTH_SHA256_RFC4868, + {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01}, + 16, + ESP_ICVLEN_HMAC_128 + }, + { + {0x76, 0x4f, 0x47, 0xc9}, /* spi */ + 0x010A0A0A, /* src */ + 0x020A0A0A, /* dst */ + 0,0, /* oseq, seq */ + ESP_128_IV_LEN, /* iv len */ + ESP_ENC_CBC_AES, + {0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, + 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}, + 16, + ESP_AUTH_SHA256_RFC4868, + {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01}, + 16, + ESP_ICVLEN_HMAC_128 + }, +}; + +static struct wolfIP_esp_sa test_out_sa_list[WOLFIP_ESP_NUM_SA] = +{ + { + {0xf6, 0xe9, 0xb8, 0x0d}, /* spi */ + 0x020A0A0A, /* src */ + 0x030A0A0A, /* dst */ + 0,0, /* oseq, seq */ + 0, /* iv len */ + 0, {0}, 0, /* null enc */ + ESP_AUTH_SHA256_RFC4868, + {0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, + 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02}, + 16, + ESP_ICVLEN_HMAC_128 + }, + + { + {0x49, 0xeb, 0xfd, 0xd4}, /* spi */ + 0x020A0A0A, /* src */ + 0x010A0A0A, /* dst */ + 0,0, /* oseq, seq */ + ESP_128_IV_LEN, /* iv len */ + ESP_ENC_CBC_AES, + {0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, + 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04}, + 16, + ESP_AUTH_SHA256_RFC4868, + {0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, + 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02}, + 16, + ESP_ICVLEN_HMAC_128 + }, + +}; + diff --git a/src/test/tcp_echo.c b/src/test/tcp_echo.c index fd63a3e..42453f8 100644 --- a/src/test/tcp_echo.c +++ b/src/test/tcp_echo.c @@ -59,7 +59,6 @@ int main() { exit(EXIT_FAILURE); } - printf("Echo server listening on port %d\n", PORT); while (1) { diff --git a/src/test/test_httpd.c b/src/test/test_httpd.c index 109ff86..5cafea5 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_linux_dhcp_dns.c b/src/test/test_linux_dhcp_dns.c index 2d859ad..7aaf0f4 100644 --- a/src/test/test_linux_dhcp_dns.c +++ b/src/test/test_linux_dhcp_dns.c @@ -42,8 +42,10 @@ static int tot_sent = 0; static int wolfIP_closing = 0; static int closed = 0; 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}; /* Client-side callback. */ static void client_cb(int fd, uint16_t event, void *arg) diff --git a/src/test/test_linux_eventloop.c b/src/test/test_linux_eventloop.c index 9e8840b..d7eb50e 100644 --- a/src/test/test_linux_eventloop.c +++ b/src/test/test_linux_eventloop.c @@ -30,6 +30,11 @@ #include "config.h" #include "wolfip.h" +#if defined(WOLFIP_ESP) + #include "wolfesp.h" + #include "esp_sa_list.c" +#endif + #define TEST_SIZE (4 * 1024) #define BUFFER_SIZE TEST_SIZE @@ -43,8 +48,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. */ @@ -172,7 +179,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) { @@ -463,6 +469,11 @@ int main(int argc, char **argv) inet_pton(AF_INET, WOLFIP_IP, &srv_ip); #endif +#if defined(WOLFIP_ESP) + esp_load_sa_list(test_in_sa_list, 2, 1); + esp_load_sa_list(test_out_sa_list, 2, 0); +#endif + /* Server side test */ test_wolfip_echoserver(s, srv_ip); diff --git a/src/test/test_native_wolfssl.c b/src/test/test_native_wolfssl.c index f2b3ad5..e3c81ad 100644 --- a/src/test/test_native_wolfssl.c +++ b/src/test/test_native_wolfssl.c @@ -44,8 +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 */ static WOLFSSL *client_ssl = NULL; @@ -286,7 +288,7 @@ void test_wolfip_echoserver(struct wolfIP *s, uint32_t srv_ip) } printf("Associating server context with wolfIP\n"); wolfSSL_SetIO_FT_CTX(server_ctx, s); - + printf("Importing server certificate\n"); ret = wolfSSL_CTX_use_certificate_buffer(server_ctx, server_der, server_der_len, SSL_FILETYPE_ASN1); diff --git a/src/test/unit/unit.c b/src/test/unit/unit.c index 0aeca38..2e17d14 100644 --- a/src/test/unit/unit.c +++ b/src/test/unit/unit.c @@ -780,7 +780,7 @@ Suite *wolf_suite(void) suite_add_tcase(s, tc_proto); tcase_add_test(tc_proto, test_arp_lookup_failure); suite_add_tcase(s, tc_proto); - + tcase_add_test(tc_utils, test_transport_checksum); suite_add_tcase(s, tc_proto); tcase_add_test(tc_utils, test_iphdr_set_checksum); @@ -792,8 +792,6 @@ Suite *wolf_suite(void) return s; } - - int main(void) { int n_fail = 0; @@ -807,5 +805,4 @@ int main(void) n_fail = srunner_ntests_failed(sr); srunner_free(sr); return (n_fail == 0) ? EXIT_SUCCESS : EXIT_FAILURE; - } diff --git a/src/wolfesp.c b/src/wolfesp.c new file mode 100644 index 0000000..1e38514 --- /dev/null +++ b/src/wolfesp.c @@ -0,0 +1,869 @@ +#if defined(WOLFIP_ESP) && !defined(WOLFESP_SRC) +#define WOLFESP_SRC + +#include "wolfesp.h" + +static WC_RNG wc_rng; +static uint8_t rng_inited = 0; + +int +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 +esp_load_sa_list(struct wolfIP_esp_sa * sa_list, uint16_t num, uint16_t in) +{ + 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 */ + +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 = WC_AES_BLOCK_SIZE; + break; + case ESP_ENC_CBC_DES3: + block_len = DES_BLOCK_SIZE; + break; + 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; +} + +static int +esp_const_memcmp(const uint8_t * vec_a, const uint8_t * vec_b, uint32_t len) +{ + uint32_t i; + 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 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\n"); + #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\n"); + #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; +} + + +/** + * 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; + int rc = 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) { + rc = esp_check_icv_hmac(esp_sa, ip->data, esp_len); + if (rc) { + 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_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? */ + 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_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 rc = 0; + + icv = ip->data + icv_offset; + + rc = esp_calc_icv_hmac(icv, esp_sa, ip->data, payload_len); + if (rc) { + 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 *s, 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); + + s->ll_dev.send(&s->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 fd97fab..af846fe 100644 --- a/src/wolfip.c +++ b/src/wolfip.c @@ -791,11 +791,39 @@ static void tcp_send_synack(struct tsocket *t) return tcp_send_syn(t, 0x12); } +#ifdef DEBUG_TCP +static void wolfIP_print_tcp(struct wolfIP_tcp_seg * tcp) +{ + printf("tcp hdr:\n"); + printf("+-----------------------------+\n"); + printf("| %8d | %8d | (sport, dport)\n", + ee16(tcp->src_port), ee16(tcp->dst_port)); + printf("+-----------------------------+\n"); + printf("| %10u | %10u | (raw seq, raw ack)\n", + ee32(tcp->seq), ee32(tcp->ack)); + printf("+-----------------------------+\n"); + printf("| %2u | %d %d %d %d %d %d | %5d | (hdrlen, flags, win)\n", + tcp->hlen >> 2, + tcp->flags >> 5 & 1, tcp->flags >> 4 & 1, tcp->flags >> 3 & 1, + tcp->flags >> 2 & 1, tcp->flags >> 1 & 1, tcp->flags >> 0 & 1, + ee16(tcp->win)); + printf("+-----------------------------+\n"); + printf("| 0x%04x | %5d | (chksum, urg)\n", + ee16(tcp->csum), ee16(tcp->urg)); + printf("+-----------------------------+\n"); + printf("\n"); +} +#endif /* DEBUG_TCP*/ + /* Add a segment to the rx buffer for the application to consume */ static void tcp_recv(struct tsocket *t, struct wolfIP_tcp_seg *seg) { uint32_t seg_len = ee16(seg->ip.len) - (IP_HEADER_LEN + (seg->hlen >> 2)); uint32_t seq = ee32(seg->seq); + + #ifdef DEBUG_TCP + wolfIP_print_tcp(seg); + #endif /* DEBUG_TCP */ if ((t->sock.tcp.state != TCP_ESTABLISHED) && (t->sock.tcp.state != TCP_CLOSE_WAIT)) { return; } @@ -869,8 +897,8 @@ static void iphdr_set_checksum(struct wolfIP_ip_packet *ip) } #ifdef ETHERNET -static int eth_output_add_header(struct wolfIP *S, const uint8_t *dst, struct wolfIP_eth_frame *eth, - uint16_t type) +static int eth_output_add_header(struct wolfIP *S, const uint8_t *dst, + struct wolfIP_eth_frame *eth, uint16_t type) { if (!dst) { /* Arp request, broadcast */ @@ -885,9 +913,15 @@ static int eth_output_add_header(struct wolfIP *S, const uint8_t *dst, struct wo } #endif -static int ip_output_add_header(struct tsocket *t, struct wolfIP_ip_packet *ip, uint8_t proto, uint16_t len) +#ifdef WOLFIP_ESP +#include "src/wolfesp.c" +#endif /* WOLFIP_ESP */ + +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; + memset(&ph, 0, sizeof(ph)); memset(ip, 0, sizeof(struct wolfIP_ip_packet)); ip->src = ee32(t->local_ip); @@ -917,8 +951,10 @@ static int ip_output_add_header(struct tsocket *t, struct wolfIP_ip_packet *ip, udp->csum = ee16(transport_checksum(&ph, &udp->src_port)); } #ifdef ETHERNET - eth_output_add_header(t->S, t->nexthop_mac, (struct wolfIP_eth_frame *)ip, ETH_TYPE_IP); + eth_output_add_header(t->S, t->nexthop_mac, (struct wolfIP_eth_frame *)ip, + ETH_TYPE_IP); #endif + return 0; } @@ -1408,7 +1444,7 @@ int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len int wolfIP_sock_send(struct wolfIP *s, int sockfd, const void *buf, size_t len, int flags) { return wolfIP_sock_sendto(s, sockfd, buf, len, flags, NULL, 0); -} +} int wolfIP_sock_write(struct wolfIP *s, int sockfd, const void *buf, size_t len) { @@ -1957,6 +1993,10 @@ static int arp_lookup(struct wolfIP *s, ip4 ip, uint8_t *mac) void wolfIP_init(struct wolfIP *s) { memset(s, 0, sizeof(struct wolfIP)); + + #ifdef WOLFIP_ESP + esp_init(); + #endif /* WOLFIP_ESP */ } struct ll *wolfIP_getdev(struct wolfIP *s) @@ -1971,11 +2011,60 @@ void wolfIP_init_static(struct wolfIP **s) return; wolfIP_init(&wolfIP_static); *s = &wolfIP_static; -} + + #ifdef WOLFIP_ESP + esp_init(); + #endif /* WOLFIP_ESP */ +} + +#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, struct wolfIP_ip_packet *ip, uint32_t len) { + #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, tcp, len); @@ -1983,11 +2072,37 @@ static inline void ip_recv(struct wolfIP *s, struct wolfIP_ip_packet *ip, else if (ip->ver_ihl == 0x45 && ip->proto == 0x11) { struct wolfIP_udp_datagram *udp = (struct wolfIP_udp_datagram *)ip; udp_try_recv(s, udp, len); - } else if (ip->ver_ihl == 0x45 && ip->proto == 0x01) { + } + else if (ip->ver_ihl == 0x45 && ip->proto == 0x01) { icmp_input(s, 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 | %5d bytes data | (eth type, payload) \n", + type[0], type[1], len); + printf("+---------------------------------------+\n"); + printf("\n"); +} +#endif /* DEBUG_ETH */ + /* Try to receive a packet from the network interface. * * This function is called either after polling the device driver @@ -1997,9 +2112,21 @@ void wolfIP_recv(struct wolfIP *s, void *buf, uint32_t len) { #ifdef ETHERNET struct wolfIP_eth_frame *eth = (struct wolfIP_eth_frame *)buf; + + #ifdef DEBUG_ETH + wolfIP_print_eth(eth, len); + #endif /* DEBUG_ETH */ + if (eth->type == ee16(0x0800)) { struct wolfIP_ip_packet *ip = (struct wolfIP_ip_packet *)eth; - if ((memcmp(eth->dst, s->ll_dev.mac, 6) != 0) && (memcmp(eth->dst, "\xff\xff\xff\xff\xff\xff", 6) != 0)) { + if ((memcmp(eth->dst, s->ll_dev.mac, 6) != 0) && + (memcmp(eth->dst, "\xff\xff\xff\xff\xff\xff", 6) != 0)) { + #ifdef DEBUG_ETH + printf("info: dropping eth frame, dst not for us: " + "%02x:%02x:%02x:%02x:%02x:%02x\n", + eth->dst[0], eth->dst[1], eth->dst[2], + eth->dst[3], eth->dst[4], eth->dst[5]); + #endif /* DEBUG_ETH */ return; /* Not for us */ } ip_recv(s, ip, len); @@ -2192,7 +2319,9 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) uint32_t size = 0; struct pkt_desc *desc; struct wolfIP_tcp_seg *tcp; + desc = fifo_peek(&ts->sock.tcp.txbuf); + while (desc) { tcp = (struct wolfIP_tcp_seg *)(ts->txmem + desc->pos + sizeof(*desc)); if (desc->flags & PKT_FLAG_SENT) { @@ -2210,6 +2339,7 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) #endif if (in_flight <= ts->sock.tcp.cwnd) { struct wolfIP_timer new_tmr = {}; + size = desc->len - ETH_HEADER_LEN; tcp = (struct wolfIP_tcp_seg *)(ts->txmem + desc->pos + sizeof(*desc)); if ((ts->sock.tcp.ack == ts->sock.tcp.last_ack) && @@ -2224,8 +2354,16 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) ts->sock.tcp.last_ack = ts->sock.tcp.ack; tcp->ack = ee32(ts->sock.tcp.ack); tcp->win = ee16(queue_space(&ts->sock.tcp.rxbuf)); - ip_output_add_header(ts, (struct wolfIP_ip_packet *)tcp, FT_IPPROTO_TCP, size); + + ip_output_add_header(ts, (struct wolfIP_ip_packet *)tcp, + FT_IPPROTO_TCP, size); + + #ifdef WOLFIP_ESP + esp_output(s, (struct wolfIP_ip_packet *)tcp, size); + #else s->ll_dev.send(&s->ll_dev, tcp, desc->len); + #endif /* WOLFIP_ESP */ + desc->flags |= PKT_FLAG_SENT; desc->time_sent = now; if (size == IP_HEADER_LEN + (uint32_t)(tcp->hlen >> 2)) { @@ -2252,6 +2390,7 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) for (i = 0; i < MAX_UDPSOCKETS; i++) { struct tsocket *t = &s->udpsockets[i]; struct pkt_desc *desc = fifo_peek(&t->sock.udp.txbuf); + while (desc) { struct wolfIP_udp_datagram *udp = (struct wolfIP_udp_datagram *)(t->txmem + desc->pos + sizeof(*desc)); #ifdef ETHERNET @@ -2264,8 +2403,11 @@ int wolfIP_poll(struct wolfIP *s, uint64_t now) if (IS_IP_BCAST(nexthop)) memset(t->nexthop_mac, 0xFF, 6); #endif len = desc->len - ETH_HEADER_LEN; - ip_output_add_header(t, (struct wolfIP_ip_packet *)udp, FT_IPPROTO_UDP, len); + + ip_output_add_header(t, (struct wolfIP_ip_packet *)udp, + FT_IPPROTO_UDP, len); s->ll_dev.send(&s->ll_dev, udp, desc->len); + fifo_pop(&t->sock.udp.txbuf); desc = fifo_peek(&t->sock.udp.txbuf); } diff --git a/wolfesp.h b/wolfesp.h new file mode 100644 index 0000000..2944130 --- /dev/null +++ b/wolfesp.h @@ -0,0 +1,66 @@ +#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 2 + +/* aes-128 */ +#define ESP_128_KEY_LEN 16 +#define ESP_128_IV_LEN 16 + +/* gcm */ +#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 \ + +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; + #if 0 + uint8_t pre_iv[ESP_GCM_RFC4106_IV_LEN]; /* unique salt that is xor'ed with + * oseq to generate the iv. */ + #endif +}; + +int esp_init(void); +void esp_load_sa_list(struct wolfIP_esp_sa * sa_list, uint16_t num, uint16_t in); + +#endif /* !WOLFESP_H */ diff --git a/wolfip.h b/wolfip.h index 8a3b0c8..5e5e14c 100644 --- a/wolfip.h +++ b/wolfip.h @@ -14,11 +14,11 @@ typedef uint32_t ip4; #ifdef DEBUG -#include -#define LOG(fmt, ...) printf(fmt, ##__VA_ARGS__) -#else -#define LOG(fmt, ...) do{}while(0) -#endif + #include + #define LOG(fmt, ...) printf(fmt, ##__VA_ARGS__) + #else + #define LOG(fmt, ...) do{}while(0) +#endif /* DEBUG */ /* Device driver interface */ /* Struct to contain a hw device description */ @@ -43,55 +43,63 @@ struct ipconf { #define MARK_TCP_SOCKET 0x100 /* Mark a socket as TCP */ #define MARK_UDP_SOCKET 0x200 /* Mark a socket as UDP */ #if (MARK_TCP_SOCKET >= MARK_UDP_SOCKET) -#error "MARK_TCP_SOCKET must be less than MARK_UDP_SOCKET" -#endif + #error "MARK_TCP_SOCKET must be less than MARK_UDP_SOCKET" +#endif /* MARK_TCP_SOCKET >= MARK_UDP_SOCKET */ #ifndef WOLF_POSIX -#define IPSTACK_SOCK_STREAM 1 -#define IPSTACK_SOCK_DGRAM 2 - - -struct wolfIP_sockaddr_in { - uint16_t sin_family; - uint16_t sin_port; - struct sin_addr { uint32_t s_addr; } sin_addr; -}; -struct wolfIP_sockaddr { uint16_t sa_family; }; -typedef uint32_t socklen_t; -#ifndef AF_INET -#define AF_INET 2 -#endif + #define IPSTACK_SOCK_STREAM 1 + #define IPSTACK_SOCK_DGRAM 2 + + struct wolfIP_sockaddr_in { + uint16_t sin_family; + uint16_t sin_port; + struct sin_addr { uint32_t s_addr; } sin_addr; + }; + struct wolfIP_sockaddr { uint16_t sa_family; }; + typedef uint32_t socklen_t; + #ifndef AF_INET + #define AF_INET 2 + #endif /* !AF_INET */ #else -#include -#include -#include -#include -#define wolfIP_sockaddr_in sockaddr_in -#define wolfIP_sockaddr sockaddr -#endif + #include + #include + #include + #include + #define wolfIP_sockaddr_in sockaddr_in + #define wolfIP_sockaddr sockaddr +#endif /* !WOLF_POSIX */ 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_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); -int wolfIP_sock_accept(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, socklen_t *addrlen); -int wolfIP_sock_connect(struct wolfIP *s, int sockfd, const struct wolfIP_sockaddr *addr, socklen_t addrlen); -int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, size_t len, int flags, const struct wolfIP_sockaddr *dest_addr, socklen_t addrlen); -int wolfIP_sock_send(struct wolfIP *s, int sockfd, const void *buf, size_t len, int flags); +int wolfIP_sock_accept(struct wolfIP *s, int sockfd, + struct wolfIP_sockaddr *addr, socklen_t *addrlen); +int wolfIP_sock_connect(struct wolfIP *s, int sockfd, + const struct wolfIP_sockaddr *addr, socklen_t addrlen); +int wolfIP_sock_sendto(struct wolfIP *s, int sockfd, const void *buf, + size_t len, int flags, const struct wolfIP_sockaddr *dest_addr, + socklen_t addrlen); +int wolfIP_sock_send(struct wolfIP *s, int sockfd, const void *buf, size_t len, + int flags); int wolfIP_sock_write(struct wolfIP *s, int sockfd, const void *buf, size_t len); -int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, int flags, struct wolfIP_sockaddr *src_addr, socklen_t *addrlen); +int wolfIP_sock_recvfrom(struct wolfIP *s, int sockfd, void *buf, size_t len, + int flags, struct wolfIP_sockaddr *src_addr, socklen_t *addrlen); int wolfIP_sock_recv(struct wolfIP *s, int sockfd, void *buf, size_t len, int flags); int wolfIP_sock_read(struct wolfIP *s, int sockfd, void *buf, size_t len); int wolfIP_sock_close(struct wolfIP *s, int sockfd); -int wolfIP_sock_getpeername(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, const socklen_t *addrlen); -int wolfIP_sock_getsockname(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); +int wolfIP_sock_getsockname(struct wolfIP *s, int sockfd, struct wolfIP_sockaddr *addr, + const socklen_t *addrlen); int dhcp_client_init(struct wolfIP *s); int dhcp_bound(struct wolfIP *s); /* DNS client */ - -int nslookup(struct wolfIP *s, const char *name, uint16_t *id, void (*lookup_cb)(uint32_t ip)); +int nslookup(struct wolfIP *s, const char *name, uint16_t *id, + void (*lookup_cb)(uint32_t ip)); /* IP stack interface */ void wolfIP_init(struct wolfIP *s); @@ -108,7 +116,9 @@ struct ll *wolfIP_getdev(struct wolfIP *s); #define CB_EVENT_TIMEOUT 0x02 /* Timeout */ #define CB_EVENT_WRITABLE 0x04 /* Connected or space available to send */ #define CB_EVENT_CLOSED 0x10 /* Connection closed by peer */ -void wolfIP_register_callback(struct wolfIP *s, int sock_fd, void (*cb)(int sock_fd, uint16_t events, void *arg), void *arg); +void wolfIP_register_callback(struct wolfIP *s, int sock_fd, + void (*cb)(int sock_fd, uint16_t events, void *arg), + void *arg); /* External requirements */ uint32_t wolfIP_getrandom(void); @@ -153,16 +163,22 @@ static inline void iptoa(ip4 ip, char *buf) } #ifdef WOLFSSL_WOLFIP -#ifdef WOLFSSL_USER_SETTINGS -#include "user_settings.h" -#else -#include -#endif -#include -#include -/* Defined in wolfssl_io.c */ -int wolfSSL_SetIO_FT(WOLFSSL* ssl, int fd); -int wolfSSL_SetIO_FT_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 + /* Defined in wolfssl_io.c */ + int wolfSSL_SetIO_FT(WOLFSSL* ssl, int fd); + int wolfSSL_SetIO_FT_CTX(WOLFSSL_CTX *ctx, struct wolfIP *s); + + #ifdef WOLFIP_ESP + #include + #include + #include + #endif /* WOLFIP_ESP */ +#endif /* WOLFSSL_WOLFIP */ + +#endif /* !WOLFIP_H */