#include "jlink.h"
#include "audio.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>

/* ---- DMA circular RX buffer ---- */
#define JLINK_RX_BUF_LEN    128u   /* must be power-of-2 */
static uint8_t  s_rx_buf[JLINK_RX_BUF_LEN];
static uint32_t s_rx_tail = 0;     /* consumer index (byte already processed) */

/* ---- HAL handles ---- */
static UART_HandleTypeDef s_uart;
static DMA_HandleTypeDef  s_dma_rx;

/* ---- TX mutex ---- */
/*
 * Issue #522: USART1 IDLE interrupt (DMA RX) fires via HAL_UART_IRQHandler
 * mid-frame during polling HAL_UART_Transmit, resetting gState and causing
 * truncated/null-prefixed frames on the Jetson link.
 *
 * Fix: disable USART1_IRQn around every blocking TX so HAL_UART_IRQHandler
 * cannot modify gState while HAL_UART_Transmit is looping.  s_tx_busy guards
 * against any re-entrant caller (ESC debug, future paths).
 */
static volatile uint8_t s_tx_busy = 0;

static void jlink_tx_locked(uint8_t *buf, uint16_t len)
{
    if (s_tx_busy) return;          /* drop if already transmitting */
    s_tx_busy = 1u;
    HAL_NVIC_DisableIRQ(USART1_IRQn);
    HAL_UART_Transmit(&s_uart, buf, len, 5u);
    HAL_NVIC_EnableIRQ(USART1_IRQn);
    s_tx_busy = 0u;
}

/* ---- Volatile state ---- */
volatile JLinkState jlink_state;

/* ---- SCHED_SET static receive buffer (Issue #550) ---- */
static JLinkSchedSetBuf s_sched_set_buf;

JLinkSchedSetBuf *jlink_get_sched_set(void)
{
    return &s_sched_set_buf;
}

/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */
static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len)
{
    uint16_t crc = 0x0000u;
    for (uint16_t i = 0; i < len; i++) {
        crc ^= (uint16_t)data[i] << 8;
        for (uint8_t b = 0; b < 8; b++) {
            if (crc & 0x8000u)
                crc = (crc << 1) ^ 0x1021u;
            else
                crc <<= 1;
        }
    }
    return crc;
}

/* ---- jlink_init() ---- */
void jlink_init(void)
{
    /* GPIO: PB6=TX AF7 (USART1_TX), PB7=RX AF7 (USART1_RX) */
    __HAL_RCC_GPIOB_CLK_ENABLE();
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin       = GPIO_PIN_6 | GPIO_PIN_7;
    gpio.Mode      = GPIO_MODE_AF_PP;
    gpio.Pull      = GPIO_PULLUP;
    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF7_USART1;
    HAL_GPIO_Init(GPIOB, &gpio);

    /* DMA2 Stream2 Channel4 -- USART1_RX circular */
    __HAL_RCC_DMA2_CLK_ENABLE();
    s_dma_rx.Instance                 = DMA2_Stream2;
    s_dma_rx.Init.Channel             = DMA_CHANNEL_4;
    s_dma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
    s_dma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
    s_dma_rx.Init.MemInc              = DMA_MINC_ENABLE;
    s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    s_dma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
    s_dma_rx.Init.Mode                = DMA_CIRCULAR;
    s_dma_rx.Init.Priority            = DMA_PRIORITY_MEDIUM;
    s_dma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
    HAL_DMA_Init(&s_dma_rx);
    __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx);

    /* USART1 at JLINK_BAUD (921600) */
    __HAL_RCC_USART1_CLK_ENABLE();
    s_uart.Instance          = USART1;
    s_uart.Init.BaudRate     = JLINK_BAUD;
    s_uart.Init.WordLength   = UART_WORDLENGTH_8B;
    s_uart.Init.StopBits     = UART_STOPBITS_1;
    s_uart.Init.Parity       = UART_PARITY_NONE;
    s_uart.Init.Mode         = UART_MODE_TX_RX;
    s_uart.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
    s_uart.Init.OverSampling = UART_OVERSAMPLING_16;
    HAL_UART_Init(&s_uart);

    /* Enable USART1 IDLE interrupt for circular buffer draining */
    __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE);
    HAL_NVIC_SetPriority(USART1_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(USART1_IRQn);

    /* DMA2_Stream2 IRQ (for error handling) */
    HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0);
    HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);

    /* Start circular DMA RX -- never stops */
    HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN);

    memset((void *)&jlink_state, 0, sizeof(jlink_state));
    memset(&s_sched_set_buf, 0, sizeof(s_sched_set_buf));
    s_rx_tail = 0;
}

