utun2/src/control_server.c

/*
 * control_server.c - Control Socket Server Implementation
 * 
 * Handles ETCP monitoring requests from clients
 */

#include "control_server.h"
#include "utun_instance.h"
#include "etcp.h"
#include "etcp_connections.h"
#include "tun_if.h"
#include "../lib/u_async.h"
#include "../lib/debug_config.h"

#include <stdlib.h>
#include <string.h>

#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#endif

#ifndef DEBUG_CATEGORY_CONTROL
#define DEBUG_CATEGORY_CONTROL 1
#endif

/* Log file name */
#define LOG_FILENAME "control_server.log"

/* ============================================================================
 * Logging Helpers
 * ============================================================================ */

#ifdef _WIN32
#include <windows.h>

static uint64_t get_timestamp_ms(void) {
    FILETIME ft;
    GetSystemTimeAsFileTime(&ft);
    uint64_t time = ((uint64_t)ft.dwHighDateTime << 32) | ft.dwLowDateTime;
    return (time / 10000) - 11644473600000ULL;
}
#else
#include <time.h>

static uint64_t get_timestamp_ms(void) {
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    return (uint64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
}
#endif

static void log_hex_data(FILE* log, const char* prefix, const uint8_t* data, size_t len) {
    if (!log || !data || len == 0) return;
    
    fprintf(log, "%llu: [%s] ", (unsigned long long)get_timestamp_ms(), prefix);
    for (size_t i = 0; i < len && i < 256; i++) {
        fprintf(log, "%02X ", data[i]);
    }
    if (len > 256) {
        fprintf(log, "... (%llu bytes total)", (unsigned long long)len);
    }
    fprintf(log, "\n");
    fflush(log);
}

/* ============================================================================
 * Forward Declarations
 * ============================================================================ */

static void accept_callback(socket_t fd, void* arg);
static void client_read_callback(socket_t fd, void* arg);
static void client_write_callback(socket_t fd, void* arg);
static void client_except_callback(socket_t fd, void* arg);
static void handle_client_data(struct control_server* server, struct control_client* client);
static void close_client(struct control_server* server, struct control_client* client);

static void send_conn_list(struct control_server* server, struct control_client* client);
static void send_metrics(struct control_server* server, struct control_client* client);
static void send_error(struct control_client* client, uint8_t error_code, const char* msg);

static struct ETCP_CONN* find_connection_by_peer_id(struct UTUN_INSTANCE* instance, uint64_t peer_id);

/* ============================================================================
 * Server Initialization
 * ============================================================================ */

int control_server_init(struct control_server* server,
                        struct UASYNC* ua,
                        struct UTUN_INSTANCE* instance,
                        struct sockaddr_storage* bind_addr,
                        uint32_t max_clients) {
    if (!server || !ua || !instance || !bind_addr) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Invalid parameters for control_server_init");
        return -1;
    }

    memset(server, 0, sizeof(*server));
    server->instance = instance;
    server->ua = ua;
    server->max_clients = max_clients ? max_clients : 8;
    memcpy(&server->bind_addr, bind_addr, sizeof(*bind_addr));
    
    /* Open log file (truncate on start) */
    server->log_file = fopen(LOG_FILENAME, "w");
    if (server->log_file) {
        fprintf(server->log_file, "%llu: [LOG] Control server log started\n", 
               (unsigned long long)get_timestamp_ms());
        fflush(server->log_file);
    }
    
    /* Create listening socket */
    int family = bind_addr->ss_family;
    server->listen_fd = socket(family, SOCK_STREAM, IPPROTO_TCP);
    
#ifdef _WIN32
    if (server->listen_fd == INVALID_SOCKET) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to create listening socket: %d", WSAGetLastError());
        return -1;
    }
    
    /* Set non-blocking mode */
    u_long nonblock = 1;
    ioctlsocket(server->listen_fd, FIONBIO, &nonblock);
    
    /* Enable address reuse */
    int reuse = 1;
    if (setsockopt(server->listen_fd, SOL_SOCKET, SO_REUSEADDR, 
                   (const char*)&reuse, sizeof(reuse)) == SOCKET_ERROR) {
        DEBUG_WARN(DEBUG_CATEGORY_CONTROL, "Failed to set SO_REUSEADDR: %d", WSAGetLastError());
    }
