utun2/src/pkt_normalizer.c

// pkt_normalizer.c - Implementation of packet normalizer for ETCP
#include "pkt_normalizer.h"
#include "etcp.h"  // For ETCP_CONN and related structures
#include "etcp_api.h"  // For etcp_recv callback
#include "routing.h"  // For routing_add_conn/routing_del_conn
#include "utun_instance.h"  // For UTUN_INSTANCE
#include "ll_queue.h"  // For queue operations
#include "u_async.h"   // For UASYNC
#include <stdlib.h>
#include <string.h>
#include <stdio.h>     // For debugging (can be removed if not needed)
#include "../lib/debug_config.h"  // For DEBUG macros

// Forward declarations
static void packer_cb(struct ll_queue* q, void* arg);
static void pn_flush_cb(void* arg);
static void etcp_input_ready_cb(struct ll_queue* q, void* arg);
static void pn_unpacker_cb(struct ll_queue* q, void* arg);
static void pn_send_to_etcp(struct PKTNORM* pn);

// Initialization
struct PKTNORM* pn_init(struct ETCP_CONN* etcp) {
    if (!etcp) return NULL;

    struct PKTNORM* pn = calloc(1, sizeof(struct PKTNORM));
    if (!pn) {
        DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_init: calloc failed");
        return NULL;
    }

    pn->etcp = etcp;
    pn->ua = etcp->instance->ua;
    pn->frag_size = etcp->mtu - 100;  // Use MTU as fixed packet size (adjust if headers need subtraction)
    pn->tx_wait_time = 10;

    pn->input = queue_new(pn->ua, 0);  // No hash needed
    if (!pn->input) {
        DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_init: queue_new(input) failed");
        pn_deinit(pn);
        return NULL;
    }

    pn->output = queue_new(pn->ua, 0); // No hash needed
    if (!pn->output) {
        DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_init: queue_new(output) failed");
        pn_deinit(pn);
        return NULL;
    }

    queue_set_callback(pn->input, packer_cb, pn);
    
    // Setup etcp_recv callback for output queue - handles all received packets
    // according to etcp_api protocol (routes by first byte ID)
    queue_set_callback(pn->output, etcp_recv, etcp);

    pn->data = NULL;
    pn->recvpart = NULL;
    pn->flush_timer = NULL;

    return pn;
}

// Deinitialization
void pn_deinit(struct PKTNORM* pn) {
    if (!pn) return;

    // Unregister from routing module
    if (pn->etcp) {
        routing_del_conn(pn->etcp);
    }

    // Drain and free queues
    if (pn->input) {
        struct ll_entry* entry;
        while ((entry = queue_data_get(pn->input)) != NULL) {
            if (entry->dgram) {
                free(entry->dgram);
            }
            queue_entry_free(entry);
        }
        queue_free(pn->input);
    }
    if (pn->output) {
        struct ll_entry* entry;
        while ((entry = queue_data_get(pn->output)) != NULL) {
            if (entry->dgram) {
                free(entry->dgram);
            }
            queue_entry_free(entry);
        }
        queue_free(pn->output);
    }

    if (pn->flush_timer) {
        uasync_cancel_timeout(pn->ua, pn->flush_timer);
    }

    if (pn->data) {
        memory_pool_free(pn->etcp->instance->data_pool, pn->data);
    }
    if (pn->recvpart) {
        queue_dgram_free(pn->recvpart);
        queue_entry_free(pn->recvpart);
    }

    free(pn);
}

// Reset unpacker state
void pn_unpacker_reset_state(struct PKTNORM* pn) {
    if (!pn) return;
    if (pn->recvpart) {
        queue_dgram_free(pn->recvpart);
        queue_entry_free(pn->recvpart);
        pn->recvpart = NULL;
    }
}

// Send data to packer (copies and adds to input queue or pending, triggering callback)
void pn_packer_send(struct PKTNORM* pn, uint8_t* data, uint16_t len) {
    if (!pn || !data || len == 0) return;

    struct ll_entry* entry = ll_alloc_lldgram(len);
    if (!entry) {
        DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_packer_send: ll_alloc_lldgram failed");
        return;
    }
    memcpy(entry->dgram, data, len);
    entry->len = len;
    entry->dgram_pool = NULL;

    // Cancel flush timer if active
    if (pn->flush_timer) {
        uasync_cancel_timeout(pn->ua, pn->flush_timer);
        pn->flush_timer = NULL;
    }

    int ret = queue_data_put(pn->input, entry, 0);
    // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer_send: queue_data_put returned %d, input count=%d", ret, queue_entry_count(pn->input));
}

// Internal: Packer callback
static void packer_cb(struct ll_queue* q, void* arg) {
    struct PKTNORM* pn = (struct PKTNORM*)arg;
    if (!pn) return;
    // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: packer_cb");

    queue_wait_threshold(pn->etcp->input_queue, 0, 0, etcp_input_ready_cb, pn);
}

// Helper to send block to ETCP as ETCP_FRAGMENT
static void pn_send_to_etcp(struct PKTNORM* pn) {
    if (!pn || !pn->data || pn->data_ptr == 0) return;

    // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: pn_send_to_etcp");
    // Allocate ETCP_FRAGMENT from io_pool
    struct ETCP_FRAGMENT* frag = (struct ETCP_FRAGMENT*)queue_entry_new_from_pool(pn->etcp->io_pool);
    if (!frag) {// drop data
        DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_packer: send to etcp alloc error");
        pn->alloc_errors++;
        pn->data_ptr = 0;
        return;
    }
    
    frag->seq = 0;
    frag->timestamp = 0;
    frag->ll.dgram = pn->data;
    frag->ll.len = pn->data_ptr;
    frag->ll.memlen = pn->etcp->instance->data_pool->object_size;
    
    queue_data_put(pn->etcp->input_queue, (struct ll_entry*)frag, 0);
    // Сбросить структуру (dgram передан во фрагмент, не освобождаем)
    pn->data = NULL;
}

