utun2/src/routing.c

// routing.c - Centralized routing module for utun
#include "routing.h"
#include "route_lib.h"
#include "tun_if.h"
#include "etcp.h"
#include "etcp_api.h"
#include "pkt_normalizer.h"
#include "utun_instance.h"
#include "../lib/ll_queue.h"
#include "../lib/debug_config.h"
#include <string.h>
#include <stdlib.h>
#include "../lib/platform_compat.h"

#define IPv4_VERSION 4
#define IPv6_VERSION 6
#define IP_HDR_VERSION_OFFSET 0
#define IP_HDR_DST_ADDR_OFFSET 16
#define IP_HDR_MIN_SIZE 20

// ETCP packet ID for routing data
#define ETCP_ID_DATA 0x00

// Extract destination IP from IPv4 packet
// Returns 0 if not IPv4 or packet too small
static uint32_t extract_dst_ip(uint8_t* data, size_t len) {
    if (!data || len < IP_HDR_MIN_SIZE) {
        return 0;
    }
    
    // Check IP version (first nibble)
    uint8_t version = (data[IP_HDR_VERSION_OFFSET] >> 4) & 0x0F;
    
    if (version == IPv6_VERSION) {
        // IPv6 - drop for now
        DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "IPv6 packet detected, dropping");
        return 0;
    }
    
    if (version != IPv4_VERSION) {
        DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Unknown IP version: %d", version);
        return 0;
    }
    
    // Extract destination IP (offset 16, 4 bytes, network byte order)
    uint32_t dst_ip;
    memcpy(&dst_ip, data + IP_HDR_DST_ADDR_OFFSET, 4);
    
    return dst_ip;  // Keep in network byte order
}

// Callback for packets from ETCP (via etcp_bind id=0)
// Format: <id 1 byte> <ip_packet ...>
static void routing_pkt_from_etcp_cb(struct ETCP_CONN* conn, struct ll_entry* entry) {
    if (!conn || !entry) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: invalid arguments");
        if (entry) {
            queue_entry_free(entry);
            queue_dgram_free(entry);
        }
        return;
    }
    
    struct UTUN_INSTANCE* instance = conn->instance;
    if (!instance) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: connection has no instance");
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    if (!instance->tun) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: no TUN interface");
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    // Check packet data
    if (!entry->dgram || entry->len < 2) {
        DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: packet too small or no data");
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    // Skip ID byte, extract IP packet (offset 1)
    uint8_t* ip_data = entry->dgram + 1;
    size_t ip_len = entry->len - 1;
    
    // Check MTU
    if (ip_len > TUN_MAX_PACKET_SIZE) {
        DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "Packet too large: %zu bytes (max %d)", 
                  ip_len, TUN_MAX_PACKET_SIZE);
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    // Log destination IP
    uint32_t dst_ip = extract_dst_ip(ip_data, ip_len);
    if (dst_ip != 0) {
        char ip_str[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &dst_ip, ip_str, sizeof(ip_str));
        DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Packet from ETCP to %s", ip_str);
    }
    
    // Allocate ETCP_FRAGMENT for TUN input_queue
    struct ETCP_FRAGMENT* pkt = (struct ETCP_FRAGMENT*)queue_entry_new_from_pool(instance->tun->pool);
    if (!pkt) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to allocate ETCP_FRAGMENT for TUN");
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    // Allocate packet data (TUN will free it)
    uint8_t* packet_data = malloc(ip_len);
    if (!packet_data) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to allocate packet data");
        memory_pool_free(instance->tun->pool, pkt);
        queue_entry_free(entry);
        queue_dgram_free(entry);
        return;
    }
    
    memcpy(packet_data, ip_data, ip_len);
    
    pkt->seq = 0;
    pkt->timestamp = 0;
    pkt->ll.dgram = packet_data;
    pkt->ll.len = ip_len;
    
    // Put to TUN input_queue
    if (queue_data_put(instance->tun->input_queue, (struct ll_entry*)pkt, 0) != 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to put packet to TUN input_queue");
        free(packet_data);
        memory_pool_free(instance->tun->pool, pkt);
    } else {
        DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Forwarded %zu bytes from ETCP to TUN", ip_len);
    }
    
    // Free original entry from ETCP
    queue_entry_free(entry);
    queue_dgram_free(entry);
}

