MicrOBU/obu-firmware/main/serial_link.c

#include "serial_link.h"
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "driver/usb_serial_jtag.h"
#include "esp_log.h"

static const char *TAG = "serial_link";

#define SYNC0  0xAA
#define SYNC1  0x55

static serial_link_cam_tx_cb_t s_on_cam_tx;
static serial_link_cam_tx_pv_cb_t s_on_cam_tx_pv;

// ---- Counters reported to the phone in every heartbeat (see SERIAL_MSG_STATUS in the header).
// Saturating rather than wrapping: "65535 drops" reads as "lots and still going", whereas a wrap
// back to a small number during a long bench run reads as "the problem went away".
static uint16_t s_oversize_drops;
static uint16_t s_tx_failures;
static uint16_t s_rx_crc_errors;
static uint16_t s_rx_queue_drops;

// Serializes send_frame(): it writes a frame as four separate usb_serial_jtag_write_bytes() calls
// and shares one static CRC scratch buffer, and it's now called from three tasks (rx_forward for
// CAM_RX, the heartbeat task for STATUS, and potentially others). Without this, two frames could
// interleave on the wire and both would be discarded by the phone's framer.
static SemaphoreHandle_t s_tx_mutex;

static inline void bump(uint16_t *counter)
{
    if (*counter < 0xFFFF) (*counter)++;
}

void serial_link_note_tx_failure(void)
{
    bump(&s_tx_failures);
}

void serial_link_note_oversize_drop(uint16_t btp_dest_port)
{
    bump(&s_oversize_drops);
    ESP_LOGW(TAG, "port %u message larger than the RX capture buffer, total oversize drops %u",
             btp_dest_port, s_oversize_drops);
}

void serial_link_note_rx_queue_drop(void)
{
    bump(&s_rx_queue_drops);
    ESP_LOGW(TAG, "rx queue full, dropped a captured frame, total rx queue drops %u", s_rx_queue_drops);
}

// ---- CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF, no reflect, no xorout) ----
// Bytewise (no table) - frames here are at most SERIAL_LINK_MAX_PAYLOAD + 3 bytes, so table
// lookup isn't worth the flash/RAM tradeoff. MUST match the Kotlin-side implementation exactly
// (see app SerialFrame.kt) or every frame will be silently rejected as corrupt.
static uint16_t crc16_ccitt_false(const uint8_t *data, size_t len)
{
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= (uint16_t)data[i] << 8;
        for (int b = 0; b < 8; b++) {
            crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
        }
    }
    return crc;
}

