/////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Name:            CommandProcessor.cpp
// Created:         Jan 2025
// Version:         v1.1
// Author(s):       Philip Smart
// Description:     Command processor — receives binary IPC commands from the RP2350 over SPI
//                  and dispatches to the appropriate handler.
//                  v1.1: Binary SPI IPC replaces UART ASCII command/response round-trips.
//                        vTaskDelay(1) polling loop eliminated; SPI slave receive blocks
//                        (CMD_SPI_POLL_TICKS timeout) — no wasted tick delays on idle bus.
//                        UART path retained for OOB handling by IO_stdinReaderTask.
//                        Binary opcode dispatch (O(1) switch) replaces std::map lookup.
// Credits:
// Copyright:       (c) 2019-2026 Philip Smart <philip.smart@net2net.org>
//
// History:   v1.00 Jan 2025 - Initial write.
//            v1.10 Mar 2026 - Binary SPI IPC, opcode dispatch, semaphore-free blocking wait.
//
// Notes:           See Makefile to enable/disable conditional components
//
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// This source file is free software: you can redistribute it and#or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This source file is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
/////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/spi_slave.h"
#include "esp_system.h"
#include "esp_log.h"
#include "sdmmc_cmd.h"
#include "driver/sdspi_host.h"
#include "driver/spi_common.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "esp_vfs_fat.h"
#include <string>
#include <unordered_map>
#include <vector>
#include <functional>
#include "IO.h"
#include "CommandProcessor.h"
#include "ipc_protocol.h"
#include "esp_attr.h"  // RTC_NOINIT_ATTR
#include "esp_timer.h"
#include "esp_rom_crc.h"  // esp_rom_crc32_le for network payload CRC

// Declared in IO.cpp — set before esp_restart() in the OOB handler so we know
// this SW reset was clean (no mid-transaction state) and can use the fast path.
#define OOB_RESTART_MAGIC 0xAA55CC33u

// Declared in IO.cpp — set by OOB handler to request SPI slave reinitalization.
extern volatile bool g_spi_clear_requested;

// ---------------------------------------------------------------------------
// ESP32→RP2350 reverse command queue.
//
// Commands are delivered to the RP2350 via the NOP poll T3 response filename
// field.  Two dispatch modes are supported:
//
//   CP_queueCmd(cmd)              — async: push and return immediately.
//                                   Use when the result is not needed.
//
//   CP_sendCmd(cmd, timeoutMs)    — sync: push and block until the NOP poll
//                                   has delivered the command to the RP2350
//                                   (i.e. it appeared in a T3 response).
//                                   Returns true if delivered within timeout.
//
// Multiple commands can be queued simultaneously up to CP_CMD_QUEUE_DEPTH.
// Async commands beyond the queue depth are silently dropped (non-blocking
// xQueueSend with timeout 0).  Sync commands wait up to 100 ms to enqueue.
// ---------------------------------------------------------------------------
#define CP_CMD_QUEUE_DEPTH 8

typedef struct
{
    char cmd[IPCF_FILENAME_LEN];
    SemaphoreHandle_t doneSem;  // NULL = async; semaphore = sync (signalled on delivery)
} t_CpCmd;

static QueueHandle_t s_cmdQueue = NULL;
static t_CpCmd s_pendingCmd = {};   // Command currently being delivered to RP2350 via NOP

// ---------------------------------------------------------------------------
// Initial SPI-done flag.
//
// Set to true the first time storeRP2350Info() succeeds (i.e. the RP2350 has
// sent an INF command, which it only does after completing its own startup
// file-load sequence).  app_main() polls this via CP_initialSpiDone() to
// delay WiFi startup until after the RP2350's initial SPI exchanges are
// complete — prevents high-priority WiFi FreeRTOS tasks from preempting the
// CommandProcessor during the critical startup window and causing T3 HS
// timeouts on the INF exchange.
// ---------------------------------------------------------------------------
static volatile bool s_initialSpiDone = false;

bool CP_initialSpiDone(void)
{
    return s_initialSpiDone;
}

void CP_markInitialSpiDone(void)
{
    s_initialSpiDone = true;
}

void CP_queueCmd(const char *cmd)
{
    if (!s_cmdQueue)
    {
        return;
    }
    t_CpCmd entry = {};
    strncpy(entry.cmd, cmd, IPCF_FILENAME_LEN - 1);
    entry.doneSem = NULL;
    xQueueSend(s_cmdQueue, &entry, 0);  // drop silently if queue full
}

