roro9stack/src/platform/console.cpp

#include "platform/console.h"

#include <Arduino.h>

#include <esp_log.h>
#include <freertos/FreeRTOS.h>

#include <algorithm>
#include <cstdio>

namespace roro {

Console console;

namespace {

// Any task may write (ESP-IDF logs come from everywhere), so the ring is behind a spinlock, held
// only for the copy.
portMUX_TYPE ringLock = portMUX_INITIALIZER_UNLOCKED;
vprintf_like_t espLogNext = nullptr;

int teeEspLog(const char* format, va_list args) {
    char line[200];
    va_list copy;
    va_copy(copy, args);
    int n = vsnprintf(line, sizeof line, format, copy);
    va_end(copy);
    if (n > 0) console.toRing(reinterpret_cast<const uint8_t*>(line), std::min<size_t>(n, sizeof line - 1));
    return espLogNext ? espLogNext(format, args) : n;  // the serial port, as before
}

}  // namespace

void Console::captureEspLogs() {
    if (!espLogNext) espLogNext = esp_log_set_vprintf(teeEspLog);  // once: it stays, and costs nothing with the ring closed
}

namespace {

// One ring: the last kRingBytes written, and how many were written in all. Under ringLock.
bool openOne(uint8_t*& ring, uint32_t& head) {
    if (ring) return true;
    auto* fresh = static_cast<uint8_t*>(malloc(Console::kRingBytes));
    if (!fresh) return false;
    portENTER_CRITICAL(&ringLock);
    head = 0;
    ring = fresh;
    portEXIT_CRITICAL(&ringLock);
    return true;
}

void closeOne(uint8_t*& ring) {
    portENTER_CRITICAL(&ringLock);
    uint8_t* old = ring;
    ring = nullptr;
    portEXIT_CRITICAL(&ringLock);
    free(old);
}

void writeOne(uint8_t* ring, uint32_t& head, const uint8_t* data, size_t len) {  // inside the lock
    if (!ring) return;
    size_t at = head % Console::kRingBytes;
    size_t first = std::min(len, Console::kRingBytes - at);
    memcpy(ring + at, data, first);
    memcpy(ring, data + first, len - first);
    head += len;
}

size_t readOne(const uint8_t* ring, uint32_t head, uint32_t& pos, uint8_t* out, size_t max, uint32_t& skipped) {
    portENTER_CRITICAL(&ringLock);
    size_t n = 0;
    skipped = 0;
    if (ring) {
        uint32_t from = head > Console::kRingBytes ? head - Console::kRingBytes : 0;
        skipped = pos < from ? from - pos : 0;
        if (pos < from) pos = from;
        n = std::min<size_t>(max, head - pos);
        size_t at = pos % Console::kRingBytes;
        size_t first = std::min(n, Console::kRingBytes - at);
        memcpy(out, ring + at, first);
        memcpy(out + first, ring, n - first);
        pos += n;
    }
    portEXIT_CRITICAL(&ringLock);
    return n;
}

}  // namespace

bool Console::openRing() { return openOne(ring_, head_); }
void Console::closeRing() { closeOne(ring_); }
bool Console::openShellRing() { return openOne(shellRing_, shellHead_); }
void Console::closeShellRing() { closeOne(shellRing_); }

void Console::toRing(const uint8_t* data, size_t len) {
    if (len > kRingBytes) {
        data += len - kRingBytes;  // only the tail can fit
        len = kRingBytes;
    }
    portENTER_CRITICAL(&ringLock);
    writeOne(ring_, head_, data, len);
    writeOne(shellRing_, shellHead_, data, len);
    portEXIT_CRITICAL(&ringLock);
}

uint32_t Console::oldest() const {
    portENTER_CRITICAL(&ringLock);
    uint32_t pos = head_ > kRingBytes ? head_ - kRingBytes : 0;
    portEXIT_CRITICAL(&ringLock);
    return pos;
}

size_t Console::readSince(uint32_t& pos, uint8_t* out, size_t max, uint32_t& skipped) {
    return readOne(ring_, head_, pos, out, max, skipped);
}

size_t Console::readShellSince(uint32_t& pos, uint8_t* out, size_t max, uint32_t& skipped) {
    return readOne(shellRing_, shellHead_, pos, out, max, skipped);
}

size_t Console::write(const uint8_t* data, size_t len) {
    toRing(data, len);
    // Never wait for the USB host. One that's attached but not reading (a VM, a closed terminal)
    // would stall the caller for up to 2 s per write while HWCDC retries; the ring keeps it anyway.
    if (Serial.availableForWrite() >= static_cast<int>(len)) Serial.write(data, len);
    return len;
}

}  // namespace roro