Log.cpp

#include "Log.h"

#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#include "Platform.h"
#include "esp_log_write.h"
#include "esp_partition.h"
#include "freertos/FreeRTOS.h"
#include "freertos/portmacro.h"
#include "freertos/semphr.h"

namespace Log {
namespace {
constexpr size_t kLogRingCapacity = 4096;
constexpr size_t kFlashRecordSize = 256;
constexpr size_t kFlashHeaderSize = 12;
constexpr size_t kFlashPayloadCapacity = kFlashRecordSize - kFlashHeaderSize;
constexpr size_t kFlashSectorSize = 4096;
constexpr size_t kFlashRecordsPerSector = kFlashSectorSize / kFlashRecordSize;
constexpr uint32_t kFlashRecordMagic = 0x31474f4cUL;
constexpr uint32_t kFlashWriteIntervalMs = 250;
constexpr uint32_t kFlashPartialFlushIntervalMs = 2000;
constexpr size_t kFlashBatchRecords = 4;
// Legacy Bluetooth troubleshooting gate; normal builds keep saving while connected.
#ifndef APP_LOG_BT_FLASH_GATE
#define APP_LOG_BT_FLASH_GATE 0
#endif
RTC_DATA_ATTR uint8_t g_logRing[kLogRingCapacity] = {};
uint32_t g_ringOldestSequence = 0;
uint32_t g_ringWriteSequence = 0;
uint32_t g_flashCommitSequence = 0;
portMUX_TYPE g_logRingMux = portMUX_INITIALIZER_UNLOCKED;

const esp_partition_t* g_flashPartition = nullptr;
StaticSemaphore_t g_flashMutexStorage = {};
SemaphoreHandle_t g_flashMutex = nullptr;
bool g_flashReady = false;
uint16_t g_flashSlotCount = 0;
uint16_t g_flashNextSlot = 0;
uint16_t g_flashOldestSlot = 0;
uint16_t g_flashRecordCount = 0;
uint32_t g_flashOldestSequence = 0;
uint32_t g_flashNextSequence = 1;
size_t g_flashBytes = 0;
uint32_t g_lastFlashFlushMs = 0;
volatile bool g_flashWriteAllowed = true;
bool g_started = false;
vprintf_like_t g_previousVprintf = nullptr;

struct __attribute__((packed)) FlashRecordHeader {
  uint32_t magic;
  uint32_t sequence;
  uint16_t length;
  uint16_t crc;
};
static_assert(sizeof(FlashRecordHeader) == kFlashHeaderSize, "flash log header size");

bool sequenceNewer(uint32_t left, uint32_t right) {
  return static_cast<int32_t>(left - right) > 0;
}

uint16_t crc16(const uint8_t* data, size_t length) {
  uint16_t crc = 0xffff;
  if (data == nullptr) return crc;
  for (size_t i = 0; i < length; ++i) {
    crc ^= static_cast<uint16_t>(data[i]) << 8;
    for (uint8_t bit = 0; bit < 8; ++bit) {
      crc = (crc & 0x8000) != 0
          ? static_cast<uint16_t>((crc << 1) ^ 0x1021)
          : static_cast<uint16_t>(crc << 1);
    }
  }
  return crc;
}

void appendRing(const char* data, size_t length) {
  if (data == nullptr || length == 0) return;
  portENTER_CRITICAL(&g_logRingMux);
  if (length >= kLogRingCapacity) {
    data += length - kLogRingCapacity;
    length = kLogRingCapacity;
  }
  for (size_t i = 0; i < length; ++i) {
    g_logRing[g_ringWriteSequence % kLogRingCapacity] =
        static_cast<uint8_t>(data[i]);
    ++g_ringWriteSequence;
  }
  const uint32_t retained = g_ringWriteSequence - g_ringOldestSequence;
  if (retained > kLogRingCapacity) {
    g_ringOldestSequence = g_ringWriteSequence - kLogRingCapacity;
  }
  // If Flash was gated long enough to fill the ring, the overwritten prefix
  // cannot be persisted later. Advancing only this cursor preserves ordering.
  if (static_cast<int32_t>(g_flashCommitSequence - g_ringOldestSequence) < 0) {
    g_flashCommitSequence = g_ringOldestSequence;
  }
  portEXIT_CRITICAL(&g_logRingMux);
}

size_t pendingSize() {
  portENTER_CRITICAL(&g_logRingMux);
  const size_t size = g_ringWriteSequence - g_flashCommitSequence;
  portEXIT_CRITICAL(&g_logRingMux);
  return size;
}

bool flashWriteAllowed() {
  return __atomic_load_n(&g_flashWriteAllowed, __ATOMIC_ACQUIRE);
}

size_t copyPending(uint8_t* destination, size_t capacity, uint32_t* startSequence) {
  if (destination == nullptr || capacity == 0) return 0;
  portENTER_CRITICAL(&g_logRingMux);
  const uint32_t start = g_flashCommitSequence;
  const size_t available = g_ringWriteSequence - start;
  const size_t count = available < capacity ? available : capacity;
  for (size_t i = 0; i < count; ++i) {
    destination[i] = g_logRing[(start + i) % kLogRingCapacity];
  }
  if (startSequence != nullptr) *startSequence = start;
  portEXIT_CRITICAL(&g_logRingMux);
  return count;
}

void commitPending(uint32_t startSequence, size_t length) {
  portENTER_CRITICAL(&g_logRingMux);
  const uint32_t end = startSequence + static_cast<uint32_t>(length);
  if (static_cast<int32_t>(end - g_flashCommitSequence) > 0) {
    g_flashCommitSequence = end;
  }
  portEXIT_CRITICAL(&g_logRingMux);
}

int capturedVprintf(const char* format, va_list args) {
  char buffer[512];
  const int required = vsnprintf(buffer, sizeof(buffer), format, args);
  if (required <= 0) return required;
  const size_t length = static_cast<size_t>(required) < sizeof(buffer)
      ? static_cast<size_t>(required)
      : sizeof(buffer) - 1;
  // Emit exactly the same bytes that enter the canonical ring. Very long
  // single ESP_LOG fragments are deliberately truncated on both outputs.
  flockfile(stdout);
  const size_t written = fwrite(buffer, 1, length, stdout);
  fflush(stdout);
  appendRing(buffer, written);
  funlockfile(stdout);
  return static_cast<int>(written);
}

bool readRecord(uint16_t slot, FlashRecordHeader* header, uint8_t* payload) {
  if (!g_flashReady || g_flashPartition == nullptr || header == nullptr ||
      slot >= g_flashSlotCount) {
    return false;
  }
  const size_t offset = static_cast<size_t>(slot) * kFlashRecordSize;
  if (esp_partition_read(g_flashPartition, offset, header, sizeof(*header)) != ESP_OK ||
      header->magic != kFlashRecordMagic || header->length > kFlashPayloadCapacity) {
    return false;
  }
  if (header->length == 0) return true;
  if (payload == nullptr ||
      esp_partition_read(
          g_flashPartition, offset + sizeof(*header), payload, header->length) != ESP_OK) {
    return false;
  }
  return crc16(payload, header->length) == header->crc;
}

void scanFlashRing() {
  if (!g_flashReady || g_flashPartition == nullptr) return;
  const size_t slotCount = g_flashPartition->size / kFlashRecordSize;
  g_flashSlotCount = static_cast<uint16_t>(
      slotCount > UINT16_MAX ? UINT16_MAX : slotCount);
  if (g_flashSlotCount == 0) {
    g_flashReady = false;
    return;
  }

  bool haveRecord = false;
  uint16_t latestSlot = 0;
  uint16_t oldestSlot = 0;
  uint32_t latestSequence = 0;
  uint32_t oldestSequence = 0;
  uint16_t count = 0;
  size_t bytes = 0;
  uint8_t payload[kFlashPayloadCapacity];
  for (uint16_t slot = 0; slot < g_flashSlotCount; ++slot) {
    FlashRecordHeader header = {};
    if (!readRecord(slot, &header, payload)) continue;
    bytes += header.length;
    ++count;
    if (!haveRecord || sequenceNewer(header.sequence, latestSequence)) {
      latestSequence = header.sequence;
      latestSlot = slot;
    }
    if (!haveRecord || sequenceNewer(oldestSequence, header.sequence)) {
      oldestSequence = header.sequence;
      oldestSlot = slot;
    }
    haveRecord = true;
  }

  g_flashRecordCount = count;
  g_flashBytes = bytes;
  if (!haveRecord) {
    g_flashOldestSlot = 0;
    g_flashNextSlot = 0;
    g_flashOldestSequence = 0;
    g_flashNextSequence = 1;
    return;
  }
  g_flashOldestSlot = oldestSlot;
  g_flashOldestSequence = oldestSequence;
  g_flashNextSlot = static_cast<uint16_t>((latestSlot + 1) % g_flashSlotCount);
  g_flashNextSequence = latestSequence + 1;
}

bool slotIsBlank(uint16_t slot) {
  uint8_t bytes[kFlashHeaderSize];
  memset(bytes, 0, sizeof(bytes));
  if (esp_partition_read(
          g_flashPartition,
          static_cast<size_t>(slot) * kFlashRecordSize,
          bytes,
          sizeof(bytes)) != ESP_OK) {
    return false;
  }
  for (uint8_t value : bytes) {
    if (value != 0xff) return false;
  }
  return true;
}

bool prepareNextSlot() {
  if (!g_flashReady || g_flashPartition == nullptr || g_flashSlotCount == 0) {
    return false;
  }
  const uint16_t slot = g_flashNextSlot;
  const bool sectorBoundary = (slot % kFlashRecordsPerSector) == 0;
  if (!sectorBoundary && slotIsBlank(slot)) return true;

  const size_t sectorOffset =
      (static_cast<size_t>(slot) / kFlashRecordsPerSector) * kFlashSectorSize;
  if (esp_partition_erase_range(g_flashPartition, sectorOffset, kFlashSectorSize) != ESP_OK) {
    return false;
  }
  scanFlashRing();
  return g_flashNextSlot == slot || slotIsBlank(g_flashNextSlot);
}

bool flushBatch() {
  if (!g_flashReady || g_flashMutex == nullptr || !flashWriteAllowed() ||
      pendingSize() == 0) {
    return false;
  }
  if (xSemaphoreTake(g_flashMutex, portMAX_DELAY) != pdTRUE) {
    return false;
  }

  // setFlashWriteAllowed(false) may have raced with the initial check.  Do
  // not start a new erase/write after the Bluetooth side has been armed.
  if (!flashWriteAllowed()) {
    xSemaphoreGive(g_flashMutex);
    return false;
  }

  if (!prepareNextSlot()) {
    xSemaphoreGive(g_flashMutex);
    return false;
  }
  // One contiguous write, never crossing an erase-sector boundary. Keep the
  // existing record format so already saved history remains readable.
  uint8_t records[kFlashRecordSize * kFlashBatchRecords];
  const size_t sectorSlots = kFlashRecordsPerSector -
      (g_flashNextSlot % kFlashRecordsPerSector);
  const size_t maxRecords = sectorSlots < kFlashBatchRecords
      ? sectorSlots : kFlashBatchRecords;
  uint32_t startSequence = 0;
  const size_t payloadLength = copyPending(
      records, maxRecords * kFlashPayloadCapacity, &startSequence);
  if (payloadLength == 0) {
    xSemaphoreGive(g_flashMutex);
    return false;
  }
  const size_t recordCount =
      (payloadLength + kFlashPayloadCapacity - 1) / kFlashPayloadCapacity;
  // Expand backwards in place to avoid a second payload buffer on the stack.
  for (size_t index = recordCount; index > 0; --index) {
    const size_t i = index - 1;
    const size_t consumed = i * kFlashPayloadCapacity;
    const size_t remaining = payloadLength - consumed;
    const size_t length = remaining < kFlashPayloadCapacity
        ? remaining : kFlashPayloadCapacity;
    FlashRecordHeader header = {
        kFlashRecordMagic,
        g_flashNextSequence + static_cast<uint32_t>(i),
        static_cast<uint16_t>(length),
        crc16(records + consumed, length)};
    memmove(records + i * kFlashRecordSize + sizeof(header),
            records + consumed, length);
    memcpy(records + i * kFlashRecordSize, &header, sizeof(header));
    memset(records + i * kFlashRecordSize + sizeof(header) + length,
           0xff, kFlashPayloadCapacity - length);
  }
    const uint16_t slot = g_flashNextSlot;
    const esp_err_t writeResult = esp_partition_write(
        g_flashPartition,
        static_cast<size_t>(slot) * kFlashRecordSize,
        records,
        recordCount * kFlashRecordSize);
    if (writeResult == ESP_OK) {
      // The slot is either blank or was removed by prepareNextSlot's sector erase.
      if (g_flashRecordCount == 0) {
        g_flashOldestSlot = slot;
        g_flashOldestSequence = g_flashNextSequence;
      }
      g_flashRecordCount += static_cast<uint16_t>(recordCount);
      g_flashBytes += payloadLength;
      g_flashNextSlot = static_cast<uint16_t>((slot + recordCount) % g_flashSlotCount);
      g_flashNextSequence += static_cast<uint32_t>(recordCount);
      commitPending(startSequence, payloadLength);
    }
  xSemaphoreGive(g_flashMutex);
  if (writeResult != ESP_OK) {
    infof("Log flash write failed: %s", esp_err_to_name(writeResult));
  }
  return writeResult == ESP_OK;
}
}  // namespace

void begin() {
  if (g_started) return;
  portENTER_CRITICAL(&g_logRingMux);
  memset(g_logRing, 0, sizeof(g_logRing));
  g_ringOldestSequence = 0;
  g_ringWriteSequence = 0;
  g_flashCommitSequence = 0;
  portEXIT_CRITICAL(&g_logRingMux);

  g_flashPartition = esp_partition_find_first(
      ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, "log");
  g_flashReady = g_flashPartition != nullptr &&
      g_flashPartition->size >= kFlashSectorSize &&
      (g_flashPartition->size % kFlashSectorSize) == 0;
  g_flashSlotCount = g_flashReady
      ? static_cast<uint16_t>(g_flashPartition->size / kFlashRecordSize)
      : 0;
  g_flashNextSlot = 0;
  g_flashOldestSlot = 0;
  g_flashRecordCount = 0;
  g_flashOldestSequence = 0;
  g_flashNextSequence = 1;
  g_flashBytes = 0;
  g_lastFlashFlushMs = millis();
  __atomic_store_n(&g_flashWriteAllowed, true, __ATOMIC_RELEASE);
  g_flashMutex = xSemaphoreCreateMutexStatic(&g_flashMutexStorage);
  if (g_flashReady && g_flashMutex != nullptr &&
      xSemaphoreTake(g_flashMutex, portMAX_DELAY) == pdTRUE) {
    scanFlashRing();
    xSemaphoreGive(g_flashMutex);
  }
  g_previousVprintf = esp_log_set_vprintf(capturedVprintf);
  (void)g_previousVprintf;
  g_started = true;
}

void setFlashWriteAllowed(bool allowed) {
#if APP_LOG_BT_FLASH_GATE
  // Publish the gate before waiting for the mutex.  If a write is already in
  // progress, the wait below lets it finish; no subsequent write can start.
  __atomic_store_n(&g_flashWriteAllowed, allowed, __ATOMIC_RELEASE);
  if (!allowed && g_flashMutex != nullptr) {
    if (xSemaphoreTake(g_flashMutex, portMAX_DELAY) == pdTRUE) {
      xSemaphoreGive(g_flashMutex);
    }
  }
#else
  (void)allowed;
#endif
}

void info(const char* message) {
  char entry[256];
  snprintf(entry, sizeof(entry), "[%lu ms] %s",
           static_cast<unsigned long>(millis()),
           message != nullptr ? message : "");
  const size_t length = strlen(entry);
  flockfile(stdout);
  fwrite(entry, 1, length, stdout);
  fwrite("\n", 1, 1, stdout);
  fflush(stdout);
  char line[sizeof(entry) + 1];
  memcpy(line, entry, length);
  line[length] = '\n';
  appendRing(line, length + 1);
  funlockfile(stdout);
}

void infof(const char* format, ...) {
  char buffer[256];
  va_list args;
  va_start(args, format);
  vsnprintf(buffer, sizeof(buffer), format, args);
  va_end(args);
  info(buffer);
}

void service(uint32_t nowMs) {
  if (!g_flashReady || !flashWriteAllowed() ||
      pendingSize() == 0) return;
  const uint32_t elapsed = nowMs - g_lastFlashFlushMs;
  const size_t pending = pendingSize();
  if (pending < kLogRingCapacity / 2 && elapsed < kFlashWriteIntervalMs) return;
  if (pending < kFlashPayloadCapacity * kFlashBatchRecords &&
      elapsed < kFlashPartialFlushIntervalMs) {
    return;
  }
  if (flushBatch()) g_lastFlashFlushMs = nowMs;
}

void flushPending() {
  if (!g_flashReady || !flashWriteAllowed()) return;
  size_t attempts = 0;
  while (flashWriteAllowed() && pendingSize() != 0 &&
         attempts++ < 32) {
    if (!flushBatch()) break;
    g_lastFlashFlushMs = millis();
  }
}

size_t copyRing(char* destination, size_t capacity) {
  if (destination == nullptr || capacity == 0) return 0;
  portENTER_CRITICAL(&g_logRingMux);
  const size_t available = g_ringWriteSequence - g_ringOldestSequence;
  const size_t count = available < capacity - 1 ? available : capacity - 1;
  const uint32_t start = g_ringWriteSequence - static_cast<uint32_t>(count);
  for (size_t i = 0; i < count; ++i) {
    destination[i] = static_cast<char>(
        g_logRing[(start + static_cast<uint32_t>(i)) % kLogRingCapacity]);
  }
  destination[count] = '\0';
  portEXIT_CRITICAL(&g_logRingMux);
  return count;
}

bool beginFlashRead(FlashReadCursor* cursor) {
  if (cursor == nullptr) return false;
  *cursor = {};
  if (g_flashMutex == nullptr ||
      xSemaphoreTake(g_flashMutex, portMAX_DELAY) != pdTRUE) {
    return false;
  }
  portENTER_CRITICAL(&g_logRingMux);
  if (g_flashReady) {
    cursor->slot = g_flashOldestSlot;
    cursor->recordsRemaining = g_flashRecordCount;
    cursor->flashNextSequence = g_flashOldestSequence;
    cursor->flashEndSequence = g_flashNextSequence;
    cursor->ramNextSequence = g_flashCommitSequence;
  } else {
    cursor->flashNextSequence = 0;
    cursor->flashEndSequence = 0;
    cursor->ramNextSequence = g_ringOldestSequence;
  }
  cursor->ramEndSequence = g_ringWriteSequence;
  portEXIT_CRITICAL(&g_logRingMux);
  xSemaphoreGive(g_flashMutex);
  const bool haveData = cursor->recordsRemaining != 0 ||
      cursor->ramNextSequence != cursor->ramEndSequence;
  return haveData;
}

size_t readFlash(FlashReadCursor* cursor, char* destination, size_t capacity) {
  if (cursor == nullptr || destination == nullptr || capacity == 0 ||
      cursor->failed || cursor->recordsRemaining == 0 || !g_flashReady ||
      g_flashMutex == nullptr) {
    return 0;
  }

  size_t written = 0;
  while (written < capacity && cursor->recordsRemaining != 0) {
    if (cursor->payloadOffset >= cursor->payloadLength) {
      FlashRecordHeader header = {};
      if (xSemaphoreTake(g_flashMutex, portMAX_DELAY) != pdTRUE) {
        cursor->failed = true;
        break;
      }
      const bool valid = readRecord(cursor->slot, &header, cursor->payload);
      xSemaphoreGive(g_flashMutex);
      if (!valid || header.sequence != cursor->flashNextSequence) {
        cursor->failed = true;
        break;
      }
      cursor->payloadLength = header.length;
      cursor->payloadOffset = 0;
      if (cursor->payloadLength == 0) {
        cursor->slot = static_cast<uint16_t>((cursor->slot + 1) % g_flashSlotCount);
        ++cursor->flashNextSequence;
        --cursor->recordsRemaining;
        continue;
      }
    }

    const size_t available = cursor->payloadLength - cursor->payloadOffset;
    const size_t count = (capacity - written) < available
        ? capacity - written
        : available;
    memcpy(destination + written, cursor->payload + cursor->payloadOffset, count);
    written += count;
    cursor->payloadOffset = static_cast<uint16_t>(cursor->payloadOffset + count);
    if (cursor->payloadOffset >= cursor->payloadLength) {
      cursor->payloadOffset = 0;
      cursor->payloadLength = 0;
      cursor->slot = static_cast<uint16_t>((cursor->slot + 1) % g_flashSlotCount);
      ++cursor->flashNextSequence;
      --cursor->recordsRemaining;
    }
  }
  return written;
}

size_t readRam(FlashReadCursor* cursor, char* destination, size_t capacity) {
  if (cursor == nullptr || destination == nullptr || capacity == 0 ||
      cursor->failed || cursor->ramNextSequence == cursor->ramEndSequence) {
    return 0;
  }

  portENTER_CRITICAL(&g_logRingMux);
  if (static_cast<int32_t>(cursor->ramNextSequence - g_ringOldestSequence) < 0) {
    // The HTTP client was too slow and the snapshotted tail was overwritten.
    cursor->failed = true;
    portEXIT_CRITICAL(&g_logRingMux);
    return 0;
  }
  const size_t available = cursor->ramEndSequence - cursor->ramNextSequence;
  const size_t count = available < capacity ? available : capacity;
  for (size_t i = 0; i < count; ++i) {
    destination[i] = static_cast<char>(g_logRing[
        (cursor->ramNextSequence + static_cast<uint32_t>(i)) % kLogRingCapacity]);
  }
  cursor->ramNextSequence += static_cast<uint32_t>(count);
  portEXIT_CRITICAL(&g_logRingMux);
  return count;
}

void endFlashRead(FlashReadCursor* cursor) {
  if (cursor == nullptr) return;
  cursor->recordsRemaining = 0;
  cursor->ramNextSequence = cursor->ramEndSequence;
}

size_t flashBytes() {
  portENTER_CRITICAL(&g_logRingMux);
  const size_t bytes = g_flashBytes;
  portEXIT_CRITICAL(&g_logRingMux);
  return bytes;
}
}  // namespace Log

// Called only on A2DP opening failures, never from the PCM render/data path.
// tBTA_AV_STATUS is distinct from esp_err_t and the public disconnect reason.
extern "C" void app_bt_connection_failure(int event, int status) {
  Log::infof("A2DP stack failure: event=%s status=%d",
             event == 1 ? "BTA_AV_REJECT_EVT" : "BTA_AV_OPEN_EVT", status);
}