// Internal: Renew sndpart buffer
static void pn_buf_renew(struct PKTNORM* pn) {
    if (pn->data) {
        int remain = pn->data_size - pn->data_ptr;
        if (remain < 3) pn_send_to_etcp(pn);
    }
    if (!pn->data) {
        pn->data = memory_pool_alloc(pn->etcp->instance->data_pool);
        if (!pn->data) {
            DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_buf_renew: memory_pool_alloc failed");
            return;
        }
        int size=pn->etcp->instance->data_pool->object_size;
        if (size>pn->frag_size) size=pn->frag_size;
        pn->data_size = size;
        pn->data_ptr=0;
        // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: new bufer size=%d bytes",size);
    }
}

// Internal: Process input when etcp->input_queue is ready (empty)
static void etcp_input_ready_cb(struct ll_queue* q, void* arg) {
    struct PKTNORM* pn = (struct PKTNORM*)arg;
    if (!pn) return;

//    DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: etcp_input_ready_cb");

    struct ll_entry* in_dgram = queue_data_get(pn->input);
    if (!in_dgram) { queue_resume_callback(pn->input); return; }

    pn_buf_renew(pn);
    // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: new pkt hdrpos=%d",pn->data_ptr);
    if (!pn->data) goto exit;  // Allocation failed
    pn->data[pn->data_ptr++] = in_dgram->len & 0xFF;
    pn->data[pn->data_ptr++] = (in_dgram->len >> 8) & 0xFF;

    uint16_t in_ptr = 0;
    while (in_ptr < in_dgram->len) {
        int remain = pn->data_size - pn->data_ptr;
        int avail = in_dgram->len - in_ptr;
        if (avail < remain) remain = avail;
        // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_packer: copy %d bytes (in_ptr=%d, out_ptr=%d)",remain, in_ptr, pn->data_ptr);
        memcpy(pn->data + pn->data_ptr, in_dgram->dgram + in_ptr, remain);
        pn->data_ptr += remain;
        in_ptr += remain;
        pn_buf_renew(pn);
    }

exit:
    queue_dgram_free(in_dgram);
    queue_entry_free(in_dgram);

    // Cancel flush timer if active
    if (pn->flush_timer) {
        uasync_cancel_timeout(pn->ua, pn->flush_timer);
        pn->flush_timer = NULL;
    }

    // Set flush timer if no more input
    if (queue_entry_count(pn->input) == 0) {
        pn->flush_timer = uasync_set_timeout(pn->ua, pn->tx_wait_time, pn, pn_flush_cb);
    }

    queue_resume_callback(pn->input);
}

// Internal: Flush callback on timeout
static void pn_flush_cb(void* arg) {
    struct PKTNORM* pn = (struct PKTNORM*)arg;
    if (!pn) return;

    pn->flush_timer = NULL;
    pn_send_to_etcp(pn);
}


// Internal: Unpacker callback (assembles fragments into original packets)
static void pn_unpacker_cb(struct ll_queue* q, void* arg) {
    struct PKTNORM* pn = (struct PKTNORM*)arg;
    if (!pn) return;

    while (1) {
        void* data = queue_data_get(pn->etcp->output_queue);
        if (!data) break;

        struct ETCP_FRAGMENT* frag = (struct ETCP_FRAGMENT*)data;  // Since data is ll_entry*
        uint8_t* payload = frag->ll.dgram;
        uint16_t len = frag->ll.len;
        uint16_t ptr = 0;
        // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker: unpacking fragment len=%d", len);

        while (ptr < len) {
            if (!pn->recvpart) {
                // Need length header for new packet
                if (len - ptr < 2) {
                    // Incomplete header, reset
                    pn_unpacker_reset_state(pn);
                    break;
                }
                uint16_t part_size = payload[ptr] | (payload[ptr + 1] << 8);
                // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker: new fragment pkt_len=%d (at %d)", part_size, ptr);
                ptr += 2;

                pn->recvpart = ll_alloc_lldgram(part_size);
                if (!pn->recvpart) {
                    DEBUG_ERROR(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker_cb: ll_alloc_lldgram failed");
                    break;
                }
                pn->recvpart->len = 0;
            }

            uint16_t rem = pn->recvpart->memlen - pn->recvpart->len;// осталось собрать байт
            uint16_t avail = len - ptr;// доступно байт сейчас
            uint16_t cp = (rem < avail) ? rem : avail;
            // DEBUG_INFO(DEBUG_CATEGORY_NORMALIZER, "pn_unpacker: copy: remain=%d avail=%d in_ptr=%d out_ptr=%d", rem, avail, ptr, pn->recvpart->len);
            memcpy(pn->recvpart->dgram + pn->recvpart->len, payload + ptr, cp);
            pn->recvpart->len += cp;
            ptr += cp;

            if (pn->recvpart->len == pn->recvpart->memlen) {
                queue_data_put(pn->output, pn->recvpart, 0);
                pn->recvpart = NULL;
            }
        }

        // Free the fragment - dgram was malloc'd in pn_send_to_etcp
        memory_pool_free(pn->etcp->instance->data_pool, frag->ll.dgram);
        memory_pool_free(pn->etcp->io_pool, frag);
    }

    queue_resume_callback(q);
}