1 // ----------------------------------------------------------------------------
2 //
3 // Copyright (C) 2006-2012 Fons Adriaensen <fons@linuxaudio.org>
4 //
5 // This program is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU General Public License as published by
7 // the Free Software Foundation; either version 3 of the License, or
8 // (at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // ----------------------------------------------------------------------------
19
20
21 #include <stdlib.h>
22 #include <stdio.h>
23 #include <string.h>
24 #include <math.h>
25 #include "resampler.h"
26
27
gcd(unsigned int a,unsigned int b)28 static unsigned int gcd (unsigned int a, unsigned int b)
29 {
30 if (a == 0) return b;
31 if (b == 0) return a;
32 while (1)
33 {
34 if (a > b)
35 {
36 a = a % b;
37 if (a == 0) return b;
38 if (a == 1) return 1;
39 }
40 else
41 {
42 b = b % a;
43 if (b == 0) return a;
44 if (b == 1) return 1;
45 }
46 }
47 return 1;
48 }
49
50
Resampler(void)51 Resampler::Resampler (void) noexcept :
52 _table (0),
53 _nchan (0),
54 _buff (0)
55 {
56 reset ();
57 }
58
59
~Resampler(void)60 Resampler::~Resampler (void)
61 {
62 clear ();
63 }
64
65
setup(unsigned int fs_inp,unsigned int fs_out,unsigned int nchan,unsigned int hlen)66 int Resampler::setup (unsigned int fs_inp,
67 unsigned int fs_out,
68 unsigned int nchan,
69 unsigned int hlen)
70 {
71 if ((hlen < 8) || (hlen > 96)) return 1;
72 return setup (fs_inp, fs_out, nchan, hlen, 1.0 - 2.6 / hlen);
73 }
74
75
setup(unsigned int fs_inp,unsigned int fs_out,unsigned int nchan,unsigned int hlen,double frel)76 int Resampler::setup (unsigned int fs_inp,
77 unsigned int fs_out,
78 unsigned int nchan,
79 unsigned int hlen,
80 double frel)
81 {
82 unsigned int g, h, k, n, s;
83 double r;
84 float *B = 0;
85 Resampler_table *T = 0;
86
87 k = s = 0;
88 if (fs_inp && fs_out && nchan)
89 {
90 r = (double) fs_out / (double) fs_inp;
91 g = gcd (fs_out, fs_inp);
92 n = fs_out / g;
93 s = fs_inp / g;
94 if ((16 * r >= 1) && (n <= 1000))
95 {
96 h = hlen;
97 k = 250;
98 if (r < 1)
99 {
100 frel *= r;
101 h = (unsigned int)(ceil (h / r));
102 k = (unsigned int)(ceil (k / r));
103 }
104 T = Resampler_table::create (frel, h, n);
105 B = new float [nchan * (2 * h - 1 + k)];
106 }
107 }
108 clear ();
109 if (T)
110 {
111 _table = T;
112 _buff = B;
113 _nchan = nchan;
114 _inmax = k;
115 _pstep = s;
116 return reset ();
117 }
118 return 1;
119 }
120
121
clear(void)122 void Resampler::clear (void)
123 {
124 Resampler_table::destroy (_table);
125 delete[] _buff;
126 _buff = 0;
127 _table = 0;
128 _nchan = 0;
129 _inmax = 0;
130 _pstep = 0;
131 reset ();
132 }
133
134
inpdist(void) const135 double Resampler::inpdist (void) const noexcept
136 {
137 if (!_table) return 0;
138 return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np;
139 }
140
141
inpsize(void) const142 unsigned int Resampler::inpsize (void) const noexcept
143 {
144 if (!_table) return 0;
145 return 2 * _table->_hl;
146 }
147
148
reset(void)149 bool Resampler::reset (void) noexcept
150 {
151 if (!_table) return false;
152
153 inp_count = 0;
154 out_count = 0;
155 inp_data = nullptr;
156 out_data = nullptr;
157 _index = 0;
158 _nread = 0;
159 _nzero = 0;
160 _phase = 0;
161 _nread = 2 * _table->_hl;
162 return true;
163 }
164
165
process(void)166 bool Resampler::process (void)
167 {
168 unsigned int hl, ph, np, dp, in, nr, nz, i, n, c;
169 float *p1, *p2;
170
171 if (!_table) return false;
172
173 hl = _table->_hl;
174 np = _table->_np;
175 dp = _pstep;
176 in = _index;
177 nr = _nread;
178 ph = _phase;
179 nz = _nzero;
180 n = (2 * hl - nr) * _nchan;
181 p1 = _buff + in * _nchan;
182 p2 = p1 + n;
183
184 while (out_count)
185 {
186 if (nr)
187 {
188 if (inp_count == 0) break;
189 if (inp_data)
190 {
191 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
192 inp_data += _nchan;
193 nz = 0;
194 }
195 else
196 {
197 for (c = 0; c < _nchan; c++) p2 [c] = 0;
198 if (nz < 2 * hl) nz++;
199 }
200 nr--;
201 p2 += _nchan;
202 inp_count--;
203 }
204 else
205 {
206 if (out_data)
207 {
208 if (nz < 2 * hl)
209 {
210 float *c1 = _table->_ctab + hl * ph;
211 float *c2 = _table->_ctab + hl * (np - ph);
212 for (c = 0; c < _nchan; c++)
213 {
214 float *q1 = p1 + c;
215 float *q2 = p2 + c;
216 float s = 1e-20f;
217 for (i = 0; i < hl; i++)
218 {
219 q2 -= _nchan;
220 s += *q1 * c1 [i] + *q2 * c2 [i];
221 q1 += _nchan;
222 }
223 *out_data++ = s - 1e-20f;
224 }
225 }
226 else
227 {
228 for (c = 0; c < _nchan; c++) *out_data++ = 0;
229 }
230 }
231 out_count--;
232
233 ph += dp;
234 if (ph >= np)
235 {
236 nr = ph / np;
237 ph -= nr * np;
238 in += nr;
239 p1 += nr * _nchan;;
240 if (in >= _inmax)
241 {
242 n = (2 * hl - nr) * _nchan;
243 memcpy (_buff, p1, n * sizeof (float));
244 in = 0;
245 p1 = _buff;
246 p2 = p1 + n;
247 }
248 }
249 }
250 }
251 _index = in;
252 _nread = nr;
253 _phase = ph;
254 _nzero = nz;
255
256 return true;
257 }
258
259
260