1 /* resampv.c -- use sinc interpolation to resample at a time-varying sample rate */
2
3
4 /* CHANGE LOG
5 * --------------------------------------------------------------------
6 * 28Apr03 dm min->MIN, max->MAX
7 */
8
9
10 #include "stdio.h"
11 #ifndef mips
12 #include "stdlib.h"
13 #endif
14 #include "xlisp.h"
15 #include "sound.h"
16
17 #include "falloc.h"
18 #include "cext.h"
19 #include "resampv.h"
20 #include "fresample.h"
21 #include "ffilterkit.h"
22 #include "fsmallfilter.h"
23 #include "assert.h"
24
25
26 /* Algorithm:
27 First compute a factor = ratio of new sample rate to original sample rate.
28 We have Time, the offset into X
29 We want Xoff = ((susp->Nmult + 1) / 2.0) * MAX(1.0, 1.0 / factor) + 10
30 samples on either side of Time before we interpolate.
31 If Xoff * 2 > Xsize, then we're in trouble because X is not big enough.
32 Assume this is a pathalogical case, raise the cutoff frequency to
33 reduce Xoff to less than Xsize/2.
34 If Time is too small, then we're in trouble because we've lost the
35 beginning of the buffer. Raise the cutoff frequency until Xoff is
36 less than Time. This should only happen if factor suddenly drops.
37 If Time is too big, we can handle it: shift X down and load X with new
38 samples. When X is shifted by N samples, N is subtracted from Time.
39 To minimize the "Time is too small" case, don't shift too far: leave
40 a cushion of Xoff * 2 samples rather than the usual Xoff.
41
42 Now compute a sample at Time using SrcUD and output it.
43
44 What is Time?
45 Time is the offset into X, so Time is g_of_now - (sum of all X shifts)
46 So, let Time = g_of_now - shift_sum
47 Whenever shift_sum or g_of_now is updated, recompute Time
48
49 To compute the next g_of_now, do a lookup of g at now + 1/sr,
50 using linear interpolation (be sure to do computation with
51 doubles to minimize sampling time jitter).
52
53 */
54
55 /* maximum ratio for downsampling (downsampling will still take place,
56 * but the lowest prefilter cutoff frequency will be
57 * (original_sample_rate/2) / MAX_FACTOR_INVERSE
58 */
59 #define MAX_FACTOR_INVERSE 64
60
61 typedef struct resamplev_susp_struct {
62 snd_susp_node susp;
63 int64_t terminate_cnt;
64 boolean logically_stopped;
65 sound_type f;
66 int f_cnt;
67 sample_block_values_type f_ptr;
68
69 sound_type g;
70 int g_cnt;
71 sample_block_values_type g_ptr;
72 double prev_g; /* data for interpolation: */
73 double next_g;
74 double phase_in_g;
75 double phase_in_g_increment;
76 double g_of_now;
77
78 float *X;
79 long Xsize;
80 double Time; /* location (offset) in X of next output sample */
81 double shift_sum; /* total amount by which we have shifted X; also, the
82 sample number of X[0] */
83 double LpScl;
84 double factor_inverse; /* computed at every sample from g */
85 /* this is the amount by which we step through the input signal, so
86 factor_inverse is the output_sample_rate / input_sample_rate, and
87 factor is the input_sample_rate / output_sample_rate. Alternatively,
88 factor is the amount to downsample and
89 factor_inverse is the amount to upsample. */
90 /* double factor; -- computed from factor_inverse */
91 sample_type *Imp;
92 sample_type *ImpD;
93 boolean interpFilt;
94 int Nmult;
95 int Nwing;
96 int Xp; /* first free location at end of X */
97 int Xoff; /* number of extra samples at beginning and end of X */
98 } resamplev_susp_node, *resamplev_susp_type;
99
100 void resamplev_free(snd_susp_type a_susp);
101 void resampv_refill(resamplev_susp_type susp);
102
103 /* Sampling rate conversion subroutine
104 * Note that this is not the same as SrcUD in resamp.c!
105 * X[] is the input signal to be resampled,
106 * dt is the ratio of sample rates; when dt=1, the skip size is
107 * Npc/dt = Npc, where Npc is how many filter coefficients to
108 * get the cutoff frequency equal to the Nyquist rate. As dt
109 * gets larger, we step through the filter more slowly, so low-pass
110 * filtering occurs. As dt gets smaller, it is X[] that limits
111 * frequency, and we use the filter to interpolate samples (upsample).
112 * Therefore, dt>1 means downsample, dt<1 means upsample.
113 * dt is how much we increment Time to compute each output sample.
114 * Time is the offset in samples, including fractional samples, of X
115 * Nwing is the size of one wing of the filter
116 * LpScl is a corrective scale factor to make the gain == 1 or whatever
117 * (Nyquist uses a gain of 0.95 to minimize clipping when peaks are
118 * interpolated.)
119 * Imp[] and ImpD[] are the filter coefficient table and table differences
120 * (for interpolation)
121 * Interp is true to interpolate filter coefficient lookup
122 */
SrcUD(float X[],double dt,double Time,int Nwing,double LpScl,float Imp[],float ImpD[],boolean Interp)123 static float SrcUD(float X[], double dt, double Time,
124 int Nwing, double LpScl,
125 float Imp[], float ImpD[], boolean Interp)
126 {
127 mem_float *Xp;
128 fast_float v;
129
130 double dh; /* Step through filter impulse response */
131 long iTime = (long) Time;
132
133 dh = MIN(Npc, Npc/dt); /* Filter sampling period */
134
135 Xp = &X[iTime]; /* Ptr to current input sample */
136 v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, Time - iTime,
137 -1, dh); /* Perform left-wing inner product */
138 v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1, (1 + iTime) - Time,
139 1, dh); /* Perform right-wing inner product */
140 v *= LpScl; /* Normalize for unity filter gain */
141 return (float) v;
142 }
143
144
resamplev__fetch(snd_susp_type a_susp,snd_list_type snd_list)145 void resamplev__fetch(snd_susp_type a_susp, snd_list_type snd_list)
146 {
147 resamplev_susp_type susp = (resamplev_susp_type) a_susp;
148 int cnt = 0; /* how many samples computed */
149 sample_block_type out;
150 /* note that in this fetch routine, out_ptr is used to remember where
151 * to put the "real" output, while X_ptr_reg is used in the inner
152 * loop that copies input samples into X, a buffer
153 */
154 register sample_block_values_type out_ptr;
155
156 falloc_sample_block(out, "resamplev__fetch");
157 out_ptr = out->samples;
158 snd_list->block = out;
159
160
161 while (cnt < max_sample_block_len) { /* outer loop */
162 /* fetch g until we have points to interpolate */
163 while (susp->phase_in_g >= 1.0) {
164 susp->prev_g = susp->next_g;
165 if (susp->g_cnt == 0) {
166 susp_get_samples(g, g_ptr, g_cnt);
167 if (susp->g->logical_stop_cnt == susp->g->current - susp->g_cnt) {
168 if (susp->susp.log_stop_cnt == UNKNOWN) {
169 susp->susp.log_stop_cnt = susp->susp.current + cnt;
170 }
171 }
172 if (susp->g_ptr == zero_block->samples &&
173 susp->terminate_cnt == UNKNOWN) {
174 susp->terminate_cnt = susp->susp.current + cnt;
175 }
176 }
177 susp->next_g = susp_fetch_sample(g, g_ptr, g_cnt);
178 susp->phase_in_g -= 1.0;
179
180 if (susp->next_g < susp->prev_g) {
181 susp->next_g = susp->prev_g; // prevent time from going backward
182 }
183 /* factor_inverse = 1/factor = how many samples of f per
184 * output sample = change-in-g / output-samples-per-g-sample
185 * = change-in-g * phase_in_g_increment
186 */
187 susp->factor_inverse = susp->phase_in_g_increment *
188 (susp->next_g - susp->prev_g);
189 if (susp->factor_inverse > MAX_FACTOR_INVERSE)
190 susp->factor_inverse = MAX_FACTOR_INVERSE;
191
192 /* update Xoff, which depends upon factor_inverse: */
193 susp->Xoff = (int) (((susp->Nmult + 1) / 2.0) *
194 MAX(1.0, susp->factor_inverse)) + 10;
195 if (susp->Xoff * 2 > susp->Xsize) {
196 /* bad because X is not big enough for filter, so we'll
197 * raise the cutoff frequency as necessary
198 */
199 susp->factor_inverse = ((susp->Xsize / 2) - 10 ) /
200 ((susp->Nmult + 1) / 2.0);
201 susp->Xoff = (susp->Xsize / 2) - 2 /* fudge factor */;
202 }
203 }
204 susp->g_of_now = susp->prev_g +
205 susp->phase_in_g * (susp->next_g - susp->prev_g);
206 susp->Time = susp->g_of_now - susp->shift_sum;
207 susp->phase_in_g += susp->phase_in_g_increment;
208
209 /* now we have a position (g_of_now) and a factor */
210 /* See if enough of f is in X */
211 if (susp->Xoff > susp->Time) {
212 /* there are not enough samples before Time in X, so
213 * modify factor_inverse to fix it
214 */
215 susp->factor_inverse = (susp->Time - 10.0) /
216 ((susp->Nmult + 1) / 2.0);
217
218 } else if ((susp->Xsize - susp->Xoff) < susp->Time) {
219 /* Time is too close to the end of the buffer, slide the samples
220 down. If there's room, leave 2*Xoff samples at beginning of
221 * buffer. Otherwise leave as little as Xoff: */
222 int i, dist, ntime;
223 ntime = susp->Xoff * 2; /* shift Time near to this index in X */
224 dist = ((int) susp->Time) - ntime;
225 if (dist < 1 && (ntime * 2 < susp->Xsize)) {
226 /* not enough room, so leave at least Xoff. */
227 ntime = susp->Xoff;
228 if (susp->Xsize / 2 - ntime > 2) {
229 /* There is some extra space. Use half to extend ntime, allowing
230 for a possible increase in Xoff that will require more history;
231 the other half reduces the amount of buffer copying needed. */
232 ntime += (susp->Xsize / 2 - ntime) / 2;
233 }
234 dist = ((int) susp->Time) - ntime;
235 }
236 /* shift everything in X by dist, adjust Time etc. */
237 for (i = 0; i < susp->Xsize - dist; i++) {
238 susp->X[i] = susp->X[i + dist];
239 }
240 susp->Xp -= dist;
241 resampv_refill(susp);
242 susp->shift_sum += dist;
243 susp->Time = susp->g_of_now - susp->shift_sum;
244 }
245
246 /* second, compute a sample to output */
247
248 /* don't run past terminate time */
249 if (susp->terminate_cnt == susp->susp.current + cnt) {
250 snd_list->block_len = cnt;
251 if (cnt > 0) {
252 susp->susp.current += cnt;
253 snd_list = snd_list->u.next;
254 snd_list->u.next = snd_list_create(&susp->susp);
255 snd_list->block = internal_zero_block;
256 snd_list_terminate(snd_list);
257 } else {
258 snd_list_terminate(snd_list);
259 }
260 return;
261 } else {
262 double scale = susp->LpScl;
263 float tmp;
264 if (susp->factor_inverse > 1) scale /= susp->factor_inverse;
265 tmp = SrcUD(susp->X, susp->factor_inverse,
266 susp->Time, susp->Nwing, scale, susp->Imp,
267 susp->ImpD, susp->interpFilt);
268 *out_ptr++ = tmp;
269 }
270 cnt++;
271 }
272 snd_list->block_len = cnt;
273 susp->susp.current += cnt;
274 assert(cnt > 0);
275 } /* resamplev__fetch */
276
277
resampv_refill(resamplev_susp_type susp)278 void resampv_refill(resamplev_susp_type susp) {
279 int togo, n;
280 register sample_type *f_ptr_reg;
281 register sample_type *X_ptr_reg;
282
283 while (susp->Xp < susp->Xsize) { /* outer loop */
284
285 /* read samples from susp->f into X */
286 togo = susp->Xsize - susp->Xp;
287
288 /* don't run past the f input sample block: */
289 susp_check_samples(f, f_ptr, f_cnt);
290 togo = MIN(togo, susp->f_cnt);
291
292 n = togo;
293 f_ptr_reg = susp->f_ptr;
294 X_ptr_reg = &(susp->X[susp->Xp]);
295 if (n) do { /* the inner sample computation loop */
296 *X_ptr_reg++ = *f_ptr_reg++;
297 } while (--n); /* inner loop */
298
299 /* using f_ptr_reg is a bad idea on RS/6000: */
300 susp->f_ptr += togo;
301 susp_took(f_cnt, togo);
302 susp->Xp += togo;
303 } /* outer loop */
304 }
305
306
307
resamplev_mark(snd_susp_type a_susp)308 void resamplev_mark(snd_susp_type a_susp)
309 {
310 resamplev_susp_type susp = (resamplev_susp_type) a_susp;
311 sound_xlmark(susp->f);
312 sound_xlmark(susp->g);
313 }
314
315
resamplev_free(snd_susp_type a_susp)316 void resamplev_free(snd_susp_type a_susp)
317 {
318 resamplev_susp_type susp = (resamplev_susp_type) a_susp;
319 sound_unref(susp->f);
320 sound_unref(susp->g);
321 free(susp->X);
322 ffree_generic(susp, sizeof(resamplev_susp_node), "resamplev_free");
323 }
324
325
resamplev_print_tree(snd_susp_type a_susp,int n)326 void resamplev_print_tree(snd_susp_type a_susp, int n)
327 {
328 resamplev_susp_type susp = (resamplev_susp_type) a_susp;
329 indent(n);
330 nyquist_printf("f:");
331 sound_print_tree_1(susp->f, n);
332
333 indent(n);
334 nyquist_printf("g:");
335 sound_print_tree_1(susp->g, n);
336 }
337
338
snd_make_resamplev(sound_type f,rate_type sr,sound_type g)339 sound_type snd_make_resamplev(sound_type f, rate_type sr, sound_type g)
340 {
341 register resamplev_susp_type susp;
342 int i;
343
344 falloc_generic(susp, resamplev_susp_node, "snd_make_resamplev");
345 susp->susp.fetch = resamplev__fetch;
346
347 susp->Nmult = SMALL_FILTER_NMULT;
348 susp->Imp = SMALL_FILTER_IMP;
349 susp->ImpD = SMALL_FILTER_IMPD;
350 susp->LpScl = SMALL_FILTER_SCALE / 32768.0;
351 susp->LpScl /= 16384.0;
352 /* this is just a fudge factor, is SMALL_FILTER_SCALE wrong? */
353 susp->LpScl /= 1.0011;
354 susp->Nwing = SMALL_FILTER_NWING;
355
356 susp->terminate_cnt = UNKNOWN;
357 /* initialize susp state */
358 susp->susp.free = resamplev_free;
359 susp->susp.sr = sr;
360 susp->susp.t0 = f->t0;
361 susp->susp.mark = resamplev_mark;
362 susp->susp.print_tree = resamplev_print_tree;
363 susp->susp.name = "resamplev";
364 susp->logically_stopped = false;
365 susp->susp.log_stop_cnt = logical_stop_cnt_cvt(f);
366 susp->susp.current = 0;
367 susp->f = f;
368 susp->f_cnt = 0;
369 susp->g = g;
370 susp->g_cnt = 0;
371 susp->next_g = 0;
372 susp->phase_in_g_increment = g->sr / sr;
373 susp->phase_in_g = 2.0;
374 /* can't use susp->factor because it is unknown and variable */
375 /* assume at most a down-sample by a factor of 2.0 and compute Xoff accordingly */
376 susp->Xoff = (int) (((susp->Nmult + 1) / 2.0) * 2.0) /* MAX(1.0, 1.0 / susp->factor) */ + 10;
377 /* this size is not critical unless it is too small */
378 /* allow the block size plus a buffer of 2*Xoff at both ends for the tails of the filter */
379 susp->Xsize = max_sample_block_len + 4 * susp->Xoff;
380 susp->X = calloc(susp->Xsize, sizeof(sample_type));
381 susp->Xp = susp->Xsize;
382 susp->shift_sum = -susp->Xsize;
383 susp->interpFilt = true;
384 for (i = 0; i < susp->Xoff; i++) susp->X[i] = 0.0F;
385 susp->LpScl *= 0.95; /* reduce probability of clipping */
386
387 return sound_create((snd_susp_type)susp, susp->susp.t0, susp->susp.sr,
388 1.0 /* scale factor */);
389 }
390
391
snd_resamplev(sound_type f,rate_type sr,sound_type g)392 sound_type snd_resamplev(sound_type f, rate_type sr, sound_type g)
393 {
394 sound_type f_copy = sound_copy(f);
395 sound_type g_copy = sound_copy(g);
396 g_copy->scale *= (float) sr; /* put g_copy in units of samples */
397 return snd_make_resamplev(f_copy, sr, g_copy);
398 }
399