From f42448d50123e376205df17bc295917e89d943f5 Mon Sep 17 00:00:00 2001 From: jacqueline Date: Fri, 5 Jul 2024 10:12:55 +1000 Subject: [PATCH] WIP start on accepting two streams in out audio output --- src/drivers/bluetooth.cpp | 87 +++++++++++++--------- src/drivers/include/drivers/bluetooth.hpp | 13 ++-- src/drivers/include/drivers/i2s_dac.hpp | 5 +- src/drivers/include/drivers/pcm_buffer.hpp | 9 ++- src/drivers/pcm_buffer.cpp | 24 ++++-- 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 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(buf); - stream->receive({samples, static_cast(buf_size / 2)}, false); + stream1->receive({samples, static_cast(buf_size / 2)}, false, false); + stream2->receive({samples, static_cast(buf_size / 2)}, true, false); // Apply software volume scaling. float factor = sVolumeFactor.load(); @@ -181,14 +184,16 @@ auto Bluetooth::PreferredDevice() -> std::optional { 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::dispatch( - bluetooth::events::SourceChanged{}); + bluetooth::events::SourcesChanged{}); } auto Bluetooth::SetVolumeFactor(float f) -> void { @@ -348,7 +353,6 @@ std::optional BluetoothState::sPreferredDevice_{}; std::optional BluetoothState::sConnectingDevice_{}; int BluetoothState::sConnectAttemptsRemaining_{0}; -std::atomic BluetoothState::sSource_; std::function BluetoothState::sEventHandler_; auto BluetoothState::Init(NvsStorage& storage) -> void { @@ -377,12 +381,13 @@ auto BluetoothState::preferred_device(std::optional addr) -> void { sPreferredDevice_ = addr; } -auto BluetoothState::source() -> PcmBuffer* { - return sSource_.load(); +auto BluetoothState::sources() -> std::pair { + 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 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(); } -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 dev) -> void; auto PreferredDevice() -> std::optional; - auto SetSource(PcmBuffer*) -> void; + auto SetSources(PcmBuffer*, PcmBuffer*) -> void; auto SetVolumeFactor(float) -> void; auto SetEventHandler(std::function 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 { static auto discovery() -> bool; static auto discovery(bool) -> void; - static auto source() -> PcmBuffer*; - static auto source(PcmBuffer*) -> void; + static auto sources() -> std::pair; + static auto sources(PcmBuffer*, PcmBuffer*) -> void; static auto event_handler(std::function) -> void; @@ -132,7 +132,7 @@ class BluetoothState : public tinyfsm::Fsm { 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 { static std::optional sConnectingDevice_; static int sConnectAttemptsRemaining_; - static std::atomic sSource_; static std::function 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; + static auto create(IGpios& expander, PcmBuffer&, PcmBuffer&) -> std::optional; 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, bool isr) -> BaseType_t; + auto receive(std::span, 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, bool isr) + auto readSingle(std::span, bool mix, bool isr) -> std::pair; 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 data) -> void { sent_ += data.size(); } -IRAM_ATTR auto PcmBuffer::receive(std::span dest, bool isr) +IRAM_ATTR auto PcmBuffer::receive(std::span 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 dest, bool isr) +IRAM_ATTR auto PcmBuffer::readSingle(std::span dest, + bool mix, + bool isr) -> std::pair { BaseType_t ret; size_t read_bytes = 0; @@ -104,7 +107,18 @@ IRAM_ATTR auto PcmBuffer::readSingle(std::span 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(dest[i]) + + static_cast(reinterpret_cast(data)[i]); + // Clip back into the range of a single sample. + dest[i] = std::clamp(sum, INT16_MIN, INT16_MAX); + } + } else { + std::memcpy(dest.data(), data, read_bytes); + } if (isr) { vRingbufferReturnItem(ringbuf_, data);