bool CP_sendCmd(const char *cmd, uint32_t timeoutMs)
{
    if (!s_cmdQueue)
    {
        return false;
    }
    SemaphoreHandle_t done = xSemaphoreCreateBinary();
    if (!done)
    {
        return false;
    }

    t_CpCmd entry = {};
    strncpy(entry.cmd, cmd, IPCF_FILENAME_LEN - 1);
    entry.doneSem = done;

    // Wait up to 100 ms to enqueue (queue full is unlikely but possible).
    if (xQueueSend(s_cmdQueue, &entry, pdMS_TO_TICKS(100)) != pdTRUE)
    {
        vSemaphoreDelete(done);
        return false;
    }

    // Block until cmdNop delivers the command to RP2350 (or timeout).
    bool delivered = (xSemaphoreTake(done, pdMS_TO_TICKS(timeoutMs)) == pdTRUE);
    vSemaphoreDelete(done);
    return delivered;
}
extern uint32_t g_oob_restart_magic;

#define CMDPROCTAG "CMDPROC"

// ---------------------------------------------------------------------------
// String splitter (unchanged — used for legacy / debug paths if needed)
// ---------------------------------------------------------------------------
std::vector<std::string> CommandProcessor::split(const std::string &s, const std::string &delimiter)
{
    size_t posStart = 0, posEnd, delimLen = delimiter.length();
    std::string token;
    std::vector<std::string> retVal;

    while ((posEnd = s.find(delimiter, posStart)) != std::string::npos)
    {
        token = s.substr(posStart, posEnd - posStart);
        posStart = posEnd + delimLen;
        retVal.push_back(token);
    }
    retVal.push_back(s.substr(posStart));
    return (retVal);
}