/* ---- IRQ handlers ---- */
void USART1_IRQHandler(void)
{
    /* Clear IDLE flag by reading SR then DR */
    if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
        __HAL_UART_CLEAR_IDLEFLAG(&s_uart);
        /* jlink_process() drains the buffer from main loop -- no work here */
    }
    HAL_UART_IRQHandler(&s_uart);
}

void DMA2_Stream2_IRQHandler(void)
{
    HAL_DMA_IRQHandler(&s_dma_rx);
}

/* ---- jlink_is_active() ---- */
bool jlink_is_active(uint32_t now_ms)
{
    if (jlink_state.last_rx_ms == 0u) return false;
    return (now_ms - jlink_state.last_rx_ms) < JLINK_HB_TIMEOUT_MS;
}

/* ---- Frame dispatch ---- */
static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
{
    /* Update heartbeat timestamp on every valid frame */
    jlink_state.last_rx_ms = HAL_GetTick();

    switch (cmd) {

    case JLINK_CMD_HEARTBEAT:
        /* Heartbeat only -- no payload action needed */
        break;

    case JLINK_CMD_DRIVE:
        if (plen == 4u) {
            int16_t spd, str;
            memcpy(&spd, payload,     2);
            memcpy(&str, payload + 2, 2);
            /* Clamp to +/-1000 */
            if (spd >  1000) spd =  1000;
            if (spd < -1000) spd = -1000;
            if (str >  1000) str =  1000;
            if (str < -1000) str = -1000;
            jlink_state.speed = spd;
            jlink_state.steer = str;
        }
        break;

    case JLINK_CMD_ARM:
        jlink_state.arm_req = 1u;
        break;

    case JLINK_CMD_DISARM:
        jlink_state.disarm_req = 1u;
        break;

    case JLINK_CMD_PID_SET:
        if (plen == 12u) {
            float kp, ki, kd;
            memcpy(&kp, payload,     4);
            memcpy(&ki, payload + 4, 4);
            memcpy(&kd, payload + 8, 4);
            /* Sanity bounds -- same as USB CDC PID handler in main.c */
            if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp;
            if (ki >= 0.0f && ki <= 50.0f)  jlink_state.pid_ki = ki;
            if (kd >= 0.0f && kd <= 50.0f)  jlink_state.pid_kd = kd;
            jlink_state.pid_updated = 1u;
        }
        break;

    case JLINK_CMD_DFU_ENTER:
        /* Payload-less; main loop checks armed state before calling ota_enter_dfu() */
        jlink_state.dfu_req = 1u;
        break;

    case JLINK_CMD_ESTOP:
        jlink_state.estop_req = 1u;
        break;

    case JLINK_CMD_AUDIO:
        /* Payload: int16 PCM samples, little-endian, 1..126 samples (2..252 bytes) */
        if (plen >= 2u && (plen & 1u) == 0u) {
            audio_write_pcm((const int16_t *)payload, plen / 2u);
        }
        break;

    case JLINK_CMD_SLEEP:
        /* Payload-less; main loop calls power_mgmt_request_sleep() */
        jlink_state.sleep_req = 1u;
        break;

    case JLINK_CMD_PID_SAVE:
        /* Payload-less; main loop calls pid_flash_save() (Issue #531) */
        jlink_state.pid_save_req = 1u;
        break;

    case JLINK_CMD_GIMBAL_POS:
        /* Payload: int16 pan_x10, int16 tilt_x10, uint16 speed (6 bytes) Issue #547 */
        if (plen == 6u) {
            int16_t  pan, tilt;
            uint16_t spd;
            memcpy(&pan,  payload,     2);
            memcpy(&tilt, payload + 2, 2);
            memcpy(&spd,  payload + 4, 2);
            jlink_state.gimbal_pan_x10  = pan;
            jlink_state.gimbal_tilt_x10 = tilt;
            jlink_state.gimbal_speed    = spd;
            jlink_state.gimbal_updated  = 1u;
        }
        break;

    case JLINK_CMD_SCHED_GET:
        /* Payload-less; main loop calls jlink_send_sched_telemetry() (Issue #550) */
        jlink_state.sched_get_req = 1u;
        break;

    case JLINK_CMD_SCHED_SET:
        /* Payload: uint8 num_bands + num_bands * sizeof(pid_sched_entry_t) bytes */
        if (plen >= 1u) {
            uint8_t nb = payload[0];
            if (nb == 0u) nb = 1u;
            if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
            uint8_t expected = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
            if (plen >= expected) {
                s_sched_set_buf.num_bands = nb;
                memcpy(s_sched_set_buf.bands, payload + 1,
                       nb * sizeof(pid_sched_entry_t));
                s_sched_set_buf.ready = 1u;
            }
        }
        break;

    case JLINK_CMD_SCHED_SAVE:
        /* Payload: float kp, float ki, float kd (12 bytes) for single-PID record */
        if (plen == 12u) {
            float kp, ki, kd;
            memcpy(&kp, payload,     4);
            memcpy(&ki, payload + 4, 4);
            memcpy(&kd, payload + 8, 4);
            if (kp >= 0.0f && kp <= 500.0f) jlink_state.sched_save_kp = kp;
            if (ki >= 0.0f && ki <= 50.0f)  jlink_state.sched_save_ki = ki;
            if (kd >= 0.0f && kd <= 50.0f)  jlink_state.sched_save_kd = kd;
            jlink_state.sched_save_req = 1u;
        }
        break;

    case JLINK_CMD_FAULT_LOG_GET:  /* Issue #565: request fault log telemetry */
        jlink_state.fault_log_req = 1u;
        break;

    case JLINK_CMD_CAN_STATS_GET:  /* Issue #597: request CAN bus statistics */
        jlink_state.can_stats_req = 1u;
        break;

    default:
        break;
    }
}

