espilon-source/espilon_bot/components/mod_canbus/canbus_isotp.c

/*
 * canbus_isotp.c
 * ISO-TP (ISO 15765-2) transport layer implementation.
 *
 * Frame types:
 *   Single Frame (SF):       [0x0N | data...]          N = length (1-7)
 *   First Frame (FF):        [0x1H 0xLL | 6 bytes]     H:L = total length (up to 4095)
 *   Consecutive Frame (CF):  [0x2N | 7 bytes]           N = sequence (0-F, wrapping)
 *   Flow Control (FC):       [0x30 BS ST]               BS=block size, ST=separation time
 */
#include "sdkconfig.h"

#if defined(CONFIG_MODULE_CANBUS) && defined(CONFIG_CANBUS_ISO_TP)

#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "esp_timer.h"
#include "esp_log.h"

#include "canbus_isotp.h"
#include "canbus_driver.h"

#define TAG "CAN_ISOTP"

/* Max ISO-TP payload (12-bit length field) */
#define ISOTP_MAX_LEN   4095

/* Reassembly buffer (static — single concurrent transfer) */
static uint8_t s_reassembly[ISOTP_MAX_LEN];

/* Synchronization: RX callback puts frame here, isotp functions wait on semaphore */
static SemaphoreHandle_t s_rx_sem = NULL;
static can_frame_t       s_rx_frame;
static volatile uint32_t s_listen_id = 0;
static volatile bool     s_listening = false;

/* Previous RX callback to chain */
static can_rx_callback_t s_prev_cb  = NULL;
static void             *s_prev_ctx = NULL;

/* ============================================================
 * Internal RX callback for ISO-TP framing
 * ============================================================ */

static void isotp_rx_callback(const can_frame_t *frame, void *ctx)
{
    (void)ctx;

    /* If we're listening for a specific ID, capture it */
    if (s_listening && frame->id == s_listen_id) {
        s_rx_frame = *frame;
        if (s_rx_sem) xSemaphoreGive(s_rx_sem);
    }

    /* Chain to previous callback (sniff/record) */
    if (s_prev_cb) s_prev_cb(frame, s_prev_ctx);
}

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

static void isotp_init_once(void)
{
    if (!s_rx_sem) {
        s_rx_sem = xSemaphoreCreateBinary();
    }
}

/* Hook our callback, saving the previous one */
static void isotp_hook_rx(uint32_t listen_id)
{
    isotp_init_once();

    s_listen_id = listen_id;
    s_listening = true;

    /* Clear any pending semaphore */
    xSemaphoreTake(s_rx_sem, 0);
}

static void isotp_unhook_rx(void)
{
    s_listening = false;
    s_listen_id = 0;
}

/* Wait for a frame with the target ID, timeout in ms */
static bool wait_frame(can_frame_t *out, int timeout_ms)
{
    if (xSemaphoreTake(s_rx_sem, pdMS_TO_TICKS(timeout_ms)) == pdTRUE) {
        *out = s_rx_frame;
        return true;
    }
    return false;
}

/* Send a single CAN frame (helper) */
static bool send_frame(uint32_t id, const uint8_t *data, uint8_t dlc)
{
    can_frame_t f = {
        .id = id,
        .dlc = dlc,
        .extended = (id > 0x7FF),
        .rtr = false,
        .timestamp_us = 0,
    };
    memcpy(f.data, data, dlc);
    return can_driver_send(&f);
}

/* Send Flow Control frame: CTS (continue to send) */
static bool send_fc(uint32_t tx_id, uint8_t block_size, uint8_t st_min)
{
    uint8_t fc[8] = { 0x30, block_size, st_min, 0, 0, 0, 0, 0 };
    return send_frame(tx_id, fc, 8);
}

/* ============================================================
 * isotp_send — Send ISO-TP message
 * ============================================================ */