// ---------------------------------------------------------------------------
// waitForCommand — main command receive loop.
//
// The RP2350 now sends all disk I/O commands as binary IPC frames over SPI.
// This loop blocks on fspi.receiveBinaryCmd() with a short timeout
// (CMD_SPI_POLL_TICKS).  On timeout it checks the UART frame queue for
// any legacy frames (OOB echo, debug, etc.) then loops back to SPI receive.
//
// The SPI receive post_setup_cb asserts HS HIGH each time a receive is
// queued, signalling the RP2350 that the ESP32 is ready for a command.
// This replaces the old vTaskDelay(1) polling loop entirely.
// ---------------------------------------------------------------------------
void CommandProcessor::waitForCommand(void)
{
    // Startup delay — behaviour depends on reset reason:
    //
    // POWERON: fresh boot; SPI slave is uninitialised, HS stays LOW, so the
    //   RP2350 simply waits. A short delay is enough to let FreeRTOS settle
    //   before the first spi_slave_transmit() call.
    //
    // PANIC / SW restart: the ESP32 may have crashed mid-transaction while the
    //   RP2350 was blocked in waitForHandShake() for T2/T3 (timeout = 3000 ms).
    //   If we raise HS (via post_setup_cb) before that timeout expires, the
    //   RP2350 mistakes the T1-ready pulse for a T2/T3 HS and sends the wrong
    //   payload into the 64-byte T1 DMA window, breaking frame synchronisation
    //   for the session. 3500 ms gives 500 ms margin beyond the RP2350 timeout.
    {
        // Proven fixed delays — simple and reliable:
        //
        //  POWERON  (2000 ms): Both RP2350 and ESP32 boot from scratch. The delay
        //                     gives the RP2350 time to reach FSPI_init() and drive
        //                     CS HIGH before we register the SPI slave ISR.
        //
        //  SW reset + OOB magic (100 ms): The RP2350 is already running with CS
        //                     firmly driven HIGH. Only used when OOB explicitly
        //                     sent (currently disabled, but kept for future use).
        //
        //  SW reset, no OOB (3500 ms): spihost corruption from RP2350 reset.
        //                     The RP2350 is rebooting — 3500 ms gives it time to
        //                     complete boot + FSPI_init + stabilize SPI bus.
        bool isOobRestart = (esp_reset_reason() != ESP_RST_POWERON) && (g_oob_restart_magic == OOB_RESTART_MAGIC);
        g_oob_restart_magic = 0u;  // consume flag

        uint32_t delayMs;
        if (esp_reset_reason() == ESP_RST_POWERON)
        {
            delayMs = 2000u;
        }
        else if (isOobRestart)
        {
            delayMs = 100u;
        }
        else
        {
            delayMs = 3500u;
        }

        ESP_LOGI(CMDPROCTAG, "Startup delay %lu ms (reset reason %d, oob=%d).", delayMs, (int) esp_reset_reason(), (int) isOobRestart);
        vTaskDelay(pdMS_TO_TICKS(delayMs));
    }

    // Initialize the SPI slave hardware after the delay.
    // By this point the RP2350 has had time to call FSPI_init() which drives
    // CS (GPIO 45) HIGH — no more floating CS noise from SPI pin glitches.
    // HS was configured LOW in setupEarly() so RP2350 knew to wait.
    if (!fspi.init())
    {
        // SPI init failed — log but do NOT restart. The ESP32 must stay online
        // for WiFi/web interface (firmware updates, config). SPI will be retried
        // if/when the RP2350 stabilizes.
        ESP_LOGE(CMDPROCTAG, "SPI slave init failed — SPI disabled, WiFi/web still active.");
    }

    t_IpcFrameHdr cmdFrame;
    int badFrameCount = 0;
    int spiErrorCount = 0;

    for (;;)
    {
        // --- Binary SPI command receive ---
        // Block indefinitely (portMAX_DELAY) waiting for the RP2350 to send a
        // command frame.  Using a finite timeout is UNSAFE: a timed-out call
        // leaves the spi_slave_transaction_t on the stack (which goes out of
        // scope) still referenced by the SPI slave ISR queue.  When a real
        // command later arrives, the ISR processes the stale (now dangling)
        // transaction, puts its result in the queue, and the next call's
        // spi_slave_get_trans_result() gets the wrong pointer → assert failure
        // (spi_slave.c:524 "ret_trans == trans_desc").
        //
        // A secondary symptom: the stale pre-arm fires post_setup_cb (HS HIGH)
        // immediately after a completed transaction's post_trans_cb (HS LOW),
        // so the LOW→HIGH gap is sub-100µs and invisible to the RP2350's 100µs
        // poll in waitForHandShake(false) → T3 starts while ESP32 still has a
        // recv transaction armed → RP2350 receives zeros → bad frameType=00.
        //
        // portMAX_DELAY guarantees exactly one transaction in flight at all times.
        memset(&cmdFrame, 0, IPCF_HEADER_SIZE);

        esp_err_t spiRet = fspi.receiveBinaryCmd((uint8_t *) &cmdFrame, portMAX_DELAY);

        // Check if OOB requested SPI slave reinit. This runs AFTER
        // spi_slave_transmit returns (transaction complete, no pending DMA),
        // so spi_slave_free is safe — no GDMA deadlock risk.
        if (g_spi_clear_requested)
        {
            g_spi_clear_requested = false;
            ESP_LOGW(CMDPROCTAG, "Reinitializing SPI slave (OOB request)...");
            spi_slave_free(FSPI_HOST);
            if (fspi.init())
            {
                ESP_LOGI(CMDPROCTAG, "SPI slave reinit OK.");
            }
            else
            {
                ESP_LOGE(CMDPROCTAG, "SPI slave reinit FAILED.");
            }
            badFrameCount = 0;
            spiErrorCount = 0;
            continue;  // Re-arm spi_slave_transmit with fresh state
        }

        if (spiRet == ESP_OK)
        {
            spiErrorCount = 0;  // SPI slave is working — reset error counter
            if (cmdFrame.frameType == IPCF_TYPE_COMMAND)
            {
                // Valid binary command — dispatch immediately.
                badFrameCount = 0;
                processCommand(cmdFrame);
            }
            else if (cmdFrame.frameType == IPCF_TYPE_NOP || cmdFrame.frameType == 0)
            {
                // NOP or zero frame — normal idle. Reset bad frame counter.
                badFrameCount = 0;
            }
            else
            {
                // Unknown/garbage frameType — protocol desync from RP2350 reset.
                // After too many consecutive bad frames, restart to reinit SPI slave.
                badFrameCount++;
                if (badFrameCount <= 10 || (badFrameCount % 100) == 0)
                    ESP_LOGW(CMDPROCTAG, "Bad frameType=0x%02X (count=%d)", cmdFrame.frameType, badFrameCount);
            }
        }
        else
        {
            // SPI slave is in an unrecoverable state (e.g. spihost NULL after a
            // spurious CS/SCK glitch during RP2350 reboot).
            //
            // Call esp_restart() immediately — no spi_slave_free(), no esp_wifi_stop().
            //
            // DO NOT call spi_slave_free() here:
            //   If spihost[FSPI_HOST] is NULL (corrupted by SCK/CS glitches during
            //   RP2350 reset), spi_slave_free() returns ESP_ERR_INVALID_ARG without
            //   deregistering the ISR.  If spihost is non-NULL but GDMA is in a bad
            //   state, spi_slave_free() holds the GDMA spinlock, esp_intr_free()
            //   re-enables interrupts, the SPI ISR fires and tries to acquire the
            //   same spinlock → deadlock → interrupt WDT.
            //
            // DO NOT call esp_wifi_stop() here:
            //   When WiFi is in a connection-retry cycle (as seen in the log), the
            //   WiFi state machine mutex is already held by the WiFi task.
            //   esp_wifi_stop() tries to take that mutex recursively →
            //   assert failed: xQueueTakeMutexRecursive queue.c:821 (pxMutex).
            //   Even outside a retry cycle, calling esp_wifi_stop() during WiFi
            //   driver init (<2 s) blocks on an internal driver lock → IWDT.
            //
            // By the time we reach this error path, the RP2350 has long since
            // completed its own reboot and is driving SCK/CS/MOSI normally —
            // no SPI ISR starvation risk.  esp_restart() (a ROM function) resets
            // the CPU without going through normal FreeRTOS task teardown; it
            // also flushes the UART TX FIFO internally so the log line below is
            // fully emitted before the reset.
            // SPI slave error — likely spihost corruption from RP2350 reset.
            // Do NOT restart — keep the ESP32 online for WiFi/web interface.
            // Just log and keep retrying. If spihost is truly NULL, every call
            // will fail, but WiFi and web interface remain functional for
            // firmware updates and config management.
            spiErrorCount++;
            if (spiErrorCount <= 5 || (spiErrorCount % 100) == 0)
                ESP_LOGE(CMDPROCTAG, "receiveBinaryCmd failed (%s), count=%d — retrying", esp_err_to_name(spiRet), spiErrorCount);
            vTaskDelay(pdMS_TO_TICKS(2000));
            continue;
        }
    }
}