#else
    if (server->listen_fd < 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to create listening socket: %s", strerror(errno));
        return -1;
    }
    
    int reuse = 1;
    if (setsockopt(server->listen_fd, SOL_SOCKET, SO_REUSEADDR, 
                   &reuse, sizeof(reuse)) < 0) {
        DEBUG_WARN(DEBUG_CATEGORY_CONTROL, "Failed to set SO_REUSEADDR: %s", strerror(errno));
    }
    
    /* Set non-blocking */
    int flags = fcntl(server->listen_fd, F_GETFL, 0);
    if (flags >= 0) {
        fcntl(server->listen_fd, F_SETFL, flags | O_NONBLOCK);
    }
#endif
    
    /* Bind to address */
    socklen_t addr_len = (family == AF_INET) ? sizeof(struct sockaddr_in) : sizeof(struct sockaddr_in6);
    
#ifdef _WIN32
    if (bind(server->listen_fd, (struct sockaddr*)bind_addr, addr_len) == SOCKET_ERROR) {
        int err = WSAGetLastError();
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to bind control socket: %d", err);
        closesocket(server->listen_fd);
        server->listen_fd = INVALID_SOCKET;
        return -1;
    }
    
    /* Listen */
    if (listen(server->listen_fd, 5) == SOCKET_ERROR) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to listen on control socket: %d", WSAGetLastError());
        closesocket(server->listen_fd);
        server->listen_fd = INVALID_SOCKET;
        return -1;
    }
#else
    if (bind(server->listen_fd, (struct sockaddr*)bind_addr, addr_len) < 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to bind control socket: %s", strerror(errno));
        close(server->listen_fd);
        server->listen_fd = -1;
        return -1;
    }
    
    if (listen(server->listen_fd, 5) < 0) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to listen on control socket: %s", strerror(errno));
        close(server->listen_fd);
        server->listen_fd = -1;
        return -1;
    }
#endif
    
    /* Register with uasync */
    server->listen_socket_id = uasync_add_socket_t(ua, server->listen_fd,
                                                      accept_callback,
                                                      NULL,  /* write callback */
                                                      NULL,  /* except callback */
                                                      server);
    
    if (!server->listen_socket_id) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to register control socket with uasync");
#ifdef _WIN32
        closesocket(server->listen_fd);
        server->listen_fd = INVALID_SOCKET;
#else
        close(server->listen_fd);
        server->listen_fd = -1;
#endif
        return -1;
    }
    
    if (family == AF_INET) {
        struct sockaddr_in* sin = (struct sockaddr_in*)bind_addr;
        DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Control server listening on %s:%d", 
                   ip_to_str(&sin->sin_addr, AF_INET).str, ntohs(sin->sin_port));
    } else {
        struct sockaddr_in6* sin6 = (struct sockaddr_in6*)bind_addr;
        DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Control server listening on [%s]:%d",
                   ip_to_str(&sin6->sin6_addr, AF_INET6).str, ntohs(sin6->sin6_port));
    }
    
    return 0;
}

/* ============================================================================
 * Server Shutdown
 * ============================================================================ */

void control_server_shutdown(struct control_server* server) {
    if (!server) return;
    
    /* Close all client connections */
    while (server->clients) {
        close_client(server, server->clients);
    }

#ifdef _WIN32
    if (server->listen_fd != INVALID_SOCKET) {
        uasync_remove_socket_t(server->ua, server->listen_fd);
        server->listen_socket_id = NULL;
        closesocket(server->listen_fd);
        server->listen_fd = INVALID_SOCKET;
    }
#else
    if (server->listen_fd >= 0) {
        uasync_remove_socket_t(server->ua, server->listen_fd);
        server->listen_socket_id = NULL;
        close(server->listen_fd);
        server->listen_fd = -1;
    }
#endif
    
    /* Close log file */
    if (server->log_file) {
        fprintf(server->log_file, "%llu: [LOG] Control server shutdown complete\n", 
               (unsigned long long)get_timestamp_ms());
        fclose(server->log_file);
        server->log_file = NULL;
    }
    
    DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Control server shutdown complete");
}

