1 files changed, 271 insertions, 0 deletions
diff --git a/src/tangara/audio/processor.cpp b/src/tangara/audio/processor.cpp
new file mode 100644
index 00000000..dd96c892
--- /dev/null
+++ b/src/tangara/audio/processor.cpp
@@ -0,0 +1,271 @@
+/*
+ * Copyright 2023 jacqueline <me@jacqueline.id.au>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "audio/processor.hpp"
+#include <stdint.h>
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <limits>
+
+#include "audio/audio_events.hpp"
+#include "audio/audio_sink.hpp"
+#include "drivers/i2s_dac.hpp"
+#include "esp_heap_caps.h"
+#include "esp_log.h"
+#include "events/event_queue.hpp"
+#include "freertos/portmacro.h"
+#include "freertos/projdefs.h"
+
+#include "audio/resample.hpp"
+#include "sample.hpp"
+#include "tasks.hpp"
+
+[[maybe_unused]] static constexpr char kTag[] = "mixer";
+
+static constexpr std::size_t kSampleBufferLength =
+    drivers::kI2SBufferLengthFrames * sizeof(sample::Sample) * 2;
+static constexpr std::size_t kSourceBufferLength = kSampleBufferLength * 2;
+
+namespace audio {
+
+SampleProcessor::SampleProcessor(StreamBufferHandle_t sink)
+    : commands_(xQueueCreate(1, sizeof(Args))),
+      resampler_(nullptr),
+      source_(xStreamBufferCreateWithCaps(kSourceBufferLength,
+                                          sizeof(sample::Sample) * 2,
+                                          MALLOC_CAP_DMA)),
+      sink_(sink),
+      leftover_bytes_(0),
+      samples_written_(0) {
+  input_buffer_ = {
+      reinterpret_cast<sample::Sample*>(heap_caps_calloc(
+          kSampleBufferLength, sizeof(sample::Sample), MALLOC_CAP_DMA)),
+      kSampleBufferLength};
+  input_buffer_as_bytes_ = {reinterpret_cast<std::byte*>(input_buffer_.data()),
+                            input_buffer_.size_bytes()};
+
+  resampled_buffer_ = {
+      reinterpret_cast<sample::Sample*>(heap_caps_calloc(
+          kSampleBufferLength, sizeof(sample::Sample), MALLOC_CAP_DMA)),
+      kSampleBufferLength};
+
+  tasks::StartPersistent<tasks::Type::kAudioConverter>([&]() { Main(); });
+}
+
+SampleProcessor::~SampleProcessor() {
+  vQueueDelete(commands_);
+  vStreamBufferDelete(source_);
+}
+
+auto SampleProcessor::SetOutput(std::shared_ptr<IAudioOutput> output) -> void {
+  assert(xStreamBufferIsEmpty(sink_));
+  // FIXME: We should add synchronisation here, but we should be careful
+  // about not impacting performance given that the output will change only
+  // very rarely (if ever).
+  output_ = output;
+  samples_written_ = output_->samplesUsed();
+}
+
+auto SampleProcessor::beginStream(std::shared_ptr<TrackInfo> track) -> void {
+  Args args{
+      .track = new std::shared_ptr<TrackInfo>(track),
+      .samples_available = 0,
+      .is_end_of_stream = false,
+      .clear_buffers = false,
+  };
+  xQueueSend(commands_, &args, portMAX_DELAY);
+}
+
+auto SampleProcessor::continueStream(std::span<sample::Sample> input) -> void {
+  Args args{
+      .track = nullptr,
+      .samples_available = input.size(),
+      .is_end_of_stream = false,
+      .clear_buffers = false,
+  };
+  xQueueSend(commands_, &args, portMAX_DELAY);
+  xStreamBufferSend(source_, input.data(), input.size_bytes(), portMAX_DELAY);
+}
+
+auto SampleProcessor::endStream(bool cancelled) -> void {
+  Args args{
+      .track = nullptr,
+      .samples_available = 0,
+      .is_end_of_stream = true,
+      .clear_buffers = cancelled,
+  };
+  xQueueSend(commands_, &args, portMAX_DELAY);
+}
+
+auto SampleProcessor::Main() -> void {
+  for (;;) {
+    Args args;
+    while (!xQueueReceive(commands_, &args, portMAX_DELAY)) {
+    }
+
+    if (args.track) {
+      handleBeginStream(*args.track);
+      delete args.track;
+    }
+    if (args.samples_available) {
+      handleContinueStream(args.samples_available);
+    }
+    if (args.is_end_of_stream) {
+      handleEndStream(args.clear_buffers);
+    }
+  }
+}
+
+auto SampleProcessor::handleBeginStream(std::shared_ptr<TrackInfo> track)
+    -> void {
+  if (track->format != source_format_) {
+    source_format_ = track->format;
+    // The new stream has a different format to the previous stream (or there
+    // was no previous stream).
+    // First, clean up our filters.
+    resampler_.reset();
+    leftover_bytes_ = 0;
+
+    // If the output is idle, then we can reconfigure it to the closest format
+    // to our new source.
+    // If the output *wasn't* idle, then we can't reconfigure without an
+    // audible gap in playback. So instead, we simply keep the same target
+    // format and begin resampling.
+    if (xStreamBufferIsEmpty(sink_)) {
+      target_format_ = output_->PrepareFormat(track->format);
+      output_->Configure(target_format_);
+    }
+  }
+
+  if (xStreamBufferIsEmpty(sink_)) {
+    samples_written_ = output_->samplesUsed();
+  }
+
+  events::Audio().Dispatch(internal::StreamStarted{
+      .track = track,
+      .sink_format = target_format_,
+      .cue_at_sample = samples_written_,
+  });
+}
+
+auto SampleProcessor::handleContinueStream(size_t samples_available) -> void {
+  // Loop until we finish reading all the bytes indicated. There might be
+  // leftovers from each iteration, and from this process as a whole,
+  // depending on the resampling stage.
+  size_t bytes_read = 0;
+  size_t bytes_to_read = samples_available * sizeof(sample::Sample);
+  while (bytes_read < bytes_to_read) {
+    // First top up the input buffer, taking care not to overwrite anything
+    // remaining from a previous iteration.
+    size_t bytes_read_this_it = xStreamBufferReceive(
+        source_, input_buffer_as_bytes_.subspan(leftover_bytes_).data(),
+        std::min(input_buffer_as_bytes_.size() - leftover_bytes_,
+                 bytes_to_read - bytes_read),
+        portMAX_DELAY);
+    bytes_read += bytes_read_this_it;
+
+    // Calculate the number of whole samples that are now in the input buffer.
+    size_t bytes_in_buffer = bytes_read_this_it + leftover_bytes_;
+    size_t samples_in_buffer = bytes_in_buffer / sizeof(sample::Sample);
+
+    size_t samples_used = handleSamples(input_buffer_.first(samples_in_buffer));
+
+    // Maybe the resampler didn't consume everything. Maybe the last few
+    // bytes we read were half a frame. Either way, we need to calculate the
+    // size of the remainder in bytes, then move it to the front of our
+    // buffer.
+    size_t bytes_used = samples_used * sizeof(sample::Sample);
+    assert(bytes_used <= bytes_in_buffer);
+
+    leftover_bytes_ = bytes_in_buffer - bytes_used;
+    if (leftover_bytes_ > 0) {
+      std::memmove(input_buffer_as_bytes_.data(),
+                   input_buffer_as_bytes_.data() + bytes_used, leftover_bytes_);
+    }
+  }
+}
+
+auto SampleProcessor::handleSamples(std::span<sample::Sample> input) -> size_t {
+  if (source_format_ == target_format_) {
+    // The happiest possible case: the input format matches the output
+    // format already.
+    sendToSink(input);
+    return input.size();
+  }
+
+  size_t samples_used = 0;
+  while (samples_used < input.size()) {
+    std::span<sample::Sample> output_source;
+    if (source_format_.sample_rate != target_format_.sample_rate) {
+      if (resampler_ == nullptr) {
+        ESP_LOGI(kTag, "creating new resampler for %lu -> %lu",
+                 source_format_.sample_rate, target_format_.sample_rate);
+        resampler_.reset(new Resampler(source_format_.sample_rate,
+                                       target_format_.sample_rate,
+                                       source_format_.num_channels));
+      }
+
+      size_t read, written;
+      std::tie(read, written) = resampler_->Process(input.subspan(samples_used),
+                                                    resampled_buffer_, false);
+      samples_used += read;
+
+      if (read == 0 && written == 0) {
+        // Zero samples used or written. We need more input.
+        break;
+      }
+      output_source = resampled_buffer_.first(written);
+    } else {
+      output_source = input;
+      samples_used = input.size();
+    }
+
+    sendToSink(output_source);
+  }
+
+  return samples_used;
+}
+
+auto SampleProcessor::handleEndStream(bool clear_bufs) -> void {
+  if (resampler_ && !clear_bufs) {
+    size_t read, written;
+    std::tie(read, written) = resampler_->Process({}, resampled_buffer_, true);
+
+    if (written > 0) {
+      sendToSink(resampled_buffer_.first(written));
+    }
+  }
+
+  if (clear_bufs) {
+    assert(xStreamBufferReset(sink_));
+    samples_written_ = output_->samplesUsed();
+  }
+
+  // FIXME: This discards any leftover samples, but there probably shouldn't be
+  // any leftover samples. Can this be an assert instead?
+  leftover_bytes_ = 0;
+
+  events::Audio().Dispatch(internal::StreamEnded{
+      .cue_at_sample = samples_written_,
+  });
+}
+
+auto SampleProcessor::sendToSink(std::span<sample::Sample> samples) -> void {
+  auto data = std::as_bytes(samples);
+  xStreamBufferSend(sink_, data.data(), data.size(), portMAX_DELAY);
+
+  uint32_t samples_before_overflow =
+      std::numeric_limits<uint32_t>::max() - samples_written_;
+  if (samples_before_overflow < samples.size()) {
+    samples_written_ = samples.size() - samples_before_overflow;
+  } else {
+    samples_written_ += samples.size();
+  }
+}
+
+}  // namespace audio