// ---------------------------------------------------------------------------
// Initialisation (called by start()).
// ---------------------------------------------------------------------------
void CommandProcessor::init(void)
{
    s_cmdQueue = xQueueCreate(CP_CMD_QUEUE_DEPTH, sizeof(t_CpCmd));
    if (!s_cmdQueue)
    {
        ESP_LOGE(CMDPROCTAG, "Failed to create reverse-command queue.");
    }
}

void CommandProcessor::cmdNop(const t_IpcFrameHdr &frame)
{
    // NOP poll from RP2350 — deliver pending ESP32→RP2350 commands via the
    // T3 response filename field.
    //
    // Reliability: the previous NOP's command is kept in s_pendingCmd until
    // THIS NOP arrives — proving the RP2350 is alive and processed it (or
    // at least received the frame).  Only then do we dequeue the next entry.
    // This prevents command loss when a CRC mismatch causes the RP2350 to
    // discard the response: the next NOP re-sends the same command.

    // --- Retire the previously-sent command (RP2350 got it) ---
    if (s_pendingCmd.cmd[0] != '\0')
    {
        if (s_pendingCmd.doneSem)
            xSemaphoreGive(s_pendingCmd.doneSem);  // unblock CP_sendCmd caller
        memset(&s_pendingCmd, 0, sizeof(s_pendingCmd));
    }

    // --- Dequeue the next command (if any) into the pending slot ---
    if (s_cmdQueue && s_pendingCmd.cmd[0] == '\0')
    {
        xQueueReceive(s_cmdQueue, &s_pendingCmd, 0);
    }

    // --- Build NOP response with any pending command embedded ---
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = IPCF_CMD_NOP;
    hdr.status = IPCF_STATUS_OK;
    hdr.payloadLen = 0;
    if (s_pendingCmd.cmd[0] != '\0')
    {
        strncpy(hdr.filename, s_pendingCmd.cmd, IPCF_FILENAME_LEN - 1);
    }

    uint32_t respSize = IPCF_HEADER_SIZE + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, NULL, 0, respSize, portMAX_DELAY);
}

// ---------------------------------------------------------------------------
// Network command handlers (Celestite W5100 emulation).
// ---------------------------------------------------------------------------