/* ============================================================================
 * Client Connection Handling
 * ============================================================================ */

static void accept_callback(socket_t fd, void* arg) {
    struct control_server* server = (struct control_server*)arg;
    
    struct sockaddr_storage client_addr;
    socklen_t addr_len = sizeof(client_addr);
    
#ifdef _WIN32
    socket_t client_fd = accept(fd, (struct sockaddr*)&client_addr, &addr_len);
    
    if (client_fd == INVALID_SOCKET) {
        int err = WSAGetLastError();
        if (err != WSAEWOULDBLOCK) {
            if (server->log_file) {
                fprintf(server->log_file, "%llu: [ERROR] Accept failed: %d\n", 
                       (unsigned long long)get_timestamp_ms(), err);
                fflush(server->log_file);
            }
            DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Accept failed: %d", err);
        }
        return;
    }
    
    /* Set non-blocking */
    u_long nonblock = 1;
    ioctlsocket(client_fd, FIONBIO, &nonblock);
    
    /* Small delay to let socket stabilize (Windows-specific workaround) */
//    Sleep(10);
#else
    socket_t client_fd = accept(fd, (struct sockaddr*)&client_addr, &addr_len);
    if (client_fd < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK) {
            if (server->log_file) {
                fprintf(server->log_file, "%llu: [ERROR] Accept failed: %s\n", 
                       (unsigned long long)get_timestamp_ms(), strerror(errno));
                fflush(server->log_file);
            }
            DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Accept failed: %s", strerror(errno));
        }
        return;
    }
    
    /* Set non-blocking */
    int flags = fcntl(client_fd, F_GETFL, 0);
    if (flags >= 0) {
        fcntl(client_fd, F_SETFL, flags | O_NONBLOCK);
    }
#endif
    DEBUG_WARN(DEBUG_CATEGORY_CONTROL, "Accept...");
    
    /* Check max clients */
    if (server->client_count >= server->max_clients) {
        if (server->log_file) {
            fprintf(server->log_file, "%llu: [ERROR] Max clients reached, rejecting connection\n", 
                   (unsigned long long)get_timestamp_ms());
            fflush(server->log_file);
        }
        DEBUG_WARN(DEBUG_CATEGORY_CONTROL, "Max clients reached, rejecting connection");
#ifdef _WIN32
        closesocket(client_fd);
#else
        close(client_fd);
#endif
        return;
    }
    
    /* Allocate client structure */
    struct control_client* client = (struct control_client*)calloc(1, sizeof(*client));
    if (!client) {
        if (server->log_file) {
            fprintf(server->log_file, "%llu: [ERROR] Failed to allocate client structure\n", 
                   (unsigned long long)get_timestamp_ms());
            fflush(server->log_file);
        }
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to allocate client structure");
#ifdef _WIN32
        closesocket(client_fd);
#else
        close(client_fd);
#endif
        return;
    }
    
    client->fd = client_fd;
    client->server = server;  /* Store back pointer to server */
    client->connected = 1;
    client->selected_peer_id = 0;
    
    /* Register with uasync */
    client->socket_id = uasync_add_socket_t(server->ua, client_fd,
                                            client_read_callback,
                                            NULL,  /* write callback - not needed for now */
                                            client_except_callback,
                                            client);
    
    if (!client->socket_id) {
        if (server->log_file) {
            fprintf(server->log_file, "%llu: [ERROR] Failed to register client socket with uasync\n", 
                   (unsigned long long)get_timestamp_ms());
            fflush(server->log_file);
        }
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to register client socket with uasync");
        free(client);
#ifdef _WIN32
        closesocket(client_fd);
#else
        close(client_fd);
#endif
        return;
    }
    
    /* Add to list */
    client->next = server->clients;
    server->clients = client;
    server->client_count++;

    const char* addr_str;
    if (client_addr.ss_family == AF_INET) {
        struct sockaddr_in* sin = (struct sockaddr_in*)&client_addr;
        addr_str = ip_to_str(&sin->sin_addr, AF_INET).str;
    } else {
        struct sockaddr_in6* sin6 = (struct sockaddr_in6*)&client_addr;
        addr_str = ip_to_str(&sin6->sin6_addr, AF_INET6).str;
    }
    
    if (server->log_file) {
        fprintf(server->log_file, "%llu: [LOG] Client connected from %s (total: %u)\n", 
               (unsigned long long)get_timestamp_ms(), addr_str, server->client_count);
        fflush(server->log_file);
    }
    
    DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Control client connected from %s (total: %u)",
               addr_str, server->client_count);
}

