Introduce a PcmBuffer abstraction for handling source draining

7 files changed, 263 insertions, 112 deletions

diff --git a/src/drivers/CMakeLists.txt b/src/drivers/CMakeLists.txt
index 91534edb..33d25894 100644
--- a/src/drivers/CMakeLists.txt
+++ b/src/drivers/CMakeLists.txt
@@ -5,7 +5,7 @@
 idf_component_register(
   SRCS "touchwheel.cpp" "i2s_dac.cpp" "gpios.cpp" "adc.cpp" "storage.cpp"
   "i2c.cpp" "bluetooth.cpp" "spi.cpp" "display.cpp" "display_init.cpp"
-  "samd.cpp" "wm8523.cpp" "nvs.cpp" "haptics.cpp" "spiffs.cpp"
+  "samd.cpp" "wm8523.cpp" "nvs.cpp" "haptics.cpp" "spiffs.cpp" "pcm_buffer.cpp"
   INCLUDE_DIRS "include"
   REQUIRES "esp_adc" "fatfs" "result" "lvgl" "nvs_flash" "spiffs" "bt"
   "tasks" "tinyfsm" "util" "libcppbor")
diff --git a/src/drivers/bluetooth.cpp b/src/drivers/bluetooth.cpp
index 4880150f..140a29d5 100644
--- a/src/drivers/bluetooth.cpp
+++ b/src/drivers/bluetooth.cpp
@@ -29,6 +29,7 @@
 
 #include "drivers/bluetooth_types.hpp"
 #include "drivers/nvs.hpp"
+#include "drivers/pcm_buffer.hpp"
 #include "memory_resource.hpp"
 #include "tasks.hpp"
 