void CommandProcessor::cmdNetCfg(const t_IpcFrameHdr &frame)
{
    // Return ESP32's WiFi STA network configuration.
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = frame.command;

    // Get the default STA interface.
    esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
    if (!netif)
    {
        hdr.status = IPCF_STATUS_ERR;
        hdr.payloadLen = 0;
        fspi.sendBinaryResp(&hdr, NULL, 0, IPCF_HEADER_SIZE + IPCF_CRC_SIZE, portMAX_DELAY);
        return;
    }

    esp_netif_ip_info_t ipInfo;
    esp_netif_get_ip_info(netif, &ipInfo);
    uint8_t mac[6];
    esp_netif_get_mac(netif, mac);

    // Pack: IP[4] + GW[4] + Subnet[4] + MAC[6] = 18 bytes.
    uint8_t payload[18];
    memcpy(payload, &ipInfo.ip.addr, 4);
    memcpy(payload + 4, &ipInfo.gw.addr, 4);
    memcpy(payload + 8, &ipInfo.netmask.addr, 4);
    memcpy(payload + 12, mac, 6);

    hdr.status = IPCF_STATUS_OK;
    hdr.payloadLen = 18;
    uint32_t respSize = IPCF_HEADER_SIZE + 18 + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, payload, 18, respSize, portMAX_DELAY);
}

void CommandProcessor::cmdNetSocket(const t_IpcFrameHdr &frame)
{
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = frame.command;

    uint8_t sockNum = frame.diskNo;
    uint8_t op = (uint8_t)(frame.sectorCount & 0xFF);
    uint8_t protocol = (uint8_t)frame.filename[2];
    uint16_t port = ((uint8_t)frame.filename[0] << 8) | (uint8_t)frame.filename[1];
    uint32_t ipAddr = frame.fileOffset;

    if (sockNum >= 4)
    {
        hdr.status = IPCF_STATUS_ERR;
        hdr.payloadLen = 0;
        fspi.sendBinaryResp(&hdr, NULL, 0, IPCF_HEADER_SIZE + IPCF_CRC_SIZE, portMAX_DELAY);
        return;
    }

    uint8_t resultStatus = 0x00;  // SOCK_CLOSED by default.

    switch (op)
    {
    case 0x01:  // OPEN
    {
        int type = (protocol == 1) ? SOCK_STREAM : SOCK_DGRAM;
        int fd = socket(AF_INET, type, 0);
        if (fd >= 0)
        {
            netSockFd[sockNum] = fd;
            resultStatus = (protocol == 1) ? 0x13 : 0x22;  // SOCK_INIT or SOCK_UDP.
            ESP_LOGI("NET", "Socket %d opened: fd=%d proto=%d", sockNum, fd, protocol);
        }
        else
        {
            ESP_LOGE("NET", "Socket %d open failed: errno=%d", sockNum, errno);
        }
        break;
    }
    case 0x04:  // CONNECT
    {
        int fd = netSockFd[sockNum];
        if (fd < 0)
            break;

        struct sockaddr_in addr;
        memset(&addr, 0, sizeof(addr));
        addr.sin_family = AF_INET;
        addr.sin_addr.s_addr = ipAddr;  // Already in network byte order.
        addr.sin_port = htons(port);

        ESP_LOGI("NET", "Socket %d connecting to %d.%d.%d.%d:%d",
                 (int)sockNum, (int)(ipAddr & 0xFF), (int)((ipAddr >> 8) & 0xFF), (int)((ipAddr >> 16) & 0xFF), (int)((ipAddr >> 24) & 0xFF), (int)port);

        // Non-blocking connect with select() timeout to avoid blocking the SPI command loop.
        int flags = fcntl(fd, F_GETFL, 0);
        fcntl(fd, F_SETFL, flags | O_NONBLOCK);

        int ret = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
        if (ret == 0)
        {
            // Immediate connect (localhost or cached).
            resultStatus = 0x17;  // SOCK_ESTABLISHED.
            ESP_LOGI("NET", "Socket %d connected (immediate)", sockNum);
        }
        else if (errno == EINPROGRESS)
        {
            // Wait for connection with 5-second timeout via select().
            fd_set wfds;
            FD_ZERO(&wfds);
            FD_SET(fd, &wfds);
            struct timeval tv = { .tv_sec = 5, .tv_usec = 0 };
            int sel = select(fd + 1, NULL, &wfds, NULL, &tv);
            if (sel > 0)
            {
                // Check if connect succeeded.
                int err = 0;
                socklen_t errLen = sizeof(err);
                getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errLen);
                if (err == 0)
                {
                    resultStatus = 0x17;  // SOCK_ESTABLISHED.
                    ESP_LOGI("NET", "Socket %d connected", sockNum);
                }
                else
                {
                    resultStatus = 0x00;
                    ESP_LOGE("NET", "Socket %d connect error: %d", sockNum, err);
                    close(fd);
                    netSockFd[sockNum] = -1;
                }
            }
            else
            {
                resultStatus = 0x00;  // Timeout or error.
                ESP_LOGE("NET", "Socket %d connect timeout", sockNum);
                close(fd);
                netSockFd[sockNum] = -1;
            }
        }
        else
        {
            resultStatus = 0x00;  // SOCK_CLOSED on failure.
            ESP_LOGE("NET", "Socket %d connect failed: errno=%d", sockNum, errno);
            close(fd);
            netSockFd[sockNum] = -1;
        }

        // Restore blocking mode if still open.
        if (netSockFd[sockNum] >= 0)
            fcntl(fd, F_SETFL, flags);
        break;
    }
    case 0x02:  // LISTEN
    {
        int fd = netSockFd[sockNum];
        if (fd < 0)
            break;

        struct sockaddr_in addr;
        memset(&addr, 0, sizeof(addr));
        addr.sin_family = AF_INET;
        addr.sin_addr.s_addr = INADDR_ANY;
        addr.sin_port = htons(port);

        if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) == 0 &&
            listen(fd, 1) == 0)
        {
            resultStatus = 0x14;  // SOCK_LISTEN.
            ESP_LOGI("NET", "Socket %d listening on port %d", sockNum, port);
        }
        else
        {
            ESP_LOGE("NET", "Socket %d listen failed: errno=%d", sockNum, errno);
        }
        break;
    }
    case 0x10:  // CLOSE
    case 0x08:  // DISCON
    {
        int fd = netSockFd[sockNum];
        if (fd >= 0)
        {
            if (op == 0x08)
                shutdown(fd, SHUT_WR);
            close(fd);
            netSockFd[sockNum] = -1;
            ESP_LOGI("NET", "Socket %d closed", sockNum);
        }
        resultStatus = 0x00;  // SOCK_CLOSED.
        break;
    }
    default:
        ESP_LOGW("NET", "Unknown socket op: 0x%02X", op);
        break;
    }

    // Send 1-byte response with new socket status.
    hdr.status = IPCF_STATUS_OK;
    hdr.payloadLen = 1;
    uint32_t respSize = IPCF_HEADER_SIZE + 1 + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, &resultStatus, 1, respSize, portMAX_DELAY);
}

