WIP start on accepting two streams in out audio output

5 files changed, 88 insertions, 50 deletions

diff --git a/src/drivers/bluetooth.cpp b/src/drivers/bluetooth.cpp
index 412cba1f..23c4f8d8 100644
--- a/src/drivers/bluetooth.cpp
+++ b/src/drivers/bluetooth.cpp
@@ -37,7 +37,8 @@ namespace drivers {
 
 [[maybe_unused]] static constexpr char kTag[] = "bluetooth";
 
-DRAM_ATTR static PcmBuffer* sStream = nullptr;
+DRAM_ATTR static PcmBuffer* sStream1 = nullptr;
+DRAM_ATTR static PcmBuffer* sStream2 = nullptr;
 DRAM_ATTR static std::atomic<float> sVolumeFactor = 1.f;
 
 static tasks::WorkerPool* sBgWorker;
@@ -96,13 +97,15 @@ IRAM_ATTR auto a2dp_data_cb(uint8_t* buf, int32_t buf_size) -> int32_t {
   if (buf == nullptr || buf_size <= 0) {
     return 0;
   }
-  PcmBuffer* stream = sStream;
-  if (stream == nullptr) {
+  PcmBuffer* stream1 = sStream1;
+  PcmBuffer* stream2 = sStream2;
+  if (stream1 == nullptr || stream2 == nullptr) {
     return 0;
   }
 
   int16_t* samples = reinterpret_cast<int16_t*>(buf);
-  stream->receive({samples, static_cast<size_t>(buf_size / 2)}, false);
+  stream1->receive({samples, static_cast<size_t>(buf_size / 2)}, false, false);
+  stream2->receive({samples, static_cast<size_t>(buf_size / 2)}, true, false);
 
   // Apply software volume scaling.
   float factor = sVolumeFactor.load();
@@ -181,14 +184,16 @@ auto Bluetooth::PreferredDevice() -> std::optional<bluetooth::MacAndName> {
   return bluetooth::BluetoothState::preferred_device();
 }
 
-auto Bluetooth::SetSource(PcmBuffer* src) -> void {
+auto Bluetooth::SetSources(PcmBuffer* src1, PcmBuffer* src2) -> void {
   auto lock = bluetooth::BluetoothState::lock();
-  if (src == bluetooth::BluetoothState::source()) {
+  PcmBuffer *cur1, *cur2;
+  std::tie(cur1, cur2) = bluetooth::BluetoothState::sources();
+  if (src1 == cur1 && src2 == cur2) {
     return;
   }
-  bluetooth::BluetoothState::source(src);
+  bluetooth::BluetoothState::sources(src1, src2);
   tinyfsm::FsmList<bluetooth::BluetoothState>::dispatch(
-      bluetooth::events::SourceChanged{});
+      bluetooth::events::SourcesChanged{});
 }
 
 auto Bluetooth::SetVolumeFactor(float f) -> void {
@@ -348,7 +353,6 @@ std::optional<MacAndName> BluetoothState::sPreferredDevice_{};
 std::optional<MacAndName> BluetoothState::sConnectingDevice_{};
 int BluetoothState::sConnectAttemptsRemaining_{0};
 
-std::atomic<PcmBuffer*> BluetoothState::sSource_;
 std::function<void(Event)> BluetoothState::sEventHandler_;
 
 auto BluetoothState::Init(NvsStorage& storage) -> void {
@@ -377,12 +381,13 @@ auto BluetoothState::preferred_device(std::optional<MacAndName> addr) -> void {
   sPreferredDevice_ = addr;
 }
 
-auto BluetoothState::source() -> PcmBuffer* {
-  return sSource_.load();
+auto BluetoothState::sources() -> std::pair<PcmBuffer*, PcmBuffer*> {
+  return {sStream1, sStream2};
 }
 
-auto BluetoothState::source(PcmBuffer* src) -> void {
-  sSource_.store(src);
+auto BluetoothState::sources(PcmBuffer* src1, PcmBuffer* src2) -> void {
+  sStream1 = src1;
+  sStream2 = src2;
 }
 
 auto BluetoothState::event_handler(std::function<void(Event)> cb) -> void {
@@ -508,11 +513,13 @@ void Disabled::react(const events::Enable&) {
   // AVRCP Target
   err = esp_avrc_tg_init();
   if (err != ESP_OK) {
-    ESP_LOGE(kTag, "Error during target init: %s %d", esp_err_to_name(err), err);
+    ESP_LOGE(kTag, "Error during target init: %s %d", esp_err_to_name(err),
+             err);
   }
   err = esp_avrc_tg_register_callback(avrcp_tg_cb);
   if (err != ESP_OK) {
-    ESP_LOGE(kTag, "Error registering AVRC tg callback: %s %d", esp_err_to_name(err), err);
+    ESP_LOGE(kTag, "Error registering AVRC tg callback: %s %d",
+             esp_err_to_name(err), err);
   }
 
   // Set the supported passthrough commands on the tg
@@ -522,19 +529,20 @@ void Disabled::react(const events::Enable&) {
   do {
     // Sleep for a bit
     vTaskDelay(pdMS_TO_TICKS(10));
-    err = esp_avrc_tg_get_psth_cmd_filter(ESP_AVRC_PSTH_FILTER_ALLOWED_CMD, &psth);
+    err = esp_avrc_tg_get_psth_cmd_filter(ESP_AVRC_PSTH_FILTER_ALLOWED_CMD,
+                                          &psth);
   } while (err != ESP_OK);
 
-  err = esp_avrc_tg_set_psth_cmd_filter(ESP_AVRC_PSTH_FILTER_SUPPORTED_CMD, &psth);
+  err = esp_avrc_tg_set_psth_cmd_filter(ESP_AVRC_PSTH_FILTER_SUPPORTED_CMD,
+                                        &psth);
   if (err != ESP_OK) {
     ESP_LOGE(kTag, "Error: %s %d", esp_err_to_name(err), err);
   }
   esp_avrc_rn_evt_cap_mask_t evt_set = {0};
   esp_avrc_rn_evt_bit_mask_operation(ESP_AVRC_BIT_MASK_OP_SET, &evt_set,
-                                      ESP_AVRC_RN_VOLUME_CHANGE);
+                                     ESP_AVRC_RN_VOLUME_CHANGE);
   assert(esp_avrc_tg_set_rn_evt_cap(&evt_set) == ESP_OK);
 
-
   // Initialise A2DP. This handles streaming audio. Currently ESP-IDF's SBC
   // encoder only supports 2 channels of interleaved 16 bit samples, at
   // 44.1kHz, so there is no additional configuration to be done for the
@@ -724,9 +732,8 @@ void Connected::react(const events::PreferredDeviceChanged& ev) {
   transit<Connecting>();
 }
 
-void Connected::react(const events::SourceChanged& ev) {
-  sStream = sSource_;
-  if (sStream != nullptr) {
+void Connected::react(const events::SourcesChanged& ev) {
+  if (sStream1 != nullptr && sStream2 != nullptr) {
     ESP_LOGI(kTag, "checking source is ready");
     esp_a2d_media_ctrl(ESP_A2D_MEDIA_CTRL_CHECK_SRC_RDY);
   } else {
@@ -775,7 +782,8 @@ void Connected::react(events::internal::Avrc ev) {
   switch (ev.type) {
     case ESP_AVRC_CT_CONNECTION_STATE_EVT:
       if (ev.param.conn_stat.connected) {
-        auto err = esp_avrc_ct_send_register_notification_cmd(4, ESP_AVRC_RN_VOLUME_CHANGE, 0);
+        auto err = esp_avrc_ct_send_register_notification_cmd(
+            4, ESP_AVRC_RN_VOLUME_CHANGE, 0);
         if (err != ESP_OK) {
           ESP_LOGE(kTag, "Error: %s %d", esp_err_to_name(err), err);
         }
@@ -787,15 +795,20 @@ void Connected::react(events::internal::Avrc ev) {
     case ESP_AVRC_CT_REMOTE_FEATURES_EVT:
       // The remote device is telling us about its capabilities! We don't
       // currently care about any of them.
-      ESP_LOGI(kTag, "Recieved capabilitites: %lu", ev.param.rmt_feats.feat_mask);
+      ESP_LOGI(kTag, "Recieved capabilitites: %lu",
+               ev.param.rmt_feats.feat_mask);
       break;
     case ESP_AVRC_CT_CHANGE_NOTIFY_EVT:
       if (ev.param.change_ntf.event_id == ESP_AVRC_RN_VOLUME_CHANGE) {
         if (sEventHandler_) {
-          std::invoke(sEventHandler_, bluetooth::RemoteVolumeChanged{.new_vol = ev.param.change_ntf.event_parameter.volume});
+          std::invoke(
+              sEventHandler_,
+              bluetooth::RemoteVolumeChanged{
+                  .new_vol = ev.param.change_ntf.event_parameter.volume});
         }
         // Resubscribe to volume facts
-        auto err = esp_avrc_ct_send_register_notification_cmd(4, ESP_AVRC_RN_VOLUME_CHANGE, 0);
+        auto err = esp_avrc_ct_send_register_notification_cmd(
+            4, ESP_AVRC_RN_VOLUME_CHANGE, 0);
         if (err != ESP_OK) {
           ESP_LOGE(kTag, "Error: %s %d", esp_err_to_name(err), err);
         }
@@ -809,16 +822,20 @@ void Connected::react(events::internal::Avrc ev) {
 void Connected::react(const events::internal::Avrctg ev) {
   switch (ev.type) {
     case ESP_AVRC_TG_CONNECTION_STATE_EVT:
-      ESP_LOGI(kTag, "Got connection event. Connected: %s", ev.param.conn_stat.connected ? "true" : "false");
+      ESP_LOGI(kTag, "Got connection event. Connected: %s",
+               ev.param.conn_stat.connected ? "true" : "false");
       if (ev.param.conn_stat.connected) {
       }
       break;
     case ESP_AVRC_TG_REMOTE_FEATURES_EVT:
-      ESP_LOGI(kTag, "Got remote features feat flag %d", ev.param.rmt_feats.ct_feat_flag);
-      ESP_LOGI(kTag, "Got remote features feat mask %lu", ev.param.rmt_feats.feat_mask);
+      ESP_LOGI(kTag, "Got remote features feat flag %d",
+               ev.param.rmt_feats.ct_feat_flag);
+      ESP_LOGI(kTag, "Got remote features feat mask %lu",
+               ev.param.rmt_feats.feat_mask);
       break;
     case ESP_AVRC_TG_PASSTHROUGH_CMD_EVT:
-      ESP_LOGI(kTag, "Got passthrough event keycode: %x, %d", ev.param.psth_cmd.key_code, ev.param.psth_cmd.key_state);
+      ESP_LOGI(kTag, "Got passthrough event keycode: %x, %d",
+               ev.param.psth_cmd.key_code, ev.param.psth_cmd.key_state);
       if (ev.param.psth_cmd.key_state == 1 && sEventHandler_) {
         switch (ev.param.psth_cmd.key_code) {
           case ESP_AVRC_PT_CMD_PLAY:
@@ -840,7 +857,8 @@ void Connected::react(const events::internal::Avrctg ev) {
             std::invoke(sEventHandler_, bluetooth::SimpleEvent::kBackward);
             break;
           default:
-            ESP_LOGI(kTag, "Unhandled passthrough cmd. Key code: %d", ev.param.psth_cmd.key_code);
+            ESP_LOGI(kTag, "Unhandled passthrough cmd. Key code: %d",
+                     ev.param.psth_cmd.key_code);
         }
       }
       break;
@@ -848,14 +866,15 @@ void Connected::react(const events::internal::Avrctg ev) {
       if (ev.param.reg_ntf.event_id == ESP_AVRC_RN_VOLUME_CHANGE) {
         // TODO: actually do this lol
         esp_avrc_rn_param_t rn_param;
-        rn_param.volume = 64; 
+        rn_param.volume = 64;
         auto err = esp_avrc_tg_send_rn_rsp(ESP_AVRC_RN_VOLUME_CHANGE,
-                                ESP_AVRC_RN_RSP_INTERIM, &rn_param);
+                                           ESP_AVRC_RN_RSP_INTERIM, &rn_param);
         if (err != ESP_OK) {
           ESP_LOGE(kTag, "Error: %s %d", esp_err_to_name(err), err);
         }
       } else {
-        ESP_LOGW(kTag, "unhandled AVRC TG Register Notification event: %u", ev.param.reg_ntf.event_id);
+        ESP_LOGW(kTag, "unhandled AVRC TG Register Notification event: %u",
+                 ev.param.reg_ntf.event_id);
       }
       break;
     }
diff --git a/src/drivers/include/drivers/bluetooth.hpp b/src/drivers/include/drivers/bluetooth.hpp
index 94a85263..b3b12ffc 100644
--- a/src/drivers/include/drivers/bluetooth.hpp
+++ b/src/drivers/include/drivers/bluetooth.hpp
@@ -43,7 +43,7 @@ class Bluetooth {
   auto SetPreferredDevice(std::optional<bluetooth::MacAndName> dev) -> void;
   auto PreferredDevice() -> std::optional<bluetooth::MacAndName>;
 
-  auto SetSource(PcmBuffer*) -> void;
+  auto SetSources(PcmBuffer*, PcmBuffer*) -> void;
   auto SetVolumeFactor(float) -> void;
 
   auto SetEventHandler(std::function<void(bluetooth::Event)> cb) -> void;
@@ -57,7 +57,7 @@ struct Disable : public tinyfsm::Event {};
 
 struct ConnectTimedOut : public tinyfsm::Event {};
 struct PreferredDeviceChanged : public tinyfsm::Event {};
-struct SourceChanged : public tinyfsm::Event {};
+struct SourcesChanged : public tinyfsm::Event {};
 struct DeviceDiscovered : public tinyfsm::Event {
   const Device& device;
 };
@@ -118,8 +118,8 @@ class BluetoothState : public tinyfsm::Fsm<BluetoothState> {
   static auto discovery() -> bool;
   static auto discovery(bool) -> void;
 
-  static auto source() -> PcmBuffer*;
-  static auto source(PcmBuffer*) -> void;
+  static auto sources() -> std::pair<PcmBuffer*, PcmBuffer*>;
+  static auto sources(PcmBuffer*, PcmBuffer*) -> void;
 
   static auto event_handler(std::function<void(Event)>) -> void;
 
@@ -132,7 +132,7 @@ class BluetoothState : public tinyfsm::Fsm<BluetoothState> {
   virtual void react(const events::Disable& ev) = 0;
   virtual void react(const events::ConnectTimedOut& ev){};
   virtual void react(const events::PreferredDeviceChanged& ev){};
-  virtual void react(const events::SourceChanged& ev){};
+  virtual void react(const events::SourcesChanged& ev){};
 
   virtual void react(const events::DeviceDiscovered&);
 
@@ -152,7 +152,6 @@ class BluetoothState : public tinyfsm::Fsm<BluetoothState> {
   static std::optional<bluetooth::MacAndName> sConnectingDevice_;
   static int sConnectAttemptsRemaining_;
 
-  static std::atomic<PcmBuffer*> sSource_;
   static std::function<void(Event)> sEventHandler_;
 
   auto connect(const bluetooth::MacAndName&) -> bool;
@@ -205,7 +204,7 @@ class Connected : public BluetoothState {
   void exit() override;
 
   void react(const events::PreferredDeviceChanged& ev) override;
-  void react(const events::SourceChanged& ev) override;
+  void react(const events::SourcesChanged& ev) override;
 
   void react(const events::Disable& ev) override;
   void react(events::internal::Gap ev) override;
diff --git a/src/drivers/include/drivers/i2s_dac.hpp b/src/drivers/include/drivers/i2s_dac.hpp
index cf9258c0..0fe462b4 100644
--- a/src/drivers/include/drivers/i2s_dac.hpp
+++ b/src/drivers/include/drivers/i2s_dac.hpp
@@ -40,7 +40,7 @@ constexpr size_t kI2SBufferLengthFrames = 1024;
  */
 class I2SDac {
  public:
-  static auto create(IGpios& expander, PcmBuffer&) -> std::optional<I2SDac*>;
+  static auto create(IGpios& expander, PcmBuffer&, PcmBuffer&) -> std::optional<I2SDac*>;
 
   I2SDac(IGpios& gpio, PcmBuffer&, i2s_chan_handle_t i2s_handle);
   ~I2SDac();
@@ -77,7 +77,8 @@ class I2SDac {
   auto set_channel(bool) -> void;
 
   IGpios& gpio_;
-  PcmBuffer& buffer_;
+  PcmBuffer& buffer1_;
+  PcmBuffer& buffer2_;
   i2s_chan_handle_t i2s_handle_;
 
   bool i2s_active_;
diff --git a/src/drivers/include/drivers/pcm_buffer.hpp b/src/drivers/include/drivers/pcm_buffer.hpp
index 6630f720..27e9eec6 100644
--- a/src/drivers/include/drivers/pcm_buffer.hpp
+++ b/src/drivers/include/drivers/pcm_buffer.hpp
@@ -35,8 +35,13 @@ class PcmBuffer {
    * 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.
+   *
+   * If `mix` is set to true then, instead of overwriting the destination span,
+   * the retrieved samples will be mixed into any existing samples contained
+   * within the destination. This mixing uses a naive sum approach, and so may
+   * introduce clipping.
    */
-  auto receive(std::span<int16_t>, bool isr) -> BaseType_t;
+  auto receive(std::span<int16_t>, bool mix, bool isr) -> BaseType_t;
 
   auto clear() -> void;
   auto isEmpty() -> bool;
@@ -58,7 +63,7 @@ class PcmBuffer {
   PcmBuffer& operator=(const PcmBuffer&) = delete;
 
  private:
-  auto readSingle(std::span<int16_t>, bool isr)
+  auto readSingle(std::span<int16_t>, bool mix, bool isr)
       -> std::pair<size_t, BaseType_t>;
 
   StaticRingbuffer_t meta_;
diff --git a/src/drivers/pcm_buffer.cpp b/src/drivers/pcm_buffer.cpp
index 3f4a0443..b619cefb 100644
--- a/src/drivers/pcm_buffer.cpp
+++ b/src/drivers/pcm_buffer.cpp
@@ -44,14 +44,15 @@ auto PcmBuffer::send(std::span<const int16_t> data) -> void {
   sent_ += data.size();
 }
 
-IRAM_ATTR auto PcmBuffer::receive(std::span<int16_t> dest, bool isr)
+IRAM_ATTR auto PcmBuffer::receive(std::span<int16_t> dest, bool mix, 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);
+  std::tie(first_read, ret1) = readSingle(dest, mix, isr);
 
   if (first_read < dest.size()) {
-    std::tie(second_read, ret2) = readSingle(dest.subspan(first_read), isr);
+    std::tie(second_read, ret2) =
+        readSingle(dest.subspan(first_read), mix, isr);
   }
 
   size_t total_read = first_read + second_read;
@@ -86,7 +87,9 @@ auto PcmBuffer::totalReceived() -> uint32_t {
   return received_;
 }
 
-IRAM_ATTR auto PcmBuffer::readSingle(std::span<int16_t> dest, bool isr)
+IRAM_ATTR auto PcmBuffer::readSingle(std::span<int16_t> dest,
+                                     bool mix,
+                                     bool isr)
     -> std::pair<size_t, BaseType_t> {
   BaseType_t ret;
   size_t read_bytes = 0;
@@ -104,7 +107,18 @@ IRAM_ATTR auto PcmBuffer::readSingle(std::span<int16_t> dest, bool isr)
     return {read_samples, ret};
   }
 
-  std::memcpy(dest.data(), data, read_bytes);
+  if (mix) {
+    for (size_t i = 0; i < read_samples; i++) {
+      // Sum the two samples in a 32 bit field so that the addition is always
+      // safe.
+      int32_t sum = static_cast<int32_t>(dest[i]) +
+                    static_cast<int32_t>(reinterpret_cast<int16_t*>(data)[i]);
+      // Clip back into the range of a single sample.
+      dest[i] = std::clamp<int32_t>(sum, INT16_MIN, INT16_MAX);
+    }
+  } else {
+    std::memcpy(dest.data(), data, read_bytes);
+  }
 
   if (isr) {
     vRingbufferReturnItem(ringbuf_, data);