static void client_read_callback(socket_t fd, void* arg) {
    struct control_client* client = (struct control_client*)arg;
    struct control_server* server = client->server;
    
    /* Read available data */
    uint8_t* buf = client->recv_buffer + client->recv_len;
    size_t buf_space = ETCPMON_MAX_MSG_SIZE - client->recv_len;
    
#ifdef _WIN32
    int received = recv(fd, (char*)buf, (int)buf_space, 0);
    
    if (received == SOCKET_ERROR) {
        int err = WSAGetLastError();
        if (err != WSAEWOULDBLOCK) {
            if (err == 10054) {
                client->connected = 0;
                return;
            }
            if (server && server->log_file) {
                fprintf(server->log_file, "%llu: [ERROR] Client recv error: %d\n", 
                       (unsigned long long)get_timestamp_ms(), err);
                fflush(server->log_file);
            }
            DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Client recv error: %d", err);
            client->connected = 0;
        }
        return;
    }
    if (received == 0) {
        /* Connection closed */
        if (server && server->log_file) {
            fprintf(server->log_file, "%llu: [LOG] Client disconnected (recv returned 0)\n", 
                   (unsigned long long)get_timestamp_ms());
            fflush(server->log_file);
        }
        client->connected = 0;
        return;
    }
#else
    ssize_t received = recv(fd, buf, buf_space, 0);
    if (received < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK) {
            if (server && server->log_file) {
                fprintf(server->log_file, "%llu: [ERROR] Client recv error: %s\n", 
                       (unsigned long long)get_timestamp_ms(), strerror(errno));
                fflush(server->log_file);
            }
            DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Client recv error: %s", strerror(errno));
            client->connected = 0;
        }
        return;
    }
    if (received == 0) {
        /* Connection closed */
        if (server && server->log_file) {
            fprintf(server->log_file, "%llu: [LOG] Client disconnected (recv returned 0)\n", 
                   (unsigned long long)get_timestamp_ms());
            fflush(server->log_file);
        }
        client->connected = 0;
        return;
    }
#endif
    
    /* Log received data */
    if (server && server->log_file) {
        log_hex_data(server->log_file, "RX", buf, received);
    }
    
    client->recv_len += received;
    
    /* Process messages */
    if (server) {
        handle_client_data(server, client);
    }
}

static void client_write_callback(socket_t fd, void* arg) {
    /* Not used for now - writes are immediate */
    (void)fd;
    (void)arg;
}

static void client_except_callback(socket_t fd, void* arg) {
    struct control_client* client = (struct control_client*)arg;
    DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Client socket exception");
    
    /* Mark for cleanup */
    client->connected = 0;
    (void)fd;
}

static void close_client(struct control_server* server, struct control_client* client) {
    if (!server || !client) return;
    
    /* Log disconnection */
    if (server->log_file) {
        fprintf(server->log_file, "%llu: [LOG] Client connection closed\n", 
               (unsigned long long)get_timestamp_ms());
        fflush(server->log_file);
    }

    /* Close socket */
#ifdef _WIN32
    if (client->fd != INVALID_SOCKET) {
        uasync_remove_socket_t(server->ua, client->fd);
        closesocket(client->fd);
    }
#else
    if (client->fd >= 0) {
        uasync_remove_socket_t(server->ua, client->fd);
        close(client->fd);
    }
#endif
    
    /* Remove from list */
    struct control_client** curr = &server->clients;
    while (*curr) {
        if (*curr == client) {
            *curr = client->next;
            break;
        }
        curr = &(*curr)->next;
    }
    
    server->client_count--;
    free(client);
    
    DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Control client disconnected (total: %u)",
               server->client_count);
}

/* ============================================================================
 * Message Handling
 * ============================================================================ */