/* ---- jlink_process() -- call from main loop every tick ---- */
/*
 * Parser state machine.
 * Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX]
 * LEN = count of CMD + PAYLOAD bytes (1..253).
 * CRC16-XModem over CMD+PAYLOAD.
 * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
 * but we cap at 64 for safety).
 */
#define JLINK_MAX_PAYLOAD   252u  /* enlarged for AUDIO chunks (126 x int16) */

typedef enum {
    PS_WAIT_STX = 0,
    PS_WAIT_LEN,
    PS_WAIT_DATA,   /* receiving CMD + PAYLOAD (len bytes total) */
    PS_WAIT_CRC_HI,
    PS_WAIT_CRC_LO,
    PS_WAIT_ETX,
} ParseState;

void jlink_process(void)
{
    static ParseState s_state   = PS_WAIT_STX;
    static uint8_t    s_len     = 0;    /* expected CMD+PAYLOAD length */
    static uint8_t    s_count   = 0;    /* bytes received so far in PS_WAIT_DATA */
    static uint8_t    s_frame[JLINK_MAX_PAYLOAD + 1u]; /* [0]=CMD, [1..]=PAYLOAD */
    static uint8_t    s_crc_hi  = 0;

    /* Compute how many bytes the DMA has written since last drain */
    uint32_t head  = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
    uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u);

    for (uint32_t i = 0; i < bytes; i++) {
        uint8_t b = s_rx_buf[s_rx_tail];
        s_rx_tail = (s_rx_tail + 1u) & (JLINK_RX_BUF_LEN - 1u);

        switch (s_state) {
        case PS_WAIT_STX:
            if (b == JLINK_STX) s_state = PS_WAIT_LEN;
            break;

        case PS_WAIT_LEN:
            if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) {
                /* Invalid length -- resync */
                s_state = PS_WAIT_STX;
            } else {
                s_len   = b;
                s_count = 0;
                s_state = PS_WAIT_DATA;
            }
            break;

        case PS_WAIT_DATA:
            s_frame[s_count++] = b;
            if (s_count == s_len) s_state = PS_WAIT_CRC_HI;
            break;

        case PS_WAIT_CRC_HI:
            s_crc_hi = b;
            s_state  = PS_WAIT_CRC_LO;
            break;

        case PS_WAIT_CRC_LO: {
            uint16_t rx_crc   = ((uint16_t)s_crc_hi << 8) | b;
            uint16_t calc_crc = crc16_xmodem(s_frame, s_len);
            if (rx_crc == calc_crc)
                s_state = PS_WAIT_ETX;
            else
                s_state = PS_WAIT_STX;   /* CRC mismatch -- drop */
            break;
        }

        case PS_WAIT_ETX:
            if (b == JLINK_ETX) {
                /* Valid frame: s_frame[0]=CMD, s_frame[1..s_len-1]=PAYLOAD */
                dispatch(s_frame + 1, s_frame[0], s_len - 1u);
            }
            /* Either way, go back to idle (resync on bad ETX) */
            s_state = PS_WAIT_STX;
            break;
        }
    }
}

