MicrOBU/microbu-esp32c5/firmware/main/serial_link.cpp

#include "serial_link.hpp"
#include <driver/usb_serial_jtag.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
#include <cstdarg>
#include <cstdio>
#include <cstring>

namespace microbu::serial {
namespace {
std::uint16_t crc16_ccitt_false_step(std::uint16_t crc, std::uint8_t octet) {
    crc ^= static_cast<std::uint16_t>(octet) << 8;
    for (int bit = 0; bit < 8; ++bit) crc = (crc & 0x8000) ? static_cast<std::uint16_t>((crc << 1) ^ 0x1021) : static_cast<std::uint16_t>(crc << 1);
    return crc;
}
}

std::uint16_t crc16_ccitt_false(const std::uint8_t* data, std::size_t length) {
    std::uint16_t crc = 0xFFFF;
    for (std::size_t i = 0; i < length; ++i) crc = crc16_ccitt_false_step(crc, data[i]);
    return crc;
}

Bytes encode_frame(FrameType type, const Bytes& payload) {
    Bytes out;
    out.reserve(payload.size() + 7);
    out.push_back(0xAA); out.push_back(0x55);
    out.push_back(static_cast<std::uint8_t>(type));
    out.push_back(payload.size() & 0xFF); out.push_back(payload.size() >> 8);
    out.insert(out.end(), payload.begin(), payload.end());
    const auto crc = crc16_ccitt_false(out.data() + 2, out.size() - 2);
    out.push_back(crc & 0xFF); out.push_back(crc >> 8);
    return out;
}

void Decoder::feed(const std::uint8_t* data, std::size_t length, const Handler& handler) {
    for (std::size_t i = 0; i < length; ++i) {
        const std::uint8_t b = data[i];
        switch (state_) {
        case State::SYNC0: state_ = (b == 0xAA) ? State::SYNC1 : State::SYNC0; break;
        case State::SYNC1: state_ = (b == 0x55) ? State::TYPE : (b == 0xAA ? State::SYNC1 : State::SYNC0); break;
        case State::TYPE: type_ = b; state_ = State::LEN_LO; break;
        case State::LEN_LO: length_ = b; state_ = State::LEN_HI; break;
        case State::LEN_HI:
            length_ |= static_cast<std::uint16_t>(b) << 8;
            payload_.clear();
            if (length_ > maximum_payload) state_ = State::SYNC0; // untrustworthy boundary: resync
            else state_ = length_ == 0 ? State::CRC_LO : State::PAYLOAD;
            break;
        case State::PAYLOAD:
            payload_.push_back(b);
            if (payload_.size() >= length_) state_ = State::CRC_LO;
            break;
        case State::CRC_LO: crc_ = b; state_ = State::CRC_HI; break;
        case State::CRC_HI: {
            crc_ |= static_cast<std::uint16_t>(b) << 8;
            // CRC over head then payload without concatenating them: same running value as encode_frame.
            const std::uint8_t head[3] = {type_, static_cast<std::uint8_t>(length_ & 0xFF), static_cast<std::uint8_t>(length_ >> 8)};
            std::uint16_t crc = 0xFFFF;
            for (auto octet : head) crc = crc16_ccitt_false_step(crc, octet);
            for (auto octet : payload_) crc = crc16_ccitt_false_step(crc, octet);
            if (crc == crc_) { ++frames_; handler(Frame {static_cast<FrameType>(type_), payload_}); }
            else ++crc_errors_;
            state_ = State::SYNC0;
            break;
        }
        }
    }
}

namespace {
SemaphoreHandle_t writer_lock = nullptr;
Counters the_counters;
Decoder::Handler frame_handler;
vprintf_like_t previous_vprintf = nullptr;

bool write_all(const Bytes& frame) {
    std::size_t sent = 0;
    while (sent < frame.size()) {
        const int count = usb_serial_jtag_write_bytes(frame.data() + sent, frame.size() - sent, pdMS_TO_TICKS(500));
        if (count <= 0) return false; // the host detects an incomplete frame by CRC
        sent += count;
    }
    return true;
}

// ESP_LOG sink: one LOG frame per call, never a raw write into the frame stream.
int log_to_frame(const char* format, va_list args) {
    char line[256];
    const int length = std::vsnprintf(line, sizeof line, format, args);
    if (length <= 0) return 0;
    std::size_t n = static_cast<std::size_t>(length) < sizeof line ? length : sizeof line - 1;
    while (n > 0 && (line[n - 1] == '\n' || line[n - 1] == '\r')) --n;
    if (n == 0) return length;
    // esp_log colour escapes are stripped by the emulator; keep the line verbatim otherwise.
    write(FrameType::LOG, Bytes(line, line + n));
    return length;
}

void reader_task(void*) {
    Decoder decoder;
    std::uint8_t buffer[512];
    for (;;) {
        const int count = usb_serial_jtag_read_bytes(buffer, sizeof buffer, pdMS_TO_TICKS(20));
        if (count > 0) {
            decoder.feed(buffer, static_cast<std::size_t>(count), frame_handler);
            the_counters.frames = decoder.frames();
            the_counters.crc_errors = decoder.crc_errors();
        }
    }
}
}

void start(const Decoder::Handler& on_frame) {
    frame_handler = on_frame;
    writer_lock = xSemaphoreCreateMutex();
    usb_serial_jtag_driver_config_t config {};
    config.rx_buffer_size = 8192;
    config.tx_buffer_size = 8192;
    ESP_ERROR_CHECK(usb_serial_jtag_driver_install(&config));
    previous_vprintf = esp_log_set_vprintf(log_to_frame);
    xTaskCreate(reader_task, "link_rx", 6144, nullptr, 12, nullptr);
}

bool write(FrameType type, const Bytes& payload) {
    if (payload.size() > maximum_payload || !writer_lock) return false;
    const auto frame = encode_frame(type, payload);
    if (xSemaphoreTake(writer_lock, pdMS_TO_TICKS(1000)) != pdTRUE) { ++the_counters.write_failures; return false; }
    const bool ok = write_all(frame);
    xSemaphoreGive(writer_lock);
    if (!ok) ++the_counters.write_failures;
    return ok;
}

Counters counters() { return the_counters; }

} // namespace microbu::serial