static bool send_frame(uint8_t type, const uint8_t *payload, int len)
{
    if (len < 0 || len > SERIAL_LINK_MAX_PAYLOAD) {
        ESP_LOGW(TAG, "send_frame: payload too large (%d)", len);
        return false;
    }

    // type(1) + length(2) + payload(len) is what the CRC covers.
    uint8_t head[3];
    head[0] = type;
    head[1] = (uint8_t)(len & 0xFF);
    head[2] = (uint8_t)((len >> 8) & 0xFF);

    // Concatenate head+payload to CRC them in one pass. static, not stack: at
    // SERIAL_LINK_MAX_PAYLOAD = 512 this buffer is 515 bytes, which is a meaningful bite out of a
    // 4 KB task stack, and send_frame() is called from several tasks. Guarded by s_tx_mutex below
    // so the shared buffer (and the four-part write) can't interleave between callers.
    static uint8_t s_crc_buf[3 + SERIAL_LINK_MAX_PAYLOAD];

    // No host on the other end: the TX buffer never drains, so every write below would block its
    // full timeout and this frame is going nowhere regardless. Bail before taking the mutex -
    // otherwise a burst of promiscuously-captured CAMs holds the lock for hundreds of ms each and
    // starves the heartbeat, which is exactly what "tx mutex timeout, dropping frame" was.
    if (!usb_serial_jtag_is_connected()) {
        return false;
    }

    // Timeout must exceed the worst-case hold below (4 writes x SERIAL_LINK_WRITE_TIMEOUT_MS),
    // or a legitimately slow-but-working host makes contending senders drop frames instead of
    // waiting their turn.
    if (s_tx_mutex && xSemaphoreTake(s_tx_mutex, pdMS_TO_TICKS(SERIAL_LINK_TX_LOCK_TIMEOUT_MS)) != pdTRUE) {
        ESP_LOGW(TAG, "send_frame: tx mutex timeout, dropping frame");
        return false;
    }

    memcpy(s_crc_buf, head, 3);
    if (len > 0) memcpy(s_crc_buf + 3, payload, (size_t)len);
    uint16_t crc = crc16_ccitt_false(s_crc_buf, (size_t)(3 + len));

    uint8_t sync[2] = {SYNC0, SYNC1};
    uint8_t crc_bytes[2] = {(uint8_t)(crc & 0xFF), (uint8_t)((crc >> 8) & 0xFF)};

    // Four separate writes rather than one assembled buffer - simplest given payload is
    // already wherever the caller has it (avoids a second copy of up to 160 bytes).
    // usb_serial_jtag_write_bytes() blocks up to the given tick timeout if the host isn't
    // reading fast enough; generous for a single frame at USB full-speed, and keeps a wedged
    // host from hanging the radio TX/RX tasks indefinitely.
    const TickType_t write_timeout = pdMS_TO_TICKS(SERIAL_LINK_WRITE_TIMEOUT_MS);
    int wrote = 0;
    wrote += usb_serial_jtag_write_bytes(sync, sizeof(sync), write_timeout);
    wrote += usb_serial_jtag_write_bytes(head, sizeof(head), write_timeout);
    if (len > 0) wrote += usb_serial_jtag_write_bytes(payload, (size_t)len, write_timeout);
    wrote += usb_serial_jtag_write_bytes(crc_bytes, sizeof(crc_bytes), write_timeout);

    if (s_tx_mutex) xSemaphoreGive(s_tx_mutex);

    return wrote == (int)(sizeof(sync) + sizeof(head) + len + sizeof(crc_bytes));
}

bool serial_link_send_v2x_rx(uint16_t btp_dest_port, int8_t rssi,
                              bool has_geo_area, bool signed_unverified,
                              int32_t geo_area_lat_tenmicrodeg,
                              int32_t geo_area_lon_tenmicrodeg,
                              uint16_t geo_area_distance_a_m,
                              const uint8_t *uper, int uper_len)
{
    if (uper_len < 0 || uper_len > SERIAL_LINK_MAX_PAYLOAD - SERIAL_V2X_RX_PREFIX_LEN) {
        // Counted, not just logged: this log line goes to the flashing port, which nobody is
        // watching during a phone bench session - so the symptom would be "that station just
        // never shows up in the app" with no visible cause.
        bump(&s_oversize_drops);
        ESP_LOGW(TAG, "send_v2x_rx: port %u payload too large (%d), total oversize drops %u",
                 btp_dest_port, uper_len, s_oversize_drops);
        return false;
    }
    // static, not stack (526 bytes at MAX_PAYLOAD 512); only rx_forward_task calls this, and
    // send_frame's mutex covers the handoff onto the wire.
    static uint8_t s_v2x_payload[SERIAL_LINK_MAX_PAYLOAD];

    // Little-endian prefix, layout documented in serial_link.h - keep in lockstep with the app's
    // SerialFrame.kt.
    s_v2x_payload[0]  = (uint8_t)(btp_dest_port & 0xFF);
    s_v2x_payload[1]  = (uint8_t)((btp_dest_port >> 8) & 0xFF);
    s_v2x_payload[2]  = (uint8_t)rssi;
    s_v2x_payload[3]  = (uint8_t)((has_geo_area ? 0x01 : 0x00) | (signed_unverified ? 0x02 : 0x00));
    uint32_t lat = (uint32_t)geo_area_lat_tenmicrodeg;
    uint32_t lon = (uint32_t)geo_area_lon_tenmicrodeg;
    s_v2x_payload[4]  = (uint8_t)(lat & 0xFF);
    s_v2x_payload[5]  = (uint8_t)((lat >> 8) & 0xFF);
    s_v2x_payload[6]  = (uint8_t)((lat >> 16) & 0xFF);
    s_v2x_payload[7]  = (uint8_t)((lat >> 24) & 0xFF);
    s_v2x_payload[8]  = (uint8_t)(lon & 0xFF);
    s_v2x_payload[9]  = (uint8_t)((lon >> 8) & 0xFF);
    s_v2x_payload[10] = (uint8_t)((lon >> 16) & 0xFF);
    s_v2x_payload[11] = (uint8_t)((lon >> 24) & 0xFF);
    s_v2x_payload[12] = (uint8_t)(geo_area_distance_a_m & 0xFF);
    s_v2x_payload[13] = (uint8_t)((geo_area_distance_a_m >> 8) & 0xFF);

    if (uper_len > 0) memcpy(s_v2x_payload + SERIAL_V2X_RX_PREFIX_LEN, uper, (size_t)uper_len);
    return send_frame(SERIAL_MSG_V2X_RX, s_v2x_payload, SERIAL_V2X_RX_PREFIX_LEN + uper_len);
}

