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 <zita-resampler/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) :
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 else 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
136 {
137 if (!_table) return 0;
138 return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np;
139 }
140
141
inpsize(void) const142 int Resampler::inpsize (void) const
143 {
144 if (!_table) return 0;
145 return 2 * _table->_hl;
146 }
147
148
reset(void)149 int Resampler::reset (void)
150 {
151 if (!_table) return 1;
152
153 inp_count = 0;
154 out_count = 0;
155 inp_data = 0;
156 out_data = 0;
157 _index = 0;
158 _nread = 0;
159 _nzero = 0;
160 _phase = 0;
161 if (_table)
162 {
163 _nread = 2 * _table->_hl;
164 return 0;
165 }
166 return 1;
167 }
168
169
process(void)170 int Resampler::process (void)
171 {
172 unsigned int hl, ph, np, dp, in, nr, nz, i, n, c;
173 float *p1, *p2;
174
175 if (!_table) return 1;
176
177 hl = _table->_hl;
178 np = _table->_np;
179 dp = _pstep;
180 in = _index;
181 nr = _nread;
182 ph = _phase;
183 nz = _nzero;
184 n = (2 * hl - nr) * _nchan;
185 p1 = _buff + in * _nchan;
186 p2 = p1 + n;
187
188 while (out_count)
189 {
190 if (nr)
191 {
192 if (inp_count == 0) break;
193 if (inp_data)
194 {
195 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
196 inp_data += _nchan;
197 nz = 0;
198 }
199 else
200 {
201 for (c = 0; c < _nchan; c++) p2 [c] = 0;
202 if (nz < 2 * hl) nz++;
203 }
204 nr--;
205 p2 += _nchan;
206 inp_count--;
207 }
208 else
209 {
210 if (out_data)
211 {
212 if (nz < 2 * hl)
213 {
214 float *c1 = _table->_ctab + hl * ph;
215 float *c2 = _table->_ctab + hl * (np - ph);
216 for (c = 0; c < _nchan; c++)
217 {
218 float *q1 = p1 + c;
219 float *q2 = p2 + c;
220 float s = 1e-20f;
221 for (i = 0; i < hl; i++)
222 {
223 q2 -= _nchan;
224 s += *q1 * c1 [i] + *q2 * c2 [i];
225 q1 += _nchan;
226 }
227 *out_data++ = s - 1e-20f;
228 }
229 }
230 else
231 {
232 for (c = 0; c < _nchan; c++) *out_data++ = 0;
233 }
234 }
235 out_count--;
236
237 ph += dp;
238 if (ph >= np)
239 {
240 nr = ph / np;
241 ph -= nr * np;
242 in += nr;
243 p1 += nr * _nchan;;
244 if (in >= _inmax)
245 {
246 n = (2 * hl - nr) * _nchan;
247 memcpy (_buff, p1, n * sizeof (float));
248 in = 0;
249 p1 = _buff;
250 p2 = p1 + n;
251 }
252 }
253 }
254 }
255 _index = in;
256 _nread = nr;
257 _phase = ph;
258 _nzero = nz;
259
260 return 0;
261 }
262
263
264