git ssb

0+

kode54 / syntrax-c



Tree: f8137a6ba7c36d2d53855c2c3c4fdd0961c4d9b6

Files: f8137a6ba7c36d2d53855c2c3c4fdd0961c4d9b6 / src / syntrax / resampler.c

8838 bytesRaw
1#include "resampler.h"
2
3#include <math.h>
4#include <stdlib.h>
5#include <string.h>
6
7/* Copyright (C) 2004-2008 Shay Green.
8 Copyright (C) 2015 Christopher Snowhill. This module is free software; you
9can redistribute it and/or modify it under the terms of the GNU Lesser
10General Public License as published by the Free Software Foundation; either
11version 2.1 of the License, or (at your option) any later version. This
12module is distributed in the hope that it will be useful, but WITHOUT ANY
13WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
14FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
15details. You should have received a copy of the GNU Lesser General Public
16License along with this module; if not, write to the Free Software Foundation,
17Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */
18
19#undef PI
20#define PI 3.1415926535897932384626433832795029
21
22enum { imp_scale = 0x7FFF };
23typedef int16_t imp_t;
24typedef int32_t imp_off_t; /* for max_res of 512 and impulse width of 32, end offsets must be 32 bits */
25
26#if RESAMPLER_BITS == 16
27typedef int32_t intermediate_t;
28#elif RESAMPLER_BITS == 32
29typedef int64_t intermediate_t;
30#endif
31
32static void gen_sinc( double rolloff, int width, double offset, double spacing, double scale,
33 int count, imp_t* out )
34{
35 double angle;
36
37 double const maxh = 256;
38 double const step = PI / maxh * spacing;
39 double const to_w = maxh * 2 / width;
40 double const pow_a_n = pow( rolloff, maxh );
41 scale /= maxh * 2;
42 angle = (count / 2 - 1 + offset) * -step;
43
44 while ( count-- )
45 {
46 double w;
47 *out++ = 0;
48 w = angle * to_w;
49 if ( fabs( w ) < PI )
50 {
51 double rolloff_cos_a = rolloff * cos( angle );
52 double num = 1 - rolloff_cos_a -
53 pow_a_n * cos( maxh * angle ) +
54 pow_a_n * rolloff * cos( (maxh - 1) * angle );
55 double den = 1 - rolloff_cos_a - rolloff_cos_a + rolloff * rolloff;
56 double sinc = scale * num / den - scale;
57
58 out [-1] = (imp_t) (cos( w ) * sinc + sinc);
59 }
60 angle += step;
61 }
62}
63
64enum { width = 32 };
65enum { max_res = 512 };
66enum { min_width = (width < 4 ? 4 : width) };
67enum { adj_width = min_width / 4 * 4 + 2 };
68enum { write_offset = adj_width };
69
70enum { buffer_size = 128 };
71
72typedef struct _resampler
73{
74 int width_;
75 int rate_;
76 int inptr;
77 int infilled;
78 int outptr;
79 int outfilled;
80
81 int latency;
82
83 imp_t const* imp;
84 imp_t impulses [max_res * (adj_width + 2 * (sizeof(imp_off_t) / sizeof(imp_t)))];
85 sample_t buffer_in[buffer_size * 2];
86 sample_t buffer_out[buffer_size];
87} resampler;
88
89void * resampler_create()
90{
91 resampler *r = (resampler *) malloc(sizeof(resampler));
92 if (r) resampler_clear(r);
93 return r;
94}
95
96void * resampler_dup(const void *_r)
97{
98 void *_t = (resampler *) malloc(sizeof(resampler));
99 if (_t) resampler_dup_inplace(_t, _r);
100 return _t;
101}
102
103void resampler_dup_inplace(void *_t, const void *_r)
104{
105 const resampler *r = (const resampler *)_r;
106 resampler *t = (resampler *)_t;
107 if (r && t)
108 {
109 memcpy(t, r, sizeof(resampler));
110 t->imp = t->impulses + (r->imp - r->impulses);
111 }
112 else if (t)
113 {
114 resampler_clear(t);
115 }
116}
117
118void resampler_destroy(void *r)
119{
120 free(r);
121}
122
123void resampler_clear(void *_r)
124{
125 resampler * r = (resampler *)_r;
126 r->width_ = adj_width;
127 r->inptr = 0;
128 r->infilled = 0;
129 r->outptr = 0;
130 r->outfilled = 0;
131 r->latency = 0;
132 r->imp = r->impulses;
133
134 resampler_set_rate(r, 1.0);
135}
136
137void resampler_set_rate( void *_r, double new_factor )
138{
139 int step; //const
140 double filter; //const
141 double fraction, pos;
142 int n;
143
144 resampler *rs = (resampler *)_r;
145 imp_t* out;
146
147 double const rolloff = 0.999;
148 double const gain = 1.0;
149
150 /* determine number of sub-phases that yield lowest error */
151 double ratio_ = 0.0;
152 int res = -1;
153 {
154 double least_error = 2;
155 double pos = 0;
156 int r;
157 for ( r = 1; r <= max_res; r++ )
158 {
159 double nearest, error;
160 pos += new_factor;
161 nearest = floor( pos + 0.5 );
162 error = fabs( pos - nearest );
163 if ( error < least_error )
164 {
165 res = r;
166 ratio_ = nearest / res;
167 least_error = error;
168 }
169 }
170 }
171 rs->rate_ = ratio_;
172
173 /* how much of input is used for each output sample */
174 step = (int) floor( ratio_ );
175 fraction = fmod( ratio_, 1.0 );
176
177 filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_;
178 pos = 0.0;
179 /*int input_per_cycle = 0;*/
180 out = rs->impulses;
181
182 for ( n = res; --n >= 0; )
183 {
184 int cur_step;
185
186 gen_sinc( rolloff, (int) (rs->width_ * filter + 1) & ~1, pos, filter,
187 (double)(imp_scale * gain * filter), (int) rs->width_, out );
188 out += rs->width_;
189
190 cur_step = step;
191 pos += fraction;
192 if ( pos >= 0.9999999 )
193 {
194 pos -= 1.0;
195 cur_step += 1;
196 }
197
198 ((imp_off_t*)out)[0] = (cur_step - rs->width_ + 2) * sizeof (sample_t);
199 ((imp_off_t*)out)[1] = 2 * sizeof (imp_t) + 2 * sizeof (imp_off_t);
200 out += 2 * (sizeof(imp_off_t) / sizeof(imp_t));
201 /*input_per_cycle += cur_step;*/
202 }
203 /* last offset moves back to beginning of impulses*/
204 ((imp_off_t*)out) [-1] -= (char*) out - (char*) rs->impulses;
205
206 rs->imp = rs->impulses;
207}
208
209int resampler_get_free(void *_r)
210{
211 resampler *r = (resampler *)_r;
212 return buffer_size - r->infilled;
213}
214
215int resampler_get_min_fill(void *_r)
216{
217 resampler *r = (resampler *)_r;
218 const int min_needed = write_offset + 1;
219 const int latency = r->latency ? 0 : adj_width;
220 int min_free = min_needed - r->infilled - latency;
221 return min_free < 0 ? 0 : min_free;
222}
223
224void resampler_write_sample(void *_r, sample_t s)
225{
226 resampler *r = (resampler *)_r;
227
228 if (!r->latency)
229 {
230 int i;
231 for ( i = 0; i < adj_width / 2; ++i)
232 {
233 r->buffer_in[r->inptr] = 0;
234 r->buffer_in[buffer_size + r->inptr] = 0;
235 r->inptr = (r->inptr + 1) % (buffer_size);
236 r->infilled += 1;
237 }
238 r->latency = 1;
239 }
240
241 if (r->infilled < buffer_size)
242 {
243 r->buffer_in[r->inptr] = s;
244 r->buffer_in[buffer_size + r->inptr + 0] = s;
245 r->inptr = (r->inptr + 1) % (buffer_size);
246 r->infilled += 1;
247 }
248}
249
250#if defined(_MSC_VER) || defined(__GNUC__)
251#define restrict __restrict
252#endif
253
254static const sample_t * resampler_inner_loop( resampler *r, sample_t** out_,
255 sample_t const* out_end, sample_t const in [], int in_size )
256{
257 in_size -= write_offset;
258 if ( in_size > 0 )
259 {
260 sample_t* restrict out = *out_;
261 sample_t const* const in_end = in + in_size;
262 imp_t const* imp = r->imp;
263
264 do
265 {
266 int n;
267 /* accumulate in extended precision*/
268 int pt = imp [0];
269 intermediate_t s = (intermediate_t)pt * (intermediate_t)(in [0]);
270 if ( out >= out_end )
271 break;
272 for ( n = (adj_width - 2) / 2; n; --n )
273 {
274 pt = imp [1];
275 s += (intermediate_t)pt * (intermediate_t)(in [1]);
276
277 /* pre-increment more efficient on some RISC processors*/
278 imp += 2;
279 pt = imp [0];
280 s += (intermediate_t)pt * (intermediate_t)(in [2]);
281 in += 2;
282 }
283 pt = imp [1];
284 s += (intermediate_t)pt * (intermediate_t)(in [1]);
285
286 /* these two "samples" after the end of the impulse give the
287 * proper offsets to the next input sample and next impulse */
288 in = (sample_t const*) ((char const*) in + ((imp_off_t*)(&imp [2]))[0]); /* some negative value */
289 imp = (imp_t const*) ((char const*) imp + ((imp_off_t*)(&imp [2]))[1]); /* small positive or large negative */
290
291 out [0] = (sample_t) (s >> 15);
292 out += 1;
293 }
294 while ( in < in_end );
295
296 r->imp = imp;
297 *out_ = out;
298 }
299 return in;
300}
301
302#undef restrict
303
304static int resampler_wrapper( resampler *r, sample_t out [], int* out_size,
305 sample_t const in [], int in_size )
306{
307 sample_t* out_ = out;
308 int result = resampler_inner_loop( r, &out_, out + *out_size, in, in_size ) - in;
309
310 *out_size = out_ - out;
311 return result;
312}
313
314static void resampler_fill( resampler *r )
315{
316 while (!r->outfilled && r->infilled)
317 {
318 int inread;
319
320 int writepos = ( r->outptr + r->outfilled ) % (buffer_size);
321 int writesize = (buffer_size) - writepos;
322 if ( writesize > ( buffer_size - r->outfilled ) )
323 writesize = buffer_size - r->outfilled;
324 inread = resampler_wrapper(r, &r->buffer_out[writepos], &writesize, &r->buffer_in[buffer_size + r->inptr - r->infilled], r->infilled);
325 r->infilled -= inread;
326 r->outfilled += writesize;
327 if (!inread)
328 break;
329 }
330}
331
332int resampler_get_avail(void *_r)
333{
334 resampler *r = (resampler *)_r;
335 if (r->outfilled < 1 && r->infilled >= r->width_)
336 resampler_fill( r );
337 return r->outfilled;
338}
339
340static void resampler_read_sample_internal( resampler *r, sample_t *s, int advance )
341{
342 if (r->outfilled < 1)
343 resampler_fill( r );
344 if (r->outfilled < 1)
345 {
346 *s = 0;
347 return;
348 }
349 *s = r->buffer_out[r->outptr];
350 if (advance)
351 {
352 r->outptr = (r->outptr + 1) % (buffer_size);
353 r->outfilled -= 1;
354 }
355}
356
357void resampler_read_sample( void *_r, sample_t *s )
358{
359 resampler *r = (resampler *)_r;
360 resampler_read_sample_internal(r, s, 1);
361}
362
363void resampler_peek_sample( void *_r, sample_t *s )
364{
365 resampler *r = (resampler *)_r;
366 resampler_read_sample_internal(r, s, 0);
367}
368

Built with git-ssb-web