static void handle_client_data(struct control_server* server, struct control_client* client) {
    while (client->recv_len >= sizeof(struct etcpmon_msg_header)) {
        struct etcpmon_msg_header* hdr = (struct etcpmon_msg_header*)client->recv_buffer;
        
        /* Validate header */
        if (etcpmon_validate_header(hdr) != 0) {
            if (server->log_file) {
                fprintf(server->log_file, "%llu: [ERROR] Invalid message header from client\n",
                       (unsigned long long)get_timestamp_ms());
                fflush(server->log_file);
            }
            DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Invalid message header from client");
            close_client(server, client);
            return;
        }
        
        /* Check if full message received */
        if (client->recv_len < hdr->size) {
            break;  /* Wait for more data */
        }
        
        /* Process message */
        uint8_t* payload = client->recv_buffer + sizeof(struct etcpmon_msg_header);
        uint16_t payload_size = hdr->size - sizeof(struct etcpmon_msg_header);
        
        if (server->log_file) {
            fprintf(server->log_file, "%llu: [LOG] Received command type=0x%02X size=%d\n",
                   (unsigned long long)get_timestamp_ms(), hdr->type, hdr->size);
            fflush(server->log_file);
        }
        
        switch (hdr->type) {
            case ETCPMON_CMD_LIST_CONN:
                send_conn_list(server, client);
                break;

            case ETCPMON_CMD_SELECT_CONN:
                if (payload_size >= sizeof(struct etcpmon_cmd_select)) {
                    struct etcpmon_cmd_select* cmd = (struct etcpmon_cmd_select*)payload;
                    client->selected_peer_id = cmd->peer_node_id;
                    if (server->log_file) {
                        fprintf(server->log_file, "%llu: [LOG] Client selected connection: %016llX\n",
                               (unsigned long long)get_timestamp_ms(), (unsigned long long)cmd->peer_node_id);
                        fflush(server->log_file);
                    }
                    DEBUG_INFO(DEBUG_CATEGORY_CONTROL, "Client selected connection: %016llX",
                               (unsigned long long)cmd->peer_node_id);
                }
                break;

            case ETCPMON_CMD_GET_METRICS:
                if (client->selected_peer_id == 0) {
                    send_error(client, ETCPMON_ERR_NO_CONN_SELECTED,
                              "No connection selected");
                } else {
                    send_metrics(server, client);
                }
                break;

            case ETCPMON_CMD_DISCONNECT:
                if (server->log_file) {
                    fprintf(server->log_file, "%llu: [LOG] Client requested disconnect\n",
                           (unsigned long long)get_timestamp_ms());
                    fflush(server->log_file);
                }
                close_client(server, client);
                return;

            default:
                if (server->log_file) {
                    fprintf(server->log_file, "%llu: [ERROR] Unknown command from client: 0x%02X\n",
                           (unsigned long long)get_timestamp_ms(), hdr->type);
                    fflush(server->log_file);
                }
                DEBUG_WARN(DEBUG_CATEGORY_CONTROL, "Unknown command from client: 0x%02X", hdr->type);
                send_error(client, ETCPMON_ERR_INVALID_CMD, "Unknown command");
                break;
        }
        
        /* Remove processed message from buffer */
        uint16_t msg_size = hdr->size;
        if (client->recv_len > msg_size) {
            memmove(client->recv_buffer, client->recv_buffer + msg_size,
                    client->recv_len - msg_size);
        }
        client->recv_len -= msg_size;
    }
}

/* ============================================================================
 * Response Builders
 * ============================================================================ */

