git ssb

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