isotp_status_t isotp_send(uint32_t tx_id, uint32_t rx_id,
                          const uint8_t *data, size_t len,
                          int timeout_ms)
{
    if (!data || len == 0 || len > ISOTP_MAX_LEN) return ISOTP_ERROR;

    isotp_init_once();

    /* Single Frame: len <= 7 */
    if (len <= 7) {
        uint8_t sf[8] = { 0 };
        sf[0] = (uint8_t)(len & 0x0F);  /* PCI: 0x0N */
        memcpy(&sf[1], data, len);
        if (!send_frame(tx_id, sf, 8)) return ISOTP_ERROR;
        return ISOTP_OK;
    }

    /* Multi-frame: First Frame + wait FC + Consecutive Frames */

    /* Send First Frame */
    uint8_t ff[8] = { 0 };
    ff[0] = 0x10 | (uint8_t)((len >> 8) & 0x0F);
    ff[1] = (uint8_t)(len & 0xFF);
    memcpy(&ff[2], data, 6);
    if (!send_frame(tx_id, ff, 8)) return ISOTP_ERROR;

    /* Wait for Flow Control */
    isotp_hook_rx(rx_id);

    can_frame_t fc;
    if (!wait_frame(&fc, timeout_ms)) {
        isotp_unhook_rx();
        ESP_LOGW(TAG, "FC timeout from 0x%03lX", (unsigned long)rx_id);
        return ISOTP_TIMEOUT;
    }

    isotp_unhook_rx();

    /* Parse FC */
    if ((fc.data[0] & 0xF0) != 0x30) {
        ESP_LOGW(TAG, "Expected FC, got PCI 0x%02X", fc.data[0]);
        return ISOTP_ERROR;
    }

    uint8_t block_size = fc.data[1];  /* 0 = no limit */
    uint8_t st_min = fc.data[2];      /* Separation time in ms */

    /* Send Consecutive Frames */
    size_t offset = 6;  /* First 6 bytes already sent in FF */
    uint8_t seq = 1;
    uint8_t blocks_sent = 0;

    while (offset < len) {
        uint8_t cf[8] = { 0 };
        cf[0] = 0x20 | (seq & 0x0F);

        size_t chunk = len - offset;
        if (chunk > 7) chunk = 7;
        memcpy(&cf[1], &data[offset], chunk);

        if (!send_frame(tx_id, cf, 8)) return ISOTP_ERROR;

        offset += chunk;
        seq = (seq + 1) & 0x0F;
        blocks_sent++;

        /* Respect separation time */
        if (st_min > 0 && st_min <= 127) {
            vTaskDelay(pdMS_TO_TICKS(st_min));
        }

        /* Block size flow control */
        if (block_size > 0 && blocks_sent >= block_size && offset < len) {
            blocks_sent = 0;
            isotp_hook_rx(rx_id);
            if (!wait_frame(&fc, timeout_ms)) {
                isotp_unhook_rx();
                return ISOTP_TIMEOUT;
            }
            isotp_unhook_rx();
            if ((fc.data[0] & 0xF0) != 0x30) return ISOTP_ERROR;
            block_size = fc.data[1];
            st_min = fc.data[2];
        }
    }

    return ISOTP_OK;
}

/* ============================================================
 * isotp_recv — Receive ISO-TP message
 * ============================================================ */

isotp_status_t isotp_recv(uint32_t rx_id,
                          uint8_t *buf, size_t buf_cap, size_t *out_len,
                          int timeout_ms)
{
    if (!buf || buf_cap == 0 || !out_len) return ISOTP_ERROR;
    *out_len = 0;

    isotp_hook_rx(rx_id);

    can_frame_t frame;
    if (!wait_frame(&frame, timeout_ms)) {
        isotp_unhook_rx();
        return ISOTP_TIMEOUT;
    }

    uint8_t pci_type = frame.data[0] & 0xF0;

    /* Single Frame */
    if (pci_type == 0x00) {
        isotp_unhook_rx();
        size_t sf_len = frame.data[0] & 0x0F;
        if (sf_len == 0 || sf_len > 7 || sf_len > buf_cap) return ISOTP_ERROR;
        memcpy(buf, &frame.data[1], sf_len);
        *out_len = sf_len;
        return ISOTP_OK;
    }

    /* First Frame */
    if (pci_type != 0x10) {
        isotp_unhook_rx();
        ESP_LOGW(TAG, "Expected SF/FF, got PCI 0x%02X", frame.data[0]);
        return ISOTP_ERROR;
    }

    size_t total_len = ((size_t)(frame.data[0] & 0x0F) << 8) | frame.data[1];
    if (total_len > buf_cap || total_len > ISOTP_MAX_LEN) {
        isotp_unhook_rx();
        return ISOTP_OVERFLOW;
    }

    /* Copy first 6 data bytes from FF */
    size_t received = (total_len < 6) ? total_len : 6;
    memcpy(buf, &frame.data[2], received);

    /* We need to figure out the TX ID to send FC back.
     * Convention: if rx_id is in 0x7E8-0x7EF range, tx_id = rx_id - 8.
     * For functional requests, FC goes to rx_id - 8.
     * Caller should use isotp_request() for proper bidirectional comms. */
    uint32_t fc_tx_id = (rx_id >= 0x7E8 && rx_id <= 0x7EF)
                      ? (rx_id - 8)
                      : (rx_id - 1);

    /* Send Flow Control: continue, no block limit, 0ms separation */
    send_fc(fc_tx_id, 0, 0);

    /* Receive Consecutive Frames */
    uint8_t expected_seq = 1;
    while (received < total_len) {
        if (!wait_frame(&frame, timeout_ms)) {
            isotp_unhook_rx();
            return ISOTP_TIMEOUT;
        }

        if ((frame.data[0] & 0xF0) != 0x20) {
            isotp_unhook_rx();
            ESP_LOGW(TAG, "Expected CF, got PCI 0x%02X", frame.data[0]);
            return ISOTP_ERROR;
        }

        uint8_t seq = frame.data[0] & 0x0F;
        if (seq != (expected_seq & 0x0F)) {
            ESP_LOGW(TAG, "CF seq mismatch: expected %u, got %u",
                     expected_seq & 0x0F, seq);
        }
        expected_seq++;

        size_t chunk = total_len - received;
        if (chunk > 7) chunk = 7;
        memcpy(&buf[received], &frame.data[1], chunk);
        received += chunk;
    }

    isotp_unhook_rx();
    *out_len = total_len;
    return ISOTP_OK;
}

