From 55462173f6c396b4db4c03cf803cb42553aa7b3a Mon Sep 17 00:00:00 2001 From: pankore <86098180+pankore@users.noreply.github.com> Date: Fri, 16 Sep 2022 22:12:38 +0800 Subject: [PATCH] [Ameba] Implement route hook (#22664) * [RouteHook] Implement route hooks to allow communication with thread devices * [Connectivity] Remove disconnection code from ConnectivityManager * [RouteHook] Fix compile error on 8720CM platform * [Restyle] Fix restyling --- .../all-clusters-app/ameba/chip_main.cmake | 4 + .../ameba/main/DeviceCallbacks.cpp | 6 +- .../ameba/route_hook/ameba_route_hook.c | 206 ++++++++++++++++++ .../ameba/route_hook/ameba_route_hook.h | 10 + .../ameba/route_hook/ameba_route_table.c | 175 +++++++++++++++ .../ameba/route_hook/ameba_route_table.h | 76 +++++++ .../Ameba/ConnectivityManagerImpl.cpp | 17 -- 7 files changed, 476 insertions(+), 18 deletions(-) create mode 100644 examples/platform/ameba/route_hook/ameba_route_hook.c create mode 100644 examples/platform/ameba/route_hook/ameba_route_hook.h create mode 100644 examples/platform/ameba/route_hook/ameba_route_table.c create mode 100644 examples/platform/ameba/route_hook/ameba_route_table.h diff --git a/examples/all-clusters-app/ameba/chip_main.cmake b/examples/all-clusters-app/ameba/chip_main.cmake index 96e025799113af..fc18f2fa9704cd 100755 --- a/examples/all-clusters-app/ameba/chip_main.cmake +++ b/examples/all-clusters-app/ameba/chip_main.cmake @@ -163,6 +163,9 @@ list( ${chip_dir}/examples/all-clusters-app/ameba/main/LEDWidget.cpp ${chip_dir}/examples/all-clusters-app/ameba/main/DsoHack.cpp + ${chip_dir}/examples/platform/ameba/route_hook/ameba_route_hook.c + ${chip_dir}/examples/platform/ameba/route_hook/ameba_route_table.c + ${chip_dir}/examples/providers/DeviceInfoProviderImpl.cpp ) @@ -204,6 +207,7 @@ target_include_directories( ${chip_dir}/examples/all-clusters-app/all-clusters-common/include ${chip_dir}/examples/all-clusters-app/ameba/main/include ${chip_dir}/examples/platform/ameba + ${chip_dir}/examples/platform/ameba/route_hook ${chip_dir}/examples/providers ${chip_dir_output}/gen/include ${chip_dir}/src/include/ diff --git a/examples/all-clusters-app/ameba/main/DeviceCallbacks.cpp b/examples/all-clusters-app/ameba/main/DeviceCallbacks.cpp index 259ff3d726f529..028dc7ca2a6370 100644 --- a/examples/all-clusters-app/ameba/main/DeviceCallbacks.cpp +++ b/examples/all-clusters-app/ameba/main/DeviceCallbacks.cpp @@ -22,8 +22,8 @@ * Implements all the callbacks to the application from the CHIP Stack * **/ -#include "DeviceCallbacks.h" +#include "DeviceCallbacks.h" #include "CHIPDeviceManager.h" #include #include @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -121,6 +122,9 @@ void DeviceCallbacks::OnInternetConnectivityChange(const ChipDeviceEvent * event { ChipLogProgress(DeviceLayer, "IPv6 Server ready..."); chip::app::DnssdServer::Instance().StartServer(); + + ChipLogProgress(DeviceLayer, "Initializing route hook..."); + ameba_route_hook_init(); #if CHIP_DEVICE_CONFIG_ENABLE_OTA_REQUESTOR // Init OTA requestor only when we have gotten IPv6 address if (!isOTAInitialized) diff --git a/examples/platform/ameba/route_hook/ameba_route_hook.c b/examples/platform/ameba/route_hook/ameba_route_hook.c new file mode 100644 index 00000000000000..0be1f71f359934 --- /dev/null +++ b/examples/platform/ameba/route_hook/ameba_route_hook.c @@ -0,0 +1,206 @@ +#include +#include + +#include "ameba_route_hook.h" +#include "ameba_route_table.h" +#include + +#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" + +typedef struct ameba_route_hook_t +{ + struct netif * netif; + struct raw_pcb * pcb; + struct ameba_route_hook_t * next; +} ameba_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 ameba_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); + + printf("Received RIO\n"); + if (rio_data_len >= prefix_len_bytes) + { + ip6_addr_t prefix; + ameba_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); + printf("prefix %s lifetime %u\n", ip6addr_ntoa(&prefix), route.lifetime_seconds); + if (ameba_route_table_add_route_entry(&route) == NULL) + { + printf("Failed to add route table entry\n"); + } + else + { + printf("Added entry to route table\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; + ameba_route_hook_t * hook = (ameba_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) + { + printf("Ignore segmented ICMP packet\n"); + return 0; + } + if (p->tot_len <= sizeof(struct ip6_hdr) + sizeof(struct icmp6_hdr)) + { + printf("Ignore invalid ICMP packet\n"); + 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; +} + +int8_t ameba_route_hook_init() +{ + struct netif * lwip_netif = &xnetif[0]; + ip_addr_t router_group = IPADDR6_INIT_HOST(0xFF020000, 0, 0, 0x02); + ameba_route_hook_t * hook = NULL; + uint8_t ret = 0; + + if (lwip_netif == NULL) + { + printf("Invalid network interface\n"); + return -1; + } + + for (ameba_route_hook_t * iter = s_hooks; iter != NULL; iter++) + { + if (iter->netif == lwip_netif) + { + printf("Hook already installed on netif, skip...\n"); + return 0; + } + } + + hook = (ameba_route_hook_t *) malloc(sizeof(ameba_route_hook_t)); + if (hook == NULL) + { + printf("Cannot allocate hook\n"); + return -1; + } + + if (mld6_joingroup_netif(lwip_netif, ip_2_ip6(&router_group)) != ERR_OK) + { + printf("Failed to join multicast group\n"); + ret = -1; + goto exit; + } + + 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 != 0 && hook != NULL) + { + free(hook); + } + return ret; +} diff --git a/examples/platform/ameba/route_hook/ameba_route_hook.h b/examples/platform/ameba/route_hook/ameba_route_hook.h new file mode 100644 index 00000000000000..802c2de5a58a46 --- /dev/null +++ b/examples/platform/ameba/route_hook/ameba_route_hook.h @@ -0,0 +1,10 @@ +#include +#ifdef __cplusplus +extern "C" { +#endif + +int8_t ameba_route_hook_init(void); + +#ifdef __cplusplus +} +#endif diff --git a/examples/platform/ameba/route_hook/ameba_route_table.c b/examples/platform/ameba/route_hook/ameba_route_table.c new file mode 100644 index 00000000000000..8446c42955ab09 --- /dev/null +++ b/examples/platform/ameba/route_hook/ameba_route_table.c @@ -0,0 +1,175 @@ +#include "ameba_route_table.h" +#include + +#include + +#include "lwip/ip6_addr.h" +#include "lwip/netif.h" +#include "lwip/timeouts.h" + +#define MAX_RIO_ROUTE 20 +#define MAX_RIO_TIMEOUT UINT32_MAX / (1000 * 4) // lwIP defined reasonable timeout value + +static ameba_route_entry_t s_route_entries[MAX_RIO_ROUTE]; + +static ameba_route_entry_t * find_route_entry(const ameba_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 ameba_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) +{ + ameba_route_entry_t * route = (ameba_route_entry_t *) arg; + + ameba_route_table_remove_route_entry(route); +} + +ameba_route_entry_t * ameba_route_table_add_route_entry(const ameba_route_entry_t * route_entry) +{ + if (route_entry == NULL || (route_entry->lifetime_seconds > MAX_RIO_TIMEOUT && route_entry->lifetime_seconds != UINT32_MAX)) + { + return NULL; + } + + ameba_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; + if (entry->lifetime_seconds != UINT32_MAX) + { + sys_timeout(entry->lifetime_seconds * 1000, route_timeout_handler, entry); + } + return entry; +} + +int8_t ameba_route_table_remove_route_entry(ameba_route_entry_t * route_entry) +{ + if (route_entry < &s_route_entries[0] || route_entry > &s_route_entries[LWIP_ARRAYSIZE(s_route_entries)]) + { + return -1; + } + route_entry->netif = NULL; + for (ameba_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 0; +} + +static inline bool is_better_route(const ameba_route_entry_t * lhs, const ameba_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 ameba_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) +{ + ameba_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) +{ + ameba_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/platform/ameba/route_hook/ameba_route_table.h b/examples/platform/ameba/route_hook/ameba_route_table.h new file mode 100644 index 00000000000000..2d1a875e078314 --- /dev/null +++ b/examples/platform/ameba/route_hook/ameba_route_table.h @@ -0,0 +1,76 @@ +#include + +#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; +} ameba_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 + * + */ +ameba_route_entry_t * ameba_route_table_add_route_entry(const ameba_route_entry_t * route_entry); + +/** + * @brief Removes an entry from the route table + * + * @param[in] route_entry The route entry to be removed + * + * @return + * - 0 + * - -1 The provided route_entry is not in the route table. + * + */ +int8_t ameba_route_table_remove_route_entry(ameba_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/src/platform/Ameba/ConnectivityManagerImpl.cpp b/src/platform/Ameba/ConnectivityManagerImpl.cpp index 8e8af6049e3c38..040471e7745f2a 100644 --- a/src/platform/Ameba/ConnectivityManagerImpl.cpp +++ b/src/platform/Ameba/ConnectivityManagerImpl.cpp @@ -502,23 +502,6 @@ void ConnectivityManagerImpl::DriveStationState() mLastStationConnectFailTime = System::Clock::kZero; OnStationConnected(); } - - // If the WiFi station interface is no longer enabled, or no longer provisioned, - // disconnect the station from the AP, unless the WiFi station mode is currently - // under application control. - if (mWiFiStationMode != kWiFiStationMode_ApplicationControlled && - (mWiFiStationMode != kWiFiStationMode_Enabled || !IsWiFiStationProvisioned())) - { - ChipLogProgress(DeviceLayer, "Disconnecting WiFi station interface"); - err = Internal::AmebaUtils::WiFiDisconnect(); - if (err != CHIP_NO_ERROR) - { - ChipLogError(DeviceLayer, "WiFiDisconnect() failed: %s", err); - return; - } - - ChangeWiFiStationState(kWiFiStationState_Disconnecting); - } } // Otherwise the station interface is NOT connected to an AP, so...