Pipeline working and outputting correctly, but noisy

10 files changed, 174 insertions, 38 deletions

diff --git a/src/audio/audio_decoder.cpp b/src/audio/audio_decoder.cpp
index af9abb94..4b9826a9 100644
--- a/src/audio/audio_decoder.cpp
+++ b/src/audio/audio_decoder.cpp
@@ -128,7 +128,10 @@ auto AudioDecoder::Process(const std::vector<InputStream>& inputs,
     }
   }
 
-  input->consume(current_codec_->GetInputPosition() - 1);
+  std::size_t pos = current_codec_->GetInputPosition();
+  if (pos > 0) {
+    input->consume(pos - 1);
+  }
 }
 
 }  // namespace audio
diff --git a/src/audio/audio_task.cpp b/src/audio/audio_task.cpp
index 3a2a5941..1670f9f6 100644
--- a/src/audio/audio_task.cpp
+++ b/src/audio/audio_task.cpp
@@ -45,7 +45,7 @@ auto StartPipeline(Pipeline* pipeline, IAudioSink* sink) -> void {
 
   ESP_LOGI(kTag, "starting audio pipeline task");
   xTaskCreatePinnedToCore(&AudioTaskMain, "pipeline", kStackSize, args,
-                          kTaskPriorityAudio, NULL, kAudioCore);
+                          kTaskPriorityAudioPipeline, NULL, kAudioCore);
 }
 
 auto StartDrain(IAudioSink* sink) -> void {
@@ -57,8 +57,8 @@ auto StartDrain(IAudioSink* sink) -> void {
   };
 
   ESP_LOGI(kTag, "starting audio drain task");
-  xTaskCreatePinnedToCore(&AudioDrainMain, "drain", kDrainStackSize, drain_args,
-                          kTaskPriorityAudio, NULL, kAudioCore);
+  xTaskCreate(&AudioDrainMain, "drain", kDrainStackSize, drain_args,
+                          kTaskPriorityAudioDrain, NULL);
 }
 
 void AudioTaskMain(void* args) {
@@ -134,7 +134,7 @@ void AudioTaskMain(void* args) {
           // The format of the stream within the sink stream has changed. We
           // need to reconfigure the sink, but shouldn't do so until we've fully
           // drained the current buffer.
-          if (xStreamBufferIsEmpty(sink->buffer())) {
+          if (xStreamBufferIsEmpty(*sink->buffer())) {
             ESP_LOGI(kTag, "reconfiguring dac");
             output_format = sink_stream.info().format;
             sink->Configure(*output_format);
@@ -149,7 +149,7 @@ void AudioTaskMain(void* args) {
           // throttle this task's CPU time. Maybe also hold off on the pipeline
           // if the buffer is already close to full?
           std::size_t sent = xStreamBufferSend(
-              sink->buffer(), sink_stream.data().data(),
+              *sink->buffer(), sink_stream.data().data(),
               sink_stream.data().size_bytes(), pdMS_TO_TICKS(10));
           sink_stream.consume(sent);
         }
@@ -172,7 +172,7 @@ void AudioDrainMain(void* args) {
 
     // TODO(jacqueline): implement PAUSE without busy-waiting.
     while (*command != QUIT) {
-      std::size_t len = xStreamBufferReceive(sink->buffer(), sDrainBuf,
+      std::size_t len = xStreamBufferReceive(*sink->buffer(), sDrainBuf,
                                              sizeof(sDrainBuf), portMAX_DELAY);
       if (len > 0) {
         sink->Send({sDrainBuf, len});
diff --git a/src/audio/i2s_audio_output.cpp b/src/audio/i2s_audio_output.cpp
index 2d336152..7e9e9353 100644
--- a/src/audio/i2s_audio_output.cpp
+++ b/src/audio/i2s_audio_output.cpp
@@ -38,9 +38,13 @@ auto I2SAudioOutput::create(drivers::GpioExpander* expander)
 
 I2SAudioOutput::I2SAudioOutput(drivers::GpioExpander* expander,
                                std::unique_ptr<drivers::AudioDac> dac)
-    : expander_(expander), dac_(std::move(dac)), current_config_() {}
+    : expander_(expander), dac_(std::move(dac)), current_config_() {
+      //dac_->SetSource(buffer());
+    }
 
-I2SAudioOutput::~I2SAudioOutput() {}
+I2SAudioOutput::~I2SAudioOutput() {
+  dac_->SetSource(nullptr);
+}
 
 auto I2SAudioOutput::Configure(const StreamInfo::Format& format) -> bool {
   if (!std::holds_alternative<StreamInfo::Pcm>(format)) {
diff --git a/src/audio/include/audio_sink.hpp b/src/audio/include/audio_sink.hpp
index 03a4690d..a11a9c92 100644
--- a/src/audio/include/audio_sink.hpp
+++ b/src/audio/include/audio_sink.hpp
@@ -1,6 +1,9 @@
 #pragma once
 
+#include <stdint.h>
 #include "audio_element.hpp"
+#include "esp_heap_caps.h"
+#include "freertos/FreeRTOS.h"
 #include "stream_info.hpp"
 namespace audio {
 
@@ -8,17 +11,30 @@ class IAudioSink {
  private:
   // TODO: tune. at least about 12KiB seems right for mp3
   static const std::size_t kDrainBufferSize = 24 * 1024;
-  StreamBufferHandle_t buffer_;
+  uint8_t *buffer_;
+  StaticStreamBuffer_t *metadata_;
+  StreamBufferHandle_t *handle_;
 
  public:
-  IAudioSink() : buffer_(xStreamBufferCreate(kDrainBufferSize, 1)) {}
-  virtual ~IAudioSink() { vStreamBufferDelete(buffer_); }
+  IAudioSink() :
+    buffer_(reinterpret_cast<uint8_t*>(heap_caps_malloc(kDrainBufferSize, MALLOC_CAP_DMA))),
+    metadata_(reinterpret_cast<StaticStreamBuffer_t*>(heap_caps_malloc(sizeof(StaticStreamBuffer_t), MALLOC_CAP_DMA))),
+    handle_(reinterpret_cast<StreamBufferHandle_t*>(heap_caps_malloc(sizeof(StreamBufferHandle_t), MALLOC_CAP_DMA))) {
+      *handle_ = xStreamBufferCreateStatic(kDrainBufferSize, 1, buffer_, metadata_);
+    }
+
+  virtual ~IAudioSink() {
+    vStreamBufferDelete(*handle_);
+    free(buffer_);
+    free(handle_);
+    free(metadata_);
+  }
 
   virtual auto Configure(const StreamInfo::Format& format) -> bool = 0;
   virtual auto Send(const cpp::span<std::byte>& data) -> void = 0;
   virtual auto Log() -> void {}
 
-  auto buffer() const -> StreamBufferHandle_t { return buffer_; }
+  auto buffer() -> StreamBufferHandle_t* { return handle_; }
 };
 
 }  // namespace audio
diff --git a/src/audio/stream_info.cpp b/src/audio/stream_info.cpp
new file mode 100644
index 00000000..7d833d25
--- /dev/null
+++ b/src/audio/stream_info.cpp
@@ -0,0 +1,70 @@
+#include "stream_info.hpp"
+
+#include <cstdint>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <type_traits>
+#include <utility>
+#include <variant>
+
+#include "result.hpp"
+#include "span.hpp"
+#include "types.hpp"
+
+namespace audio {
+
+void InputStream::consume(std::size_t bytes) const {
+  assert(raw_->info->bytes_in_stream >= bytes);
+  auto new_data = raw_->data.subspan(bytes);
+  std::move(new_data.begin(), new_data.end(), raw_->data.begin());
+  raw_->info->bytes_in_stream = new_data.size_bytes();
+}
+
+void InputStream::mark_incomplete() const {
+  raw_->is_incomplete = true;
+}
+
+const StreamInfo& InputStream::info() const {
+  return *raw_->info;
+}
+
+cpp::span<const std::byte> InputStream::data() const {
+  return raw_->data.first(raw_->info->bytes_in_stream);
+}
+
+void OutputStream::add(std::size_t bytes) const {
+  assert(raw_->info->bytes_in_stream + bytes <= raw_->data.size_bytes());
+  raw_->info->bytes_in_stream += bytes;
+}
+
+bool OutputStream::prepare(const StreamInfo::Format& new_format) {
+  if (std::holds_alternative<std::monostate>(raw_->info->format)) {
+    raw_->info->format = new_format;
+    raw_->info->bytes_in_stream = 0;
+    return true;
+  }
+  if (new_format == raw_->info->format) {
+    return true;
+  }
+  if (raw_->is_incomplete) {
+    raw_->info->format = new_format;
+    raw_->info->bytes_in_stream = 0;
+    return true;
+  }
+  return false;
+}
+
+const StreamInfo& OutputStream::info() const {
+  return *raw_->info;
+}
+
+cpp::span<std::byte> OutputStream::data() const {
+  return raw_->data.subspan(raw_->info->bytes_in_stream);
+}
+
+bool OutputStream::is_incomplete() const {
+  return raw_->is_incomplete;
+}
+
+}  // namespace audio
diff --git a/src/codecs/mad.cpp b/src/codecs/mad.cpp
index 020e0d57..7e5b350b 100644
--- a/src/codecs/mad.cpp
+++ b/src/codecs/mad.cpp
@@ -9,7 +9,7 @@
 
 namespace codecs {
 
-static int scaleTo24Bits(mad_fixed_t sample) {
+static int scaleTo16Bits(mad_fixed_t sample) {
   // Round the bottom bits.
   sample += (1L << (MAD_F_FRACBITS - 16));
 
@@ -19,7 +19,7 @@ static int scaleTo24Bits(mad_fixed_t sample) {
   else if (sample < -MAD_F_ONE)
     sample = -MAD_F_ONE;
 
-  /* quantize */
+  // Quantize.
   return sample >> (MAD_F_FRACBITS + 1 - 16);
 }
 
@@ -119,10 +119,10 @@ auto MadMp3Decoder::WriteOutputSamples(cpp::span<std::byte> output)
     }
 
     for (int channel = 0; channel < synth_.pcm.channels; channel++) {
-      uint32_t sample_24 =
-          scaleTo24Bits(synth_.pcm.samples[channel][current_sample_]);
-      output[output_byte++] = static_cast<std::byte>((sample_24 >> 8) & 0xFF);
-      output[output_byte++] = static_cast<std::byte>((sample_24)&0xFF);
+      uint16_t sample_16 =
+          scaleTo16Bits(synth_.pcm.samples[channel][current_sample_]);
+      output[output_byte++] = static_cast<std::byte>((sample_16 >> 8) & 0xFF);
+      output[output_byte++] = static_cast<std::byte>((sample_16)&0xFF);
     }
     current_sample_++;
   }
diff --git a/src/drivers/dac.cpp b/src/drivers/dac.cpp
index c99c88b0..0fe75a5e 100644
--- a/src/drivers/dac.cpp
+++ b/src/drivers/dac.cpp
@@ -5,7 +5,8 @@
 
 #include "assert.h"
 #include "driver/i2c.h"
-#include "driver/i2s_types_legacy.h"
+#include "driver/i2s_common.h"
+#include "driver/i2s_std.h"
 #include "esp_attr.h"
 #include "esp_err.h"
 #include "esp_log.h"
@@ -24,13 +25,8 @@ namespace drivers {
 
 static const char* kTag = "AUDIODAC";
 static const uint8_t kPcm5122Address = 0x4C;
-static const uint8_t kPcm5122Timeout = pdMS_TO_TICKS(100);
 static const i2s_port_t kI2SPort = I2S_NUM_0;
 
-static const AudioDac::SampleRate kDefaultSampleRate =
-    AudioDac::SAMPLE_RATE_44_1;
-static const AudioDac::BitsPerSample kDefaultBps = AudioDac::BPS_16;
-
 auto AudioDac::create(GpioExpander* expander)
     -> cpp::result<std::unique_ptr<AudioDac>, Error> {
   // TODO: tune.
@@ -119,7 +115,7 @@ AudioDac::AudioDac(GpioExpander* gpio, i2s_chan_handle_t i2s_handle)
       i2s_active_(false),
       active_page_(),
       clock_config_(I2S_STD_CLK_DEFAULT_CONFIG(44100)),
-      slot_config_(I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_16BIT,
+      slot_config_(I2S_STD_MSB_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_16BIT,
                                                        I2S_SLOT_MODE_STEREO)) {
   clock_config_.clk_src = I2S_CLK_SRC_PLL_160M;
   gpio_->set_pin(GpioExpander::AMP_EN, true);
@@ -171,11 +167,24 @@ auto AudioDac::Reconfigure(BitsPerSample bps, SampleRate rate) -> void {
   }
 
   // I2S reconfiguration.
-
-  slot_config_.slot_bit_width = I2S_SLOT_BIT_WIDTH_16BIT;
+  uint8_t bps_bits = 0;
+  switch (bps) {
+    case BPS_16:
+      slot_config_.data_bit_width = I2S_DATA_BIT_WIDTH_16BIT;
+      bps_bits = 0;
+      break;
+    case BPS_24:
+      slot_config_.data_bit_width = I2S_DATA_BIT_WIDTH_24BIT;
+      bps_bits = 0b10;
+      break;
+    case BPS_32:
+      slot_config_.data_bit_width = I2S_DATA_BIT_WIDTH_32BIT;
+      bps_bits = 0b11;
+      break;
+  }
   ESP_ERROR_CHECK(i2s_channel_reconfig_std_slot(i2s_handle_, &slot_config_));
 
-  clock_config_.sample_rate_hz = 44100;
+  clock_config_.sample_rate_hz = rate;
   // If we have an MCLK/SCK, then it must be a multiple of both the sample rate
   // and the bit clock. At 24 BPS, we therefore have to change the MCLK multiple
   // to avoid issues at some sample rates. (e.g. 48KHz)
@@ -183,19 +192,14 @@ auto AudioDac::Reconfigure(BitsPerSample bps, SampleRate rate) -> void {
       bps == BPS_24 ? I2S_MCLK_MULTIPLE_384 : I2S_MCLK_MULTIPLE_256;
   ESP_ERROR_CHECK(i2s_channel_reconfig_std_clock(i2s_handle_, &clock_config_));
 
-  // TODO: base on BPS
-  // WriteRegister(Register::I2S_FORMAT, 0b110000);
+  WriteRegister(pcm512x::I2S_1, (0b11 << 4) | bps_bits);
+  WriteRegister(pcm512x::I2S_2, 0);
 
   // Configuration is all done, so we can now bring the DAC and I2S stream back
   // up. I2S first, since otherwise the DAC will see that there's no clocks and
   // shut itself down.
   ESP_ERROR_CHECK(i2s_channel_enable(i2s_handle_));
   WriteRegister(pcm512x::POWER, 0);
-  WriteRegister(pcm512x::SYNCHRONIZE, 1);
-  vTaskDelay(pdMS_TO_TICKS(10));
-  WriteRegister(pcm512x::SYNCHRONIZE, 0);
-  vTaskDelay(pdMS_TO_TICKS(10));
-  LogStatus();
   i2s_active_ = true;
 }
 
@@ -208,6 +212,41 @@ auto AudioDac::WriteData(const cpp::span<const std::byte>& data) -> void {
   }
 }
 
+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;
+  }
+  StreamBufferHandle_t *src = reinterpret_cast<StreamBufferHandle_t*>(user_ctx);
+  BaseType_t ret = false;
+  std::size_t bytes_received = xStreamBufferReceiveFromISR(*src, event->data, event->size, &ret);
+  if (bytes_received < event->size) {
+    // TODO(jacqueline): zero-pad.
+  }
+  return ret;
+}
+
+auto AudioDac::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 AudioDac::Stop() -> void {
   LogStatus();
   WriteRegister(pcm512x::POWER, 1 << 4);
diff --git a/src/drivers/include/dac.hpp b/src/drivers/include/dac.hpp
index b84f9bdb..f2ee9b49 100644
--- a/src/drivers/include/dac.hpp
+++ b/src/drivers/include/dac.hpp
@@ -12,6 +12,7 @@
 #include "esp_err.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/portmacro.h"
+#include "freertos/stream_buffer.h"
 #include "result.hpp"
 #include "span.hpp"
 
@@ -157,6 +158,7 @@ class AudioDac {
   auto Reconfigure(BitsPerSample bps, SampleRate rate) -> void;
 
   auto WriteData(const cpp::span<const std::byte>& data) -> void;
+  auto SetSource(StreamBufferHandle_t *buffer) -> void;
 
   auto Stop() -> void;
   auto LogStatus() -> void;
diff --git a/src/tasks/tasks.cpp b/src/tasks/tasks.cpp
index 32de431a..b9fce7ec 100644
--- a/src/tasks/tasks.cpp
+++ b/src/tasks/tasks.cpp
@@ -1,4 +1,5 @@
 #include "tasks.hpp"
 
 const UBaseType_t kTaskPriorityLvgl = 4;
-const UBaseType_t kTaskPriorityAudio = 5;
+const UBaseType_t kTaskPriorityAudioPipeline = 5;
+const UBaseType_t kTaskPriorityAudioDrain = 6;
diff --git a/src/tasks/tasks.hpp b/src/tasks/tasks.hpp
index 24f8509a..47668aea 100644
--- a/src/tasks/tasks.hpp
+++ b/src/tasks/tasks.hpp
@@ -3,4 +3,5 @@
 #include "freertos/portmacro.h"
 
 extern const UBaseType_t kTaskPriorityLvgl;
-extern const UBaseType_t kTaskPriorityAudio;
+extern const UBaseType_t kTaskPriorityAudioPipeline;
+extern const UBaseType_t kTaskPriorityAudioDrain;