From bb835d7bafe727b5cf808dc1098c330dc6a2abca Mon Sep 17 00:00:00 2001 From: Jiacheng Guo Date: Wed, 3 Nov 2021 16:26:14 +0800 Subject: [PATCH] Support ICMPv6 RIO handling * Add support for ICMPv6 RIO handling for ESP32 * Update ESP-IDF release to v4.4 for route hook support --- .gitlab-ci.yml | 2 +- components/route_hook/CMakeLists.txt | 8 + .../route_hook/include/esp_route_hook.h | 18 ++ .../private_include/esp_route_table.h | 80 ++++++++ components/route_hook/src/esp_route_hook.c | 191 ++++++++++++++++++ components/route_hook/src/esp_route_table.c | 156 ++++++++++++++ examples/light/main/CMakeLists.txt | 2 +- examples/light/main/app_matter.cpp | 4 + examples/light/sdkconfig.defaults | 4 + examples/rainmaker_light/main/CMakeLists.txt | 2 +- examples/rainmaker_light/main/app_matter.cpp | 2 + examples/rainmaker_light/sdkconfig.defaults | 4 + 12 files changed, 470 insertions(+), 3 deletions(-) create mode 100644 components/route_hook/CMakeLists.txt create mode 100644 components/route_hook/include/esp_route_hook.h create mode 100644 components/route_hook/private_include/esp_route_table.h create mode 100644 components/route_hook/src/esp_route_hook.c create mode 100644 components/route_hook/src/esp_route_table.c diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 0bd789a48..6bcebf1e7 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -12,7 +12,7 @@ variables: .setup_idf: &setup_idf # - cd $REPOS_PATH # IDF_PATH is set to something else already? - # using v4.3 tag of espressif/esp-idf github repo + # using release/v4.4 branch of espressif/esp-idf github repo - git clone --recursive --single-branch -b release/v4.4 --reference-if-able /local_references/github/ https://github.com/espressif/esp-idf.git - cd esp-idf - ./install.sh diff --git a/components/route_hook/CMakeLists.txt b/components/route_hook/CMakeLists.txt new file mode 100644 index 000000000..19cd48bc9 --- /dev/null +++ b/components/route_hook/CMakeLists.txt @@ -0,0 +1,8 @@ +if (NOT CONFIG_OPENTHREAD_ENABLED) + set(src_dirs src) +endif() + +idf_component_register(SRC_DIRS ${src_dirs} + INCLUDE_DIRS include + PRIV_INCLUDE_DIRS private_include + REQUIRES lwip) diff --git a/components/route_hook/include/esp_route_hook.h b/components/route_hook/include/esp_route_hook.h new file mode 100644 index 000000000..f35411261 --- /dev/null +++ b/components/route_hook/include/esp_route_hook.h @@ -0,0 +1,18 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_err.h" +#include "esp_netif.h" + +#ifdef __cplusplus +extern "C" { +#endif + +esp_err_t esp_route_hook_init(esp_netif_t *netif); + +#ifdef __cplusplus +} +#endif diff --git a/components/route_hook/private_include/esp_route_table.h b/components/route_hook/private_include/esp_route_table.h new file mode 100644 index 000000000..31db85e9f --- /dev/null +++ b/components/route_hook/private_include/esp_route_table.h @@ -0,0 +1,80 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_err.h" +#include "lwip/ip6_addr.h" +#include "lwip/netif.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Route table entry + * + */ +typedef struct { + ip6_addr_t prefix; + uint8_t prefix_length; + ip6_addr_t gateway; + int8_t preference; + uint32_t lifetime_seconds; + struct netif *netif; +} esp_route_entry_t; + +/** + * @brief Adds an entry to the route table + * + * @param[in] route_entry The route entry to be added + * + * @return + * - The pointer to the added route entry on success + * - NULL on failure + * + */ +esp_route_entry_t *esp_route_table_add_route_entry(const esp_route_entry_t *route_entry); + +/** + * @brief Removes an entry from the route table + * + * @param[in] route_entry The route entry to be removed + * + * @return + * - ESP_OK + * - ESP_ERR_INVALID_ARG The provided route_entry is not in the route table. + * + */ +esp_err_t esp_route_table_remove_route_entry(esp_route_entry_t *route_entry); + +/** + * @brief The lwIP ip6 route hook, called by the lwIP function ip6_route when sending packets. + * + * @param[in] src The source address + * @param[in] dest The destination address + * + * @return + * - The target interface when route found + * - NULL when route not found + * + */ +struct netif *lwip_hook_ip6_route(const ip6_addr_t *src, const ip6_addr_t *dest); + +/** + * @brief The lwIP gateway hook, called by the lwIP when deciding next hop. + * + * @param[in] netif The output network interface + * @param[in] dest The destination address + * + * @return + * - The gateway address when route found + * - NULL when route not found + * + */ +const ip6_addr_t *lwip_hook_nd6_get_gw(struct netif *netif, const ip6_addr_t *dest); + +#ifdef __cplusplus +} +#endif diff --git a/components/route_hook/src/esp_route_hook.c b/components/route_hook/src/esp_route_hook.c new file mode 100644 index 000000000..1eed8251c --- /dev/null +++ b/components/route_hook/src/esp_route_hook.c @@ -0,0 +1,191 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_route_hook.h" + +#include +#include + +#include "esp_check.h" +#include "esp_err.h" +#include "esp_netif.h" +#include "esp_route_table.h" + +#include "lwip/icmp6.h" +#include "lwip/mld6.h" +#include "lwip/netif.h" +#include "lwip/prot/icmp6.h" +#include "lwip/prot/ip6.h" +#include "lwip/prot/nd6.h" +#include "lwip/raw.h" + +#define HOPLIM_MAX 255 +#define PIO_FLAG_ON_LINK (1 << 7) +#define PIO_FLAG_AUTO_CONFIG (1 << 6) + +#define TAG "ROUTE_HOOK" + +typedef struct esp_route_hook_t { + struct netif *netif; + struct raw_pcb *pcb; + struct esp_route_hook_t *next; +} esp_route_hook_t; + +PACK_STRUCT_BEGIN +struct rio_header_t { + PACK_STRUCT_FLD_8(u8_t type); + PACK_STRUCT_FLD_8(u8_t length); + PACK_STRUCT_FLD_8(u8_t prefix_length); + PACK_STRUCT_FLD_8(u8_t preference); + PACK_STRUCT_FIELD(u32_t route_lifetime); +} PACK_STRUCT_STRUCT; +PACK_STRUCT_END + +typedef struct rio_header_t rio_header_t; + +static esp_route_hook_t *s_hooks; + +static bool is_self_address(struct netif *netif, const ip6_addr_t *addr) +{ + for (size_t i = 0; i < LWIP_ARRAYSIZE(netif->ip6_addr); i++) { + if (ip6_addr_isvalid(netif_ip6_addr_state(netif, i)) && + memcmp(addr->addr, netif_ip6_addr(netif, i)->addr, sizeof(addr->addr)) == 0) { + return true; + } + } + return false; +} + +static void ra_recv_handler(struct netif *netif, const uint8_t *icmp_payload, uint16_t payload_len, + const ip6_addr_t *src_addr) +{ + if (payload_len < sizeof(struct ra_header)) { + return; + } + icmp_payload += sizeof(struct ra_header); + payload_len -= sizeof(struct ra_header); + + while (payload_len >= 2) { + uint8_t opt_type = icmp_payload[0]; + uint8_t opt_len = icmp_payload[1] << 3; + + if (opt_type == ND6_OPTION_TYPE_ROUTE_INFO && opt_len >= sizeof(rio_header_t) && + !is_self_address(netif, src_addr) && payload_len >= opt_len) { + rio_header_t rio_header; + memcpy(&rio_header, icmp_payload, sizeof(rio_header)); + + // skip if prefix is longer than IPv6 address. + if (rio_header.prefix_length > 128) { + break; + } + uint8_t prefix_len_bytes = (rio_header.prefix_length + 7) / 8; + int8_t preference = -2 * ((rio_header.preference >> 4) & 1) + (((rio_header.preference) >> 3) & 1); + const uint8_t *rio_data = &icmp_payload[sizeof(rio_header_t)]; + uint8_t rio_data_len = opt_len - sizeof(rio_header_t); + + ESP_LOGI(TAG, "Received RIO"); + if (rio_data_len >= prefix_len_bytes) { + ip6_addr_t prefix; + esp_route_entry_t route; + + memset(&prefix, 0, sizeof(prefix)); + memcpy(&prefix.addr, rio_data, prefix_len_bytes); + route.netif = netif; + route.gateway = *src_addr; + route.prefix_length = rio_header.prefix_length; + route.prefix = prefix; + route.preference = preference; + route.lifetime_seconds = lwip_ntohl(rio_header.route_lifetime); + ESP_LOGI(TAG, "prefix %s lifetime %u\n", ip6addr_ntoa(&prefix), route.lifetime_seconds); + if (esp_route_table_add_route_entry(&route) == NULL) { + ESP_LOGI(TAG, "Failed to add route table entry\n"); + } + } + } + icmp_payload += opt_len; + payload_len -= opt_len; + } +} + +static uint8_t icmp6_raw_recv_handler(void *arg, struct raw_pcb *pcb, struct pbuf *p, const ip_addr_t *addr) +{ + uint8_t *icmp_payload = NULL; + uint16_t icmp_payload_len; + struct ip6_hdr *ip6_header = (struct ip6_hdr *)p->payload; + struct icmp6_hdr *icmp6_header; + ip6_addr_t src; + ip6_addr_t dest; + esp_route_hook_t *hook = (esp_route_hook_t *)arg; + + memcpy(src.addr, ip6_header->src.addr, sizeof(src.addr)); + memcpy(dest.addr, ip6_header->dest.addr, sizeof(dest.addr)); +#if LWIP_IPV6_SCOPES + src.zone = 0; +#endif + + if (p->tot_len != p->len) { + ESP_LOGW(TAG, "Ignore segmented ICMP packet"); + return 0; + } + if (p->tot_len <= sizeof(struct ip6_hdr) + sizeof(struct icmp6_hdr)) { + ESP_LOGW(TAG, "Ignore invalid ICMP packet"); + return 0; + } + if (!ip6_addr_islinklocal(&dest) && !ip6_addr_isallnodes_linklocal(&dest) && + !ip6_addr_isallrouters_linklocal(&dest)) { + return 0; + } + + icmp_payload_len = p->tot_len - sizeof(struct ip6_hdr); + icmp_payload = p->payload + sizeof(struct ip6_hdr); + + icmp6_header = (struct icmp6_hdr *)icmp_payload; + if (icmp6_header->type == ICMP6_TYPE_RA) { + ra_recv_handler(hook->netif, icmp_payload, icmp_payload_len, &src); + } + return 0; +} + +esp_err_t esp_route_hook_init(esp_netif_t *netif) +{ + struct netif *lwip_netif; + ip_addr_t router_group = IPADDR6_INIT_HOST(0xFF020000, 0, 0, 0x02); + esp_route_hook_t *hook = NULL; + esp_err_t ret = ESP_OK; + + ESP_RETURN_ON_FALSE(netif != NULL, ESP_ERR_INVALID_ARG, TAG, "Invalid network interface"); + lwip_netif = netif_get_by_index(esp_netif_get_netif_impl_index(netif)); + ESP_RETURN_ON_FALSE(lwip_netif != NULL, ESP_ERR_INVALID_ARG, TAG, "Invalid network interface"); + + for (esp_route_hook_t *iter = s_hooks; iter != NULL; iter++) { + if (iter->netif == lwip_netif) { + ESP_LOGI(TAG, "Hook already installed on netif, skip..."); + return ESP_OK; + } + } + + hook = (esp_route_hook_t *)malloc(sizeof(esp_route_hook_t)); + ESP_RETURN_ON_FALSE(hook != NULL, ESP_ERR_NO_MEM, TAG, "Cannot allocate hook"); + + ESP_GOTO_ON_FALSE(mld6_joingroup_netif(lwip_netif, ip_2_ip6(&router_group)) == ESP_OK, ESP_FAIL, exit, TAG, + "Failed to join multicast group"); + hook->netif = lwip_netif; + hook->pcb = raw_new_ip_type(IPADDR_TYPE_V6, IP6_NEXTH_ICMP6); + hook->pcb->flags |= RAW_FLAGS_MULTICAST_LOOP; + hook->pcb->chksum_reqd = 1; + // The ICMPv6 header checksum offset + hook->pcb->chksum_offset = 2; + raw_bind_netif(hook->pcb, lwip_netif); + raw_recv(hook->pcb, icmp6_raw_recv_handler, hook); + hook->next = s_hooks; + s_hooks = hook; + +exit: + if (ret != ESP_OK && hook != NULL) { + free(hook); + } + return ret; +} diff --git a/components/route_hook/src/esp_route_table.c b/components/route_hook/src/esp_route_table.c new file mode 100644 index 000000000..48aaa4f33 --- /dev/null +++ b/components/route_hook/src/esp_route_table.c @@ -0,0 +1,156 @@ +/* + * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_route_table.h" + +#include + +#include "esp_err.h" +#include "esp_log.h" +#include "lwip/ip6_addr.h" +#include "lwip/netif.h" +#include "lwip/timeouts.h" + +#define MAX_RIO_ROUTE 20 + +#define TAG "ROUTE_HOOK" + +static esp_route_entry_t s_route_entries[MAX_RIO_ROUTE]; + +static esp_route_entry_t *find_route_entry(const esp_route_entry_t *route_entry) +{ + for (size_t i = 0; i < LWIP_ARRAYSIZE(s_route_entries); i++) { + if (s_route_entries[i].netif == NULL) { + break; + } + if (s_route_entries[i].netif == route_entry->netif && + s_route_entries[i].prefix_length == route_entry->prefix_length && + memcmp(s_route_entries[i].gateway.addr, route_entry->gateway.addr, sizeof(route_entry->gateway.addr)) == + 0 && + memcmp(s_route_entries[i].prefix.addr, route_entry->prefix.addr, route_entry->prefix_length / 8) == 0) { + return &s_route_entries[i]; + } + } + return NULL; +} + +static esp_route_entry_t *find_empty_route_entry(void) +{ + for (size_t i = 0; i < LWIP_ARRAYSIZE(s_route_entries); i++) { + if (s_route_entries[i].netif == NULL) { + return &s_route_entries[i]; + } + } + return NULL; +} + +static void route_timeout_handler(void *arg) +{ + esp_route_entry_t *route = (esp_route_entry_t *)arg; + + esp_route_table_remove_route_entry(route); +} + +esp_route_entry_t *esp_route_table_add_route_entry(const esp_route_entry_t *route_entry) +{ + if (route_entry == NULL) { + return NULL; + } + + esp_route_entry_t *entry = find_route_entry(route_entry); + + if (entry == NULL) { + entry = find_empty_route_entry(); + if (entry == NULL) { + return NULL; + } + entry->netif = route_entry->netif; + entry->gateway = route_entry->gateway; + ip6_addr_assign_zone(&entry->gateway, IP6_UNICAST, entry->netif); + entry->prefix = route_entry->prefix; + entry->prefix_length = route_entry->prefix_length; + } else { + sys_untimeout(route_timeout_handler, entry); + } + entry->preference = route_entry->preference; + entry->lifetime_seconds = route_entry->lifetime_seconds; + sys_timeout(entry->lifetime_seconds * 1000, route_timeout_handler, entry); + return entry; +} + +esp_err_t esp_route_table_remove_route_entry(esp_route_entry_t *route_entry) +{ + if (route_entry < &s_route_entries[0] || route_entry > &s_route_entries[LWIP_ARRAYSIZE(s_route_entries)]) { + return ESP_ERR_INVALID_ARG; + } + route_entry->netif = NULL; + for (esp_route_entry_t *moved = route_entry; moved < &s_route_entries[LWIP_ARRAYSIZE(s_route_entries) - 1]; + moved++) { + *moved = *(moved + 1); + if (moved->netif == NULL) { + break; + } + } + return ESP_OK; +} + +static inline bool is_better_route(const esp_route_entry_t *lhs, const esp_route_entry_t *rhs) +{ + if (rhs == NULL) { + return true; + } + if (lhs == NULL) { + return false; + } + return (lhs->prefix_length > rhs->prefix_length) || + (lhs->prefix_length == rhs->prefix_length && lhs->preference > rhs->preference); +} + +static inline bool route_match(const esp_route_entry_t *route, const ip6_addr_t *dest) +{ + return memcmp(dest, route->prefix.addr, route->prefix_length / 8) == 0; +} + +struct netif *lwip_hook_ip6_route(const ip6_addr_t *src, const ip6_addr_t *dest) +{ + esp_route_entry_t *route = NULL; + + for (size_t i = 0; i < LWIP_ARRAYSIZE(s_route_entries); i++) { + if (s_route_entries[i].netif == NULL) { + break; + } + if (route_match(&s_route_entries[i], dest) && is_better_route(&s_route_entries[i], route)) { + route = &s_route_entries[i]; + } + } + + if (route) { + return route->netif; + } else { + return NULL; + } +} + +const ip6_addr_t *lwip_hook_nd6_get_gw(struct netif *netif, const ip6_addr_t *dest) +{ + esp_route_entry_t *route = NULL; + + for (size_t i = 0; i < LWIP_ARRAYSIZE(s_route_entries); i++) { + if (s_route_entries[i].netif == NULL) { + break; + } + if (s_route_entries[i].netif == netif && route_match(&s_route_entries[i], dest) && + is_better_route(&s_route_entries[i], route)) { + route = &s_route_entries[i]; + } + } + + if (route) { + return &route->gateway; + } else { + return NULL; + } +} diff --git a/examples/light/main/CMakeLists.txt b/examples/light/main/CMakeLists.txt index 895b050d1..030ea2097 100644 --- a/examples/light/main/CMakeLists.txt +++ b/examples/light/main/CMakeLists.txt @@ -26,7 +26,7 @@ set(PRIV_INCLUDE_DIRS_LIST "${CMAKE_CURRENT_LIST_DIR}" "${MATTER_SDK_PATH}/zzz_generated/app-common") set(PRIV_REQUIRES_LIST - chip bt esp32_mbedtls esp_matter esp_matter_console app_driver app_qrcode app_openthread) + chip bt esp32_mbedtls esp_matter esp_matter_console app_driver app_qrcode app_openthread route_hook) if ("${IDF_TARGET}" STREQUAL "esp32h2") list(APPEND PRIV_REQUIRES_LIST openthread) diff --git a/examples/light/main/app_matter.cpp b/examples/light/main/app_matter.cpp index 40cce95b0..9d9757d21 100644 --- a/examples/light/main/app_matter.cpp +++ b/examples/light/main/app_matter.cpp @@ -13,6 +13,7 @@ #include "esp_heap_caps.h" #include "esp_log.h" +#include "esp_route_hook.h" #include "app-common/zap-generated/att-storage.h" #include "app-common/zap-generated/attribute-id.h" @@ -244,7 +245,10 @@ esp_err_t app_matter_attribute_set(const char *endpoint, const char *attribute, static void on_device_event(const ChipDeviceEvent *event, intptr_t arg) { if (event->Type == PublicEventTypes::kInterfaceIpAddressChanged) { +#if !CHIP_DEVICE_CONFIG_ENABLE_THREAD chip::app::DnssdServer::Instance().StartServer(); + esp_route_hook_init(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF")); +#endif } ESP_LOGI(TAG, "Current free heap: %zu", heap_caps_get_free_size(MALLOC_CAP_8BIT)); } diff --git a/examples/light/sdkconfig.defaults b/examples/light/sdkconfig.defaults index 522a6fa78..8e4871aca 100644 --- a/examples/light/sdkconfig.defaults +++ b/examples/light/sdkconfig.defaults @@ -19,3 +19,7 @@ CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" # Enable chip shell CONFIG_ENABLE_CHIP_SHELL=y + +#enable lwIP route hooks +CONFIG_LWIP_HOOK_IP6_ROUTE_DEFAULT=y +CONFIG_LWIP_HOOK_ND6_GET_GW_DEFAULT=y diff --git a/examples/rainmaker_light/main/CMakeLists.txt b/examples/rainmaker_light/main/CMakeLists.txt index cdefb6511..8bcfb630f 100644 --- a/examples/rainmaker_light/main/CMakeLists.txt +++ b/examples/rainmaker_light/main/CMakeLists.txt @@ -26,7 +26,7 @@ set(PRIV_INCLUDE_DIRS_LIST "${CMAKE_CURRENT_LIST_DIR}" "${MATTER_SDK_PATH}/src" "${MATTER_SDK_PATH}/zzz_generated/app-common") -set(PRIV_REQUIRES_LIST chip bt esp32_mbedtls esp_matter esp_matter_console app_driver app_qrcode esp_rainmaker) +set(PRIV_REQUIRES_LIST chip bt esp32_mbedtls esp_matter esp_matter_console app_driver app_qrcode esp_rainmaker route_hook) idf_component_register(SRC_DIRS ${SRC_DIRS_LIST} PRIV_INCLUDE_DIRS ${PRIV_INCLUDE_DIRS_LIST} diff --git a/examples/rainmaker_light/main/app_matter.cpp b/examples/rainmaker_light/main/app_matter.cpp index 0098dc8c3..7fc20e534 100644 --- a/examples/rainmaker_light/main/app_matter.cpp +++ b/examples/rainmaker_light/main/app_matter.cpp @@ -13,6 +13,7 @@ #include "esp_heap_caps.h" #include "esp_log.h" +#include "esp_route_hook.h" #include #include "app-common/zap-generated/att-storage.h" @@ -236,6 +237,7 @@ static void on_device_event(const ChipDeviceEvent *event, intptr_t arg) { if (event->Type == PublicEventTypes::kInterfaceIpAddressChanged) { chip::app::DnssdServer::Instance().StartServer(); + esp_route_hook_init(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF")); } ESP_LOGI(TAG, "Current free heap: %zu", heap_caps_get_free_size(MALLOC_CAP_8BIT)); } diff --git a/examples/rainmaker_light/sdkconfig.defaults b/examples/rainmaker_light/sdkconfig.defaults index 59362f807..42631b422 100644 --- a/examples/rainmaker_light/sdkconfig.defaults +++ b/examples/rainmaker_light/sdkconfig.defaults @@ -29,3 +29,7 @@ CONFIG_MBEDTLS_DYNAMIC_FREE_CONFIG_DATA=y # Temporary Fix for Timer Overflows CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=3120 + +#enable lwIP route hooks +CONFIG_LWIP_HOOK_IP6_ROUTE_DEFAULT=y +CONFIG_LWIP_HOOK_ND6_GET_GW_DEFAULT=y