1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 3 * 4 * Copyright (c) 2005-2009 Ariff Abdullah <ariff@FreeBSD.org> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 */ 28 29 /* 30 * feeder_rate: (Codename: Z Resampler), which means any effort to create 31 * future replacement for this resampler are simply absurd unless 32 * the world decide to add new alphabet after Z. 33 * 34 * FreeBSD bandlimited sinc interpolator, technically based on 35 * "Digital Audio Resampling" by Julius O. Smith III 36 * - http://ccrma.stanford.edu/~jos/resample/ 37 * 38 * The Good: 39 * + all out fixed point integer operations, no soft-float or anything like 40 * that. 41 * + classic polyphase converters with high quality coefficient's polynomial 42 * interpolators. 43 * + fast, faster, or the fastest of its kind. 44 * + compile time configurable. 45 * + etc etc.. 46 * 47 * The Bad: 48 * - The z, z_, and Z_ . Due to mental block (or maybe just 0x7a69), I 49 * couldn't think of anything simpler than that (feeder_rate_xxx is just 50 * too long). Expect possible clashes with other zitizens (any?). 51 */ 52 53 #ifdef _KERNEL 54 #ifdef HAVE_KERNEL_OPTION_HEADERS 55 #include "opt_snd.h" 56 #endif 57 #include <dev/sound/pcm/sound.h> 58 #include <dev/sound/pcm/pcm.h> 59 #include "feeder_if.h" 60 61 #define SND_USE_FXDIV 62 #include "snd_fxdiv_gen.h" 63 64 SND_DECLARE_FILE("$FreeBSD$"); 65 #endif 66 67 #include "feeder_rate_gen.h" 68 69 #if !defined(_KERNEL) && defined(SND_DIAGNOSTIC) 70 #undef Z_DIAGNOSTIC 71 #define Z_DIAGNOSTIC 1 72 #elif defined(_KERNEL) 73 #undef Z_DIAGNOSTIC 74 #endif 75 76 #ifndef Z_QUALITY_DEFAULT 77 #define Z_QUALITY_DEFAULT Z_QUALITY_LINEAR 78 #endif 79 80 #define Z_RESERVOIR 2048 81 #define Z_RESERVOIR_MAX 131072 82 83 #define Z_SINC_MAX 0x3fffff 84 #define Z_SINC_DOWNMAX 48 /* 384000 / 8000 */ 85 86 #ifdef _KERNEL 87 #define Z_POLYPHASE_MAX 183040 /* 286 taps, 640 phases */ 88 #else 89 #define Z_POLYPHASE_MAX 1464320 /* 286 taps, 5120 phases */ 90 #endif 91 92 #define Z_RATE_DEFAULT 48000 93 94 #define Z_RATE_MIN FEEDRATE_RATEMIN 95 #define Z_RATE_MAX FEEDRATE_RATEMAX 96 #define Z_ROUNDHZ FEEDRATE_ROUNDHZ 97 #define Z_ROUNDHZ_MIN FEEDRATE_ROUNDHZ_MIN 98 #define Z_ROUNDHZ_MAX FEEDRATE_ROUNDHZ_MAX 99 100 #define Z_RATE_SRC FEEDRATE_SRC 101 #define Z_RATE_DST FEEDRATE_DST 102 #define Z_RATE_QUALITY FEEDRATE_QUALITY 103 #define Z_RATE_CHANNELS FEEDRATE_CHANNELS 104 105 #define Z_PARANOID 1 106 107 #define Z_MULTIFORMAT 1 108 109 #ifdef _KERNEL 110 #undef Z_USE_ALPHADRIFT 111 #define Z_USE_ALPHADRIFT 1 112 #endif 113 114 #define Z_FACTOR_MIN 1 115 #define Z_FACTOR_MAX Z_MASK 116 #define Z_FACTOR_SAFE(v) (!((v) < Z_FACTOR_MIN || (v) > Z_FACTOR_MAX)) 117 118 struct z_info; 119 120 typedef void (*z_resampler_t)(struct z_info *, uint8_t *); 121 122 struct z_info { 123 int32_t rsrc, rdst; /* original source / destination rates */ 124 int32_t src, dst; /* rounded source / destination rates */ 125 int32_t channels; /* total channels */ 126 int32_t bps; /* bytes-per-sample */ 127 int32_t quality; /* resampling quality */ 128 129 int32_t z_gx, z_gy; /* interpolation / decimation ratio */ 130 int32_t z_alpha; /* output sample time phase / drift */ 131 uint8_t *z_delay; /* FIR delay line / linear buffer */ 132 int32_t *z_coeff; /* FIR coefficients */ 133 int32_t *z_dcoeff; /* FIR coefficients differences */ 134 int32_t *z_pcoeff; /* FIR polyphase coefficients */ 135 int32_t z_scale; /* output scaling */ 136 int32_t z_dx; /* input sample drift increment */ 137 int32_t z_dy; /* output sample drift increment */ 138 #ifdef Z_USE_ALPHADRIFT 139 int32_t z_alphadrift; /* alpha drift rate */ 140 int32_t z_startdrift; /* buffer start position drift rate */ 141 #endif 142 int32_t z_mask; /* delay line full length mask */ 143 int32_t z_size; /* half width of FIR taps */ 144 int32_t z_full; /* full size of delay line */ 145 int32_t z_alloc; /* largest allocated full size of delay line */ 146 int32_t z_start; /* buffer processing start position */ 147 int32_t z_pos; /* current position for the next feed */ 148 #ifdef Z_DIAGNOSTIC 149 uint32_t z_cycle; /* output cycle, purely for statistical */ 150 #endif 151 int32_t z_maxfeed; /* maximum feed to avoid 32bit overflow */ 152 153 z_resampler_t z_resample; 154 }; 155 156 int feeder_rate_min = Z_RATE_MIN; 157 int feeder_rate_max = Z_RATE_MAX; 158 int feeder_rate_round = Z_ROUNDHZ; 159 int feeder_rate_quality = Z_QUALITY_DEFAULT; 160 161 static int feeder_rate_polyphase_max = Z_POLYPHASE_MAX; 162 163 #ifdef _KERNEL 164 static char feeder_rate_presets[] = FEEDER_RATE_PRESETS; 165 SYSCTL_STRING(_hw_snd, OID_AUTO, feeder_rate_presets, CTLFLAG_RD, 166 &feeder_rate_presets, 0, "compile-time rate presets"); 167 SYSCTL_INT(_hw_snd, OID_AUTO, feeder_rate_polyphase_max, CTLFLAG_RWTUN, 168 &feeder_rate_polyphase_max, 0, "maximum allowable polyphase entries"); 169 170 static int 171 sysctl_hw_snd_feeder_rate_min(SYSCTL_HANDLER_ARGS) 172 { 173 int err, val; 174 175 val = feeder_rate_min; 176 err = sysctl_handle_int(oidp, &val, 0, req); 177 178 if (err != 0 || req->newptr == NULL || val == feeder_rate_min) 179 return (err); 180 181 if (!(Z_FACTOR_SAFE(val) && val < feeder_rate_max)) 182 return (EINVAL); 183 184 feeder_rate_min = val; 185 186 return (0); 187 } 188 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_min, CTLTYPE_INT | CTLFLAG_RWTUN, 189 0, sizeof(int), sysctl_hw_snd_feeder_rate_min, "I", 190 "minimum allowable rate"); 191 192 static int 193 sysctl_hw_snd_feeder_rate_max(SYSCTL_HANDLER_ARGS) 194 { 195 int err, val; 196 197 val = feeder_rate_max; 198 err = sysctl_handle_int(oidp, &val, 0, req); 199 200 if (err != 0 || req->newptr == NULL || val == feeder_rate_max) 201 return (err); 202 203 if (!(Z_FACTOR_SAFE(val) && val > feeder_rate_min)) 204 return (EINVAL); 205 206 feeder_rate_max = val; 207 208 return (0); 209 } 210 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_max, CTLTYPE_INT | CTLFLAG_RWTUN, 211 0, sizeof(int), sysctl_hw_snd_feeder_rate_max, "I", 212 "maximum allowable rate"); 213 214 static int 215 sysctl_hw_snd_feeder_rate_round(SYSCTL_HANDLER_ARGS) 216 { 217 int err, val; 218 219 val = feeder_rate_round; 220 err = sysctl_handle_int(oidp, &val, 0, req); 221 222 if (err != 0 || req->newptr == NULL || val == feeder_rate_round) 223 return (err); 224 225 if (val < Z_ROUNDHZ_MIN || val > Z_ROUNDHZ_MAX) 226 return (EINVAL); 227 228 feeder_rate_round = val - (val % Z_ROUNDHZ); 229 230 return (0); 231 } 232 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_round, CTLTYPE_INT | CTLFLAG_RWTUN, 233 0, sizeof(int), sysctl_hw_snd_feeder_rate_round, "I", 234 "sample rate converter rounding threshold"); 235 236 static int 237 sysctl_hw_snd_feeder_rate_quality(SYSCTL_HANDLER_ARGS) 238 { 239 struct snddev_info *d; 240 struct pcm_channel *c; 241 struct pcm_feeder *f; 242 int i, err, val; 243 244 val = feeder_rate_quality; 245 err = sysctl_handle_int(oidp, &val, 0, req); 246 247 if (err != 0 || req->newptr == NULL || val == feeder_rate_quality) 248 return (err); 249 250 if (val < Z_QUALITY_MIN || val > Z_QUALITY_MAX) 251 return (EINVAL); 252 253 feeder_rate_quality = val; 254 255 /* 256 * Traverse all available channels on each device and try to 257 * set resampler quality if and only if it is exist as 258 * part of feeder chains and the channel is idle. 259 */ 260 for (i = 0; pcm_devclass != NULL && 261 i < devclass_get_maxunit(pcm_devclass); i++) { 262 d = devclass_get_softc(pcm_devclass, i); 263 if (!PCM_REGISTERED(d)) 264 continue; 265 PCM_LOCK(d); 266 PCM_WAIT(d); 267 PCM_ACQUIRE(d); 268 CHN_FOREACH(c, d, channels.pcm) { 269 CHN_LOCK(c); 270 f = chn_findfeeder(c, FEEDER_RATE); 271 if (f == NULL || f->data == NULL || CHN_STARTED(c)) { 272 CHN_UNLOCK(c); 273 continue; 274 } 275 (void)FEEDER_SET(f, FEEDRATE_QUALITY, val); 276 CHN_UNLOCK(c); 277 } 278 PCM_RELEASE(d); 279 PCM_UNLOCK(d); 280 } 281 282 return (0); 283 } 284 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_quality, CTLTYPE_INT | CTLFLAG_RWTUN, 285 0, sizeof(int), sysctl_hw_snd_feeder_rate_quality, "I", 286 "sample rate converter quality ("__XSTRING(Z_QUALITY_MIN)"=low .. " 287 __XSTRING(Z_QUALITY_MAX)"=high)"); 288 #endif /* _KERNEL */ 289 290 291 /* 292 * Resampler type. 293 */ 294 #define Z_IS_ZOH(i) ((i)->quality == Z_QUALITY_ZOH) 295 #define Z_IS_LINEAR(i) ((i)->quality == Z_QUALITY_LINEAR) 296 #define Z_IS_SINC(i) ((i)->quality > Z_QUALITY_LINEAR) 297 298 /* 299 * Macroses for accurate sample time drift calculations. 300 * 301 * gy2gx : given the amount of output, return the _exact_ required amount of 302 * input. 303 * gx2gy : given the amount of input, return the _maximum_ amount of output 304 * that will be generated. 305 * drift : given the amount of input and output, return the elapsed 306 * sample-time. 307 */ 308 #define _Z_GCAST(x) ((uint64_t)(x)) 309 310 #if defined(__GNUCLIKE_ASM) && defined(__i386__) 311 /* 312 * This is where i386 being beaten to a pulp. Fortunately this function is 313 * rarely being called and if it is, it will decide the best (hopefully) 314 * fastest way to do the division. If we can ensure that everything is dword 315 * aligned, letting the compiler to call udivdi3 to do the division can be 316 * faster compared to this. 317 * 318 * amd64 is the clear winner here, no question about it. 319 */ 320 static __inline uint32_t 321 Z_DIV(uint64_t v, uint32_t d) 322 { 323 uint32_t hi, lo, quo, rem; 324 325 hi = v >> 32; 326 lo = v & 0xffffffff; 327 328 /* 329 * As much as we can, try to avoid long division like a plague. 330 */ 331 if (hi == 0) 332 quo = lo / d; 333 else 334 __asm("divl %2" 335 : "=a" (quo), "=d" (rem) 336 : "r" (d), "0" (lo), "1" (hi)); 337 338 return (quo); 339 } 340 #else 341 #define Z_DIV(x, y) ((x) / (y)) 342 #endif 343 344 #define _Z_GY2GX(i, a, v) \ 345 Z_DIV(((_Z_GCAST((i)->z_gx) * (v)) + ((i)->z_gy - (a) - 1)), \ 346 (i)->z_gy) 347 348 #define _Z_GX2GY(i, a, v) \ 349 Z_DIV(((_Z_GCAST((i)->z_gy) * (v)) + (a)), (i)->z_gx) 350 351 #define _Z_DRIFT(i, x, y) \ 352 ((_Z_GCAST((i)->z_gy) * (x)) - (_Z_GCAST((i)->z_gx) * (y))) 353 354 #define z_gy2gx(i, v) _Z_GY2GX(i, (i)->z_alpha, v) 355 #define z_gx2gy(i, v) _Z_GX2GY(i, (i)->z_alpha, v) 356 #define z_drift(i, x, y) _Z_DRIFT(i, x, y) 357 358 /* 359 * Macroses for SINC coefficients table manipulations.. whatever. 360 */ 361 #define Z_SINC_COEFF_IDX(i) ((i)->quality - Z_QUALITY_LINEAR - 1) 362 363 #define Z_SINC_LEN(i) \ 364 ((int32_t)(((uint64_t)z_coeff_tab[Z_SINC_COEFF_IDX(i)].len << \ 365 Z_SHIFT) / (i)->z_dy)) 366 367 #define Z_SINC_BASE_LEN(i) \ 368 ((z_coeff_tab[Z_SINC_COEFF_IDX(i)].len - 1) >> (Z_DRIFT_SHIFT - 1)) 369 370 /* 371 * Macroses for linear delay buffer operations. Alignment is not 372 * really necessary since we're not using true circular buffer, but it 373 * will help us guard against possible trespasser. To be honest, 374 * the linear block operations does not need guarding at all due to 375 * accurate drifting! 376 */ 377 #define z_align(i, v) ((v) & (i)->z_mask) 378 #define z_next(i, o, v) z_align(i, (o) + (v)) 379 #define z_prev(i, o, v) z_align(i, (o) - (v)) 380 #define z_fetched(i) (z_align(i, (i)->z_pos - (i)->z_start) - 1) 381 #define z_free(i) ((i)->z_full - (i)->z_pos) 382 383 /* 384 * Macroses for Bla Bla .. :) 385 */ 386 #define z_copy(src, dst, sz) (void)memcpy(dst, src, sz) 387 #define z_feed(...) FEEDER_FEED(__VA_ARGS__) 388 389 static __inline uint32_t 390 z_min(uint32_t x, uint32_t y) 391 { 392 393 return ((x < y) ? x : y); 394 } 395 396 static int32_t 397 z_gcd(int32_t x, int32_t y) 398 { 399 int32_t w; 400 401 while (y != 0) { 402 w = x % y; 403 x = y; 404 y = w; 405 } 406 407 return (x); 408 } 409 410 static int32_t 411 z_roundpow2(int32_t v) 412 { 413 int32_t i; 414 415 i = 1; 416 417 /* 418 * Let it overflow at will.. 419 */ 420 while (i > 0 && i < v) 421 i <<= 1; 422 423 return (i); 424 } 425 426 /* 427 * Zero Order Hold, the worst of the worst, an insult against quality, 428 * but super fast. 429 */ 430 static void 431 z_feed_zoh(struct z_info *info, uint8_t *dst) 432 { 433 #if 0 434 z_copy(info->z_delay + 435 (info->z_start * info->channels * info->bps), dst, 436 info->channels * info->bps); 437 #else 438 uint32_t cnt; 439 uint8_t *src; 440 441 cnt = info->channels * info->bps; 442 src = info->z_delay + (info->z_start * cnt); 443 444 /* 445 * This is a bit faster than doing bcopy() since we're dealing 446 * with possible unaligned samples. 447 */ 448 do { 449 *dst++ = *src++; 450 } while (--cnt != 0); 451 #endif 452 } 453 454 /* 455 * Linear Interpolation. This at least sounds better (perceptually) and fast, 456 * but without any proper filtering which means aliasing still exist and 457 * could become worst with a right sample. Interpolation centered within 458 * Z_LINEAR_ONE between the present and previous sample and everything is 459 * done with simple 32bit scaling arithmetic. 460 */ 461 #define Z_DECLARE_LINEAR(SIGN, BIT, ENDIAN) \ 462 static void \ 463 z_feed_linear_##SIGN##BIT##ENDIAN(struct z_info *info, uint8_t *dst) \ 464 { \ 465 int32_t z; \ 466 intpcm_t x, y; \ 467 uint32_t ch; \ 468 uint8_t *sx, *sy; \ 469 \ 470 z = ((uint32_t)info->z_alpha * info->z_dx) >> Z_LINEAR_UNSHIFT; \ 471 \ 472 sx = info->z_delay + (info->z_start * info->channels * \ 473 PCM_##BIT##_BPS); \ 474 sy = sx - (info->channels * PCM_##BIT##_BPS); \ 475 \ 476 ch = info->channels; \ 477 \ 478 do { \ 479 x = _PCM_READ_##SIGN##BIT##_##ENDIAN(sx); \ 480 y = _PCM_READ_##SIGN##BIT##_##ENDIAN(sy); \ 481 x = Z_LINEAR_INTERPOLATE_##BIT(z, x, y); \ 482 _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, x); \ 483 sx += PCM_##BIT##_BPS; \ 484 sy += PCM_##BIT##_BPS; \ 485 dst += PCM_##BIT##_BPS; \ 486 } while (--ch != 0); \ 487 } 488 489 /* 490 * Userland clipping diagnostic check, not enabled in kernel compilation. 491 * While doing sinc interpolation, unrealistic samples like full scale sine 492 * wav will clip, but for other things this will not make any noise at all. 493 * Everybody should learn how to normalized perceived loudness of their own 494 * music/sounds/samples (hint: ReplayGain). 495 */ 496 #ifdef Z_DIAGNOSTIC 497 #define Z_CLIP_CHECK(v, BIT) do { \ 498 if ((v) > PCM_S##BIT##_MAX) { \ 499 fprintf(stderr, "Overflow: v=%jd, max=%jd\n", \ 500 (intmax_t)(v), (intmax_t)PCM_S##BIT##_MAX); \ 501 } else if ((v) < PCM_S##BIT##_MIN) { \ 502 fprintf(stderr, "Underflow: v=%jd, min=%jd\n", \ 503 (intmax_t)(v), (intmax_t)PCM_S##BIT##_MIN); \ 504 } \ 505 } while (0) 506 #else 507 #define Z_CLIP_CHECK(...) 508 #endif 509 510 #define Z_CLAMP(v, BIT) \ 511 (((v) > PCM_S##BIT##_MAX) ? PCM_S##BIT##_MAX : \ 512 (((v) < PCM_S##BIT##_MIN) ? PCM_S##BIT##_MIN : (v))) 513 514 /* 515 * Sine Cardinal (SINC) Interpolation. Scaling is done in 64 bit, so 516 * there's no point to hold the plate any longer. All samples will be 517 * shifted to a full 32 bit, scaled and restored during write for 518 * maximum dynamic range (only for downsampling). 519 */ 520 #define _Z_SINC_ACCUMULATE(SIGN, BIT, ENDIAN, adv) \ 521 c += z >> Z_SHIFT; \ 522 z &= Z_MASK; \ 523 coeff = Z_COEFF_INTERPOLATE(z, z_coeff[c], z_dcoeff[c]); \ 524 x = _PCM_READ_##SIGN##BIT##_##ENDIAN(p); \ 525 v += Z_NORM_##BIT((intpcm64_t)x * coeff); \ 526 z += info->z_dy; \ 527 p adv##= info->channels * PCM_##BIT##_BPS 528 529 /* 530 * XXX GCC4 optimization is such a !@#$%, need manual unrolling. 531 */ 532 #if defined(__GNUC__) && __GNUC__ >= 4 533 #define Z_SINC_ACCUMULATE(...) do { \ 534 _Z_SINC_ACCUMULATE(__VA_ARGS__); \ 535 _Z_SINC_ACCUMULATE(__VA_ARGS__); \ 536 } while (0) 537 #define Z_SINC_ACCUMULATE_DECR 2 538 #else 539 #define Z_SINC_ACCUMULATE(...) do { \ 540 _Z_SINC_ACCUMULATE(__VA_ARGS__); \ 541 } while (0) 542 #define Z_SINC_ACCUMULATE_DECR 1 543 #endif 544 545 #define Z_DECLARE_SINC(SIGN, BIT, ENDIAN) \ 546 static void \ 547 z_feed_sinc_##SIGN##BIT##ENDIAN(struct z_info *info, uint8_t *dst) \ 548 { \ 549 intpcm64_t v; \ 550 intpcm_t x; \ 551 uint8_t *p; \ 552 int32_t coeff, z, *z_coeff, *z_dcoeff; \ 553 uint32_t c, center, ch, i; \ 554 \ 555 z_coeff = info->z_coeff; \ 556 z_dcoeff = info->z_dcoeff; \ 557 center = z_prev(info, info->z_start, info->z_size); \ 558 ch = info->channels * PCM_##BIT##_BPS; \ 559 dst += ch; \ 560 \ 561 do { \ 562 dst -= PCM_##BIT##_BPS; \ 563 ch -= PCM_##BIT##_BPS; \ 564 v = 0; \ 565 z = info->z_alpha * info->z_dx; \ 566 c = 0; \ 567 p = info->z_delay + (z_next(info, center, 1) * \ 568 info->channels * PCM_##BIT##_BPS) + ch; \ 569 for (i = info->z_size; i != 0; i -= Z_SINC_ACCUMULATE_DECR) \ 570 Z_SINC_ACCUMULATE(SIGN, BIT, ENDIAN, +); \ 571 z = info->z_dy - (info->z_alpha * info->z_dx); \ 572 c = 0; \ 573 p = info->z_delay + (center * info->channels * \ 574 PCM_##BIT##_BPS) + ch; \ 575 for (i = info->z_size; i != 0; i -= Z_SINC_ACCUMULATE_DECR) \ 576 Z_SINC_ACCUMULATE(SIGN, BIT, ENDIAN, -); \ 577 if (info->z_scale != Z_ONE) \ 578 v = Z_SCALE_##BIT(v, info->z_scale); \ 579 else \ 580 v >>= Z_COEFF_SHIFT - Z_GUARD_BIT_##BIT; \ 581 Z_CLIP_CHECK(v, BIT); \ 582 _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, Z_CLAMP(v, BIT)); \ 583 } while (ch != 0); \ 584 } 585 586 #define Z_DECLARE_SINC_POLYPHASE(SIGN, BIT, ENDIAN) \ 587 static void \ 588 z_feed_sinc_polyphase_##SIGN##BIT##ENDIAN(struct z_info *info, uint8_t *dst) \ 589 { \ 590 intpcm64_t v; \ 591 intpcm_t x; \ 592 uint8_t *p; \ 593 int32_t ch, i, start, *z_pcoeff; \ 594 \ 595 ch = info->channels * PCM_##BIT##_BPS; \ 596 dst += ch; \ 597 start = z_prev(info, info->z_start, (info->z_size << 1) - 1) * ch; \ 598 \ 599 do { \ 600 dst -= PCM_##BIT##_BPS; \ 601 ch -= PCM_##BIT##_BPS; \ 602 v = 0; \ 603 p = info->z_delay + start + ch; \ 604 z_pcoeff = info->z_pcoeff + \ 605 ((info->z_alpha * info->z_size) << 1); \ 606 for (i = info->z_size; i != 0; i--) { \ 607 x = _PCM_READ_##SIGN##BIT##_##ENDIAN(p); \ 608 v += Z_NORM_##BIT((intpcm64_t)x * *z_pcoeff); \ 609 z_pcoeff++; \ 610 p += info->channels * PCM_##BIT##_BPS; \ 611 x = _PCM_READ_##SIGN##BIT##_##ENDIAN(p); \ 612 v += Z_NORM_##BIT((intpcm64_t)x * *z_pcoeff); \ 613 z_pcoeff++; \ 614 p += info->channels * PCM_##BIT##_BPS; \ 615 } \ 616 if (info->z_scale != Z_ONE) \ 617 v = Z_SCALE_##BIT(v, info->z_scale); \ 618 else \ 619 v >>= Z_COEFF_SHIFT - Z_GUARD_BIT_##BIT; \ 620 Z_CLIP_CHECK(v, BIT); \ 621 _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, Z_CLAMP(v, BIT)); \ 622 } while (ch != 0); \ 623 } 624 625 #define Z_DECLARE(SIGN, BIT, ENDIAN) \ 626 Z_DECLARE_LINEAR(SIGN, BIT, ENDIAN) \ 627 Z_DECLARE_SINC(SIGN, BIT, ENDIAN) \ 628 Z_DECLARE_SINC_POLYPHASE(SIGN, BIT, ENDIAN) 629 630 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 631 Z_DECLARE(S, 16, LE) 632 Z_DECLARE(S, 32, LE) 633 #endif 634 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 635 Z_DECLARE(S, 16, BE) 636 Z_DECLARE(S, 32, BE) 637 #endif 638 #ifdef SND_FEEDER_MULTIFORMAT 639 Z_DECLARE(S, 8, NE) 640 Z_DECLARE(S, 24, LE) 641 Z_DECLARE(S, 24, BE) 642 Z_DECLARE(U, 8, NE) 643 Z_DECLARE(U, 16, LE) 644 Z_DECLARE(U, 24, LE) 645 Z_DECLARE(U, 32, LE) 646 Z_DECLARE(U, 16, BE) 647 Z_DECLARE(U, 24, BE) 648 Z_DECLARE(U, 32, BE) 649 #endif 650 651 enum { 652 Z_RESAMPLER_ZOH, 653 Z_RESAMPLER_LINEAR, 654 Z_RESAMPLER_SINC, 655 Z_RESAMPLER_SINC_POLYPHASE, 656 Z_RESAMPLER_LAST 657 }; 658 659 #define Z_RESAMPLER_IDX(i) \ 660 (Z_IS_SINC(i) ? Z_RESAMPLER_SINC : (i)->quality) 661 662 #define Z_RESAMPLER_ENTRY(SIGN, BIT, ENDIAN) \ 663 { \ 664 AFMT_##SIGN##BIT##_##ENDIAN, \ 665 { \ 666 [Z_RESAMPLER_ZOH] = z_feed_zoh, \ 667 [Z_RESAMPLER_LINEAR] = z_feed_linear_##SIGN##BIT##ENDIAN, \ 668 [Z_RESAMPLER_SINC] = z_feed_sinc_##SIGN##BIT##ENDIAN, \ 669 [Z_RESAMPLER_SINC_POLYPHASE] = \ 670 z_feed_sinc_polyphase_##SIGN##BIT##ENDIAN \ 671 } \ 672 } 673 674 static const struct { 675 uint32_t format; 676 z_resampler_t resampler[Z_RESAMPLER_LAST]; 677 } z_resampler_tab[] = { 678 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 679 Z_RESAMPLER_ENTRY(S, 16, LE), 680 Z_RESAMPLER_ENTRY(S, 32, LE), 681 #endif 682 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) 683 Z_RESAMPLER_ENTRY(S, 16, BE), 684 Z_RESAMPLER_ENTRY(S, 32, BE), 685 #endif 686 #ifdef SND_FEEDER_MULTIFORMAT 687 Z_RESAMPLER_ENTRY(S, 8, NE), 688 Z_RESAMPLER_ENTRY(S, 24, LE), 689 Z_RESAMPLER_ENTRY(S, 24, BE), 690 Z_RESAMPLER_ENTRY(U, 8, NE), 691 Z_RESAMPLER_ENTRY(U, 16, LE), 692 Z_RESAMPLER_ENTRY(U, 24, LE), 693 Z_RESAMPLER_ENTRY(U, 32, LE), 694 Z_RESAMPLER_ENTRY(U, 16, BE), 695 Z_RESAMPLER_ENTRY(U, 24, BE), 696 Z_RESAMPLER_ENTRY(U, 32, BE), 697 #endif 698 }; 699 700 #define Z_RESAMPLER_TAB_SIZE \ 701 ((int32_t)(sizeof(z_resampler_tab) / sizeof(z_resampler_tab[0]))) 702 703 static void 704 z_resampler_reset(struct z_info *info) 705 { 706 707 info->src = info->rsrc - (info->rsrc % ((feeder_rate_round > 0 && 708 info->rsrc > feeder_rate_round) ? feeder_rate_round : 1)); 709 info->dst = info->rdst - (info->rdst % ((feeder_rate_round > 0 && 710 info->rdst > feeder_rate_round) ? feeder_rate_round : 1)); 711 info->z_gx = 1; 712 info->z_gy = 1; 713 info->z_alpha = 0; 714 info->z_resample = NULL; 715 info->z_size = 1; 716 info->z_coeff = NULL; 717 info->z_dcoeff = NULL; 718 if (info->z_pcoeff != NULL) { 719 free(info->z_pcoeff, M_DEVBUF); 720 info->z_pcoeff = NULL; 721 } 722 info->z_scale = Z_ONE; 723 info->z_dx = Z_FULL_ONE; 724 info->z_dy = Z_FULL_ONE; 725 #ifdef Z_DIAGNOSTIC 726 info->z_cycle = 0; 727 #endif 728 if (info->quality < Z_QUALITY_MIN) 729 info->quality = Z_QUALITY_MIN; 730 else if (info->quality > Z_QUALITY_MAX) 731 info->quality = Z_QUALITY_MAX; 732 } 733 734 #ifdef Z_PARANOID 735 static int32_t 736 z_resampler_sinc_len(struct z_info *info) 737 { 738 int32_t c, z, len, lmax; 739 740 if (!Z_IS_SINC(info)) 741 return (1); 742 743 /* 744 * A rather careful (or useless) way to calculate filter length. 745 * Z_SINC_LEN() itself is accurate enough to do its job. Extra 746 * sanity checking is not going to hurt though.. 747 */ 748 c = 0; 749 z = info->z_dy; 750 len = 0; 751 lmax = z_coeff_tab[Z_SINC_COEFF_IDX(info)].len; 752 753 do { 754 c += z >> Z_SHIFT; 755 z &= Z_MASK; 756 z += info->z_dy; 757 } while (c < lmax && ++len > 0); 758 759 if (len != Z_SINC_LEN(info)) { 760 #ifdef _KERNEL 761 printf("%s(): sinc l=%d != Z_SINC_LEN=%d\n", 762 __func__, len, Z_SINC_LEN(info)); 763 #else 764 fprintf(stderr, "%s(): sinc l=%d != Z_SINC_LEN=%d\n", 765 __func__, len, Z_SINC_LEN(info)); 766 return (-1); 767 #endif 768 } 769 770 return (len); 771 } 772 #else 773 #define z_resampler_sinc_len(i) (Z_IS_SINC(i) ? Z_SINC_LEN(i) : 1) 774 #endif 775 776 #define Z_POLYPHASE_COEFF_SHIFT 0 777 778 /* 779 * Pick suitable polynomial interpolators based on filter oversampled ratio 780 * (2 ^ Z_DRIFT_SHIFT). 781 */ 782 #if !(defined(Z_COEFF_INTERP_ZOH) || defined(Z_COEFF_INTERP_LINEAR) || \ 783 defined(Z_COEFF_INTERP_QUADRATIC) || defined(Z_COEFF_INTERP_HERMITE) || \ 784 defined(Z_COEFF_INTER_BSPLINE) || defined(Z_COEFF_INTERP_OPT32X) || \ 785 defined(Z_COEFF_INTERP_OPT16X) || defined(Z_COEFF_INTERP_OPT8X) || \ 786 defined(Z_COEFF_INTERP_OPT4X) || defined(Z_COEFF_INTERP_OPT2X)) 787 #if Z_DRIFT_SHIFT >= 6 788 #define Z_COEFF_INTERP_BSPLINE 1 789 #elif Z_DRIFT_SHIFT >= 5 790 #define Z_COEFF_INTERP_OPT32X 1 791 #elif Z_DRIFT_SHIFT == 4 792 #define Z_COEFF_INTERP_OPT16X 1 793 #elif Z_DRIFT_SHIFT == 3 794 #define Z_COEFF_INTERP_OPT8X 1 795 #elif Z_DRIFT_SHIFT == 2 796 #define Z_COEFF_INTERP_OPT4X 1 797 #elif Z_DRIFT_SHIFT == 1 798 #define Z_COEFF_INTERP_OPT2X 1 799 #else 800 #error "Z_DRIFT_SHIFT screwed!" 801 #endif 802 #endif 803 804 /* 805 * In classic polyphase mode, the actual coefficients for each phases need to 806 * be calculated based on default prototype filters. For highly oversampled 807 * filter, linear or quadradatic interpolator should be enough. Anything less 808 * than that require 'special' interpolators to reduce interpolation errors. 809 * 810 * "Polynomial Interpolators for High-Quality Resampling of Oversampled Audio" 811 * by Olli Niemitalo 812 * - http://www.student.oulu.fi/~oniemita/dsp/deip.pdf 813 * 814 */ 815 static int32_t 816 z_coeff_interpolate(int32_t z, int32_t *z_coeff) 817 { 818 int32_t coeff; 819 #if defined(Z_COEFF_INTERP_ZOH) 820 821 /* 1-point, 0th-order (Zero Order Hold) */ 822 z = z; 823 coeff = z_coeff[0]; 824 #elif defined(Z_COEFF_INTERP_LINEAR) 825 int32_t zl0, zl1; 826 827 /* 2-point, 1st-order Linear */ 828 zl0 = z_coeff[0]; 829 zl1 = z_coeff[1] - z_coeff[0]; 830 831 coeff = Z_RSHIFT((int64_t)zl1 * z, Z_SHIFT) + zl0; 832 #elif defined(Z_COEFF_INTERP_QUADRATIC) 833 int32_t zq0, zq1, zq2; 834 835 /* 3-point, 2nd-order Quadratic */ 836 zq0 = z_coeff[0]; 837 zq1 = z_coeff[1] - z_coeff[-1]; 838 zq2 = z_coeff[1] + z_coeff[-1] - (z_coeff[0] << 1); 839 840 coeff = Z_RSHIFT((Z_RSHIFT((int64_t)zq2 * z, Z_SHIFT) + 841 zq1) * z, Z_SHIFT + 1) + zq0; 842 #elif defined(Z_COEFF_INTERP_HERMITE) 843 int32_t zh0, zh1, zh2, zh3; 844 845 /* 4-point, 3rd-order Hermite */ 846 zh0 = z_coeff[0]; 847 zh1 = z_coeff[1] - z_coeff[-1]; 848 zh2 = (z_coeff[-1] << 1) - (z_coeff[0] * 5) + (z_coeff[1] << 2) - 849 z_coeff[2]; 850 zh3 = z_coeff[2] - z_coeff[-1] + ((z_coeff[0] - z_coeff[1]) * 3); 851 852 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((int64_t)zh3 * z, Z_SHIFT) + 853 zh2) * z, Z_SHIFT) + zh1) * z, Z_SHIFT + 1) + zh0; 854 #elif defined(Z_COEFF_INTERP_BSPLINE) 855 int32_t zb0, zb1, zb2, zb3; 856 857 /* 4-point, 3rd-order B-Spline */ 858 zb0 = Z_RSHIFT(0x15555555LL * (((int64_t)z_coeff[0] << 2) + 859 z_coeff[-1] + z_coeff[1]), 30); 860 zb1 = z_coeff[1] - z_coeff[-1]; 861 zb2 = z_coeff[-1] + z_coeff[1] - (z_coeff[0] << 1); 862 zb3 = Z_RSHIFT(0x15555555LL * (((z_coeff[0] - z_coeff[1]) * 3) + 863 z_coeff[2] - z_coeff[-1]), 30); 864 865 coeff = (Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((int64_t)zb3 * z, Z_SHIFT) + 866 zb2) * z, Z_SHIFT) + zb1) * z, Z_SHIFT) + zb0 + 1) >> 1; 867 #elif defined(Z_COEFF_INTERP_OPT32X) 868 int32_t zoz, zoe1, zoe2, zoe3, zoo1, zoo2, zoo3; 869 int32_t zoc0, zoc1, zoc2, zoc3, zoc4, zoc5; 870 871 /* 6-point, 5th-order Optimal 32x */ 872 zoz = z - (Z_ONE >> 1); 873 zoe1 = z_coeff[1] + z_coeff[0]; 874 zoe2 = z_coeff[2] + z_coeff[-1]; 875 zoe3 = z_coeff[3] + z_coeff[-2]; 876 zoo1 = z_coeff[1] - z_coeff[0]; 877 zoo2 = z_coeff[2] - z_coeff[-1]; 878 zoo3 = z_coeff[3] - z_coeff[-2]; 879 880 zoc0 = Z_RSHIFT((0x1ac2260dLL * zoe1) + (0x0526cdcaLL * zoe2) + 881 (0x00170c29LL * zoe3), 30); 882 zoc1 = Z_RSHIFT((0x14f8a49aLL * zoo1) + (0x0d6d1109LL * zoo2) + 883 (0x008cd4dcLL * zoo3), 30); 884 zoc2 = Z_RSHIFT((-0x0d3e94a4LL * zoe1) + (0x0bddded4LL * zoe2) + 885 (0x0160b5d0LL * zoe3), 30); 886 zoc3 = Z_RSHIFT((-0x0de10cc4LL * zoo1) + (0x019b2a7dLL * zoo2) + 887 (0x01cfe914LL * zoo3), 30); 888 zoc4 = Z_RSHIFT((0x02aa12d7LL * zoe1) + (-0x03ff1bb3LL * zoe2) + 889 (0x015508ddLL * zoe3), 30); 890 zoc5 = Z_RSHIFT((0x051d29e5LL * zoo1) + (-0x028e7647LL * zoo2) + 891 (0x0082d81aLL * zoo3), 30); 892 893 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT( 894 (int64_t)zoc5 * zoz, Z_SHIFT) + 895 zoc4) * zoz, Z_SHIFT) + zoc3) * zoz, Z_SHIFT) + 896 zoc2) * zoz, Z_SHIFT) + zoc1) * zoz, Z_SHIFT) + zoc0; 897 #elif defined(Z_COEFF_INTERP_OPT16X) 898 int32_t zoz, zoe1, zoe2, zoe3, zoo1, zoo2, zoo3; 899 int32_t zoc0, zoc1, zoc2, zoc3, zoc4, zoc5; 900 901 /* 6-point, 5th-order Optimal 16x */ 902 zoz = z - (Z_ONE >> 1); 903 zoe1 = z_coeff[1] + z_coeff[0]; 904 zoe2 = z_coeff[2] + z_coeff[-1]; 905 zoe3 = z_coeff[3] + z_coeff[-2]; 906 zoo1 = z_coeff[1] - z_coeff[0]; 907 zoo2 = z_coeff[2] - z_coeff[-1]; 908 zoo3 = z_coeff[3] - z_coeff[-2]; 909 910 zoc0 = Z_RSHIFT((0x1ac2260dLL * zoe1) + (0x0526cdcaLL * zoe2) + 911 (0x00170c29LL * zoe3), 30); 912 zoc1 = Z_RSHIFT((0x14f8a49aLL * zoo1) + (0x0d6d1109LL * zoo2) + 913 (0x008cd4dcLL * zoo3), 30); 914 zoc2 = Z_RSHIFT((-0x0d3e94a4LL * zoe1) + (0x0bddded4LL * zoe2) + 915 (0x0160b5d0LL * zoe3), 30); 916 zoc3 = Z_RSHIFT((-0x0de10cc4LL * zoo1) + (0x019b2a7dLL * zoo2) + 917 (0x01cfe914LL * zoo3), 30); 918 zoc4 = Z_RSHIFT((0x02aa12d7LL * zoe1) + (-0x03ff1bb3LL * zoe2) + 919 (0x015508ddLL * zoe3), 30); 920 zoc5 = Z_RSHIFT((0x051d29e5LL * zoo1) + (-0x028e7647LL * zoo2) + 921 (0x0082d81aLL * zoo3), 30); 922 923 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT( 924 (int64_t)zoc5 * zoz, Z_SHIFT) + 925 zoc4) * zoz, Z_SHIFT) + zoc3) * zoz, Z_SHIFT) + 926 zoc2) * zoz, Z_SHIFT) + zoc1) * zoz, Z_SHIFT) + zoc0; 927 #elif defined(Z_COEFF_INTERP_OPT8X) 928 int32_t zoz, zoe1, zoe2, zoe3, zoo1, zoo2, zoo3; 929 int32_t zoc0, zoc1, zoc2, zoc3, zoc4, zoc5; 930 931 /* 6-point, 5th-order Optimal 8x */ 932 zoz = z - (Z_ONE >> 1); 933 zoe1 = z_coeff[1] + z_coeff[0]; 934 zoe2 = z_coeff[2] + z_coeff[-1]; 935 zoe3 = z_coeff[3] + z_coeff[-2]; 936 zoo1 = z_coeff[1] - z_coeff[0]; 937 zoo2 = z_coeff[2] - z_coeff[-1]; 938 zoo3 = z_coeff[3] - z_coeff[-2]; 939 940 zoc0 = Z_RSHIFT((0x1aa9b47dLL * zoe1) + (0x053d9944LL * zoe2) + 941 (0x0018b23fLL * zoe3), 30); 942 zoc1 = Z_RSHIFT((0x14a104d1LL * zoo1) + (0x0d7d2504LL * zoo2) + 943 (0x0094b599LL * zoo3), 30); 944 zoc2 = Z_RSHIFT((-0x0d22530bLL * zoe1) + (0x0bb37a2cLL * zoe2) + 945 (0x016ed8e0LL * zoe3), 30); 946 zoc3 = Z_RSHIFT((-0x0d744b1cLL * zoo1) + (0x01649591LL * zoo2) + 947 (0x01dae93aLL * zoo3), 30); 948 zoc4 = Z_RSHIFT((0x02a7ee1bLL * zoe1) + (-0x03fbdb24LL * zoe2) + 949 (0x0153ed07LL * zoe3), 30); 950 zoc5 = Z_RSHIFT((0x04cf9b6cLL * zoo1) + (-0x0266b378LL * zoo2) + 951 (0x007a7c26LL * zoo3), 30); 952 953 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT( 954 (int64_t)zoc5 * zoz, Z_SHIFT) + 955 zoc4) * zoz, Z_SHIFT) + zoc3) * zoz, Z_SHIFT) + 956 zoc2) * zoz, Z_SHIFT) + zoc1) * zoz, Z_SHIFT) + zoc0; 957 #elif defined(Z_COEFF_INTERP_OPT4X) 958 int32_t zoz, zoe1, zoe2, zoe3, zoo1, zoo2, zoo3; 959 int32_t zoc0, zoc1, zoc2, zoc3, zoc4, zoc5; 960 961 /* 6-point, 5th-order Optimal 4x */ 962 zoz = z - (Z_ONE >> 1); 963 zoe1 = z_coeff[1] + z_coeff[0]; 964 zoe2 = z_coeff[2] + z_coeff[-1]; 965 zoe3 = z_coeff[3] + z_coeff[-2]; 966 zoo1 = z_coeff[1] - z_coeff[0]; 967 zoo2 = z_coeff[2] - z_coeff[-1]; 968 zoo3 = z_coeff[3] - z_coeff[-2]; 969 970 zoc0 = Z_RSHIFT((0x1a8eda43LL * zoe1) + (0x0556ee38LL * zoe2) + 971 (0x001a3784LL * zoe3), 30); 972 zoc1 = Z_RSHIFT((0x143d863eLL * zoo1) + (0x0d910e36LL * zoo2) + 973 (0x009ca889LL * zoo3), 30); 974 zoc2 = Z_RSHIFT((-0x0d026821LL * zoe1) + (0x0b837773LL * zoe2) + 975 (0x017ef0c6LL * zoe3), 30); 976 zoc3 = Z_RSHIFT((-0x0cef1502LL * zoo1) + (0x01207a8eLL * zoo2) + 977 (0x01e936dbLL * zoo3), 30); 978 zoc4 = Z_RSHIFT((0x029fe643LL * zoe1) + (-0x03ef3fc8LL * zoe2) + 979 (0x014f5923LL * zoe3), 30); 980 zoc5 = Z_RSHIFT((0x043a9d08LL * zoo1) + (-0x02154febLL * zoo2) + 981 (0x00670dbdLL * zoo3), 30); 982 983 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT( 984 (int64_t)zoc5 * zoz, Z_SHIFT) + 985 zoc4) * zoz, Z_SHIFT) + zoc3) * zoz, Z_SHIFT) + 986 zoc2) * zoz, Z_SHIFT) + zoc1) * zoz, Z_SHIFT) + zoc0; 987 #elif defined(Z_COEFF_INTERP_OPT2X) 988 int32_t zoz, zoe1, zoe2, zoe3, zoo1, zoo2, zoo3; 989 int32_t zoc0, zoc1, zoc2, zoc3, zoc4, zoc5; 990 991 /* 6-point, 5th-order Optimal 2x */ 992 zoz = z - (Z_ONE >> 1); 993 zoe1 = z_coeff[1] + z_coeff[0]; 994 zoe2 = z_coeff[2] + z_coeff[-1]; 995 zoe3 = z_coeff[3] + z_coeff[-2]; 996 zoo1 = z_coeff[1] - z_coeff[0]; 997 zoo2 = z_coeff[2] - z_coeff[-1]; 998 zoo3 = z_coeff[3] - z_coeff[-2]; 999 1000 zoc0 = Z_RSHIFT((0x19edb6fdLL * zoe1) + (0x05ebd062LL * zoe2) + 1001 (0x00267881LL * zoe3), 30); 1002 zoc1 = Z_RSHIFT((0x1223af76LL * zoo1) + (0x0de3dd6bLL * zoo2) + 1003 (0x00d683cdLL * zoo3), 30); 1004 zoc2 = Z_RSHIFT((-0x0c3ee068LL * zoe1) + (0x0a5c3769LL * zoe2) + 1005 (0x01e2aceaLL * zoe3), 30); 1006 zoc3 = Z_RSHIFT((-0x0a8ab614LL * zoo1) + (-0x0019522eLL * zoo2) + 1007 (0x022cefc7LL * zoo3), 30); 1008 zoc4 = Z_RSHIFT((0x0276187dLL * zoe1) + (-0x03a801e8LL * zoe2) + 1009 (0x0131d935LL * zoe3), 30); 1010 zoc5 = Z_RSHIFT((0x02c373f5LL * zoo1) + (-0x01275f83LL * zoo2) + 1011 (0x0018ee79LL * zoo3), 30); 1012 1013 coeff = Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT((Z_RSHIFT( 1014 (int64_t)zoc5 * zoz, Z_SHIFT) + 1015 zoc4) * zoz, Z_SHIFT) + zoc3) * zoz, Z_SHIFT) + 1016 zoc2) * zoz, Z_SHIFT) + zoc1) * zoz, Z_SHIFT) + zoc0; 1017 #else 1018 #error "Interpolation type screwed!" 1019 #endif 1020 1021 #if Z_POLYPHASE_COEFF_SHIFT > 0 1022 coeff = Z_RSHIFT(coeff, Z_POLYPHASE_COEFF_SHIFT); 1023 #endif 1024 return (coeff); 1025 } 1026 1027 static int 1028 z_resampler_build_polyphase(struct z_info *info) 1029 { 1030 int32_t alpha, c, i, z, idx; 1031 1032 /* Let this be here first. */ 1033 if (info->z_pcoeff != NULL) { 1034 free(info->z_pcoeff, M_DEVBUF); 1035 info->z_pcoeff = NULL; 1036 } 1037 1038 if (feeder_rate_polyphase_max < 1) 1039 return (ENOTSUP); 1040 1041 if (((int64_t)info->z_size * info->z_gy * 2) > 1042 feeder_rate_polyphase_max) { 1043 #ifndef _KERNEL 1044 fprintf(stderr, "Polyphase entries exceed: [%d/%d] %jd > %d\n", 1045 info->z_gx, info->z_gy, 1046 (intmax_t)info->z_size * info->z_gy * 2, 1047 feeder_rate_polyphase_max); 1048 #endif 1049 return (E2BIG); 1050 } 1051 1052 info->z_pcoeff = malloc(sizeof(int32_t) * 1053 info->z_size * info->z_gy * 2, M_DEVBUF, M_NOWAIT | M_ZERO); 1054 if (info->z_pcoeff == NULL) 1055 return (ENOMEM); 1056 1057 for (alpha = 0; alpha < info->z_gy; alpha++) { 1058 z = alpha * info->z_dx; 1059 c = 0; 1060 for (i = info->z_size; i != 0; i--) { 1061 c += z >> Z_SHIFT; 1062 z &= Z_MASK; 1063 idx = (alpha * info->z_size * 2) + 1064 (info->z_size * 2) - i; 1065 info->z_pcoeff[idx] = 1066 z_coeff_interpolate(z, info->z_coeff + c); 1067 z += info->z_dy; 1068 } 1069 z = info->z_dy - (alpha * info->z_dx); 1070 c = 0; 1071 for (i = info->z_size; i != 0; i--) { 1072 c += z >> Z_SHIFT; 1073 z &= Z_MASK; 1074 idx = (alpha * info->z_size * 2) + i - 1; 1075 info->z_pcoeff[idx] = 1076 z_coeff_interpolate(z, info->z_coeff + c); 1077 z += info->z_dy; 1078 } 1079 } 1080 1081 #ifndef _KERNEL 1082 fprintf(stderr, "Polyphase: [%d/%d] %d entries\n", 1083 info->z_gx, info->z_gy, info->z_size * info->z_gy * 2); 1084 #endif 1085 1086 return (0); 1087 } 1088 1089 static int 1090 z_resampler_setup(struct pcm_feeder *f) 1091 { 1092 struct z_info *info; 1093 int64_t gy2gx_max, gx2gy_max; 1094 uint32_t format; 1095 int32_t align, i, z_scale; 1096 int adaptive; 1097 1098 info = f->data; 1099 z_resampler_reset(info); 1100 1101 if (info->src == info->dst) 1102 return (0); 1103 1104 /* Shrink by greatest common divisor. */ 1105 i = z_gcd(info->src, info->dst); 1106 info->z_gx = info->src / i; 1107 info->z_gy = info->dst / i; 1108 1109 /* Too big, or too small. Bail out. */ 1110 if (!(Z_FACTOR_SAFE(info->z_gx) && Z_FACTOR_SAFE(info->z_gy))) 1111 return (EINVAL); 1112 1113 format = f->desc->in; 1114 adaptive = 0; 1115 z_scale = 0; 1116 1117 /* 1118 * Setup everything: filter length, conversion factor, etc. 1119 */ 1120 if (Z_IS_SINC(info)) { 1121 /* 1122 * Downsampling, or upsampling scaling factor. As long as the 1123 * factor can be represented by a fraction of 1 << Z_SHIFT, 1124 * we're pretty much in business. Scaling is not needed for 1125 * upsampling, so we just slap Z_ONE there. 1126 */ 1127 if (info->z_gx > info->z_gy) 1128 /* 1129 * If the downsampling ratio is beyond sanity, 1130 * enable semi-adaptive mode. Although handling 1131 * extreme ratio is possible, the result of the 1132 * conversion is just pointless, unworthy, 1133 * nonsensical noises, etc. 1134 */ 1135 if ((info->z_gx / info->z_gy) > Z_SINC_DOWNMAX) 1136 z_scale = Z_ONE / Z_SINC_DOWNMAX; 1137 else 1138 z_scale = ((uint64_t)info->z_gy << Z_SHIFT) / 1139 info->z_gx; 1140 else 1141 z_scale = Z_ONE; 1142 1143 /* 1144 * This is actually impossible, unless anything above 1145 * overflow. 1146 */ 1147 if (z_scale < 1) 1148 return (E2BIG); 1149 1150 /* 1151 * Calculate sample time/coefficients index drift. It is 1152 * a constant for upsampling, but downsampling require 1153 * heavy duty filtering with possible too long filters. 1154 * If anything goes wrong, revisit again and enable 1155 * adaptive mode. 1156 */ 1157 z_setup_adaptive_sinc: 1158 if (info->z_pcoeff != NULL) { 1159 free(info->z_pcoeff, M_DEVBUF); 1160 info->z_pcoeff = NULL; 1161 } 1162 1163 if (adaptive == 0) { 1164 info->z_dy = z_scale << Z_DRIFT_SHIFT; 1165 if (info->z_dy < 1) 1166 return (E2BIG); 1167 info->z_scale = z_scale; 1168 } else { 1169 info->z_dy = Z_FULL_ONE; 1170 info->z_scale = Z_ONE; 1171 } 1172 1173 #if 0 1174 #define Z_SCALE_DIV 10000 1175 #define Z_SCALE_LIMIT(s, v) \ 1176 ((((uint64_t)(s) * (v)) + (Z_SCALE_DIV >> 1)) / Z_SCALE_DIV) 1177 1178 info->z_scale = Z_SCALE_LIMIT(info->z_scale, 9780); 1179 #endif 1180 1181 /* Smallest drift increment. */ 1182 info->z_dx = info->z_dy / info->z_gy; 1183 1184 /* 1185 * Overflow or underflow. Try adaptive, let it continue and 1186 * retry. 1187 */ 1188 if (info->z_dx < 1) { 1189 if (adaptive == 0) { 1190 adaptive = 1; 1191 goto z_setup_adaptive_sinc; 1192 } 1193 return (E2BIG); 1194 } 1195 1196 /* 1197 * Round back output drift. 1198 */ 1199 info->z_dy = info->z_dx * info->z_gy; 1200 1201 for (i = 0; i < Z_COEFF_TAB_SIZE; i++) { 1202 if (Z_SINC_COEFF_IDX(info) != i) 1203 continue; 1204 /* 1205 * Calculate required filter length and guard 1206 * against possible abusive result. Note that 1207 * this represents only 1/2 of the entire filter 1208 * length. 1209 */ 1210 info->z_size = z_resampler_sinc_len(info); 1211 1212 /* 1213 * Multiple of 2 rounding, for better accumulator 1214 * performance. 1215 */ 1216 info->z_size &= ~1; 1217 1218 if (info->z_size < 2 || info->z_size > Z_SINC_MAX) { 1219 if (adaptive == 0) { 1220 adaptive = 1; 1221 goto z_setup_adaptive_sinc; 1222 } 1223 return (E2BIG); 1224 } 1225 info->z_coeff = z_coeff_tab[i].coeff + Z_COEFF_OFFSET; 1226 info->z_dcoeff = z_coeff_tab[i].dcoeff; 1227 break; 1228 } 1229 1230 if (info->z_coeff == NULL || info->z_dcoeff == NULL) 1231 return (EINVAL); 1232 } else if (Z_IS_LINEAR(info)) { 1233 /* 1234 * Don't put much effort if we're doing linear interpolation. 1235 * Just center the interpolation distance within Z_LINEAR_ONE, 1236 * and be happy about it. 1237 */ 1238 info->z_dx = Z_LINEAR_FULL_ONE / info->z_gy; 1239 } 1240 1241 /* 1242 * We're safe for now, lets continue.. Look for our resampler 1243 * depending on configured format and quality. 1244 */ 1245 for (i = 0; i < Z_RESAMPLER_TAB_SIZE; i++) { 1246 int ridx; 1247 1248 if (AFMT_ENCODING(format) != z_resampler_tab[i].format) 1249 continue; 1250 if (Z_IS_SINC(info) && adaptive == 0 && 1251 z_resampler_build_polyphase(info) == 0) 1252 ridx = Z_RESAMPLER_SINC_POLYPHASE; 1253 else 1254 ridx = Z_RESAMPLER_IDX(info); 1255 info->z_resample = z_resampler_tab[i].resampler[ridx]; 1256 break; 1257 } 1258 1259 if (info->z_resample == NULL) 1260 return (EINVAL); 1261 1262 info->bps = AFMT_BPS(format); 1263 align = info->channels * info->bps; 1264 1265 /* 1266 * Calculate largest value that can be fed into z_gy2gx() and 1267 * z_gx2gy() without causing (signed) 32bit overflow. z_gy2gx() will 1268 * be called early during feeding process to determine how much input 1269 * samples that is required to generate requested output, while 1270 * z_gx2gy() will be called just before samples filtering / 1271 * accumulation process based on available samples that has been 1272 * calculated using z_gx2gy(). 1273 * 1274 * Now that is damn confusing, I guess ;-) . 1275 */ 1276 gy2gx_max = (((uint64_t)info->z_gy * INT32_MAX) - info->z_gy + 1) / 1277 info->z_gx; 1278 1279 if ((gy2gx_max * align) > SND_FXDIV_MAX) 1280 gy2gx_max = SND_FXDIV_MAX / align; 1281 1282 if (gy2gx_max < 1) 1283 return (E2BIG); 1284 1285 gx2gy_max = (((uint64_t)info->z_gx * INT32_MAX) - info->z_gy) / 1286 info->z_gy; 1287 1288 if (gx2gy_max > INT32_MAX) 1289 gx2gy_max = INT32_MAX; 1290 1291 if (gx2gy_max < 1) 1292 return (E2BIG); 1293 1294 /* 1295 * Ensure that z_gy2gx() at its largest possible calculated value 1296 * (alpha = 0) will not cause overflow further late during z_gx2gy() 1297 * stage. 1298 */ 1299 if (z_gy2gx(info, gy2gx_max) > _Z_GCAST(gx2gy_max)) 1300 return (E2BIG); 1301 1302 info->z_maxfeed = gy2gx_max * align; 1303 1304 #ifdef Z_USE_ALPHADRIFT 1305 info->z_startdrift = z_gy2gx(info, 1); 1306 info->z_alphadrift = z_drift(info, info->z_startdrift, 1); 1307 #endif 1308 1309 i = z_gy2gx(info, 1); 1310 info->z_full = z_roundpow2((info->z_size << 1) + i); 1311 1312 /* 1313 * Too big to be true, and overflowing left and right like mad .. 1314 */ 1315 if ((info->z_full * align) < 1) { 1316 if (adaptive == 0 && Z_IS_SINC(info)) { 1317 adaptive = 1; 1318 goto z_setup_adaptive_sinc; 1319 } 1320 return (E2BIG); 1321 } 1322 1323 /* 1324 * Increase full buffer size if its too small to reduce cyclic 1325 * buffer shifting in main conversion/feeder loop. 1326 */ 1327 while (info->z_full < Z_RESERVOIR_MAX && 1328 (info->z_full - (info->z_size << 1)) < Z_RESERVOIR) 1329 info->z_full <<= 1; 1330 1331 /* Initialize buffer position. */ 1332 info->z_mask = info->z_full - 1; 1333 info->z_start = z_prev(info, info->z_size << 1, 1); 1334 info->z_pos = z_next(info, info->z_start, 1); 1335 1336 /* 1337 * Allocate or reuse delay line buffer, whichever makes sense. 1338 */ 1339 i = info->z_full * align; 1340 if (i < 1) 1341 return (E2BIG); 1342 1343 if (info->z_delay == NULL || info->z_alloc < i || 1344 i <= (info->z_alloc >> 1)) { 1345 if (info->z_delay != NULL) 1346 free(info->z_delay, M_DEVBUF); 1347 info->z_delay = malloc(i, M_DEVBUF, M_NOWAIT | M_ZERO); 1348 if (info->z_delay == NULL) 1349 return (ENOMEM); 1350 info->z_alloc = i; 1351 } 1352 1353 /* 1354 * Zero out head of buffer to avoid pops and clicks. 1355 */ 1356 memset(info->z_delay, sndbuf_zerodata(f->desc->out), 1357 info->z_pos * align); 1358 1359 #ifdef Z_DIAGNOSTIC 1360 /* 1361 * XXX Debuging mess !@#$%^ 1362 */ 1363 #define dumpz(x) fprintf(stderr, "\t%12s = %10u : %-11d\n", \ 1364 "z_"__STRING(x), (uint32_t)info->z_##x, \ 1365 (int32_t)info->z_##x) 1366 fprintf(stderr, "\n%s():\n", __func__); 1367 fprintf(stderr, "\tchannels=%d, bps=%d, format=0x%08x, quality=%d\n", 1368 info->channels, info->bps, format, info->quality); 1369 fprintf(stderr, "\t%d (%d) -> %d (%d), ", 1370 info->src, info->rsrc, info->dst, info->rdst); 1371 fprintf(stderr, "[%d/%d]\n", info->z_gx, info->z_gy); 1372 fprintf(stderr, "\tminreq=%d, ", z_gy2gx(info, 1)); 1373 if (adaptive != 0) 1374 z_scale = Z_ONE; 1375 fprintf(stderr, "factor=0x%08x/0x%08x (%f)\n", 1376 z_scale, Z_ONE, (double)z_scale / Z_ONE); 1377 fprintf(stderr, "\tbase_length=%d, ", Z_SINC_BASE_LEN(info)); 1378 fprintf(stderr, "adaptive=%s\n", (adaptive != 0) ? "YES" : "NO"); 1379 dumpz(size); 1380 dumpz(alloc); 1381 if (info->z_alloc < 1024) 1382 fprintf(stderr, "\t%15s%10d Bytes\n", 1383 "", info->z_alloc); 1384 else if (info->z_alloc < (1024 << 10)) 1385 fprintf(stderr, "\t%15s%10d KBytes\n", 1386 "", info->z_alloc >> 10); 1387 else if (info->z_alloc < (1024 << 20)) 1388 fprintf(stderr, "\t%15s%10d MBytes\n", 1389 "", info->z_alloc >> 20); 1390 else 1391 fprintf(stderr, "\t%15s%10d GBytes\n", 1392 "", info->z_alloc >> 30); 1393 fprintf(stderr, "\t%12s %10d (min output samples)\n", 1394 "", 1395 (int32_t)z_gx2gy(info, info->z_full - (info->z_size << 1))); 1396 fprintf(stderr, "\t%12s %10d (min allocated output samples)\n", 1397 "", 1398 (int32_t)z_gx2gy(info, (info->z_alloc / align) - 1399 (info->z_size << 1))); 1400 fprintf(stderr, "\t%12s = %10d\n", 1401 "z_gy2gx()", (int32_t)z_gy2gx(info, 1)); 1402 fprintf(stderr, "\t%12s = %10d -> z_gy2gx() -> %d\n", 1403 "Max", (int32_t)gy2gx_max, (int32_t)z_gy2gx(info, gy2gx_max)); 1404 fprintf(stderr, "\t%12s = %10d\n", 1405 "z_gx2gy()", (int32_t)z_gx2gy(info, 1)); 1406 fprintf(stderr, "\t%12s = %10d -> z_gx2gy() -> %d\n", 1407 "Max", (int32_t)gx2gy_max, (int32_t)z_gx2gy(info, gx2gy_max)); 1408 dumpz(maxfeed); 1409 dumpz(full); 1410 dumpz(start); 1411 dumpz(pos); 1412 dumpz(scale); 1413 fprintf(stderr, "\t%12s %10f\n", "", 1414 (double)info->z_scale / Z_ONE); 1415 dumpz(dx); 1416 fprintf(stderr, "\t%12s %10f\n", "", 1417 (double)info->z_dx / info->z_dy); 1418 dumpz(dy); 1419 fprintf(stderr, "\t%12s %10d (drift step)\n", "", 1420 info->z_dy >> Z_SHIFT); 1421 fprintf(stderr, "\t%12s %10d (scaling differences)\n", "", 1422 (z_scale << Z_DRIFT_SHIFT) - info->z_dy); 1423 fprintf(stderr, "\t%12s = %u bytes\n", 1424 "intpcm32_t", sizeof(intpcm32_t)); 1425 fprintf(stderr, "\t%12s = 0x%08x, smallest=%.16lf\n", 1426 "Z_ONE", Z_ONE, (double)1.0 / (double)Z_ONE); 1427 #endif 1428 1429 return (0); 1430 } 1431 1432 static int 1433 z_resampler_set(struct pcm_feeder *f, int what, int32_t value) 1434 { 1435 struct z_info *info; 1436 int32_t oquality; 1437 1438 info = f->data; 1439 1440 switch (what) { 1441 case Z_RATE_SRC: 1442 if (value < feeder_rate_min || value > feeder_rate_max) 1443 return (E2BIG); 1444 if (value == info->rsrc) 1445 return (0); 1446 info->rsrc = value; 1447 break; 1448 case Z_RATE_DST: 1449 if (value < feeder_rate_min || value > feeder_rate_max) 1450 return (E2BIG); 1451 if (value == info->rdst) 1452 return (0); 1453 info->rdst = value; 1454 break; 1455 case Z_RATE_QUALITY: 1456 if (value < Z_QUALITY_MIN || value > Z_QUALITY_MAX) 1457 return (EINVAL); 1458 if (value == info->quality) 1459 return (0); 1460 /* 1461 * If we failed to set the requested quality, restore 1462 * the old one. We cannot afford leaving it broken since 1463 * passive feeder chains like vchans never reinitialize 1464 * itself. 1465 */ 1466 oquality = info->quality; 1467 info->quality = value; 1468 if (z_resampler_setup(f) == 0) 1469 return (0); 1470 info->quality = oquality; 1471 break; 1472 case Z_RATE_CHANNELS: 1473 if (value < SND_CHN_MIN || value > SND_CHN_MAX) 1474 return (EINVAL); 1475 if (value == info->channels) 1476 return (0); 1477 info->channels = value; 1478 break; 1479 default: 1480 return (EINVAL); 1481 break; 1482 } 1483 1484 return (z_resampler_setup(f)); 1485 } 1486 1487 static int 1488 z_resampler_get(struct pcm_feeder *f, int what) 1489 { 1490 struct z_info *info; 1491 1492 info = f->data; 1493 1494 switch (what) { 1495 case Z_RATE_SRC: 1496 return (info->rsrc); 1497 break; 1498 case Z_RATE_DST: 1499 return (info->rdst); 1500 break; 1501 case Z_RATE_QUALITY: 1502 return (info->quality); 1503 break; 1504 case Z_RATE_CHANNELS: 1505 return (info->channels); 1506 break; 1507 default: 1508 break; 1509 } 1510 1511 return (-1); 1512 } 1513 1514 static int 1515 z_resampler_init(struct pcm_feeder *f) 1516 { 1517 struct z_info *info; 1518 int ret; 1519 1520 if (f->desc->in != f->desc->out) 1521 return (EINVAL); 1522 1523 info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO); 1524 if (info == NULL) 1525 return (ENOMEM); 1526 1527 info->rsrc = Z_RATE_DEFAULT; 1528 info->rdst = Z_RATE_DEFAULT; 1529 info->quality = feeder_rate_quality; 1530 info->channels = AFMT_CHANNEL(f->desc->in); 1531 1532 f->data = info; 1533 1534 ret = z_resampler_setup(f); 1535 if (ret != 0) { 1536 if (info->z_pcoeff != NULL) 1537 free(info->z_pcoeff, M_DEVBUF); 1538 if (info->z_delay != NULL) 1539 free(info->z_delay, M_DEVBUF); 1540 free(info, M_DEVBUF); 1541 f->data = NULL; 1542 } 1543 1544 return (ret); 1545 } 1546 1547 static int 1548 z_resampler_free(struct pcm_feeder *f) 1549 { 1550 struct z_info *info; 1551 1552 info = f->data; 1553 if (info != NULL) { 1554 if (info->z_pcoeff != NULL) 1555 free(info->z_pcoeff, M_DEVBUF); 1556 if (info->z_delay != NULL) 1557 free(info->z_delay, M_DEVBUF); 1558 free(info, M_DEVBUF); 1559 } 1560 1561 f->data = NULL; 1562 1563 return (0); 1564 } 1565 1566 static uint32_t 1567 z_resampler_feed_internal(struct pcm_feeder *f, struct pcm_channel *c, 1568 uint8_t *b, uint32_t count, void *source) 1569 { 1570 struct z_info *info; 1571 int32_t alphadrift, startdrift, reqout, ocount, reqin, align; 1572 int32_t fetch, fetched, start, cp; 1573 uint8_t *dst; 1574 1575 info = f->data; 1576 if (info->z_resample == NULL) 1577 return (z_feed(f->source, c, b, count, source)); 1578 1579 /* 1580 * Calculate sample size alignment and amount of sample output. 1581 * We will do everything in sample domain, but at the end we 1582 * will jump back to byte domain. 1583 */ 1584 align = info->channels * info->bps; 1585 ocount = SND_FXDIV(count, align); 1586 if (ocount == 0) 1587 return (0); 1588 1589 /* 1590 * Calculate amount of input samples that is needed to generate 1591 * exact amount of output. 1592 */ 1593 reqin = z_gy2gx(info, ocount) - z_fetched(info); 1594 1595 #ifdef Z_USE_ALPHADRIFT 1596 startdrift = info->z_startdrift; 1597 alphadrift = info->z_alphadrift; 1598 #else 1599 startdrift = _Z_GY2GX(info, 0, 1); 1600 alphadrift = z_drift(info, startdrift, 1); 1601 #endif 1602 1603 dst = b; 1604 1605 do { 1606 if (reqin != 0) { 1607 fetch = z_min(z_free(info), reqin); 1608 if (fetch == 0) { 1609 /* 1610 * No more free spaces, so wind enough 1611 * samples back to the head of delay line 1612 * in byte domain. 1613 */ 1614 fetched = z_fetched(info); 1615 start = z_prev(info, info->z_start, 1616 (info->z_size << 1) - 1); 1617 cp = (info->z_size << 1) + fetched; 1618 z_copy(info->z_delay + (start * align), 1619 info->z_delay, cp * align); 1620 info->z_start = 1621 z_prev(info, info->z_size << 1, 1); 1622 info->z_pos = 1623 z_next(info, info->z_start, fetched + 1); 1624 fetch = z_min(z_free(info), reqin); 1625 #ifdef Z_DIAGNOSTIC 1626 if (1) { 1627 static uint32_t kk = 0; 1628 fprintf(stderr, 1629 "Buffer Move: " 1630 "start=%d fetched=%d cp=%d " 1631 "cycle=%u [%u]\r", 1632 start, fetched, cp, info->z_cycle, 1633 ++kk); 1634 } 1635 info->z_cycle = 0; 1636 #endif 1637 } 1638 if (fetch != 0) { 1639 /* 1640 * Fetch in byte domain and jump back 1641 * to sample domain. 1642 */ 1643 fetched = SND_FXDIV(z_feed(f->source, c, 1644 info->z_delay + (info->z_pos * align), 1645 fetch * align, source), align); 1646 /* 1647 * Prepare to convert fetched buffer, 1648 * or mark us done if we cannot fulfill 1649 * the request. 1650 */ 1651 reqin -= fetched; 1652 info->z_pos += fetched; 1653 if (fetched != fetch) 1654 reqin = 0; 1655 } 1656 } 1657 1658 reqout = z_min(z_gx2gy(info, z_fetched(info)), ocount); 1659 if (reqout != 0) { 1660 ocount -= reqout; 1661 1662 /* 1663 * Drift.. drift.. drift.. 1664 * 1665 * Notice that there are 2 methods of doing the drift 1666 * operations: The former is much cleaner (in a sense 1667 * of mathematical readings of my eyes), but slower 1668 * due to integer division in z_gy2gx(). Nevertheless, 1669 * both should give the same exact accurate drifting 1670 * results, so the later is favourable. 1671 */ 1672 do { 1673 info->z_resample(info, dst); 1674 #if 0 1675 startdrift = z_gy2gx(info, 1); 1676 alphadrift = z_drift(info, startdrift, 1); 1677 info->z_start += startdrift; 1678 info->z_alpha += alphadrift; 1679 #else 1680 info->z_alpha += alphadrift; 1681 if (info->z_alpha < info->z_gy) 1682 info->z_start += startdrift; 1683 else { 1684 info->z_start += startdrift - 1; 1685 info->z_alpha -= info->z_gy; 1686 } 1687 #endif 1688 dst += align; 1689 #ifdef Z_DIAGNOSTIC 1690 info->z_cycle++; 1691 #endif 1692 } while (--reqout != 0); 1693 } 1694 } while (reqin != 0 && ocount != 0); 1695 1696 /* 1697 * Back to byte domain.. 1698 */ 1699 return (dst - b); 1700 } 1701 1702 static int 1703 z_resampler_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b, 1704 uint32_t count, void *source) 1705 { 1706 uint32_t feed, maxfeed, left; 1707 1708 /* 1709 * Split count to smaller chunks to avoid possible 32bit overflow. 1710 */ 1711 maxfeed = ((struct z_info *)(f->data))->z_maxfeed; 1712 left = count; 1713 1714 do { 1715 feed = z_resampler_feed_internal(f, c, b, 1716 z_min(maxfeed, left), source); 1717 b += feed; 1718 left -= feed; 1719 } while (left != 0 && feed != 0); 1720 1721 return (count - left); 1722 } 1723 1724 static struct pcm_feederdesc feeder_rate_desc[] = { 1725 { FEEDER_RATE, 0, 0, 0, 0 }, 1726 { 0, 0, 0, 0, 0 }, 1727 }; 1728 1729 static kobj_method_t feeder_rate_methods[] = { 1730 KOBJMETHOD(feeder_init, z_resampler_init), 1731 KOBJMETHOD(feeder_free, z_resampler_free), 1732 KOBJMETHOD(feeder_set, z_resampler_set), 1733 KOBJMETHOD(feeder_get, z_resampler_get), 1734 KOBJMETHOD(feeder_feed, z_resampler_feed), 1735 KOBJMETHOD_END 1736 }; 1737 1738 FEEDER_DECLARE(feeder_rate, NULL); 1739