/* ---- jlink_send_telemetry() ---- */
void jlink_send_telemetry(const jlink_tlm_status_t *status)
{
    /*
     * Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 20 (payload) = 21
     * Total frame length = 1+1+1+20+2+1 = 26 bytes
     * At 921600 baud (10 bits/byte): 26x10/921600 ~0.28ms -- safe to block.
     */
    static uint8_t frame[26];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_status_t);  /* 20 */
    const uint8_t  len  = 1u + plen;                            /* 21 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_STATUS;
    memcpy(&frame[3], status, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);  /* over CMD + PAYLOAD */
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_power_telemetry() ---- */
void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
{
    /*
     * Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes
     * At 921600 baud: 17x10/921600 ~0.18 ms -- safe to block.
     */
    static uint8_t frame[17];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_power_t);  /* 11 */
    const uint8_t  len  = 1u + plen;                            /* 12 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_POWER;
    memcpy(&frame[3], power, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_pid_result() -- Issue #531 ---- */
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
{
    /*
     * Frame: [STX][LEN][0x83][13 bytes PID_RESULT][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 13 (payload) = 14; total frame = 19 bytes.
     * At 921600 baud: 19x10/921600 ~0.21 ms -- safe to block.
     */
    static uint8_t frame_pid[19];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_pid_result_t);  /* 13 */
    const uint8_t  flen = 1u + plen;                                 /* 14 */

    frame_pid[0] = JLINK_STX;
    frame_pid[1] = flen;
    frame_pid[2] = JLINK_TLM_PID_RESULT;
    memcpy(&frame_pid[3], result, plen);

    uint16_t crc_pid = crc16_xmodem(&frame_pid[2], flen);
    frame_pid[3 + plen]     = (uint8_t)(crc_pid >> 8);
    frame_pid[3 + plen + 1] = (uint8_t)(crc_pid & 0xFFu);
    frame_pid[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame_pid, sizeof(frame_pid));
}