void CommandProcessor::cmdNetSend(const t_IpcFrameHdr &frame)
{
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = frame.command;

    uint8_t sockNum = frame.diskNo;
    if (sockNum >= 4 || netSockFd[sockNum] < 0)
    {
        hdr.status = IPCF_STATUS_ERR;
        hdr.payloadLen = 0;
        fspi.sendBinaryResp(&hdr, NULL, 0, IPCF_HEADER_SIZE + IPCF_CRC_SIZE, portMAX_DELAY);
        return;
    }

    // Receive the T2 write payload (data to send over network).
    // Inline T2 receive: same logic as SDCard::receiveWritePayload but without private access.
    uint32_t txLen = frame.payloadLen;
    bool gotPayload = false;
    if (txLen > 0 && txLen <= IPCF_MAX_PAYLOAD)
    {
        uint32_t frameLen = (txLen + IPCF_CRC_SIZE + 3u) & ~3u;
        spi_slave_transaction_t t2 = {};
        t2.length = frameLen * 8;
        t2.rx_buffer = fspi.ipcCmdBuf;
        t2.tx_buffer = fspi.ipcRespBuf;
        if (spi_slave_transmit(FSPI_HOST, &t2, portMAX_DELAY) == ESP_OK)
        {
            // Verify CRC32.
            uint32_t recvCrc, calcCrc;
            memcpy(&recvCrc, fspi.ipcCmdBuf + txLen, IPCF_CRC_SIZE);
            calcCrc = esp_rom_crc32_le(0, fspi.ipcCmdBuf, txLen);
            if (calcCrc == recvCrc)
                gotPayload = true;
            else
                ESP_LOGE("NET", "Send payload CRC mismatch: calc=%08lx recv=%08lx", (unsigned long)calcCrc, (unsigned long)recvCrc);
        }
    }

    int32_t sent = 0;
    if (gotPayload && txLen > 0)
    {
        sent = send(netSockFd[sockNum], fspi.ipcCmdBuf, txLen, 0);
        if (sent < 0)
        {
            ESP_LOGE("NET", "Socket %d send failed: errno=%d", sockNum, errno);
            sent = 0;
        }
        else
        {
            ESP_LOGI("NET", "Socket %d sent %d bytes", sockNum, (int)sent);
        }
    }

    // Response: header + CRC only (no payload).
    // FSPI_sendBinaryCmd expects write-command responses to have no payload
    // (it calculates T3 size as IPCF_HEADER_SIZE + CRC_SIZE for write ops).
    // Including a payload causes CRC offset mismatch.
    hdr.status = (sent > 0) ? IPCF_STATUS_OK : IPCF_STATUS_ERR;
    hdr.payloadLen = 0;
    uint32_t respSize = IPCF_HEADER_SIZE + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, NULL, 0, respSize, portMAX_DELAY);
}