static void send_conn_list(struct control_server* server, struct control_client* client) {
    struct UTUN_INSTANCE* instance = server->instance;
    
    /* Count connections */
    uint8_t count = 0;
    struct ETCP_CONN* conn = instance->connections;
    while (conn && count < ETCPMON_MAX_CONNECTIONS) {
        count++;
        conn = conn->next;
    }
    
    /* Build response */
    uint16_t rsp_size = ETCPMON_CONN_LIST_SIZE(count);
    uint8_t* buffer = (uint8_t*)malloc(rsp_size);
    if (!buffer) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to allocate connection list buffer");
        return;
    }
    
    struct etcpmon_msg_header* hdr = (struct etcpmon_msg_header*)buffer;
    etcpmon_build_header(hdr, sizeof(struct etcpmon_rsp_conn_list) + 
                              count * sizeof(struct etcpmon_conn_info),
                         ETCPMON_RSP_CONN_LIST);
    
    struct etcpmon_rsp_conn_list* rsp = (struct etcpmon_rsp_conn_list*)(buffer + sizeof(*hdr));
    rsp->count = count;
    
    struct etcpmon_conn_info* info = (struct etcpmon_conn_info*)(buffer + sizeof(*hdr) + sizeof(*rsp));
    conn = instance->connections;
    for (uint8_t i = 0; i < count && conn; i++) {
        info[i].peer_node_id = conn->peer_node_id;
        strncpy(info[i].name, conn->log_name, ETCPMON_MAX_CONN_NAME - 1);
        info[i].name[ETCPMON_MAX_CONN_NAME - 1] = '\0';
        conn = conn->next;
    }
    
    /* Log and send response */
    if (server->log_file) {
        log_hex_data(server->log_file, "TX", buffer, rsp_size);
        fprintf(server->log_file, "%llu: [LOG] Sent RSP_CONN_LIST count=%d\n",
               (unsigned long long)get_timestamp_ms(), count);
        fflush(server->log_file);
    }
    
#ifdef _WIN32
    send(client->fd, (const char*)buffer, rsp_size, 0);
#else
    send(client->fd, buffer, rsp_size, 0);
#endif
    
    free(buffer);
}

static void send_metrics(struct control_server* server, struct control_client* client) {
    struct UTUN_INSTANCE* instance = server->instance;
    
    /* Find selected connection */
    struct ETCP_CONN* conn = find_connection_by_peer_id(instance, client->selected_peer_id);
    if (!conn) {
        send_error(client, ETCPMON_ERR_INVALID_CONN, "Connection not found");
        return;
    }
    
    /* Count links */
    uint8_t links_count = 0;
    struct ETCP_LINK* link = conn->links;
    while (link && links_count < ETCPMON_MAX_LINKS) {
        links_count++;
        link = link->next;
    }
    
    /* Build response */
    uint16_t rsp_size = ETCPMON_METRICS_SIZE(links_count);
    uint8_t* buffer = (uint8_t*)malloc(rsp_size);
    if (!buffer) {
        DEBUG_ERROR(DEBUG_CATEGORY_CONTROL, "Failed to allocate metrics buffer");
        return;
    }
    
    struct etcpmon_msg_header* hdr = (struct etcpmon_msg_header*)buffer;
    etcpmon_build_header(hdr, sizeof(struct etcpmon_rsp_metrics) + 
                              links_count * sizeof(struct etcpmon_link_metrics),
                         ETCPMON_RSP_METRICS);
    
    struct etcpmon_rsp_metrics* rsp = (struct etcpmon_rsp_metrics*)(buffer + sizeof(*hdr));
    
    /* Fill ETCP metrics */
    rsp->etcp.rtt_last = conn->rtt_last;
    rsp->etcp.rtt_avg_10 = conn->rtt_avg_10;
    rsp->etcp.rtt_avg_100 = conn->rtt_avg_100;
    rsp->etcp.jitter = conn->jitter;
    rsp->etcp.bytes_sent_total = conn->bytes_sent_total;
    rsp->etcp.retrans_count = conn->retransmissions_count;
    rsp->etcp.ack_count = conn->ack_packets_count;
    rsp->etcp.unacked_bytes = conn->unacked_bytes;
    rsp->etcp.optimal_inflight = conn->optimal_inflight;
    rsp->etcp.links_count = links_count;
    
    /* Fill TUN metrics */
    if (instance->tun) {
        rsp->tun.bytes_read = instance->tun->bytes_read;
        rsp->tun.bytes_written = instance->tun->bytes_written;
        rsp->tun.packets_read = instance->tun->packets_read;
        rsp->tun.packets_written = instance->tun->packets_written;
        rsp->tun.read_errors = instance->tun->read_errors;
        rsp->tun.write_errors = instance->tun->write_errors;
    } else {
        memset(&rsp->tun, 0, sizeof(rsp->tun));
    }
    
    /* Fill link metrics */
    struct etcpmon_link_metrics* link_info = (struct etcpmon_link_metrics*)(buffer + sizeof(*hdr) + sizeof(*rsp));
    link = conn->links;
    for (uint8_t i = 0; i < links_count && link; i++) {
        link_info[i].local_link_id = link->local_link_id;
        link_info[i].status = link->link_status;
        link_info[i].encrypt_errors = (uint32_t)link->encrypt_errors;
        link_info[i].decrypt_errors = (uint32_t)link->decrypt_errors;
        link_info[i].send_errors = (uint32_t)link->send_errors;
        link_info[i].recv_errors = (uint32_t)link->recv_errors;
        link_info[i].total_encrypted = link->total_encrypted;
        link_info[i].total_decrypted = link->total_decrypted;
        link_info[i].bandwidth = link->bandwidth;
        link_info[i].nat_changes_count = link->nat_changes_count;
        link_info[i].rtt_last = link->rtt_last;
        link_info[i].tt_last = link->tt_last;
        link = link->next;
    }
    
    /* Log and send response */
    if (server->log_file) {
        log_hex_data(server->log_file, "TX", buffer, rsp_size);
        fprintf(server->log_file, "%llu: [LOG] Sent RSP_METRICS rtt=%u bytes=%llu links=%d\n",
               (unsigned long long)get_timestamp_ms(),
               rsp->etcp.rtt_last,
               (unsigned long long)rsp->etcp.bytes_sent_total,
               links_count);
        fflush(server->log_file);
    }
    
#ifdef _WIN32
    send(client->fd, (const char*)buffer, rsp_size, 0);
#else
    send(client->fd, buffer, rsp_size, 0);
#endif
    
    free(buffer);
}

