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 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
64
enum { width = 32 };
65
enum { stereo = 2 };
66
enum { max_res = 512 };
67
enum { min_width = (width < 4 ? 4 : width) };
68
enum { adj_width = min_width / 4 * 4 + 2 };
69
enum { write_offset = adj_width * stereo };
70
71
enum { buffer_size = 128 };
72
73
typedef struct _resampler
74
{
75
	int width_;
76
	int rate_;
77
	int inptr;
78
	int infilled;
79
	int outptr;
80
	int outfilled;
81
82
	int latency;
83
84
	imp_t const* imp;
85
	imp_t impulses [max_res * (adj_width + 2 * (sizeof(imp_off_t) / sizeof(imp_t)))];
86
	sample_t buffer_in[buffer_size * stereo * 2];
87
	sample_t buffer_out[buffer_size * stereo];
88
} resampler;
89
90
void * resampler_create()
91
{
92
	resampler *r = (resampler *) malloc(sizeof(resampler));
93
	if (r) resampler_clear(r);
94
	return r;
95
}
96
97
void * resampler_dup(const void *_r)
98
{
99
    void *_t = (resampler *) malloc(sizeof(resampler));
100
    if (_t) resampler_dup_inplace(_t, _r);
101
    return _t;
102
}
103
104
void resampler_dup_inplace(void *_t, const void *_r)
105
{
106
	const resampler *r = (const resampler *)_r;
107
	resampler *t = (resampler *)_t;
108
	if (r && t)
109
	{
110
		memcpy(t, r, sizeof(resampler));
111
		t->imp = t->impulses + (r->imp - r->impulses);
112
	}
113
	else if (t)
114
	{
115
		resampler_clear(t);
116
	}
117
}
118
119
void resampler_destroy(void *r)
120
{
121
	free(r);
122
}
123
124
void resampler_clear(void *_r)
125
{
126
	resampler * r = (resampler *)_r;
127
	r->width_ = adj_width;
128
	r->inptr = 0;
129
	r->infilled = 0;
130
	r->outptr = 0;
131
	r->outfilled = 0;
132
	r->latency = 0;
133
	r->imp = r->impulses;
134
135
	resampler_set_rate(r, 1.0);
136
}
137
138
void resampler_set_rate( void *_r, double new_factor )
139
{
140
	int step; //const
141
	double filter; //const
142
	double fraction, pos;
143
    int n;
144
145
	resampler *rs = (resampler *)_r;
146
    imp_t* out;
147
148
	double const rolloff = 0.999;
149
	double const gain = 1.0;
150
151
	/* determine number of sub-phases that yield lowest error */
152
	double ratio_ = 0.0;
153
	int res = -1;
154
	{
155
		double least_error = 2;
156
		double pos = 0;
157
		int r;
158
		for ( r = 1; r <= max_res; r++ )
159
		{
160
			double nearest, error;
161
			pos += new_factor;
162
			nearest = floor( pos + 0.5 );
163
			error = fabs( pos - nearest );
164
			if ( error < least_error )
165
			{
166
				res = r;
167
				ratio_ = nearest / res;
168
				least_error = error;
169
			}
170
		}
171
	}
172
	rs->rate_ = ratio_;
173
174
	/* how much of input is used for each output sample */
175
	step = stereo * (int) floor( ratio_ );
176
	fraction = fmod( ratio_, 1.0 );
177
178
	filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_;
179
	pos = 0.0;
180
	/*int input_per_cycle = 0;*/
181
	out = rs->impulses;
182
	
183
	for ( n = res; --n >= 0; )
184
	{
185
        int cur_step;
186
        
187
		gen_sinc( rolloff, (int) (rs->width_ * filter + 1) & ~1, pos, filter,
188
				(double)(imp_scale * gain * filter), (int) rs->width_, out );
189
		out += rs->width_;
190
191
		cur_step = step;
192
		pos += fraction;
193
		if ( pos >= 0.9999999 )
194
		{
195
			pos -= 1.0;
196
			cur_step += stereo;
197
		}
198
199
		((imp_off_t*)out)[0] = (cur_step - rs->width_ * 2 + 4) * sizeof (sample_t);
200
		((imp_off_t*)out)[1] = 2 * sizeof (imp_t) + 2 * sizeof (imp_off_t);
201
		out += 2 * (sizeof(imp_off_t) / sizeof(imp_t));
202
		/*input_per_cycle += cur_step;*/
203
	}
204
	/* last offset moves back to beginning of impulses*/
205
	((imp_off_t*)out) [-1] -= (char*) out - (char*) rs->impulses;
206
207
	rs->imp = rs->impulses;
208
}
209
210
int resampler_get_free(void *_r)
211
{
212
	resampler *r = (resampler *)_r;
213
	return buffer_size * stereo - r->infilled;
214
}
215
216
int resampler_get_min_fill(void *_r)
217
{
218
	resampler *r = (resampler *)_r;
219
	const int min_needed = write_offset + stereo;
220
	const int latency = r->latency ? 0 : adj_width;
221
	int min_free = min_needed - r->infilled - latency;
222
	return min_free < 0 ? 0 : min_free;
223
}
224
225
void resampler_write_pair(void *_r, sample_t ls, sample_t rs)
226
{
227
	resampler *r = (resampler *)_r;
228
229
	if (!r->latency)
230
	{
231
	    int i;
232
		for ( i = 0; i < adj_width / 2; ++i)
233
		{
234
			r->buffer_in[r->inptr + 0] = 0;
235
			r->buffer_in[r->inptr + 1] = 0;
236
			r->buffer_in[buffer_size * stereo + r->inptr + 0] = 0;
237
			r->buffer_in[buffer_size * stereo + r->inptr + 1] = 0;
238
			r->inptr = (r->inptr + stereo) % (buffer_size * stereo);
239
			r->infilled += stereo;
240
		}
241
		r->latency = 1;
242
	}
243
244
	if (r->infilled < buffer_size * stereo)
245
	{
246
		r->buffer_in[r->inptr + 0] = ls;
247
		r->buffer_in[r->inptr + 1] = rs;
248
		r->buffer_in[buffer_size * stereo + r->inptr + 0] = ls;
249
		r->buffer_in[buffer_size * stereo + r->inptr + 1] = rs;
250
		r->inptr = (r->inptr + stereo) % (buffer_size * stereo);
251
		r->infilled += stereo;
252
	}
253
}
254
255
#if defined(_MSC_VER) || defined(__GNUC__)
256
#define restrict __restrict
257
#endif
258
259
static const sample_t * resampler_inner_loop( resampler *r, sample_t** out_,
260
		sample_t const* out_end, sample_t const in [], int in_size )