void CommandProcessor::cmdNetRecv(const t_IpcFrameHdr &frame)
{
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = frame.command;

    uint8_t sockNum = frame.diskNo;
    if (sockNum >= 4 || netSockFd[sockNum] < 0)
    {
        hdr.status = IPCF_STATUS_OK;
        hdr.payloadLen = 0;
        fspi.sendBinaryResp(&hdr, NULL, 0, IPCF_HEADER_SIZE + IPCF_CRC_SIZE, portMAX_DELAY);
        return;
    }

    // Set non-blocking for poll.
    int flags = fcntl(netSockFd[sockNum], F_GETFL, 0);
    fcntl(netSockFd[sockNum], F_SETFL, flags | O_NONBLOCK);

    static uint8_t recvBuf[IPCF_MAX_PAYLOAD];
    uint16_t reqSize = frame.sectorCount;
    if (reqSize == 0 || reqSize > IPCF_MAX_PAYLOAD)
        reqSize = IPCF_MAX_PAYLOAD;

    int recvd = recv(netSockFd[sockNum], recvBuf, reqSize, 0);

    // Restore blocking mode.
    fcntl(netSockFd[sockNum], F_SETFL, flags);

    if (recvd <= 0)
    {
        // No data or error — return empty response.
        // Check if connection was closed by peer.
        if (recvd == 0)
        {
            ESP_LOGI("NET", "Socket %d: peer closed connection", sockNum);
        }
        hdr.status = IPCF_STATUS_OK;
        hdr.payloadLen = 0;
        // Use flags to signal peer close: flags bit 0 = peer disconnected.
        if (recvd == 0)
            hdr.flags = 0x04;  // Signal peer close.
        fspi.sendBinaryResp(&hdr, NULL, 0, IPCF_HEADER_SIZE + IPCF_CRC_SIZE, portMAX_DELAY);
        return;
    }

    ESP_LOGI("NET", "Socket %d recv %d bytes", sockNum, recvd);
    hdr.status = IPCF_STATUS_OK;
    hdr.payloadLen = (uint16_t)recvd;
    uint32_t respSize = IPCF_HEADER_SIZE + recvd + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, recvBuf, recvd, respSize, portMAX_DELAY);
}

void CommandProcessor::cmdNetPing(const t_IpcFrameHdr &frame)
{
    t_IpcFrameHdr hdr = {};
    hdr.frameType = IPCF_TYPE_RESPONSE;
    hdr.command = frame.command;

    uint32_t targetIp = frame.fileOffset;

    // Use lwIP raw socket for ICMP echo.
    int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
    uint32_t rtt = 0xFFFFFFFF;  // Timeout sentinel.

    if (sock >= 0)
    {
        struct timeval tv = { .tv_sec = 3, .tv_usec = 0 };
        setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

        struct sockaddr_in addr;
        memset(&addr, 0, sizeof(addr));
        addr.sin_family = AF_INET;
        addr.sin_addr.s_addr = targetIp;

        // Build ICMP echo request.
        uint8_t icmpPkt[64];
        memset(icmpPkt, 0, sizeof(icmpPkt));
        icmpPkt[0] = 8;   // Type: Echo Request.
        icmpPkt[1] = 0;   // Code: 0.
        icmpPkt[4] = 0x12; // ID high.
        icmpPkt[5] = 0x34; // ID low.
        icmpPkt[6] = 0;    // Seq high.
        icmpPkt[7] = 1;    // Seq low.
        // Checksum.
        uint32_t sum = 0;
        for (int i = 0; i < 64; i += 2)
            sum += (icmpPkt[i] << 8) | icmpPkt[i + 1];
        while (sum >> 16)
            sum = (sum & 0xFFFF) + (sum >> 16);
        uint16_t cksum = ~sum;
        icmpPkt[2] = (uint8_t)(cksum >> 8);
        icmpPkt[3] = (uint8_t)(cksum & 0xFF);

        int64_t t0 = esp_timer_get_time();
        int sent = sendto(sock, icmpPkt, 64, 0, (struct sockaddr *)&addr, sizeof(addr));
        if (sent > 0)
        {
            uint8_t pingRecvBuf[128];
            struct sockaddr_in from;
            socklen_t fromLen = sizeof(from);
            int pingRecvd = recvfrom(sock, pingRecvBuf, sizeof(pingRecvBuf), 0, (struct sockaddr *)&from, &fromLen);
            if (pingRecvd > 0)
            {
                int64_t t1 = esp_timer_get_time();
                rtt = (uint32_t)((t1 - t0) / 1000);  // Convert us to ms.
                ESP_LOGI("NET", "Ping %d.%d.%d.%d: RTT=%d ms",
                         (int)(targetIp & 0xFF), (int)((targetIp >> 8) & 0xFF), (int)((targetIp >> 16) & 0xFF), (int)((targetIp >> 24) & 0xFF), (int)rtt);
            }
            else
            {
                ESP_LOGW("NET", "Ping timeout");
            }
        }
        close(sock);
    }
    else
    {
        ESP_LOGE("NET", "Cannot create ICMP socket: errno=%d", errno);
    }

    // Response: no payload (FSPI expects write-style response for non-sector commands).
    // RTT encoded in sectorCount field (16-bit, ms). 0xFFFF = timeout.
    hdr.status = (rtt < 0xFFFFFFFF) ? IPCF_STATUS_OK : IPCF_STATUS_ERR;
    hdr.payloadLen = 0;
    hdr.sectorCount = (uint16_t)(rtt < 0xFFFF ? rtt : 0xFFFF);
    uint32_t respSize = IPCF_HEADER_SIZE + IPCF_CRC_SIZE;
    fspi.sendBinaryResp(&hdr, NULL, 0, respSize, portMAX_DELAY);
}