/* ---- jlink_send_battery_telemetry() -- Issue #533 ---- */
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
{
    /*
     * Frame: [STX][LEN][0x82][10 bytes BATTERY][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
     * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
     */
    static uint8_t frame[16];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_battery_t);  /* 10 */
    const uint8_t  len  = 1u + plen;                              /* 11 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_BATTERY;
    memcpy(&frame[3], batt, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_gimbal_state() -- Issue #547 ---- */
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
{
    /*
     * Frame: [STX][LEN][0x84][10 bytes GIMBAL_STATE][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
     * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
     */
    static uint8_t frame[16];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_gimbal_state_t);  /* 10 */
    const uint8_t  len  = 1u + plen;                                   /* 11 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_GIMBAL_STATE;
    memcpy(&frame[3], state, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_motor_current_tlm() -- Issue #584 ---- */
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm)
{
    /*
     * Frame: [STX][LEN][0x86][8 bytes MOTOR_CURRENT][CRC_hi][CRC_lo][ETX]
     * LEN = 1 (CMD) + 8 (payload) = 9; total frame = 14 bytes.
     * At 921600 baud: 14x10/921600 ~0.15 ms -- safe to block.
     */
    static uint8_t frame[14];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_motor_current_t);  /* 8 */
    const uint8_t  len  = 1u + plen;                                    /* 9 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_MOTOR_CURRENT;
    memcpy(&frame[3], tlm, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
{
    /*
     * Frame: [STX][LEN][0x85][1+N*16 bytes SCHED][CRC_hi][CRC_lo][ETX]
     * Actual payload = 1 (num_bands) + tlm->num_bands * 16 bytes.
     * Max payload = 1 + 6*16 = 97; max frame = 103 bytes.
     * At 921600 baud: 103x10/921600 ~1.1 ms -- safe to block.
     */
    uint8_t  nb   = tlm->num_bands;
    if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
    uint8_t  plen = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
    uint8_t  len  = 1u + plen;  /* CMD + payload */
    /* frame: STX + LEN + CMD + payload + CRC_hi + CRC_lo + ETX */
    uint8_t  frame[103];

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_SCHED;
    frame[3] = nb;
    memcpy(&frame[4], tlm->bands, nb * sizeof(pid_sched_entry_t));

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
}

/* ---- jlink_send_slope_tlm() -- Issue #600 ---- */
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
{
    /*
     * Frame: [STX][LEN][0x88][4 bytes SLOPE][CRC_hi][CRC_lo][ETX]
     * Total: 1+1+1+4+2+1 = 10 bytes
     */
    static uint8_t frame[10];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_slope_t);  /* 4 */
    const uint8_t  len  = 1u + plen;  /* CMD byte + payload */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_SLOPE;
    memcpy(&frame[3], tlm, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_fault_log() -- Issue #565 ---- */
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl)
{
    /*
     * Frame: [STX][LEN][0x87][20 bytes fault_log][CRC_hi][CRC_lo][ETX]
     * LEN = 1 + 20 = 21; total = 26 bytes
     * At 921600 baud: 26x10/921600 ~0.28 ms -- safe to block.
     */
    static uint8_t frame[26];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_fault_log_t);  /* 20 */
    const uint8_t  len  = 1u + plen;  /* CMD byte + payload */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_FAULT_LOG;
    memcpy(&frame[3], fl, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_can_stats() -- Issue #597 ---- */
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm)
{
    /*
     * Frame: [STX][LEN][0x89][16 bytes CAN_STATS][CRC_hi][CRC_lo][ETX]
     * LEN = 1 + 16 = 17; total = 22 bytes
     * At 921600 baud: 22x10/921600 ~0.24 ms -- safe to block.
     */
    static uint8_t frame[22];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_can_stats_t);  /* 16 */
    const uint8_t  len  = 1u + plen;                                /* 17 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_CAN_STATS;
    memcpy(&frame[3], tlm, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}

/* ---- jlink_send_lvc_tlm() -- Issue #613 ---- */
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm)
{
    /*
     * Frame: [STX][LEN][0x8B][4 bytes LVC][CRC_hi][CRC_lo][ETX]
     * LEN = 1 + 4 = 5; total = 10 bytes
     */
    static uint8_t frame[10];
    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_lvc_t);  /* 4 */
    const uint8_t  len  = 1u + plen;                          /* 5 */

    frame[0] = JLINK_STX;
    frame[1] = len;
    frame[2] = JLINK_TLM_LVC;
    memcpy(&frame[3], tlm, plen);

    uint16_t crc = crc16_xmodem(&frame[2], len);
    frame[3 + plen]     = (uint8_t)(crc >> 8);
    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
    frame[3 + plen + 2] = JLINK_ETX;

    jlink_tx_locked(frame, sizeof(frame));
}