Do our own resampling

1 files changed, 64 insertions, 147 deletions

diff --git a/src/audio/sink_mixer.cpp b/src/audio/sink_mixer.cpp
index 79e6f3d3..ba306626 100644
--- a/src/audio/sink_mixer.cpp
+++ b/src/audio/sink_mixer.cpp
@@ -13,6 +13,8 @@
 #include "esp_log.h"
 #include "freertos/portmacro.h"
 #include "freertos/projdefs.h"
+#include "resample.hpp"
+#include "sample.hpp"
 #include "samplerate.h"
 
 #include "stream_info.hpp"
@@ -21,10 +23,7 @@
 static constexpr char kTag[] = "mixer";
 
 static constexpr std::size_t kSourceBufferLength = 2 * 1024;
-static constexpr std::size_t kInputBufferLength = 2 * 1024;
-static constexpr std::size_t kReformatBufferLength = 8 * 1024;
-static constexpr std::size_t kResampleBufferLength = kReformatBufferLength;
-static constexpr std::size_t kQuantisedBufferLength = 1 * 1024;
+static constexpr std::size_t kSampleBufferLength = 4 * 1024;
 
 namespace audio {
 
@@ -34,20 +33,8 @@ SinkMixer::SinkMixer(StreamBufferHandle_t dest)
       resampler_(nullptr),
       source_(xStreamBufferCreate(kSourceBufferLength, 1)),
       sink_(dest) {
-  input_stream_.reset(new RawStream(kInputBufferLength));
-  floating_point_stream_.reset(new RawStream(kReformatBufferLength, MALLOC_CAP_SPIRAM));
-  resampled_stream_.reset(new RawStream(kResampleBufferLength, MALLOC_CAP_SPIRAM));
-
-  quantisation_buffer_ = {
-      reinterpret_cast<std::byte*>(heap_caps_malloc(
-          kQuantisedBufferLength, MALLOC_CAP_INTERNAL | MALLOC_CAP_8BIT)),
-      kQuantisedBufferLength};
-  quantisation_buffer_as_ints_ = {
-      reinterpret_cast<int*>(quantisation_buffer_.data()),
-      quantisation_buffer_.size_bytes() / 4};
-  quantisation_buffer_as_shorts_ = {
-      reinterpret_cast<short*>(quantisation_buffer_.data()),
-      quantisation_buffer_.size_bytes() / 2};
+  input_stream_.reset(new RawStream(kSampleBufferLength));
+  resampled_stream_.reset(new RawStream(kSampleBufferLength));
 
   tasks::StartPersistent<tasks::Type::kMixer>([&]() { Main(); });
 }
@@ -56,10 +43,6 @@ SinkMixer::~SinkMixer() {
   vQueueDelete(commands_);
   vSemaphoreDelete(is_idle_);
   vStreamBufferDelete(source_);
-  heap_caps_free(quantisation_buffer_.data());
-  if (resampler_ != nullptr) {
-    src_delete(resampler_);
-  }
 }
 
 auto SinkMixer::MixAndSend(InputStream& input, const StreamInfo::Pcm& target)
@@ -109,10 +92,12 @@ auto SinkMixer::Main() -> void {
       case Command::kSetSourceFormat:
         ESP_LOGI(kTag, "setting source format");
         input_receiver.prepare(args.format, {});
+        resampler_.reset();
         break;
       case Command::kSetTargetFormat:
         ESP_LOGI(kTag, "setting target format");
         target_format_ = args.format;
+        resampler_.reset();
         break;
       case Command::kReadBytes:
         xSemaphoreTake(is_idle_, 0);
@@ -150,152 +135,84 @@ auto SinkMixer::HandleBytes() -> void {
     return;
   }
 
-  // Work out the resampling ratio using floating point arithmetic, since
-  // relying on the FPU for this will be much faster, and the difference in
-  // accuracy is unlikely to be noticeable.
-  float src_ratio = static_cast<float>(target_format_.sample_rate) /
-                    static_cast<float>(pcm->sample_rate);
-
-  // Loop until we don't have any complete frames left in the input stream,
-  // where a 'frame' is one complete sample per channel.
   while (!input_stream_->empty()) {
-    // The first step of both resampling and requantising is to convert the
-    // fixed point pcm input data into 32 bit floating point samples.
-    OutputStream floating_writer{floating_point_stream_.get()};
-    if (pcm->bits_per_sample == 16) {
-      ConvertFixedToFloating<short>(input, floating_writer);
+    RawStream* output_source;
+    if (pcm->sample_rate != target_format_.sample_rate) {
+      OutputStream resampled_writer{resampled_stream_.get()};
+      if (Resample(input, resampled_writer)) {
+        // Zero samples used or written. We need more input.
+        break;
+      }
+      output_source = resampled_stream_.get();
     } else {
-      // FIXME: We should consider treating 24 bit and 32 bit samples
-      // differently.
-      ConvertFixedToFloating<int>(input, floating_writer);
+      output_source = input_stream_.get();
     }
 
-    InputStream floating_reader{floating_point_stream_.get()};
+    if (target_format_.bits_per_sample == 16) {
+      // This is slightly scary; we're basically reaching into the internals of
+      // the stream buffer to do in-place conversion of samples. Saving an
+      // extra buffer + copy into that buffer is certainly worth it however.
+      cpp::span<sample::Sample> src =
+          output_source->data_as<sample::Sample>().first(
+              output_source->info().bytes_in_stream() / sizeof(sample::Sample));
+      cpp::span<int16_t> dest = output_source->data_as<int16_t>().first(
+          output_source->info().bytes_in_stream() / sizeof(int16_t));
 
-    while (!floating_point_stream_->empty()) {
-      RawStream* quantisation_source;
-      if (pcm->sample_rate != target_format_.sample_rate) {
-        // The input data needs to be resampled before being sent to the sink.
-        OutputStream resample_writer{resampled_stream_.get()};
-        Resample(src_ratio, pcm->channels, floating_reader, resample_writer);
-        quantisation_source = resampled_stream_.get();
-      } else {
-        // The input data already has an acceptable sample rate. All we need to
-        // do is quantise it.
-        quantisation_source = floating_point_stream_.get();
-      }
+      ApplyDither(src, 16);
+      Downscale(src, dest);
 
-      InputStream quantise_reader{quantisation_source};
-      while (!quantisation_source->empty()) {
-        std::size_t samples_available;
-        if (target_format_.bits_per_sample == 16) {
-          samples_available = Quantise<short>(quantise_reader);
-        } else {
-          samples_available = Quantise<int>(quantise_reader);
-        }
+      output_source->info().bytes_in_stream() /= 2;
+    }
 
-        assert(samples_available * target_format_.real_bytes_per_sample() <=
-               quantisation_buffer_.size_bytes());
+    InputStream output{output_source};
+    cpp::span<const std::byte> buf = output.data();
 
-        std::size_t bytes_sent = xStreamBufferSend(
-            sink_, quantisation_buffer_.data(),
-            samples_available * target_format_.real_bytes_per_sample(),
-            portMAX_DELAY);
-        assert(bytes_sent ==
-               samples_available * target_format_.real_bytes_per_sample());
-      }
+    size_t bytes_sent = 0;
+    while (bytes_sent < buf.size_bytes()) {
+      auto cropped = buf.subspan(bytes_sent);
+      bytes_sent += xStreamBufferSend(sink_, cropped.data(),
+                                      cropped.size_bytes(), portMAX_DELAY);
     }
+    output.consume(bytes_sent);
   }
 }
 
-template <>
-auto SinkMixer::ConvertFixedToFloating<short>(InputStream& in_str,
-                                              OutputStream& out_str) -> void {
-  auto in = in_str.data_as<short>();
-  auto out = out_str.data_as<float>();
-  std::size_t samples_converted = std::min(in.size(), out.size());
+auto SinkMixer::Resample(InputStream& in, OutputStream& out) -> bool {
+  if (resampler_ == nullptr) {
+    ESP_LOGI(kTag, "creating new resampler");
+    auto format = in.info().format_as<StreamInfo::Pcm>();
+    resampler_.reset(new Resampler(
+        format->sample_rate, target_format_.sample_rate, format->channels));
+  }
 
-  src_short_to_float_array(in.data(), out.data(), samples_converted);
+  auto res = resampler_->Process(in.data_as<sample::Sample>(),
+                                 out.data_as<sample::Sample>(), false);
 
-  in_str.consume(samples_converted * sizeof(short));
-  out_str.add(samples_converted * sizeof(float));
-}
+  ESP_LOGI(kTag, "resampler sent %u samples, consumed %u, produced %u",
+           in.data().size(), res.first, res.second);
 
-template <>
-auto SinkMixer::ConvertFixedToFloating<int>(InputStream& in_str,
-                                            OutputStream& out_str) -> void {
-  auto in = in_str.data_as<int>();
-  auto out = out_str.data_as<float>();
-  std::size_t samples_converted = std::min(in.size(), out.size());
+  in.consume(res.first * sizeof(sample::Sample));
+  out.add(res.first * sizeof(sample::Sample));
 
-  src_int_to_float_array(in.data(), out.data(), samples_converted);
-
-  in_str.consume(samples_converted * sizeof(int));
-  out_str.add(samples_converted * sizeof(float));
+  return res.first == 0 && res.second == 0;
 }
 
-auto SinkMixer::Resample(float src_ratio,
-                         int channels,
-                         InputStream& in,
-                         OutputStream& out) -> void {
-  if (resampler_ == nullptr || src_get_channels(resampler_) != channels) {
-    if (resampler_ != nullptr) {
-      src_delete(resampler_);
-    }
-
-    ESP_LOGI(kTag, "creating new resampler with %u channels", channels);
-
-    int err = 0;
-    resampler_ = src_new(SRC_LINEAR, channels, &err);
-    assert(resampler_ != NULL);
-    assert(err == 0);
+auto SinkMixer::Downscale(cpp::span<sample::Sample> samples,
+                          cpp::span<int16_t> output) -> void {
+  for (size_t i = 0; i < samples.size(); i++) {
+    output[i] = sample::ToSigned16Bit(samples[i]);
   }
-
-  auto in_buf = in.data_as<float>();
-  auto out_buf = out.data_as<float>();
-
-  src_set_ratio(resampler_, src_ratio);
-  SRC_DATA args{
-      .data_in = in_buf.data(),
-      .data_out = out_buf.data(),
-      .input_frames = static_cast<long>(in_buf.size()),
-      .output_frames = static_cast<long>(out_buf.size()),
-      .input_frames_used = 0,
-      .output_frames_gen = 0,
-      .end_of_input = 0,
-      .src_ratio = src_ratio,
-  };
-  int err = src_process(resampler_, &args);
-  if (err != 0) {
-    ESP_LOGE(kTag, "resampler error: %s", src_strerror(err));
-  }
-
-  in.consume(args.input_frames_used * sizeof(float));
-  out.add(args.output_frames_gen * sizeof(float));
 }
 
-template <>
-auto SinkMixer::Quantise<short>(InputStream& in) -> std::size_t {
-  auto src = in.data_as<float>();
-  cpp::span<short> dest = quantisation_buffer_as_shorts_;
-  dest = dest.first(std::min(src.size(), dest.size()));
-
-  src_float_to_short_array(src.data(), dest.data(), dest.size());
-
-  in.consume(dest.size() * sizeof(float));
-  return dest.size();
-}
-
-template <>
-auto SinkMixer::Quantise<int>(InputStream& in) -> std::size_t {
-  auto src = in.data_as<float>();
-  cpp::span<int> dest = quantisation_buffer_as_ints_;
-  dest = dest.first(std::min<int>(src.size(), dest.size()));
-
-  src_float_to_int_array(src.data(), dest.data(), dest.size());
-
-  in.consume(dest.size() * sizeof(float));
-  return dest.size();
+auto SinkMixer::ApplyDither(cpp::span<sample::Sample> samples,
+                            uint_fast8_t bits) -> void {
+  static uint32_t prnd;
+  for (auto& s : samples) {
+    prnd = (prnd * 0x19660dL + 0x3c6ef35fL) & 0xffffffffL;
+    s = sample::Clip(
+        static_cast<int64_t>(s) +
+        (static_cast<int>(prnd) >> (sizeof(sample::Sample) - bits)));
+  }
 }
 
 }  // namespace audio