// ---------------------------------------------------------------------------
// start — spawn the waitForCommand task.
// ---------------------------------------------------------------------------
void CommandProcessor::start(void)
{
    init();
    xTaskCreate([](void *param) { static_cast<CommandProcessor *>(param)->waitForCommand(); }, "waitForCommand", TASK_STACK_SIZE, this, 12, NULL);
}

// ---------------------------------------------------------------------------
// processCommand — binary opcode dispatch.
//
// O(1) switch dispatch on frame.command (uint8_t), replacing the previous
// O(log n) std::map lookup on 3-char ASCII strings.
// ---------------------------------------------------------------------------
void CommandProcessor::processCommand(const t_IpcFrameHdr &frame)
{
    ESP_LOGD(CMDPROCTAG, "CMD opcode=%02X seq=%u file=%.32s", frame.command, frame.seqNum, frame.filename);

    // Any non-NOP command proves the RP2350 is alive and has moved past the
    // last NOP exchange — retire any pending reverse command.
    if (frame.command != IPCF_CMD_NOP && s_pendingCmd.cmd[0] != '\0')
    {
        if (s_pendingCmd.doneSem)
            xSemaphoreGive(s_pendingCmd.doneSem);
        memset(&s_pendingCmd, 0, sizeof(s_pendingCmd));
    }

    switch (frame.command)
    {
    case IPCF_CMD_NOP:
        cmdNop(frame);
        break;
    case IPCF_CMD_RDS:
        cmdReadSector(frame);
        break;
    case IPCF_CMD_RBURST:
        cmdReadBurst(frame);
        break;
    case IPCF_CMD_WRS:
        cmdWriteSector(frame);
        break;
    case IPCF_CMD_WBURST:
        cmdWriteBurst(frame);
        break;
    case IPCF_CMD_RFILE:
    case IPCF_CMD_RFD:
    case IPCF_CMD_RQD:
    case IPCF_CMD_RRF:
        cmdReadFile(frame);
        break;
    case IPCF_CMD_WFILE:
        cmdWriteFile(frame);
        break;
    case IPCF_CMD_INF:
        cmdReadInfo(frame);
        break;
    case IPCF_CMD_DIR:
        cmdReadDir(frame);
        break;
    case IPCF_CMD_NET_CFG:
        cmdNetCfg(frame);
        break;
    case IPCF_CMD_NET_SOCK:
        cmdNetSocket(frame);
        break;
    case IPCF_CMD_NET_SEND:
        cmdNetSend(frame);
        break;
    case IPCF_CMD_NET_RECV:
        cmdNetRecv(frame);
        break;
    case IPCF_CMD_NET_PING:
        cmdNetPing(frame);
        break;
    default:
        cmdUnknown(frame);
        break;
    }
}