Add very limited resampling (it's slow as shit)

5 files changed, 131 insertions, 7 deletions

diff --git a/src/audio/include/audio_sink.hpp b/src/audio/include/audio_sink.hpp
index 261f7c79..20bb6da6 100644
--- a/src/audio/include/audio_sink.hpp
+++ b/src/audio/include/audio_sink.hpp
@@ -38,8 +38,8 @@ class IAudioSink {
   virtual auto AdjustVolumeUp() -> bool = 0;
   virtual auto AdjustVolumeDown() -> bool = 0;
 
-  virtual auto Configure(const StreamInfo::Pcm& format) -> bool = 0;
-  virtual auto Send(const cpp::span<std::byte>& data) -> void = 0;
+  virtual auto PrepareFormat(const StreamInfo::Pcm&) -> StreamInfo::Pcm = 0;
+  virtual auto Configure(const StreamInfo::Pcm& format) -> void = 0;
 
   auto stream() -> StreamBufferHandle_t { return stream_; }
 };
diff --git a/src/audio/include/audio_task.hpp b/src/audio/include/audio_task.hpp
index f6e9789b..b27aa039 100644
--- a/src/audio/include/audio_task.hpp
+++ b/src/audio/include/audio_task.hpp
@@ -14,6 +14,7 @@
 #include "audio_source.hpp"
 #include "codec.hpp"
 #include "pipeline.hpp"
+#include "sink_mixer.hpp"
 #include "stream_info.hpp"
 
 namespace audio {
@@ -63,18 +64,20 @@ class AudioTask {
   auto ForwardPcmStream(StreamInfo::Pcm&, cpp::span<const std::byte>) -> bool;
 
   auto ConfigureSink(const StreamInfo::Pcm&, const Duration&) -> bool;
+  auto SendToSink(InputStream&) -> void;
 
   IAudioSource* source_;
   IAudioSink* sink_;
   std::unique_ptr<codecs::ICodec> codec_;
+  std::unique_ptr<SinkMixer> mixer_;
   std::unique_ptr<Timer> timer_;
 
   bool has_begun_decoding_;
   std::optional<StreamInfo::Format> current_input_format_;
   std::optional<StreamInfo::Pcm> current_output_format_;
+  std::optional<StreamInfo::Pcm> current_sink_format_;
 
-  std::byte* sample_buffer_;
-  std::size_t sample_buffer_len_;
+  std::unique_ptr<RawStream> codec_buffer_;
 };
 
 }  // namespace audio
diff --git a/src/audio/include/i2s_audio_output.hpp b/src/audio/include/i2s_audio_output.hpp
index 43155711..e0f791c5 100644
--- a/src/audio/include/i2s_audio_output.hpp
+++ b/src/audio/include/i2s_audio_output.hpp
@@ -35,8 +35,8 @@ class I2SAudioOutput : public IAudioSink {
   auto AdjustVolumeUp() -> bool override;
   auto AdjustVolumeDown() -> bool override;
 
-  auto Configure(const StreamInfo::Pcm& format) -> bool override;
-  auto Send(const cpp::span<std::byte>& data) -> void override;
+  auto PrepareFormat(const StreamInfo::Pcm&) -> StreamInfo::Pcm override;
+  auto Configure(const StreamInfo::Pcm& format) -> void override;
 
   I2SAudioOutput(const I2SAudioOutput&) = delete;
   I2SAudioOutput& operator=(const I2SAudioOutput&) = delete;
diff --git a/src/audio/include/sink_mixer.hpp b/src/audio/include/sink_mixer.hpp
new file mode 100644
index 00000000..632ffa2e
--- /dev/null
+++ b/src/audio/include/sink_mixer.hpp
@@ -0,0 +1,88 @@
+/*
+ * Copyright 2023 jacqueline <me@jacqueline.id.au>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#pragma once
+
+#include <sys/_stdint.h>
+#include <cstdint>
+#include <memory>
+
+#include "samplerate.h"
+
+#include "audio_decoder.hpp"
+#include "audio_sink.hpp"
+#include "audio_source.hpp"
+#include "codec.hpp"
+#include "pipeline.hpp"
+#include "stream_info.hpp"
+
+namespace audio {
+
+/*
+ * Handles the final downmix + resample + quantisation stage of audio,
+ * generation sending the result directly to an IAudioSink.
+ */
+class SinkMixer {
+ public:
+  SinkMixer(StreamBufferHandle_t dest);
+  ~SinkMixer();
+
+  auto MixAndSend(InputStream&, const StreamInfo::Pcm&) -> std::size_t;
+
+ private:
+  auto Main() -> void;
+
+  auto SetTargetFormat(const StreamInfo::Pcm& format) -> void;
+  auto HandleBytes() -> void;
+
+  template <typename T>
+  auto ConvertFixedToFloating(InputStream&, OutputStream&) -> void;
+  auto Resample(float, int, InputStream&, OutputStream&) -> void;
+  template <typename T>
+  auto Quantise(InputStream&) -> std::size_t;
+
+  enum class Command {
+    kReadBytes,
+    kSetSourceFormat,
+    kSetTargetFormat,
+  };
+
+  struct Args {
+    Command cmd;
+    StreamInfo::Pcm format;
+  };
+
+  QueueHandle_t commands_;
+  SemaphoreHandle_t is_idle_;
+
+  SRC_STATE* resampler_;
+
+  std::unique_ptr<RawStream> input_stream_;
+  std::unique_ptr<RawStream> floating_point_stream_;
+  std::unique_ptr<RawStream> resampled_stream_;
+
+  cpp::span<std::byte> quantisation_buffer_;
+  cpp::span<short> quantisation_buffer_as_shorts_;
+  cpp::span<int> quantisation_buffer_as_ints_;
+
+  StreamInfo::Pcm target_format_;
+  StreamBufferHandle_t source_;
+  StreamBufferHandle_t sink_;
+};
+
+template <>
+auto SinkMixer::ConvertFixedToFloating<short>(InputStream&, OutputStream&)
+    -> void;
+template <>
+auto SinkMixer::ConvertFixedToFloating<int>(InputStream&, OutputStream&)
+    -> void;
+
+template <>
+auto SinkMixer::Quantise<short>(InputStream&) -> std::size_t;
+template <>
+auto SinkMixer::Quantise<int>(InputStream&) -> std::size_t;
+
+}  // namespace audio
diff --git a/src/audio/include/stream_info.hpp b/src/audio/include/stream_info.hpp
index d48c39a8..d31e035c 100644
--- a/src/audio/include/stream_info.hpp
+++ b/src/audio/include/stream_info.hpp
@@ -56,6 +56,18 @@ class StreamInfo {
     bool operator==(const Encoded&) const = default;
   };
 
+  /*
+   * Two-channel, interleaved, 32-bit floating point pcm samples.
+   */
+  struct FloatingPointPcm {
+    // Number of channels in this stream.
+    uint8_t channels;
+    // The sample rate.
+    uint32_t sample_rate;
+
+    bool operator==(const FloatingPointPcm&) const = default;
+  };
+
   struct Pcm {
     // Number of channels in this stream.
     uint8_t channels;
@@ -64,10 +76,14 @@ class StreamInfo {
     // The sample rate.
     uint32_t sample_rate;
 
+    auto real_bytes_per_sample() const -> uint8_t {
+      return bits_per_sample == 16 ? 2 : 4;
+    }
+
     bool operator==(const Pcm&) const = default;
   };
 
-  typedef std::variant<std::monostate, Encoded, Pcm> Format;
+  typedef std::variant<std::monostate, Encoded, FloatingPointPcm, Pcm> Format;
   auto format() const -> const Format& { return format_; }
   auto set_format(Format f) -> void { format_ = f; }
 
@@ -98,6 +114,12 @@ class RawStream {
 
   auto info() -> StreamInfo& { return info_; }
   auto data() -> cpp::span<std::byte>;
+  template <typename T>
+  auto data_as() -> cpp::span<T> {
+    auto orig = data();
+    return {reinterpret_cast<T*>(orig.data()), orig.size_bytes() / sizeof(T)};
+  }
+  auto empty() const -> bool { return info_.bytes_in_stream() == 0; }
 
  private:
   StreamInfo info_;
@@ -114,6 +136,12 @@ class InputStream {
   const StreamInfo& info() const;
 
   cpp::span<const std::byte> data() const;
+  template <typename T>
+  auto data_as() const -> cpp::span<const T> {
+    auto orig = data();
+    return {reinterpret_cast<const T*>(orig.data()),
+            orig.size_bytes() / sizeof(T)};
+  }
 
  private:
   RawStream* raw_;
@@ -131,6 +159,11 @@ class OutputStream {
   const StreamInfo& info() const;
 
   cpp::span<std::byte> data() const;
+  template <typename T>
+  auto data_as() const -> cpp::span<T> {
+    auto orig = data();
+    return {reinterpret_cast<T*>(orig.data()), orig.size_bytes() / sizeof(T)};
+  }
 
  private:
   RawStream* raw_;