bool serial_link_send_status(uint8_t status)
{
    // [status:1][oversize_drops:2 LE][tx_failures:2 LE][rx_crc_errors:2 LE][capabilities:1]
    // [rx_queue_drops:2 LE] - keep in lockstep with EspLinkStatus.parse() in the app's
    // SerialFrame.kt.
    uint8_t payload[10];
    payload[0] = status;
    payload[1] = (uint8_t)(s_oversize_drops & 0xFF);
    payload[2] = (uint8_t)((s_oversize_drops >> 8) & 0xFF);
    payload[3] = (uint8_t)(s_tx_failures & 0xFF);
    payload[4] = (uint8_t)((s_tx_failures >> 8) & 0xFF);
    payload[5] = (uint8_t)(s_rx_crc_errors & 0xFF);
    payload[6] = (uint8_t)((s_rx_crc_errors >> 8) & 0xFF);
    // What this firmware accepts. The app reads it to decide whether it may send CAM_TX_PV, which
    // is what lets a new app keep working against firmware that predates that message.
    payload[7] = SERIAL_CAP_CAM_TX_PV;
    payload[8] = (uint8_t)(s_rx_queue_drops & 0xFF);
    payload[9] = (uint8_t)((s_rx_queue_drops >> 8) & 0xFF);
    return send_frame(SERIAL_MSG_STATUS, payload, sizeof(payload));
}

