|
|
|
|
@ -51,155 +51,154 @@ static uint32_t extract_dst_ip(uint8_t* data, size_t len) {
|
|
|
|
|
return ntohl(dst_ip); // Convert to host 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"); |
|
|
|
|
// entry format: <id 1 byte> <ip_packet ...>
|
|
|
|
|
static void route_pkt(struct UTUN_INSTANCE* instance, struct ll_entry* entry) { |
|
|
|
|
if (!instance || !entry) { |
|
|
|
|
DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "route_pkt: 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"); |
|
|
|
|
|
|
|
|
|
if (!instance->rt) { |
|
|
|
|
DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "route_pkt: no route table"); |
|
|
|
|
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"); |
|
|
|
|
DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "route_pkt: no TUN interface"); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Check packet data
|
|
|
|
|
|
|
|
|
|
// Check packet data (skip cmd byte)
|
|
|
|
|
if (!entry->dgram || entry->len < 2) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: packet too small or no data"); |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: packet too small or no data"); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Skip ID byte, extract IP packet (offset 1)
|
|
|
|
|
|
|
|
|
|
// Skip cmd 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)",
|
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: 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
|
|
|
|
|
|
|
|
|
|
// Extract destination IP
|
|
|
|
|
uint32_t dst_ip = extract_dst_ip(ip_data, ip_len); |
|
|
|
|
if (dst_ip != 0) { |
|
|
|
|
struct in_addr addr; |
|
|
|
|
addr.s_addr = dst_ip; |
|
|
|
|
DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Packet from ETCP to %s", ip_to_str(&addr, AF_INET).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"); |
|
|
|
|
if (dst_ip == 0) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: failed to extract destination IP, dropping"); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Allocate packet data (TUN will free it)
|
|
|
|
|
uint8_t* packet_data = u_malloc(ip_len); |
|
|
|
|
if (!packet_data) { |
|
|
|
|
DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "Failed to allocate packet data"); |
|
|
|
|
memory_pool_free(instance->tun->pool, pkt); |
|
|
|
|
|
|
|
|
|
struct in_addr addr; |
|
|
|
|
addr.s_addr = htonl(dst_ip); |
|
|
|
|
DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "route_pkt: dst=%s len=%zu", ip_to_str(&addr, AF_INET).str, ip_len); |
|
|
|
|
|
|
|
|
|
// Lookup route
|
|
|
|
|
struct ROUTE_ARRAY* routes = route_table_lookup(instance->rt, dst_ip); |
|
|
|
|
if (!routes || routes->routes == 0) { |
|
|
|
|
DEBUG_INFO(DEBUG_CATEGORY_ROUTING, "route_pkt: no route to %s, dropping", ip_to_str(&addr, AF_INET).str); |
|
|
|
|
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"); |
|
|
|
|
u_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); |
|
|
|
|
if (!pkt) return; |
|
|
|
|
|
|
|
|
|
do { |
|
|
|
|
if (!pkt->ll.dgram || pkt->ll.len <= 0) break; |
|
|
|
|
|
|
|
|
|
dump_ip_packet("TUN->routing", (const uint8_t*)pkt->ll.dgram, pkt->ll.len); |
|
|
|
|
// Check route type
|
|
|
|
|
if (routes->entries[0]->type == ROUTE_TYPE_LOCAL) { |
|
|
|
|
// Local route - send to TUN (entry has [cmd=0][IP data], TUN skips cmd byte)
|
|
|
|
|
if (queue_data_put(instance->tun->input_queue, entry, 0) != 0) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: failed to put to TUN input_queue"); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
// Entry sent to TUN, don't free here
|
|
|
|
|
DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "route_pkt: sent %zu bytes to TUN", ip_len); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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"); break; } |
|
|
|
|
// Learned route - use next_hop
|
|
|
|
|
struct ETCP_CONN* conn = routes->entries[0]->next_hop; |
|
|
|
|
if (!conn) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: route to %s has no next_hop, dropping", ip_to_str(&addr, AF_INET).str); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct in_addr addr; |
|
|
|
|
addr.s_addr = htonl(dst_ip); |
|
|
|
|
if (!conn->normalizer) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: connection for %s has no normalizer, dropping", ip_to_str(&addr, AF_INET).str); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
struct ROUTE_ARRAY* routes = route_table_lookup(instance->rt, dst_ip); |
|
|
|
|
if (!routes || routes->routes == 0) { DEBUG_INFO(DEBUG_CATEGORY_ROUTING, "No route to %s, dropping packet", ip_to_str(&addr, AF_INET).str); break; } |
|
|
|
|
// Send to ETCP
|
|
|
|
|
DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "route_pkt: sending %zu bytes to ETCP %s", ip_len, conn->log_name); |
|
|
|
|
if (etcp_send(conn, entry) != 0) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "route_pkt: etcp_send failed for %s", ip_to_str(&addr, AF_INET).str); |
|
|
|
|
queue_entry_free(entry); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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_to_str(&addr, AF_INET).str); break; } |
|
|
|
|
// Entry sent successfully, don't free here
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!conn->normalizer) { DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "Connection for %s has no normalizer, dropping packet", ip_to_str(&addr, AF_INET).str); break; } |
|
|
|
|
// Callback for packets from ETCP (via etcp_bind id=0)
|
|
|
|
|
static void routing_pkt_from_etcp_cb(struct ETCP_CONN* conn, struct ll_entry* pkt) { |
|
|
|
|
if (!conn || !pkt) { |
|
|
|
|
DEBUG_ERROR(DEBUG_CATEGORY_ROUTING, "routing_pkt_from_etcp_cb: invalid arguments"); |
|
|
|
|
if (pkt) { |
|
|
|
|
queue_entry_free(pkt); |
|
|
|
|
queue_dgram_free(pkt); |
|
|
|
|
} |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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"); break; } |
|
|
|
|
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(pkt); |
|
|
|
|
queue_dgram_free(pkt); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
entry->dgram[0] = ETCP_ID_DATA; |
|
|
|
|
memcpy(entry->dgram + 1, pkt->ll.dgram, pkt->ll.len); |
|
|
|
|
entry->len = etcp_data_len; |
|
|
|
|
route_pkt(instance, pkt); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
DEBUG_DEBUG(DEBUG_CATEGORY_ROUTING, "Sending dgram to ETCP %s len=%d", conn->log_name, entry->len); |
|
|
|
|
if (etcp_send(conn, entry) != 0) { |
|
|
|
|
DEBUG_WARN(DEBUG_CATEGORY_ROUTING, "etcp_send failed for %s", ip_to_str(&addr, AF_INET).str); |
|
|
|
|
queue_dgram_free(entry); |
|
|
|
|
queue_entry_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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} while (0); |
|
|
|
|
struct ll_entry* pkt = queue_data_get(instance->tun->output_queue); |
|
|
|
|
if (!pkt) return; |
|
|
|
|
|
|
|
|
|
if (pkt->ll.dgram) { |
|
|
|
|
u_free(pkt->ll.dgram); |
|
|
|
|
pkt->ll.dgram = NULL; |
|
|
|
|
} |
|
|
|
|
memory_pool_free(instance->tun->pool, pkt); |
|
|
|
|
// Route packet (unified function handles everything)
|
|
|
|
|
route_pkt(instance, pkt); |
|
|
|
|
|
|
|
|
|
// Resume callback for next packet
|
|
|
|
|
queue_resume_callback(instance->tun->output_queue); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|