1 // Copyright 2015 Emilie Gillet. 2 // 3 // Author: Emilie Gillet (emilie.o.gillet@gmail.com) 4 // 5 // Permission is hereby granted, free of charge, to any person obtaining a copy 6 // of this software and associated documentation files (the "Software"), to deal 7 // in the Software without restriction, including without limitation the rights 8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 // copies of the Software, and to permit persons to whom the Software is 10 // furnished to do so, subject to the following conditions: 11 // 12 // The above copyright notice and this permission notice shall be included in 13 // all copies or substantial portions of the Software. 14 // 15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 // THE SOFTWARE. 22 // 23 // See http://creativecommons.org/licenses/MIT/ for more information. 24 // 25 // ----------------------------------------------------------------------------- 26 // 27 // Sample rate converter. 28 29 #ifndef STMLIB_DSP_SAMPLE_RATE_CONVERTER_H_ 30 #define STMLIB_DSP_SAMPLE_RATE_CONVERTER_H_ 31 32 #include "stmlib/stmlib.h" 33 34 #include <algorithm> 35 36 namespace stmlib { 37 38 enum SampleRateConversionDirection { 39 SRC_UP, 40 SRC_DOWN 41 }; 42 43 template <SampleRateConversionDirection direction, int32_t ratio, int32_t length> 44 struct SRC_FIR { }; 45 46 template<int32_t N> 47 struct FilterState { 48 public: 49 enum { 50 n = N 51 }; PushFilterState52 inline void Push(float value) { 53 tail.Push(head); 54 head = value; 55 } 56 ReadFilterState57 template<int32_t i> inline float Read() const { 58 return i == 0 ? head : tail.template Read<i - 1>(); 59 } 60 LoadFilterState61 inline void Load(const float* x_state) { 62 head = x_state[0]; 63 tail.Load(x_state + 1); 64 } 65 SaveFilterState66 inline void Save(float* x_state) { 67 x_state[0] = head; 68 tail.Save(x_state + 1); 69 } 70 71 private: 72 float head; 73 FilterState<N-1> tail; 74 }; 75 76 template<> 77 class FilterState<1> { 78 public: 79 enum { 80 n = 1 81 }; Push(float value)82 inline void Push(float value) { 83 head = value; 84 } 85 Read()86 template<int32_t i> inline float Read() const { 87 return head; 88 } 89 Load(const float * x_state)90 inline void Load(const float* x_state) { 91 head = x_state[0]; 92 } 93 Save(float * x_state)94 inline void Save(float* x_state) { 95 x_state[0] = head; 96 } 97 private: 98 float head; 99 }; 100 101 template<int32_t N, int32_t x_stride, int32_t h_stride, int32_t mirror = 0, int32_t i = 0, int32_t h_offset = 0> 102 struct Accumulator { 103 enum { 104 h_index = mirror != 0 && h_offset + i * h_stride >= mirror / 2 ? 105 mirror - 1 - i * h_stride - h_offset : h_offset + i * h_stride 106 }; 107 108 template<typename IR> operatorAccumulator109 inline float operator()(const float* x, const IR& h) const { 110 Accumulator<N - 1, x_stride, h_stride, mirror, i + 1, h_offset> a; 111 return x[i * x_stride] * h.template Read<h_index>() + a(x, h); 112 } 113 114 template<int32_t NN, typename IR> operatorAccumulator115 inline float operator()(const FilterState<NN>& x, const IR& h) const { 116 Accumulator<N - 1, x_stride, h_stride, mirror, i + 1, h_offset> a; 117 return x.template Read<i * x_stride>() * h.template Read<h_index>() + a(x, h); 118 } 119 }; 120 121 template<int32_t x_stride, int32_t h_stride, int32_t mirror, int32_t i, int32_t h_offset> 122 struct Accumulator<0, x_stride, h_stride, mirror, i, h_offset> { 123 template<typename IR> 124 inline float operator()(const float* x, const IR& h) const { 125 return 0.0f; 126 } 127 128 template<int32_t NN, typename IR> 129 inline float operator()(const FilterState<NN>& x, const IR& h) const { 130 return 0.0f; 131 } 132 }; 133 134 template<int32_t K, int32_t mirror = 0, int32_t remaining = K> 135 struct PolyphaseStage { 136 template<typename T, typename IR> 137 inline void operator()(float* &y, const T& x, const IR& h) const { 138 Accumulator<T::n, 1, K, mirror, 0, K - remaining> a; 139 *y++ = a(x, h); 140 PolyphaseStage<K, mirror, remaining - 1> p; 141 p(y, x, h); 142 } 143 }; 144 145 template<int32_t K, int32_t mirror> 146 struct PolyphaseStage<K, mirror, 0> { 147 template<typename T, typename IR> 148 inline void operator()(float* &y, const T& x, const IR& h) const { } 149 }; 150 151 template< 152 SampleRateConversionDirection direction, 153 int32_t ratio, 154 int32_t filter_size> 155 class SampleRateConverter { }; 156 157 template<int32_t filter_size> 158 class SampleRateConverter<SRC_UP, 1, filter_size> { 159 public: 160 SampleRateConverter() { } 161 ~SampleRateConverter() { } 162 163 inline void Init() { } 164 inline int32_t delay() const { return 0; } 165 inline void Process(const float* in, float* out, size_t input_size) { 166 std::copy(&in[0], &in[input_size], &out[0]); 167 } 168 private: 169 DISALLOW_COPY_AND_ASSIGN(SampleRateConverter); 170 }; 171 172 template<int32_t filter_size> 173 class SampleRateConverter<SRC_DOWN, 1, filter_size> { 174 public: 175 SampleRateConverter() { } 176 ~SampleRateConverter() { } 177 178 inline void Init() { } 179 inline int32_t delay() const { return 0; } 180 inline void Process(const float* in, float* out, size_t input_size) { 181 std::copy(&in[0], &in[input_size], &out[0]); 182 } 183 private: 184 DISALLOW_COPY_AND_ASSIGN(SampleRateConverter); 185 }; 186 187 template<int32_t ratio, int32_t filter_size> 188 class SampleRateConverter<SRC_UP, ratio, filter_size> { 189 private: 190 enum { 191 N = filter_size / ratio, 192 K = ratio 193 }; 194 195 public: 196 SampleRateConverter() { } 197 ~SampleRateConverter() { } 198 199 inline void Init() { 200 std::fill(&x_[0], &x_[N], 0); 201 }; 202 203 inline int32_t delay() const { return filter_size / ratio / 2; } 204 205 inline void Process(const float* in, float* out, size_t input_size) { 206 SRC_FIR<SRC_UP, ratio, filter_size> ir; 207 FilterState<N> x; 208 x.Load(x_); 209 while (input_size--) { 210 x.Push(*in++); 211 PolyphaseStage<K, filter_size> polyphase_stage; 212 polyphase_stage(out, x, ir); 213 } 214 x.Save(x_); 215 } 216 217 private: 218 float x_[N]; 219 220 DISALLOW_COPY_AND_ASSIGN(SampleRateConverter); 221 }; 222 223 template<int32_t ratio, int32_t filter_size> 224 class SampleRateConverter<SRC_DOWN, ratio, filter_size> { 225 private: 226 enum { 227 N = filter_size, 228 K = ratio 229 }; 230 231 public: 232 SampleRateConverter() { } 233 ~SampleRateConverter() { } 234 235 inline void Init() { 236 std::fill(&x_[0], &x_[2 * N], 0); 237 x_ptr_ = &x_[N - 1]; 238 }; 239 240 inline int32_t delay() const { return filter_size / 2; } 241 242 inline void Process(const float* in, float* out, size_t input_size) { 243 // When downsampling, the number of input samples must be a multiple 244 // of the downsampling ratio. 245 if ((input_size % ratio) != 0) { 246 return; 247 } 248 249 SRC_FIR<SRC_DOWN, ratio, filter_size> ir; 250 if (input_size >= 8 * filter_size) { 251 std::copy(&in[0], &in[N], &x_[N - 1]); 252 253 // Generate the samples which require access to the history buffer. 254 for (int32_t i = 0; i < N; i += ratio) { 255 Accumulator<N, -1, 1, filter_size> accumulator; 256 *out++ = accumulator(&x_[N - 1 + i], ir); 257 in += ratio; 258 input_size -= ratio; 259 } 260 261 // From now on, all the samples we need to access are located inside 262 // the input buffer passed as an argument, and since the filter 263 // is small, we can unroll the summation loop. 264 if ((input_size / ratio) & 1) { 265 while (input_size) { 266 Accumulator<N, -1, 1, filter_size> accumulator; 267 *out++ = accumulator(in, ir); 268 input_size -= ratio; 269 in += ratio; 270 } 271 } else { 272 while (input_size) { 273 Accumulator<N, -1, 1, filter_size> accumulator; 274 *out++ = accumulator(in, ir); 275 *out++ = accumulator(in + ratio, ir); 276 input_size -= 2 * ratio; 277 in += 2 * ratio; 278 } 279 } 280 281 // Copy last input samples to history buffer. 282 std::copy(&in[-N + 1], &in[0], &x_[0]); 283 } else { 284 // Variant which uses a circular buffer to store history. 285 while (input_size) { 286 for (int32_t i = 0; i < ratio; ++i) { 287 x_ptr_[0] = x_ptr_[N] = *in++; 288 --x_ptr_; 289 if (x_ptr_ < x_) { 290 x_ptr_ += N; 291 } 292 } 293 input_size -= ratio; 294 295 Accumulator<N, 1, 1, filter_size> accumulator; 296 *out++ = accumulator(&x_ptr_[1], ir); 297 } 298 } 299 } 300 301 private: 302 float x_[2 * N]; 303 float* x_ptr_; 304 305 DISALLOW_COPY_AND_ASSIGN(SampleRateConverter); 306 }; 307 308 } // namespace stmlib 309 310 #endif // STMLIB_DSP_SAMPLE_RATE_CONVERTER_H_ 311