// Callback for packets from TUN output queue
static void routing_pkt_from_tun_cb(struct ll_queue* q, void* arg) {
    (void)q;
    struct UTUN_INSTANCE* instance = (struct UTUN_INSTANCE*)arg;
    if (!instance) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_tun_cb: invalid instance");
        return;
    }
    
    if (!instance->rt) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_tun_cb: no route table");
        return;
    }
    
    struct ETCP_FRAGMENT* pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
    while (pkt) {
        if (pkt->ll.dgram && pkt->ll.len > 0) {
            // Extract destination IP for routing
            uint32_t dst_ip = extract_dst_ip(pkt->ll.dgram, pkt->ll.len);
            if (dst_ip == 0) {
                DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "Failed to extract destination IP, dropping packet");
                free(pkt->ll.dgram);
                memory_pool_free(instance->tun->pool, pkt);
                pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
                continue;
            }
            
            char ip_str[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, &dst_ip, ip_str, sizeof(ip_str));
            
            // Lookup route in routing table
            struct ROUTE_ARRAY* routes = route_table_lookup(instance->rt, dst_ip);
            if (!routes || routes->routes == 0) {
                DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "No route to %s, dropping packet", ip_str);
                free(pkt->ll.dgram);
                memory_pool_free(instance->tun->pool, pkt);
                pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
                continue;
            }
            
            // Use first (best) route
            struct ETCP_CONN* conn = routes->entries[0]->next_hop;
            if (!conn) {
                DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "Route to %s has no next_hop, dropping packet", ip_str);
                free(pkt->ll.dgram);
                memory_pool_free(instance->tun->pool, pkt);
                pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
                continue;
            }
            
            // Check connection has normalizer
            if (!conn->normalizer) {
                DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "Connection for %s has no normalizer, dropping packet", ip_str);
                free(pkt->ll.dgram);
                memory_pool_free(instance->tun->pool, pkt);
                pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
                continue;
            }
            
            // Create ll_entry with ID prefix for ETCP
            // Format: <id 1 byte> <ip_packet ...>
            size_t etcp_data_len = 1 + pkt->ll.len;
            struct ll_entry* entry = ll_alloc_lldgram(etcp_data_len);
            if (!entry) {
                DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to allocate entry for ETCP send");
                free(pkt->ll.dgram);
                memory_pool_free(instance->tun->pool, pkt);
                pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
                continue;
            }
            
            // Set ID byte and copy IP packet
            entry->dgram[0] = ETCP_ID_DATA;
            memcpy(entry->dgram + 1, pkt->ll.dgram, pkt->ll.len);
            entry->len = etcp_data_len;
            
            // Send via etcp_send
            if (etcp_send(conn, entry) != 0) {
                DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "etcp_send failed for %s", ip_str);
                queue_entry_free(entry);
                queue_dgram_free(entry);
            } else {
                DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Forwarded packet to %s via ETCP conn [%s]", 
                           ip_str, conn->log_name);
            }
            
            // Free original TUN packet
            free(pkt->ll.dgram);
            memory_pool_free(instance->tun->pool, pkt);
        } else {
            // Empty packet - just free structure
            if (pkt->ll.dgram) {
                free(pkt->ll.dgram);
            }
            memory_pool_free(instance->tun->pool, pkt);
        }
        
        // Get next packet
        pkt = (struct ETCP_FRAGMENT*)queue_data_get(instance->tun->output_queue);
    }
}

// Initialize routing module for instance
int routing_create(struct UTUN_INSTANCE* instance) {
    if (!instance) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_create: instance is NULL");
        return -1;
    }
    
    // Create route table
    instance->rt = route_table_create();
    if (!instance->rt) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to create route table");
        return -1;
    }
    
    // Bind ID=0 for receiving data packets from ETCP
    if (etcp_bind(instance, ETCP_ID_DATA, routing_pkt_from_etcp_cb) != 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to bind ETCP ID=0");
        route_table_destroy(instance->rt);
        instance->rt = NULL;
        return -1;
    }
    
    DEBUG_INFO(DEBUG_CATEGORY_ROUTING, "Routing module initialized for instance");
    
    return 0;
}

// Destroy routing module for instance
void routing_destroy(struct UTUN_INSTANCE* instance) {
    if (!instance) return;
    
    DEBUG_INFO(DEBUG_CATEGORY_ROUTING, "Destroying routing module");
    
    // Unbind ETCP ID=0
    etcp_unbind(instance, ETCP_ID_DATA);
    
    // Clean up route table if exists
    if (instance->rt) {
        route_table_destroy(instance->rt);
        instance->rt = NULL;
    }
}

// Register ETCP connection with routing (deprecated, kept for compatibility)
void routing_add_conn(struct ETCP_CONN* etcp) {
    (void)etcp;
    // No-op: connections are now handled via routing table entries
    DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "routing_add_conn: deprecated, no action taken");
}

// Unregister ETCP connection from routing (deprecated, kept for compatibility)
void routing_del_conn(struct ETCP_CONN* etcp) {
    (void)etcp;
    // No-op: connections are now handled via routing table entries
    DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "routing_del_conn: deprecated, no action taken");
}

// Set TUN interface for routing
void routing_set_tun(struct UTUN_INSTANCE* instance) {
    if (!instance) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_set_tun: instance is NULL");
        return;
    }
    
    if (!instance->tun) {
        DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_set_tun: instance has no TUN");
        return;
    }
    
    // Set callback on TUN output queue (packets from TUN to routing)
    queue_set_callback(instance->tun->output_queue, routing_pkt_from_tun_cb, instance);
    
    DEBUG_INFO(DEBUG_CATEGORY_ROUTING, "TUN interface registered in routing module");
}