summaryrefslogtreecommitdiff
path: root/src/audio/resample.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/audio/resample.cpp')
-rw-r--r--src/audio/resample.cpp260
1 files changed, 260 insertions, 0 deletions
diff --git a/src/audio/resample.cpp b/src/audio/resample.cpp
new file mode 100644
index 00000000..93ea1034
--- /dev/null
+++ b/src/audio/resample.cpp
@@ -0,0 +1,260 @@
+#include "resample.hpp"
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <algorithm>
+#include <numeric>
+
+#include "esp_log.h"
+
+#include "sample.hpp"
+#include "stream_info.hpp"
+
+namespace audio {
+
+static constexpr size_t kFilterSize = 1536;
+
+constexpr auto calc_deltas(const std::array<int32_t, kFilterSize>& filter)
+ -> std::array<int32_t, kFilterSize> {
+ std::array<int32_t, kFilterSize> deltas;
+ for (size_t n = 0; n < kFilterSize - 1; n++)
+ deltas[n] = filter[n + 1] - filter[n];
+ return deltas;
+}
+
+static const std::array<int32_t, kFilterSize> kFilter{
+#include "fir.h"
+};
+
+static const std::array<int32_t, kFilterSize> kFilterDeltas =
+ calc_deltas(kFilter);
+
+class Channel {
+ public:
+ Channel(uint32_t src_rate,
+ uint32_t dest_rate,
+ size_t chunk_size,
+ size_t skip);
+ ~Channel();
+
+ auto output_chunk_size() -> size_t { return output_chunk_size_; }
+
+ auto FlushSamples(cpp::span<sample::Sample> out) -> size_t;
+ auto AddSample(sample::Sample, cpp::span<sample::Sample> out) -> std::size_t;
+ auto ApplyFilter() -> sample::Sample;
+
+ private:
+ size_t output_chunk_size_;
+ size_t skip_;
+
+ uint32_t factor_; /* factor */
+
+ uint32_t time_; /* time */
+
+ uint32_t time_per_filter_iteration_; /* output step */
+ uint32_t filter_step_; /* filter step */
+ uint32_t filter_end_; /* filter end */
+
+ int32_t unity_scale_; /* unity scale */
+
+ int32_t samples_per_filter_wing_; /* extra samples */
+ int32_t latest_sample_; /* buffer index */
+ cpp::span<int32_t> sample_buffer_; /* the buffer */
+};
+
+enum {
+ Nl = 8, /* 2^Nl samples per zero crossing in fir */
+ Nη = 8, /* phase bits for filter interpolation */
+ kPhaseBits = Nl + Nη, /* phase bits (fract of fixed point) */
+ One = 1 << kPhaseBits,
+};
+
+Channel::Channel(uint32_t irate, uint32_t orate, size_t count, size_t skip)
+ : skip_(skip) {
+ factor_ = ((uint64_t)orate << kPhaseBits) / irate;
+ if (factor_ != One) {
+ time_per_filter_iteration_ = ((uint64_t)irate << kPhaseBits) / orate;
+ filter_step_ = 1 << (Nl + Nη);
+ filter_end_ = kFilterSize << Nη;
+ samples_per_filter_wing_ = 1 + (filter_end_ / filter_step_);
+ unity_scale_ = 13128; /* unity scale factor for fir */
+ if (factor_ < One) {
+ unity_scale_ *= factor_;
+ unity_scale_ >>= kPhaseBits;
+ filter_step_ *= factor_;
+ filter_step_ >>= kPhaseBits;
+ samples_per_filter_wing_ *= time_per_filter_iteration_;
+ samples_per_filter_wing_ >>= kPhaseBits;
+ }
+ latest_sample_ = samples_per_filter_wing_;
+ time_ = latest_sample_ << kPhaseBits;
+
+ size_t buf_size = samples_per_filter_wing_ * 2 + count;
+ int32_t* buf = new int32_t[buf_size];
+ sample_buffer_ = {buf, buf_size};
+ count += buf_size; /* account for buffer accumulation */
+ }
+ output_chunk_size_ = ((uint64_t)count * factor_) >> kPhaseBits;
+}
+
+Channel::~Channel() {
+ delete sample_buffer_.data();
+}
+
+auto Channel::ApplyFilter() -> sample::Sample {
+ uint32_t iteration, p, i;
+ int32_t *sample, a;
+
+ int64_t value = 0;
+
+ // I did my best, but I'll be honest with you I've no idea about any of this
+ // maths stuff.
+
+ // Left wing of the filter.
+ sample = &sample_buffer_[time_ >> kPhaseBits];
+ p = time_ & ((1 << kPhaseBits) - 1);
+ iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p;
+ while (iteration < filter_end_) {
+ i = iteration >> Nη;
+ a = iteration & ((1 << Nη) - 1);
+ iteration += filter_step_;
+ a *= kFilterDeltas[i];
+ a >>= Nη;
+ a += kFilter[i];
+ value += static_cast<int64_t>(*--sample) * a;
+ }
+
+ // Right wing of the filter.
+ sample = &sample_buffer_[time_ >> kPhaseBits];
+ p = (One - p) & ((1 << kPhaseBits) - 1);
+ iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p;
+ if (p == 0) /* skip h[0] as it was already been summed above if p == 0 */
+ iteration += filter_step_;
+ while (iteration < filter_end_) {
+ i = iteration >> Nη;
+ a = iteration & ((1 << Nη) - 1);
+ iteration += filter_step_;
+ a *= kFilterDeltas[i];
+ a >>= Nη;
+ a += kFilter[i];
+ value += static_cast<int64_t>(*sample++) * a;
+ }
+
+ /* scale */
+ value >>= 2;
+ value *= unity_scale_;
+ value >>= 27;
+
+ return sample::Clip(value);
+}
+
+auto Channel::FlushSamples(cpp::span<sample::Sample> out) -> size_t {
+ size_t zeroes_needed = (2 * samples_per_filter_wing_) - latest_sample_;
+ size_t produced = 0;
+ while (zeroes_needed > 0) {
+ produced += AddSample(0, out.subspan(produced));
+ zeroes_needed--;
+ }
+ return produced;
+}
+
+auto Channel::AddSample(sample::Sample in, cpp::span<sample::Sample> out)
+ -> size_t {
+ // Add the latest sample to our working buffer.
+ sample_buffer_[latest_sample_++] = in;
+
+ // If we don't have enough samples to run the filter, then bail out and wait
+ // for more.
+ if (latest_sample_ < 2 * samples_per_filter_wing_) {
+ return 0;
+ }
+
+ // Apply the filter to the buffered samples. First, we work out how long (in
+ // samples) we can run the filter for before running out. This isn't as
+ // trivial as it might look; e.g. depending on the resampling factor we might
+ // be doubling the number of samples, or halving them.
+ uint32_t max_time = (latest_sample_ - samples_per_filter_wing_) << kPhaseBits;
+ size_t samples_output = 0;
+ while (time_ < max_time) {
+ out[skip_ * samples_output++] = ApplyFilter();
+ time_ += time_per_filter_iteration_;
+ }
+
+ // If we are approaching the end of our buffer, we need to shift all the data
+ // in it down to the front to make room for more samples.
+ int32_t current_sample = time_ >> kPhaseBits;
+ if (current_sample >= (sample_buffer_.size() - samples_per_filter_wing_)) {
+ // NB: bit shifting back and forth means we're only modifying `time` by
+ // whole samples.
+ time_ -= current_sample << kPhaseBits;
+ time_ += samples_per_filter_wing_ << kPhaseBits;
+
+ int32_t new_current_sample = time_ >> kPhaseBits;
+ new_current_sample -= samples_per_filter_wing_;
+ current_sample -= samples_per_filter_wing_;
+
+ int32_t samples_to_move = latest_sample_ - current_sample;
+ if (samples_to_move > 0) {
+ auto samples = sample_buffer_.subspan(current_sample, samples_to_move);
+ std::copy_backward(samples.begin(), samples.end(),
+ sample_buffer_.first(new_current_sample).end());
+ latest_sample_ = new_current_sample + samples_to_move;
+ } else {
+ latest_sample_ = new_current_sample;
+ }
+ }
+
+ return samples_output;
+}
+
+static const size_t kChunkSizeSamples = 256;
+
+Resampler::Resampler(uint32_t source_sample_rate,
+ uint32_t target_sample_rate,
+ uint8_t num_channels)
+ : source_sample_rate_(source_sample_rate),
+ target_sample_rate_(target_sample_rate),
+ factor_(((uint64_t)target_sample_rate << kPhaseBits) /
+ source_sample_rate),
+ num_channels_(num_channels),
+ channels_() {
+ for (int i = 0; i < num_channels; i++) {
+ channels_.emplace_back(source_sample_rate, target_sample_rate,
+ kChunkSizeSamples, num_channels);
+ }
+}
+
+Resampler::~Resampler() {}
+
+auto Resampler::Process(cpp::span<const sample::Sample> input,
+ cpp::span<sample::Sample> output,
+ bool end_of_data) -> std::pair<size_t, size_t> {
+ size_t samples_used = 0;
+ std::vector<size_t> samples_produced = {num_channels_, 0};
+ size_t total_samples_produced = 0;
+
+ size_t slop = (factor_ >> kPhaseBits) + 1;
+
+ uint_fast8_t cur_channel = 0;
+
+ while (input.size() > samples_used &&
+ output.size() > total_samples_produced + slop) {
+ // Work out where the next set of samples should be placed.
+ size_t next_output_index =
+ (samples_produced[cur_channel] * num_channels_) + cur_channel;
+
+ // Generate the next samples
+ size_t new_samples = channels_[cur_channel].AddSample(
+ input[samples_used++], output.subspan(next_output_index));
+
+ samples_produced[cur_channel] += new_samples;
+ total_samples_produced += new_samples;
+
+ cur_channel = (cur_channel + 1) % num_channels_;
+ }
+
+ return {samples_used, total_samples_produced};
+}
+
+} // namespace audio
generated by cgit v1.2.3 (git 2.25.1) at 2026-04-29 22:18:41 +0000