261
{
262
	in_size -= write_offset;
263
	if ( in_size > 0 )
264
	{
265
		sample_t* restrict out = *out_;
266
		sample_t const* const in_end = in + in_size;
267
		imp_t const* imp = r->imp;
268
269
		do
270
		{
271
			int n;
272
			/* accumulate in extended precision*/
273
			int pt = imp [0];
274
			intermediate_t l = (intermediate_t)pt * (intermediate_t)(in [0]);
275
			intermediate_t r = (intermediate_t)pt * (intermediate_t)(in [1]);
276
			if ( out >= out_end )
277
				break;
278
			for ( n = (adj_width - 2) / 2; n; --n )
279
			{
280
				pt = imp [1];
281
				l += (intermediate_t)pt * (intermediate_t)(in [2]);
282
				r += (intermediate_t)pt * (intermediate_t)(in [3]);
283
284
				/* pre-increment more efficient on some RISC processors*/
285
				imp += 2;
286
				pt = imp [0];
287
				r += (intermediate_t)pt * (intermediate_t)(in [5]);
288
				in += 4;
289
				l += (intermediate_t)pt * (intermediate_t)(in [0]);
290
			}
291
			pt = imp [1];
292
			l += (intermediate_t)pt * (intermediate_t)(in [2]);
293
			r += (intermediate_t)pt * (intermediate_t)(in [3]);
294
295
			/* these two "samples" after the end of the impulse give the
296
			 * proper offsets to the next input sample and next impulse */
297
			in  = (sample_t const*) ((char const*) in  + ((imp_off_t*)(&imp [2]))[0]); /* some negative value */
298
			imp = (imp_t const*) ((char const*) imp + ((imp_off_t*)(&imp [2]))[1]); /* small positive or large negative */
299
300
			out [0] = (sample_t) (l >> 15);
301
			out [1] = (sample_t) (r >> 15);
302
			out += 2;
303
		}
304
		while ( in < in_end );
305
306
		r->imp = imp;
307
		*out_ = out;
308
	}
309
	return in;
310
}
311
312
#undef restrict
313
314
static int resampler_wrapper( resampler *r, sample_t out [], int* out_size,
315
		sample_t const in [], int in_size )
316
{
317
	sample_t* out_ = out;
318
	int result = resampler_inner_loop( r, &out_, out + *out_size, in, in_size ) - in;
319
320
	*out_size = out_ - out;
321
	return result;
322
}
323
324
static void resampler_fill( resampler *r )
325
{
326
	while (!r->outfilled && r->infilled)
327
	{
328
		int inread;
329
330
		int writepos = ( r->outptr + r->outfilled ) % (buffer_size * stereo);
331
		int writesize = (buffer_size * stereo) - writepos;
332
		if ( writesize > ( buffer_size * stereo - r->outfilled ) )
333
				writesize = buffer_size * stereo - r->outfilled;
334
		inread = resampler_wrapper(r, &r->buffer_out[writepos], &writesize, &r->buffer_in[buffer_size * stereo + r->inptr - r->infilled], r->infilled);
335
		r->infilled -= inread;
336
		r->outfilled += writesize;
337
		if (!inread)
338
			break;
339
	}
340
}
341
342
int resampler_get_avail(void *_r)
343
{
344
	resampler *r = (resampler *)_r;
345
	if (r->outfilled < stereo && r->infilled >= r->width_)
346
	  resampler_fill( r );
347
	return r->outfilled;
348
}
349
350
static void resampler_read_pair_internal( resampler *r, sample_t *ls, sample_t *rs, int advance )
351
{
352
	if (r->outfilled < stereo)
353
	  resampler_fill( r );
354
	if (r->outfilled < stereo)
355
	{
356
		*ls = 0;
357
		*rs = 0;
358
		return;
359
	}
360
	*ls = r->buffer_out[r->outptr + 0];
361
	*rs = r->buffer_out[r->outptr + 1];
362
	if (advance)
363
	{
364
		r->outptr = (r->outptr + 2) % (buffer_size * stereo);
365
		r->outfilled -= stereo;
366
	}
367
}
368
369
void resampler_read_pair( void *_r, sample_t *ls, sample_t *rs )
370
{
371
	resampler *r = (resampler *)_r;
372
	resampler_read_pair_internal(r, ls, rs, 1);
373
}
374
375
void resampler_peek_pair( void *_r, sample_t *ls, sample_t *rs )
376
{
377
	resampler *r = (resampler *)_r;
378
	resampler_read_pair_internal(r, ls, rs, 0);
379
}
380