static void send_error(struct control_client* client, uint8_t error_code, const char* msg) {
    struct control_server* server = client->server;
    uint16_t msg_len = (uint16_t)strlen(msg);
    uint16_t rsp_size = ETCPMON_ERROR_SIZE(msg_len);

    uint8_t* buffer = (uint8_t*)malloc(rsp_size);
    if (!buffer) return;

    struct etcpmon_msg_header* hdr = (struct etcpmon_msg_header*)buffer;
    etcpmon_build_header(hdr, sizeof(struct etcpmon_rsp_error) + msg_len + 1,
                         ETCPMON_RSP_ERROR);

    struct etcpmon_rsp_error* rsp = (struct etcpmon_rsp_error*)(buffer + sizeof(*hdr));
    rsp->error_code = error_code;
    memcpy(buffer + sizeof(*hdr) + sizeof(*rsp), msg, msg_len + 1);

    /* Log error response */
    if (server && server->log_file) {
        log_hex_data(server->log_file, "TX", buffer, rsp_size);
        fprintf(server->log_file, "%llu: [LOG] Sent RSP_ERROR code=%d msg='%s'\n",
               (unsigned long long)get_timestamp_ms(), error_code, msg);
        fflush(server->log_file);
    }

#ifdef _WIN32
    send(client->fd, (const char*)buffer, rsp_size, 0);
#else
    send(client->fd, buffer, rsp_size, 0);
#endif

    free(buffer);
}

/* ============================================================================
 * Helper Functions
 * ============================================================================ */

static struct ETCP_CONN* find_connection_by_peer_id(struct UTUN_INSTANCE* instance, 
                                                     uint64_t peer_id) {
    struct ETCP_CONN* conn = instance->connections;
    while (conn) {
        if (conn->peer_node_id == peer_id) {
            return conn;
        }
        conn = conn->next;
    }
    return NULL;
}

/* ============================================================================
 * Public API Implementation
 * ============================================================================ */

void control_server_process_updates(struct control_server* server) {
    if (!server) return;
    
    /* Process any pending data for all clients */
    struct control_client* client = server->clients;
    while (client) {
        struct control_client* next = client->next;
        
        if (!client->connected) {
            close_client(server, client);
        } else if (client->recv_len > 0) {
            handle_client_data(server, client);
        }
        
        client = next;
    }
}

uint32_t control_server_get_client_count(const struct control_server* server) {
    return server ? server->client_count : 0;
}