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 #include <stdlib.h>
21 #include <stdio.h>
22 #include <string.h>
23 #include <math.h>
24
25 #include "zita-resampler/resampler.h"
26
27 using namespace ArdourZita;
28
gcd(unsigned int a,unsigned int b)29 static unsigned int gcd (unsigned int a, unsigned int b)
30 {
31 if (a == 0) return b;
32 if (b == 0) return a;
33 while (1) {
34 if (a > b) {
35 a = a % b;
36 if (a == 0) return b;
37 if (a == 1) return 1;
38 } else {
39 b = b % a;
40 if (b == 0) return a;
41 if (b == 1) return 1;
42 }
43 }
44 return 1;
45 }
46
Resampler(void)47 Resampler::Resampler (void)
48 : _table (0)
49 , _nchan (0)
50 , _buff (0)
51 {
52 reset ();
53 }
54
~Resampler(void)55 Resampler::~Resampler (void)
56 {
57 clear ();
58 }
59
60 int
setup(unsigned int fs_inp,unsigned int fs_out,unsigned int nchan,unsigned int hlen)61 Resampler::setup (unsigned int fs_inp,
62 unsigned int fs_out,
63 unsigned int nchan,
64 unsigned int hlen)
65 {
66 if ((hlen < 8) || (hlen > 96)) return 1;
67 return setup (fs_inp, fs_out, nchan, hlen, 1.0 - 2.6 / hlen);
68 }
69
70 int
setup(unsigned int fs_inp,unsigned int fs_out,unsigned int nchan,unsigned int hlen,double frel)71 Resampler::setup (unsigned int fs_inp,
72 unsigned int fs_out,
73 unsigned int nchan,
74 unsigned int hlen,
75 double frel)
76 {
77 unsigned int g, h, k, n, s;
78 double r;
79 float *B = 0;
80 Resampler_table *T = 0;
81
82 k = s = 0;
83 if (fs_inp && fs_out && nchan) {
84 r = (double) fs_out / (double) fs_inp;
85 g = gcd (fs_out, fs_inp);
86 n = fs_out / g;
87 s = fs_inp / g;
88 if ((16 * r >= 1) && (n <= 1000)) {
89 h = hlen;
90 k = 250;
91 if (r < 1) {
92 frel *= r;
93 h = (unsigned int)(ceil (h / r));
94 k = (unsigned int)(ceil (k / r));
95 }
96 T = Resampler_table::create (frel, h, n);
97 B = new float [nchan * (2 * h - 1 + k)];
98 }
99 }
100 clear ();
101 if (T) {
102 _table = T;
103 _buff = B;
104 _nchan = nchan;
105 _inmax = k;
106 _pstep = s;
107 return reset ();
108 }
109 else return 1;
110 }
111
112 void
clear(void)113 Resampler::clear (void)
114 {
115 Resampler_table::destroy (_table);
116 delete[] _buff;
117 _buff = 0;
118 _table = 0;
119 _nchan = 0;
120 _inmax = 0;
121 _pstep = 0;
122 reset ();
123 }
124
125 double
inpdist(void) const126 Resampler::inpdist (void) const
127 {
128 if (!_table) return 0;
129 return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np;
130 }
131
132 int
inpsize(void) const133 Resampler::inpsize (void) const
134 {
135 if (!_table) return 0;
136 return 2 * _table->_hl;
137 }
138
139 int
reset(void)140 Resampler::reset (void)
141 {
142 if (!_table) return 1;
143
144 inp_count = 0;
145 out_count = 0;
146 inp_data = 0;
147 out_data = 0;
148 _index = 0;
149 _nread = 0;
150 _nzero = 0;
151 _phase = 0;
152 if (_table) {
153 _nread = 2 * _table->_hl;
154 return 0;
155 }
156 return 1;
157 }
158
159 int
process(void)160 Resampler::process (void)
161 {
162 unsigned int hl, ph, np, dp, in, nr, nz, i, n, c;
163 float *p1, *p2;
164
165 if (!_table) return 1;
166
167 hl = _table->_hl;
168 np = _table->_np;
169 dp = _pstep;
170 in = _index;
171 nr = _nread;
172 ph = _phase;
173 nz = _nzero;
174 n = (2 * hl - nr) * _nchan;
175 p1 = _buff + in * _nchan;
176 p2 = p1 + n;
177
178 while (out_count) {
179 if (nr) {
180 if (inp_count == 0) break;
181 if (inp_data) {
182 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
183 inp_data += _nchan;
184 nz = 0;
185 } else {
186 for (c = 0; c < _nchan; c++) p2 [c] = 0;
187 if (nz < 2 * hl) nz++;
188 }
189 nr--;
190 p2 += _nchan;
191 inp_count--;
192 } else {
193 if (out_data) {
194 if (nz < 2 * hl) {
195 float *c1 = _table->_ctab + hl * ph;
196 float *c2 = _table->_ctab + hl * (np - ph);
197 for (c = 0; c < _nchan; c++) {
198 float *q1 = p1 + c;
199 float *q2 = p2 + c;
200 float s = 1e-20f;
201 for (i = 0; i < hl; i++) {
202 q2 -= _nchan;
203 s += *q1 * c1 [i] + *q2 * c2 [i];
204 q1 += _nchan;
205 }
206 *out_data++ = s - 1e-20f;
207 }
208 } else {
209 for (c = 0; c < _nchan; c++) *out_data++ = 0;
210 }
211 }
212 out_count--;
213
214 ph += dp;
215 if (ph >= np) {
216 nr = ph / np;
217 ph -= nr * np;
218 in += nr;
219 p1 += nr * _nchan;;
220 if (in >= _inmax) {
221 n = (2 * hl - nr) * _nchan;
222 memcpy (_buff, p1, n * sizeof (float));
223 in = 0;
224 p1 = _buff;
225 p2 = p1 + n;
226 }
227 }
228 }
229 }
230 _index = in;
231 _nread = nr;
232 _phase = ph;
233 _nzero = nz;
234
235 return 0;
236 }
237