@@ -36,9 +37,8 @@ namespace drivers {
 
 [[maybe_unused]] static constexpr char kTag[] = "bluetooth";
 
-DRAM_ATTR static StreamBufferHandle_t sStream = nullptr;
+DRAM_ATTR static PcmBuffer* sStream = nullptr;
 DRAM_ATTR static std::atomic<float> sVolumeFactor = 1.f;
-DRAM_ATTR static std::atomic<uint32_t> sSamplesUsed = 0;
 
 static tasks::WorkerPool* sBgWorker;
 
@@ -68,27 +68,21 @@ IRAM_ATTR auto a2dp_data_cb(uint8_t* buf, int32_t buf_size) -> int32_t {
   if (buf == nullptr || buf_size <= 0) {
     return 0;
   }
-  StreamBufferHandle_t stream = sStream;
+  PcmBuffer* stream = sStream;
   if (stream == nullptr) {
     return 0;
   }
-  size_t bytes_received = xStreamBufferReceive(stream, buf, buf_size, 0);
 
-  size_t samples_received = bytes_received / 2;
-  if (UINT32_MAX - sSamplesUsed < samples_received) {
-    sSamplesUsed = samples_received - (UINT32_MAX - sSamplesUsed);
-  } else {
-    sSamplesUsed += samples_received;
-  }
+  int16_t* samples = reinterpret_cast<int16_t*>(buf);
+  stream->receive({samples, static_cast<size_t>(buf_size / 2)}, false);
 
   // Apply software volume scaling.
-  int16_t* samples = reinterpret_cast<int16_t*>(buf);
   float factor = sVolumeFactor.load();
-  for (size_t i = 0; i < bytes_received / 2; i++) {
+  for (size_t i = 0; i < buf_size / 2; i++) {
     samples[i] *= factor;
   }
 
-  return bytes_received;
+  return buf_size;
 }
 
 Bluetooth::Bluetooth(NvsStorage& storage, tasks::WorkerPool& bg_worker) {
@@ -159,7 +153,7 @@ auto Bluetooth::PreferredDevice() -> std::optional<bluetooth::MacAndName> {
   return bluetooth::BluetoothState::preferred_device();
 }
 
-auto Bluetooth::SetSource(StreamBufferHandle_t src) -> void {
+auto Bluetooth::SetSource(PcmBuffer* src) -> void {
   auto lock = bluetooth::BluetoothState::lock();
   if (src == bluetooth::BluetoothState::source()) {
     return;
@@ -173,10 +167,6 @@ auto Bluetooth::SetVolumeFactor(float f) -> void {
   sVolumeFactor = f;
 }
 
-auto Bluetooth::SamplesUsed() -> uint32_t {
-  return sSamplesUsed;
-}
-
 auto Bluetooth::SetEventHandler(std::function<void(bluetooth::Event)> cb)
     -> void {
   auto lock = bluetooth::BluetoothState::lock();
@@ -333,7 +323,7 @@ std::optional<MacAndName> BluetoothState::sPreferredDevice_{};
 std::optional<MacAndName> BluetoothState::sConnectingDevice_{};
 int BluetoothState::sConnectAttemptsRemaining_{0};
 
-std::atomic<StreamBufferHandle_t> BluetoothState::sSource_;
+std::atomic<PcmBuffer*> BluetoothState::sSource_;
 std::function<void(Event)> BluetoothState::sEventHandler_;
 
 auto BluetoothState::Init(NvsStorage& storage) -> void {
@@ -362,11 +352,11 @@ auto BluetoothState::preferred_device(std::optional<MacAndName> addr) -> void {
   sPreferredDevice_ = addr;
 }
 
-auto BluetoothState::source() -> StreamBufferHandle_t {
+auto BluetoothState::source() -> PcmBuffer* {
   return sSource_.load();
 }
 
-auto BluetoothState::source(StreamBufferHandle_t src) -> void {
+auto BluetoothState::source(PcmBuffer* src) -> void {
   sSource_.store(src);
 }
 
@@ -409,7 +399,7 @@ auto BluetoothState::connect(const MacAndName& dev) -> bool {
            dev.mac[5]);
   if (esp_a2d_source_connect(sConnectingDevice_->mac.data()) != ESP_OK) {
     ESP_LOGI(kTag, "Connecting failed...");
-    if (sConnectAttemptsRemaining_>1) {
+    if (sConnectAttemptsRemaining_ > 1) {
       ESP_LOGI(kTag, "Will retry.");
     }
   }
diff --git a/src/drivers/i2s_dac.cpp b/src/drivers/i2s_dac.cpp
index e5efe198..b479d572 100644
--- a/src/drivers/i2s_dac.cpp
+++ b/src/drivers/i2s_dac.cpp
@@ -17,6 +17,7 @@
 #include "driver/i2s_common.h"
 #include "driver/i2s_std.h"
 #include "driver/i2s_types.h"
+#include "drivers/pcm_buffer.hpp"
 #include "esp_attr.h"
 #include "esp_err.h"
 #include "esp_log.h"
@@ -37,7 +38,43 @@ namespace drivers {
 [[maybe_unused]] static const char* kTag = "i2s_dac";
 static const i2s_port_t kI2SPort = I2S_NUM_0;
 
-auto I2SDac::create(IGpios& expander) -> std::optional<I2SDac*> {
+DRAM_ATTR static volatile bool sSwapWords = false;
+
+extern "C" IRAM_ATTR auto callback(i2s_chan_handle_t handle,
+                                   i2s_event_data_t* event,
+                                   void* user_ctx) -> bool {
+  if (event == nullptr || user_ctx == nullptr) {
+    return false;
+  }
+  if (event->data == nullptr || event->size == 0) {
+    return false;
+  }
+  assert(event->size % 4 == 0);
+
+  uint8_t* buf = *reinterpret_cast<uint8_t**>(event->data);
+  auto* src = reinterpret_cast<PcmBuffer*>(user_ctx);
+
+  BaseType_t ret =
+      src->receive({reinterpret_cast<int16_t*>(buf), event->size / 2}, true);
+
+  // The ESP32's I2S peripheral has a different endianness to its processors.
+  // ESP-IDF handles this difference for stereo channels, but not for mono
+  // channels. We therefore sometimes need to swap each pair of words as they're
+  // written to the DMA buffer.
+  if (sSwapWords) {
+    uint16_t* buf_as_words = reinterpret_cast<uint16_t*>(buf);
+    for (size_t i = 0; i + 1 < event->size / 2; i += 2) {
+      uint16_t temp = buf_as_words[i];
+      buf_as_words[i] = buf_as_words[i + 1];
+      buf_as_words[i + 1] = temp;
+    }
+  }
+
+  return ret;
+}
+
+auto I2SDac::create(IGpios& expander, PcmBuffer& buf)
+    -> std::optional<I2SDac*> {
   i2s_chan_handle_t i2s_handle;
   i2s_chan_config_t channel_config{
       .id = kI2SPort,
@@ -52,7 +89,8 @@ auto I2SDac::create(IGpios& expander) -> std::optional<I2SDac*> {
 
   // First, instantiate the instance so it can do all of its power on
   // configuration.
-  std::unique_ptr<I2SDac> dac = std::make_unique<I2SDac>(expander, i2s_handle);
+  std::unique_ptr<I2SDac> dac =
+      std::make_unique<I2SDac>(expander, buf, i2s_handle);
 
   // Whilst we wait for the initial boot, we can work on installing the I2S
   // driver.
@@ -78,11 +116,20 @@ auto I2SDac::create(IGpios& expander) -> std::optional<I2SDac*> {
     return {};
   }
 
+  i2s_event_callbacks_t callbacks{
+      .on_recv = NULL,
+      .on_recv_q_ovf = NULL,
+      .on_sent = callback,
+      .on_send_q_ovf = NULL,
+  };
+  i2s_channel_register_event_callback(i2s_handle, &callbacks, &buf);
+
   return dac.release();
 }
 
-I2SDac::I2SDac(IGpios& gpio, i2s_chan_handle_t i2s_handle)
+I2SDac::I2SDac(IGpios& gpio, PcmBuffer& buf, i2s_chan_handle_t i2s_handle)
     : gpio_(gpio),
+      buffer_(buf),
       i2s_handle_(i2s_handle),
       i2s_active_(false),
       clock_config_(I2S_STD_CLK_DEFAULT_CONFIG(48000)),
@@ -141,8 +188,6 @@ auto I2SDac::SetPaused(bool paused) -> void {
   }
 }
 
-DRAM_ATTR static volatile bool sSwapWords = false;
-
 auto I2SDac::Reconfigure(Channels ch, BitsPerSample bps, SampleRate rate)
     -> void {
   std::lock_guard<std::mutex> lock(configure_mutex_);
@@ -217,79 +262,6 @@ auto I2SDac::WriteData(const std::span<const std::byte>& data) -> void {
   }
 }
 
-DRAM_ATTR static volatile uint32_t sSamplesRead = 0;
-
-extern "C" IRAM_ATTR auto callback(i2s_chan_handle_t handle,
-                                   i2s_event_data_t* event,
-                                   void* user_ctx) -> bool {
-  if (event == nullptr || user_ctx == nullptr) {
-    return false;
-  }
-  if (event->data == nullptr || event->size == 0) {
-    return false;
-  }
-  assert(event->size % 4 == 0);
-
-  uint8_t* buf = *reinterpret_cast<uint8_t**>(event->data);
-  auto src = reinterpret_cast<StreamBufferHandle_t>(user_ctx);
-
-  BaseType_t ret = false;
-  size_t bytes_written =
-      xStreamBufferReceiveFromISR(src, buf, event->size, &ret);
-
-  // Assume 16 bit samples.
-  size_t samples = bytes_written / 2;
-  if (UINT32_MAX - sSamplesRead < samples) {
-    sSamplesRead = samples - (UINT32_MAX - sSamplesRead);
-  } else {
-    sSamplesRead = sSamplesRead + samples;
-  }
-
-  // The ESP32's I2S peripheral has a different endianness to its processors.
-  // ESP-IDF handles this difference for stereo channels, but not for mono
-  // channels. We therefore sometimes need to swap each pair of words as they're
-  // written to the DMA buffer.
-  if (sSwapWords) {
-    uint16_t* buf_as_words = reinterpret_cast<uint16_t*>(buf);
-    for (size_t i = 0; i + 1 < bytes_written / 2; i += 2) {
-      uint16_t temp = buf_as_words[i];
-      buf_as_words[i] = buf_as_words[i + 1];
-      buf_as_words[i + 1] = temp;
-    }
-  }
-
-  // If we ran out of data, then make sure we clear out the DMA buffers rather
-  // than continuing to repreat the last few samples.
-  if (bytes_written < event->size) {
-    std::memset(buf + bytes_written, 0, event->size - bytes_written);
-  }
-
-  return ret;
-}
-
-auto I2SDac::SetSource(StreamBufferHandle_t buffer) -> void {
-  if (i2s_active_) {
-    ESP_ERROR_CHECK(i2s_channel_disable(i2s_handle_));
-  }
-  i2s_event_callbacks_t callbacks{
-      .on_recv = NULL,
-      .on_recv_q_ovf = NULL,
-      .on_sent = NULL,
-      .on_send_q_ovf = NULL,
-  };
-  if (buffer != nullptr) {
-    callbacks.on_sent = &callback;
-  }
-  i2s_channel_register_event_callback(i2s_handle_, &callbacks, buffer);
-  if (i2s_active_) {
-    ESP_ERROR_CHECK(i2s_channel_enable(i2s_handle_));
-  }
-}
-
-auto I2SDac::SamplesUsed() -> uint32_t {
-  return sSamplesRead;
-}
-
 auto I2SDac::set_channel(bool enabled) -> void {
   if (i2s_active_ == enabled) {
     return;
diff --git a/src/drivers/include/drivers/bluetooth.hpp b/src/drivers/include/drivers/bluetooth.hpp
index ad61fcc1..030907d9 100644
--- a/src/drivers/include/drivers/bluetooth.hpp
+++ b/src/drivers/include/drivers/bluetooth.hpp
@@ -14,6 +14,7 @@
 #include <stdint.h>
 #include "drivers/bluetooth_types.hpp"
 #include "drivers/nvs.hpp"
+#include "drivers/pcm_buffer.hpp"
 #include "esp_a2dp_api.h"
 #include "esp_avrc_api.h"
 #include "esp_gap_bt_api.h"
@@ -42,9 +43,8 @@ class Bluetooth {
   auto SetPreferredDevice(std::optional<bluetooth::MacAndName> dev) -> void;
   auto PreferredDevice() -> std::optional<bluetooth::MacAndName>;
 
-  auto SetSource(StreamBufferHandle_t) -> void;
+  auto SetSource(PcmBuffer*) -> void;
   auto SetVolumeFactor(float) -> void;
-  auto SamplesUsed() -> uint32_t;
 
   auto SetEventHandler(std::function<void(bluetooth::Event)> cb) -> void;
 };
@@ -114,8 +114,8 @@ class BluetoothState : public tinyfsm::Fsm<BluetoothState> {
   static auto discovery() -> bool;
   static auto discovery(bool) -> void;
 
-  static auto source() -> StreamBufferHandle_t;
-  static auto source(StreamBufferHandle_t) -> void;
+  static auto source() -> PcmBuffer*;
+  static auto source(PcmBuffer*) -> void;
 
   static auto event_handler(std::function<void(Event)>) -> void;
 
@@ -147,7 +147,7 @@ class BluetoothState : public tinyfsm::Fsm<BluetoothState> {
   static std::optional<bluetooth::MacAndName> sConnectingDevice_;
   static int sConnectAttemptsRemaining_;
 
-  static std::atomic<StreamBufferHandle_t> sSource_;
+  static std::atomic<PcmBuffer*> sSource_;
   static std::function<void(Event)> sEventHandler_;
 
   auto connect(const bluetooth::MacAndName&) -> bool;
diff --git a/src/drivers/include/drivers/i2s_dac.hpp b/src/drivers/include/drivers/i2s_dac.hpp
index 0776dbab..138a0c03 100644
--- a/src/drivers/include/drivers/i2s_dac.hpp
+++ b/src/drivers/include/drivers/i2s_dac.hpp
@@ -16,6 +16,7 @@
 
 #include "driver/i2s_std.h"
 #include "driver/i2s_types.h"
+#include "drivers/pcm_buffer.hpp"
 #include "esp_err.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/portmacro.h"
@@ -39,9 +40,9 @@ constexpr size_t kI2SBufferLengthFrames = 1024;
  */
 class I2SDac {
  public:
-  static auto create(IGpios& expander) -> std::optional<I2SDac*>;
+  static auto create(IGpios& expander, PcmBuffer&) -> std::optional<I2SDac*>;
 
-  I2SDac(IGpios& gpio, i2s_chan_handle_t i2s_handle);
+  I2SDac(IGpios& gpio, PcmBuffer&, i2s_chan_handle_t i2s_handle);
   ~I2SDac();
 
   auto Start() -> void;
@@ -69,9 +70,6 @@ class I2SDac {
   auto Reconfigure(Channels ch, BitsPerSample bps, SampleRate rate) -> void;
 
   auto WriteData(const std::span<const std::byte>& data) -> void;
-  auto SetSource(StreamBufferHandle_t buffer) -> void;
-
-  auto SamplesUsed() -> uint32_t;
 
   // Not copyable or movable.
   I2SDac(const I2SDac&) = delete;
@@ -81,9 +79,10 @@ class I2SDac {
   auto set_channel(bool) -> void;
 
   IGpios& gpio_;
+  PcmBuffer& buffer_;
   i2s_chan_handle_t i2s_handle_;
+
   bool i2s_active_;
-  StreamBufferHandle_t buffer_;
   std::mutex configure_mutex_;
 
   i2s_std_clk_config_t clock_config_;
diff --git a/src/drivers/include/drivers/pcm_buffer.hpp b/src/drivers/include/drivers/pcm_buffer.hpp
new file mode 100644
index 00000000..6630f720
--- /dev/null
+++ b/src/drivers/include/drivers/pcm_buffer.hpp
@@ -0,0 +1,72 @@
+/*
+ * Copyright 2024 jacqueline <me@jacqueline.id.au>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <atomic>
+#include <cstddef>
+#include <span>
+
+#include "freertos/FreeRTOS.h"
+
+#include "freertos/ringbuf.h"
+#include "portmacro.h"
+
+namespace drivers {
+
+/*
+ * A circular buffer of signed, 16-bit PCM samples. PcmBuffers are the main
+ * data structure used for shuffling large amounts of read-to-play samples
+ * throughout the system.
+ */
+class PcmBuffer {
+ public:
+  PcmBuffer(size_t size_in_samples);
+  ~PcmBuffer();
+
+  /* Adds samples to the buffer. */
+  auto send(std::span<const int16_t>) -> void;
+
+  /*
+   * Fills the given span with samples. If enough samples are available in
+   * the buffer, then the span will be filled with samples from the buffer. Any
+   * shortfall is made up by padding the given span with zeroes.
+   */
+  auto receive(std::span<int16_t>, bool isr) -> BaseType_t;
+
+  auto clear() -> void;
+  auto isEmpty() -> bool;
+
+  /*
+   * How many samples have been added to this buffer since it was created. This
+   * method overflows by wrapping around to zero.
+   */
+  auto totalSent() -> uint32_t;
+
+  /*
+   * How many samples have been removed from this buffer since it was created.
+   * This method overflows by wrapping around to zero.
+   */
+  auto totalReceived() -> uint32_t;
+
+  // Not copyable or movable.
+  PcmBuffer(const PcmBuffer&) = delete;
+  PcmBuffer& operator=(const PcmBuffer&) = delete;
+
+ private:
+  auto readSingle(std::span<int16_t>, bool isr)
+      -> std::pair<size_t, BaseType_t>;
+
+  StaticRingbuffer_t meta_;
+  uint8_t* buf_;
+
+  std::atomic<uint32_t> sent_;
+  std::atomic<uint32_t> received_;
+  RingbufHandle_t ringbuf_;
+};
+
+}  // namespace drivers
diff --git a/src/drivers/pcm_buffer.cpp b/src/drivers/pcm_buffer.cpp
new file mode 100644
index 00000000..3f4a0443
--- /dev/null
+++ b/src/drivers/pcm_buffer.cpp
@@ -0,0 +1,118 @@
+/*
+ * Copyright 2024 jacqueline <me@jacqueline.id.au>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "drivers/pcm_buffer.hpp"
+#include <stdint.h>
+
+#include <algorithm>
+#include <cstddef>
+#include <cstring>
+#include <span>
+#include <tuple>
+
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+
+#include "esp_heap_caps.h"
+#include "freertos/ringbuf.h"
+#include "portmacro.h"
+
+namespace drivers {
+
+[[maybe_unused]] static const char kTag[] = "pcmbuf";
+
+PcmBuffer::PcmBuffer(size_t size_in_samples) : sent_(0), received_(0) {
+  size_t size_in_bytes = size_in_samples * sizeof(int16_t);
+  ESP_LOGI(kTag, "allocating pcm buffer of size %u (%uKiB)", size_in_samples,
+           size_in_bytes / 1024);
+  buf_ = reinterpret_cast<uint8_t*>(
+      heap_caps_malloc(size_in_bytes, MALLOC_CAP_SPIRAM));
+  ringbuf_ = xRingbufferCreateStatic(size_in_bytes, RINGBUF_TYPE_BYTEBUF, buf_,
+                                     &meta_);
+}
+
+PcmBuffer::~PcmBuffer() {
+  vRingbufferDelete(ringbuf_);
+  heap_caps_free(buf_);
+}
+
+auto PcmBuffer::send(std::span<const int16_t> data) -> void {
+  xRingbufferSend(ringbuf_, data.data(), data.size_bytes(), portMAX_DELAY);
+  sent_ += data.size();
+}
+
+IRAM_ATTR auto PcmBuffer::receive(std::span<int16_t> dest, bool isr)
+    -> BaseType_t {
+  size_t first_read = 0, second_read = 0;
+  BaseType_t ret1 = false, ret2 = false;
+  std::tie(first_read, ret1) = readSingle(dest, isr);
+
+  if (first_read < dest.size()) {
+    std::tie(second_read, ret2) = readSingle(dest.subspan(first_read), isr);
+  }
+
+  size_t total_read = first_read + second_read;
+  if (total_read < dest.size()) {
+    std::fill_n(dest.begin() + total_read, dest.size() - total_read, 0);
+  }
+
+  received_ += first_read + second_read;
+
+  return ret1 || ret2;
+}
+
+auto PcmBuffer::clear() -> void {
+  while (!isEmpty()) {
+    size_t bytes_cleared;
+    void* data = xRingbufferReceive(ringbuf_, &bytes_cleared, 0);
+    vRingbufferReturnItem(ringbuf_, data);
+    received_ += bytes_cleared / sizeof(int16_t);
+  }
+}
+
+auto PcmBuffer::isEmpty() -> bool {
+  return xRingbufferGetMaxItemSize(ringbuf_) ==
+         xRingbufferGetCurFreeSize(ringbuf_);
+}
+
+auto PcmBuffer::totalSent() -> uint32_t {
+  return sent_;
+}
+
+auto PcmBuffer::totalReceived() -> uint32_t {
+  return received_;
+}
+
+IRAM_ATTR auto PcmBuffer::readSingle(std::span<int16_t> dest, bool isr)
+    -> std::pair<size_t, BaseType_t> {
+  BaseType_t ret;
+  size_t read_bytes = 0;
+  void* data;
+  if (isr) {
+    data =
+        xRingbufferReceiveUpToFromISR(ringbuf_, &read_bytes, dest.size_bytes());
+  } else {
+    data = xRingbufferReceiveUpTo(ringbuf_, &read_bytes, 0, dest.size_bytes());
+  }
+
+  size_t read_samples = read_bytes / sizeof(int16_t);
+
+  if (!data) {
+    return {read_samples, ret};
+  }
+
+  std::memcpy(dest.data(), data, read_bytes);
+
+  if (isr) {
+    vRingbufferReturnItem(ringbuf_, data);
+  } else {
+    vRingbufferReturnItemFromISR(ringbuf_, data, &ret);
+  }
+
+  return {read_samples, ret};
+}
+
+}  // namespace drivers