// 1 Hz heartbeat. This is what lets the phone tell "link alive, radio quiet" from "link dead" -
// the app's watchdog (UsbSerialTransport.kt) marks the link ERROR after 3 missed beats. Keep the
// period in step with LINK_TIMEOUT_MS over there.
static void status_task(void *arg)
{
    (void)arg;
    while (1) {
        serial_link_send_status(0);
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

// ---- RX framing state machine ----
// Runs in its own task, byte-at-a-time off the USB Serial/JTAG driver's RX ring buffer (via
// usb_serial_jtag_read_bytes with a short timeout, not raw ISR access - simplest correct option
// for a link this slow/small; revisit if CAM traffic volume ever makes this a bottleneck).
typedef enum {
    WAIT_SYNC0,
    WAIT_SYNC1,
    WAIT_TYPE,
    WAIT_LEN_LO,
    WAIT_LEN_HI,
    WAIT_PAYLOAD,
    WAIT_CRC_LO,
    WAIT_CRC_HI,
} rx_state_t;

static void rx_task(void *arg)
{
    (void)arg;
    rx_state_t state = WAIT_SYNC0;
    uint8_t type = 0;
    uint16_t len = 0;
    uint16_t payload_idx = 0;
    uint16_t crc_recv = 0;
    // static, not stack: at SERIAL_LINK_MAX_PAYLOAD = 512 these two are >1 KB together, a quarter
    // of this task's 4 KB stack. Safe as statics because rx_task is a singleton - one instance,
    // created once in serial_link_init().
    static uint8_t payload[SERIAL_LINK_MAX_PAYLOAD];
    static uint8_t crc_buf[3 + SERIAL_LINK_MAX_PAYLOAD];

    uint8_t byte;
    while (1) {
        int n = usb_serial_jtag_read_bytes(&byte, 1, pdMS_TO_TICKS(50));
        if (n <= 0) continue;

        switch (state) {
        case WAIT_SYNC0:
            state = (byte == SYNC0) ? WAIT_SYNC1 : WAIT_SYNC0;
            break;
        case WAIT_SYNC1:
            state = (byte == SYNC1) ? WAIT_TYPE : (byte == SYNC0 ? WAIT_SYNC1 : WAIT_SYNC0);
            break;
        case WAIT_TYPE:
            type = byte;
            state = WAIT_LEN_LO;
            break;
        case WAIT_LEN_LO:
            len = byte;
            state = WAIT_LEN_HI;
            break;
        case WAIT_LEN_HI:
            len |= (uint16_t)byte << 8;
            if (len > SERIAL_LINK_MAX_PAYLOAD) {
                ESP_LOGW(TAG, "rx: length %u exceeds max, resyncing", len);
                state = WAIT_SYNC0; // can't trust this frame boundary at all - drop to resync
            } else if (len == 0) {
                payload_idx = 0;
                state = WAIT_CRC_LO;
            } else {
                payload_idx = 0;
                state = WAIT_PAYLOAD;
            }
            break;
        case WAIT_PAYLOAD:
            payload[payload_idx++] = byte;
            if (payload_idx >= len) state = WAIT_CRC_LO;
            break;
        case WAIT_CRC_LO:
            crc_recv = byte;
            state = WAIT_CRC_HI;
            break;
        case WAIT_CRC_HI: {
            crc_recv |= (uint16_t)byte << 8;

            crc_buf[0] = type;
            crc_buf[1] = (uint8_t)(len & 0xFF);
            crc_buf[2] = (uint8_t)((len >> 8) & 0xFF);
            if (len > 0) memcpy(crc_buf + 3, payload, len);
            uint16_t crc_calc = crc16_ccitt_false(crc_buf, (size_t)(3 + len));

            if (crc_calc == crc_recv) {
                if (type == SERIAL_MSG_CAM_TX) {
                    if (s_on_cam_tx) s_on_cam_tx(payload, len);
                } else if (type == SERIAL_MSG_CAM_TX_PV) {
                    // A frame that is all prefix has nothing to transmit.
                    if (len > SERIAL_CAM_TX_PV_PREFIX_LEN) {
                        if (s_on_cam_tx_pv) {
                            s_on_cam_tx_pv(payload, payload + SERIAL_CAM_TX_PV_PREFIX_LEN,
                                           len - SERIAL_CAM_TX_PV_PREFIX_LEN);
                        }
                    } else {
                        ESP_LOGW(TAG, "rx: CAM_TX_PV of %u bytes carries no CAM, ignoring", len);
                    }
                } else {
                    ESP_LOGW(TAG, "rx: unexpected frame type 0x%02x from phone, ignoring", type);
                }
            } else {
                bump(&s_rx_crc_errors);
                ESP_LOGW(TAG, "rx: CRC mismatch (got %04x want %04x), dropping frame (total %u)",
                         crc_recv, crc_calc, s_rx_crc_errors);
            }
            state = WAIT_SYNC0;
            break;
        }
        }
    }
}

void serial_link_init(serial_link_cam_tx_cb_t on_cam_tx,
                      serial_link_cam_tx_pv_cb_t on_cam_tx_pv)
{
    s_on_cam_tx = on_cam_tx;
    s_on_cam_tx_pv = on_cam_tx_pv;

    s_tx_mutex = xSemaphoreCreateMutex();
    if (!s_tx_mutex) {
        // Fail loudly rather than silently running unserialized: interleaved frames would look
        // like random CRC errors on the phone, which is a miserable thing to debug.
        ESP_LOGE(TAG, "failed to create tx mutex");
        abort();
    }

    usb_serial_jtag_driver_config_t cfg = {
        .tx_buffer_size = SERIAL_LINK_USB_BUF_SIZE,
        .rx_buffer_size = SERIAL_LINK_USB_BUF_SIZE,
    };
    ESP_ERROR_CHECK(usb_serial_jtag_driver_install(&cfg));

    xTaskCreate(rx_task, "serial_link_rx", 4096, NULL, 6, NULL);
    xTaskCreate(status_task, "serial_link_status", 2560, NULL, 4, NULL);
    ESP_LOGI(TAG, "serial_link up on native USB Serial/JTAG (VID 0x303A / PID 0x1001), "
                  "max payload %d, 1 Hz heartbeat", SERIAL_LINK_MAX_PAYLOAD);
}