


/*




* Rational Sample Rate Conversion




* Copyright (C) 2012, 2013 Thomas Tsou <tom@tsou.cc>




*




* SPDXLicenseIdentifier: LGPL2.1+




*




* This library is free software; you can redistribute it and/or




* modify it under the terms of the GNU Lesser General Public




* License as published by the Free Software Foundation; either




* version 2.1 of the License, or (at your option) any later version.




*




* This library is distributed in the hope that it will be useful,




* but WITHOUT ANY WARRANTY; without even the implied warranty of




* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU




* Lesser General Public License for more details.




*/








#include <stdlib.h>




#include <math.h>




#include <string.h>




#include <malloc.h>




#include <iostream>




#include <algorithm>








#include "Resampler.h"








extern "C" {




#include "convolve.h"




}








#ifndef M_PI




#define M_PI 3.14159265358979323846264338327f




#endif








#define MAX_OUTPUT_LEN 4096








using namespace std;








static float sinc(float x)




{




if (x == 0.0)




return 0.9999999999;








return sin(M_PI * x) / (M_PI * x);




}








void Resampler::initFilters(float bw)




{




float cutoff;




float sum = 0.0f, scale = 0.0f;








/*




* Allocate partition filters and the temporary prototype filter




* according to numerator of the rational rate. Coefficients are




* real only and must be 16byte memory aligned for SSE usage.




*/




auto proto = vector<float>(p * filt_len);




for (auto &part : partitions)




part = (complex<float> *) memalign(16, filt_len * sizeof(complex<float>));








/*




* Generate the prototype filter with a Blackmanharris window.




* Scale coefficients with DC filter gain set to unity divided




* by the number of filter partitions.




*/




float a0 = 0.35875;




float a1 = 0.48829;




float a2 = 0.14128;




float a3 = 0.01168;








if (p > q)




cutoff = (float) p;




else




cutoff = (float) q;








float midpt = (proto.size()  1) / 2.0;




for (size_t i = 0; i < proto.size(); i++) {




proto[i] = sinc(((float) i  midpt) / cutoff * bw);




proto[i] *= a0 




a1 * cos(2 * M_PI * i / (proto.size()  1)) +




a2 * cos(4 * M_PI * i / (proto.size()  1)) 




a3 * cos(6 * M_PI * i / (proto.size()  1));




sum += proto[i];




}




scale = p / sum;








/* Populate filter partitions from the prototype filter */




for (size_t i = 0; i < filt_len; i++) {




for (size_t n = 0; n < p; n++)




partitions[n][i] = complex<float>(proto[i * p + n] * scale);




}








/* Store filter taps in reverse */




for (auto &part : partitions)




reverse(&part[0], &part[filt_len]);




}








#ifndef __OPTIMIZE__




static bool check_vec_len(int in_len, int out_len, int p, int q)




{




if (in_len % q) {




std::cerr << "Invalid input length " << in_len




<< " is not multiple of " << q << std::endl;




return false;




}








if (out_len % p) {




std::cerr << "Invalid output length " << out_len




<< " is not multiple of " << p << std::endl;




return false;




}








if ((in_len / q) != (out_len / p)) {




std::cerr << "Input/output block length mismatch" << std::endl;




std::cerr << "P = " << p << ", Q = " << q << std::endl;




std::cerr << "Input len: " << in_len << std::endl;




std::cerr << "Output len: " << out_len << std::endl;




return false;




}








if (out_len > MAX_OUTPUT_LEN) {




std::cerr << "Block length of " << out_len




<< " exceeds max of " << MAX_OUTPUT_LEN << std::endl;




return false;




}








return true;




}




#endif








int Resampler::rotate(const float *in, size_t in_len, float *out, size_t out_len)




{




int n, path;




#ifndef __OPTIMIZE__




if (!check_vec_len(in_len, out_len, p, q))




return 1;




#endif




/* Generate output from precomputed input/output paths */




for (size_t i = 0; i < out_len; i++) {




n = in_index[i];




path = out_path[i];








convolve_real(in, in_len,




reinterpret_cast<float *>(partitions[path]),




filt_len, &out[2 * i], out_len  i,




n, 1);




}








return out_len;




}








bool Resampler::init(float bw)




{




if (p == 0  q == 0  filt_len == 0) return false;








/* Filterbank filter internals */




initFilters(bw);








/* Precompute filterbank paths */




int i = 0;




for (auto &index : in_index)




index = (q * i++) / p;




i = 0;




for (auto &path : out_path)




path = (q * i++) % p;








return true;




}








size_t Resampler::len()




{




return filt_len;




}








Resampler::Resampler(size_t p, size_t q, size_t filt_len)




: in_index(MAX_OUTPUT_LEN), out_path(MAX_OUTPUT_LEN), partitions(p)




{




this>p = p;




this>q = q;




this>filt_len = filt_len;




}








Resampler::~Resampler()




{




for (auto &part : partitions)




free(part);




}
