1 /* resample.c: see resample.h for interesting stuff */
15 /* Some systems don't define this */
17 #define M_PI 3.14159265358979323846
20 static int hcf(int arg1, int arg2)
24 while (~(arg1 | arg2) & 1)
25 arg1 >>= 1, arg2 >>= 1, mult <<= 1;
29 if (~(arg1 & arg2) & 1)
35 arg2 = (arg2 - arg1) >> 1;
37 arg1 = (arg1 - arg2) >> 1;
44 static void filt_sinc(float *dest, int N, int step, double fc, double gain, int width)
48 float *endpoint = dest + N,
68 *dest = (x ? sin(x * M_PI * s) / (x * M_PI) * step : fc) * gain;
74 assert(dest == origdest + width);
78 static double I_zero(double x)
91 } while (u > 1e-21 * s);
97 static void win_kaiser(float *dest, int N, double alpha, int width)
99 double I_alpha, midsq;
101 float *endpoint = dest + N,
111 if (dest >= endpoint)
117 midsq = (double)(x - 1) * (double)(x - 1);
118 I_alpha = I_zero(alpha);
122 *dest *= I_zero(alpha * sqrt(1.0 - ((double)x * (double)x) / midsq)) / I_alpha;
125 if (dest >= endpoint)
128 assert(dest == origdest + width);
132 int res_init(res_state *state, int channels, int outfreq, int infreq, res_parameter op1, ...)
142 assert(channels > 0);
147 if (state == NULL || channels <= 0 || outfreq <= 0 || infreq <= 0 || taps <= 0)
159 gain = va_arg(argp, double);
163 cutoff = va_arg(argp, double);
164 assert(cutoff > 0.01 && cutoff <= 1.0);
168 taps = va_arg(argp, int);
169 assert(taps > 2 && taps < 1000);
173 beta = va_arg(argp, double);
177 assert("arglist" == "valid");
180 op1 = va_arg(argp, res_parameter);
181 } while (op1 != RES_END);
185 factor = hcf(infreq, outfreq);
189 /* adjust to rational values for downsampling */
190 if (outfreq < infreq)
192 /* push the cutoff frequency down to the output frequency */
193 cutoff = cutoff * outfreq / infreq;
195 /* compensate for the sharper roll-off requirement
196 * by using a bigger hammer */
197 taps = taps * infreq/outfreq;
200 assert(taps >= (infreq + outfreq - 1) / outfreq);
202 if ((state->table = calloc(outfreq * taps, sizeof(float))) == NULL)
204 if ((state->pool = calloc(channels * taps, sizeof(SAMPLE))) == NULL)
211 state->poolfill = taps / 2 + 1;
212 state->channels = channels;
213 state->outfreq = outfreq;
214 state->infreq = infreq;
218 filt_sinc(state->table, outfreq * taps, outfreq, cutoff, gain, taps);
219 win_kaiser(state->table, outfreq * taps, beta, taps);
225 static SAMPLE sum(float const *scale, int count, SAMPLE const *source, SAMPLE const *trigger, SAMPLE const *reset, int srcstep)
231 total += *source * *scale;
233 if (source == trigger)
234 source = reset, srcstep = 1;
243 static int push(res_state const * const state, SAMPLE *pool, int * const poolfill, int * const offset, SAMPLE *dest, int dststep, SAMPLE const *source, int srcstep, size_t srclen)
245 SAMPLE * const destbase = dest,
246 *poolhead = pool + *poolfill,
247 *poolend = pool + state->taps,
249 SAMPLE const *refill, *base, *endpoint;
259 assert(state->poolfill != -1);
261 lencheck = res_push_check(state, srclen);
263 /* fill the pool before diving in */
264 while (poolhead < poolend && srclen > 0)
266 *poolhead++ = *source;
271 if ((long)srclen <= 0)
275 endpoint = source + srclen * srcstep;
277 while (source < endpoint)
279 *dest = sum(state->table + *offset * state->taps, state->taps, source, base, poolend, srcstep);
281 *offset += state->infreq;
282 while (*offset >= state->outfreq)
284 *offset -= state->outfreq;
289 assert(dest == destbase + lencheck * dststep);
291 /* pretend that source has that underrun data we're not going to get */
292 srclen += (source - endpoint) / srcstep;
294 /* if we didn't get enough to completely replace the pool, then shift things about a bit */
295 if (srclen < state->taps)
297 refill = pool + srclen;
298 while (refill < poolend)
299 *newpool++ = *refill++;
301 refill = source - srclen * srcstep;
304 refill = source - state->taps * srcstep;
306 /* pull in fresh pool data */
307 while (refill < endpoint)
309 *newpool++ = *refill;
313 assert(newpool > pool);
314 assert(newpool <= poolend);
316 *poolfill = newpool - pool;
318 return (dest - destbase) / dststep;
322 int res_push_max_input(res_state const * const state, size_t maxoutput)
324 return maxoutput * state->infreq / state->outfreq;
328 int res_push_check(res_state const * const state, size_t srclen)
330 if (state->poolfill < state->taps)
331 srclen -= state->taps - state->poolfill;
333 return (srclen * state->outfreq - state->offset + state->infreq - 1) / state->infreq;
337 int res_push(res_state *state, SAMPLE **dstlist, SAMPLE const **srclist, size_t srclen)
339 int result = -1, poolfill = -1, offset = -1, i;
344 assert(state->poolfill >= 0);
346 for (i = 0; i < state->channels; i++)
348 poolfill = state->poolfill;
349 offset = state->offset;
350 result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, srclist[i], 1, srclen);
352 state->poolfill = poolfill;
353 state->offset = offset;
359 int res_push_interleaved(res_state *state, SAMPLE *dest, SAMPLE const *source, size_t srclen)
361 int result = -1, poolfill = -1, offset = -1, i;
366 assert(state->poolfill >= 0);
368 for (i = 0; i < state->channels; i++)
370 poolfill = state->poolfill;
371 offset = state->offset;
372 result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, source + i, state->channels, srclen);
374 state->poolfill = poolfill;
375 state->offset = offset;
381 int res_drain(res_state *state, SAMPLE **dstlist)
384 int result = -1, poolfill = -1, offset = -1, i;
388 assert(state->poolfill >= 0);
390 if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
393 for (i = 0; i < state->channels; i++)
395 poolfill = state->poolfill;
396 offset = state->offset;
397 result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, tail, 1, state->taps / 2 - 1);
402 state->poolfill = -1;
408 int res_drain_interleaved(res_state *state, SAMPLE *dest)
411 int result = -1, poolfill = -1, offset = -1, i;
415 assert(state->poolfill >= 0);
417 if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
420 for (i = 0; i < state->channels; i++)
422 poolfill = state->poolfill;
423 offset = state->offset;
424 result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, tail, 1, state->taps / 2 - 1);
429 state->poolfill = -1;
435 void res_clear(res_state *state)
438 assert(state->table);
443 memset(state, 0, sizeof(*state));