ESP32RouteHook.c

/*
 * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <platform/ESP32/route_hook/ESP32RouteHook.h>

#include <platform/ESP32/route_hook/ESP32RouteTable.h>
#include <stdint.h>
#include <string.h>

#include "esp_check.h"
#include "esp_err.h"
#include "esp_netif.h"

#include "lwip/icmp6.h"
#include "lwip/mld6.h"
#include "lwip/netif.h"
#include "lwip/opt.h"
#include "lwip/prot/icmp6.h"
#include "lwip/prot/ip6.h"
#include "lwip/prot/nd6.h"
#include "lwip/raw.h"
#include "lwip/tcpip.h"

#define HOPLIM_MAX 255
#define PIO_FLAG_ON_LINK (1 << 7)
#define PIO_FLAG_AUTO_CONFIG (1 << 6)

typedef struct esp_route_hook_t
{
    struct netif * netif;
    struct raw_pcb * pcb;
    struct esp_route_hook_t * next;
} esp_route_hook_t;

typedef struct route_option route_option_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 = (uint16_t) (payload_len - sizeof(struct ra_header));

    while (payload_len >= 2)
    {
        uint8_t opt_type = icmp_payload[0];
        uint8_t opt_len  = (uint8_t) (icmp_payload[1] << 3);
        if (opt_len == 0 || opt_len > payload_len)
        {
            ESP_LOGE(TAG, "Invalid ND6 option length");
            break;
        }

        if (opt_type == ND6_OPTION_TYPE_ROUTE_INFO && opt_len >= sizeof(route_option_t) - sizeof(ip6_addr_p_t) &&
            !is_self_address(netif, src_addr) && payload_len >= opt_len)
        {
            route_option_t route_option;
            memcpy(&route_option, icmp_payload, sizeof(route_option));

            // skip if prefix is longer than IPv6 address.
            if (route_option.prefix_length > 128)
            {
                break;
            }
            uint8_t prefix_len_bytes = (uint8_t) ((route_option.prefix_length + 7) / 8);
            int8_t preference    = (int8_t) (-2 * ((route_option.preference >> 4) & 1) + (((route_option.preference) >> 3) & 1));
            uint8_t rio_data_len = (uint8_t) (opt_len - sizeof(route_option) + sizeof(ip6_addr_p_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, route_option.prefix.addr, prefix_len_bytes);
                route.netif            = netif;
                route.gateway          = *src_addr;
                route.prefix_length    = route_option.prefix_length;
                route.prefix           = prefix;
                route.preference       = preference;
                route.lifetime_seconds = lwip_ntohl(route_option.route_lifetime);
                ESP_LOGI(TAG, "prefix %s lifetime %" PRIu32, ip6addr_ntoa(&prefix), route.lifetime_seconds);
                if (esp_route_table_add_route_entry(&route) == NULL)
                {
                    ESP_LOGI(TAG, "Failed to add route table entry");
                }
            }
        }
        icmp_payload += opt_len;
        payload_len = (uint16_t) (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_LOGI(TAG, "Ignore segmented ICMP packet");
        return 0;
    }
    if (p->tot_len <= sizeof(struct ip6_hdr) + sizeof(struct icmp6_hdr))
    {
        ESP_LOGI(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 = (uint16_t) (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");

    LOCK_TCPIP_CORE();

    int netif_idx = esp_netif_get_netif_impl_index(netif);
    if (netif_idx < 0 || netif_idx > UINT8_MAX)
    {
        UNLOCK_TCPIP_CORE();
        return ESP_ERR_INVALID_SIZE;
    }
    lwip_netif = netif_get_by_index((uint8_t) netif_idx);

    if (lwip_netif == NULL)
    {
        UNLOCK_TCPIP_CORE();
        ESP_LOGE(TAG, "Invalid network interface");
        return ESP_ERR_INVALID_ARG;
    }

    for (esp_route_hook_t * iter = s_hooks; iter != NULL; iter = iter->next)
    {
        if (iter->netif == lwip_netif)
        {
            UNLOCK_TCPIP_CORE();
            ESP_LOGI(TAG, "Hook already installed on netif, skip...");
            return ESP_OK;
        }
    }

    hook = (esp_route_hook_t *) malloc(sizeof(esp_route_hook_t));
    if (hook == NULL)
    {
        UNLOCK_TCPIP_CORE();
        ESP_LOGE(TAG, "Cannot allocate hook");
        return ESP_ERR_NO_MEM;
    }

    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:

    UNLOCK_TCPIP_CORE();

    if (ret != ESP_OK && hook != NULL)
    {
        free(hook);
    }
    return ret;
}