/* ============================================================
 * isotp_request — Send + Receive (UDS request-response pattern)
 * ============================================================ */

isotp_status_t isotp_request(uint32_t tx_id, uint32_t rx_id,
                             const uint8_t *req, size_t req_len,
                             uint8_t *resp, size_t resp_cap, size_t *resp_len,
                             int timeout_ms)
{
    if (!resp || !resp_len) return ISOTP_ERROR;
    *resp_len = 0;

    isotp_init_once();

    /* For request-response, we need to listen before sending
     * (the response may come very quickly after the request) */
    isotp_hook_rx(rx_id);

    /* Send request */
    isotp_status_t st;

    if (req_len <= 7) {
        /* Single frame — send directly and wait for response */
        uint8_t sf[8] = { 0 };
        sf[0] = (uint8_t)(req_len & 0x0F);
        memcpy(&sf[1], req, req_len);
        if (!send_frame(tx_id, sf, 8)) {
            isotp_unhook_rx();
            return ISOTP_ERROR;
        }
    } else {
        /* Multi-frame send — unhook first since isotp_send hooks itself */
        isotp_unhook_rx();
        st = isotp_send(tx_id, rx_id, req, req_len, timeout_ms);
        if (st != ISOTP_OK) return st;
        isotp_hook_rx(rx_id);
    }

    /* Wait for response (may be SF or FF+CF) */
    can_frame_t frame;
    if (!wait_frame(&frame, timeout_ms)) {
        isotp_unhook_rx();
        return ISOTP_TIMEOUT;
    }

    uint8_t pci_type = frame.data[0] & 0xF0;

    /* Single Frame response */
    if (pci_type == 0x00) {
        isotp_unhook_rx();
        size_t sf_len = frame.data[0] & 0x0F;
        if (sf_len == 0 || sf_len > 7 || sf_len > resp_cap) return ISOTP_ERROR;
        memcpy(resp, &frame.data[1], sf_len);
        *resp_len = sf_len;
        return ISOTP_OK;
    }

    /* First Frame response */
    if (pci_type == 0x10) {
        size_t total_len = ((size_t)(frame.data[0] & 0x0F) << 8) | frame.data[1];
        if (total_len > resp_cap || total_len > ISOTP_MAX_LEN) {
            isotp_unhook_rx();
            return ISOTP_OVERFLOW;
        }

        size_t received = (total_len < 6) ? total_len : 6;
        memcpy(resp, &frame.data[2], received);

        /* Send FC */
        send_fc(tx_id, 0, 0);

        /* Receive CFs */
        uint8_t expected_seq = 1;
        while (received < total_len) {
            if (!wait_frame(&frame, timeout_ms)) {
                isotp_unhook_rx();
                return ISOTP_TIMEOUT;
            }
            if ((frame.data[0] & 0xF0) != 0x20) {
                isotp_unhook_rx();
                return ISOTP_ERROR;
            }
            expected_seq++;

            size_t chunk = total_len - received;
            if (chunk > 7) chunk = 7;
            memcpy(&resp[received], &frame.data[1], chunk);
            received += chunk;
        }

        isotp_unhook_rx();
        *resp_len = total_len;
        return ISOTP_OK;
    }

    isotp_unhook_rx();
    ESP_LOGW(TAG, "Unexpected PCI type 0x%02X in response", frame.data[0]);
    return ISOTP_ERROR;
}

/* ============================================================
 * Install ISO-TP RX hook into the CAN driver
 * ============================================================ */

void isotp_install_hook(void)
{
    isotp_init_once();
    /* Save the current callback so we can chain to it */
    can_driver_get_rx_callback(&s_prev_cb, &s_prev_ctx);
    can_driver_set_rx_callback(isotp_rx_callback, NULL);
}

#endif /* CONFIG_MODULE_CANBUS && CONFIG_CANBUS_ISO_TP */