1 /*- 2 * Copyright (c) 2003 Orion Hodson <orion@freebsd.org> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 * 26 * MAINTAINER: Orion Hodson <orion@freebsd.org> 27 * 28 * This rate conversion code uses linear interpolation without any 29 * pre- or post- interpolation filtering to combat aliasing. This 30 * greatly limits the sound quality and should be addressed at some 31 * stage in the future. 32 * 33 * Since this accuracy of interpolation is sensitive and examination 34 * of the algorithm output is harder from the kernel, th code is 35 * designed to be compiled in the kernel and in a userland test 36 * harness. This is done by selectively including and excluding code 37 * with several portions based on whether _KERNEL is defined. It's a 38 * little ugly, but exceedingly useful. The testsuite and its 39 * revisions can be found at: 40 * http://people.freebsd.org/~orion/feedrate/ 41 * 42 * Special thanks to Ken Marx for exposing flaws in the code and for 43 * testing revisions. 44 */ 45 46 #ifdef _KERNEL 47 48 #include <dev/sound/pcm/sound.h> 49 #include "feeder_if.h" 50 51 SND_DECLARE_FILE("$FreeBSD$"); 52 53 #endif /* _KERNEL */ 54 55 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); 56 57 #ifndef RATE_ASSERT 58 #define RATE_ASSERT(x, y) /* KASSERT(x) */ 59 #endif /* RATE_ASSERT */ 60 61 #ifndef RATE_TRACE 62 #define RATE_TRACE(x...) /* printf(x) */ 63 #endif 64 65 /*****************************************************************************/ 66 67 /* The following coefficients are coupled. They are chosen to be 68 * guarantee calculable factors for the interpolation routine. They 69 * have been tested over the range of RATEMIN-RATEMAX Hz. Decreasing 70 * the granularity increases the required buffer size and affects the 71 * gain values at different points in the space. These values were 72 * found by running the test program with -p (probe) and some trial 73 * and error. 74 * 75 * ROUNDHZ the granularity of sample rates (fits n*11025 and n*8000). 76 * FEEDBUFSZ the amount of buffer space. 77 * MINGAIN the minimum acceptable gain in coefficients search. 78 */ 79 #define ROUNDHZ 25 80 #define FEEDBUFSZ 8192 81 #define MINGAIN 92 82 83 #define RATEMIN 4000 84 #define RATEMAX 48000 85 86 struct feed_rate_info; 87 88 typedef int (*rate_convert_method)(struct feed_rate_info *, 89 uint32_t, uint32_t, int16_t *); 90 91 static int 92 convert_stereo_up(struct feed_rate_info *info, 93 uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); 94 95 static int 96 convert_stereo_down(struct feed_rate_info *info, 97 uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); 98 99 struct feed_rate_info { 100 uint32_t src, dst; /* source and destination rates */ 101 uint16_t buffer_ticks; /* number of available samples in buffer */ 102 uint16_t buffer_pos; /* next available sample in buffer */ 103 uint16_t rounds; /* maximum number of cycle rounds w buffer */ 104 uint16_t alpha; /* interpolation distance */ 105 uint16_t sscale; /* src clock scale */ 106 uint16_t dscale; /* dst clock scale */ 107 uint16_t mscale; /* scale factor to avoid divide per sample */ 108 uint16_t mroll; /* roll to again avoid divide per sample */ 109 uint16_t channels; /* 1 = mono, 2 = stereo */ 110 111 rate_convert_method convert; 112 int16_t buffer[FEEDBUFSZ]; 113 }; 114 115 #define bytes_per_sample 2 116 #define src_ticks_per_cycle(info) (info->dscale * info->rounds) 117 #define dst_ticks_per_cycle(info) (info->sscale * info->rounds) 118 #define bytes_per_tick(info) (info->channels * bytes_per_sample) 119 #define src_bytes_per_cycle(info) \ 120 (src_ticks_per_cycle(info) * bytes_per_tick(info)) 121 #define dst_bytes_per_cycle(info) \ 122 (dst_ticks_per_cycle(info) * bytes_per_tick(info)) 123 124 static uint32_t 125 gcd(uint32_t x, uint32_t y) 126 { 127 uint32_t w; 128 while (y != 0) { 129 w = x % y; 130 x = y; 131 y = w; 132 } 133 return x; 134 } 135 136 static int 137 feed_rate_setup(struct pcm_feeder *f) 138 { 139 struct feed_rate_info *info = f->data; 140 uint32_t mscale, mroll, l, r, g; 141 142 /* Beat sample rates down by greatest common divisor */ 143 g = gcd(info->src, info->dst); 144 info->sscale = info->dst / g; 145 info->dscale = info->src / g; 146 147 info->alpha = 0; 148 info->buffer_ticks = 0; 149 info->buffer_pos = 0; 150 151 /* Pick suitable conversion routine */ 152 if (info->src > info->dst) { 153 info->convert = convert_stereo_down; 154 } else { 155 info->convert = convert_stereo_up; 156 } 157 158 /* 159 * Determine number of conversion rounds that will fit into 160 * buffer. NB Must set info->rounds to one before using 161 * src_ticks_per_cycle here since it used by src_ticks_per_cycle. 162 */ 163 info->rounds = 1; 164 r = (FEEDBUFSZ - bytes_per_tick(info)) / 165 (src_ticks_per_cycle(info) * bytes_per_tick(info)); 166 if (r == 0) { 167 RATE_TRACE("Insufficient buffer space for conversion %d -> %d " 168 "(%d < %d)\n", info->src, info->dst, FEEDBUFSZ, 169 src_ticks_per_cycle(info) * bytes_per_tick(info)); 170 return -1; 171 } 172 info->rounds = r; 173 174 /* 175 * Find scale and roll combination that allows us to trade 176 * costly divide operations in the main loop for multiply-rolls. 177 */ 178 for (l = 96; l >= MINGAIN; l -= 3) { 179 for (mroll = 0; mroll < 16; mroll ++) { 180 mscale = (1 << mroll) / info->sscale; 181 182 r = (mscale * info->sscale * 100) >> mroll; 183 if (r > l && r <= 100) { 184 info->mscale = mscale; 185 info->mroll = mroll; 186 RATE_TRACE("Converting %d to %d with " 187 "mscale = %d and mroll = %d " 188 "(gain = %d / 100)\n", 189 info->src, info->dst, 190 info->mscale, info->mroll, r); 191 return 0; 192 } 193 } 194 } 195 196 RATE_TRACE("Failed to find a converter within %d%% gain for " 197 "%d to %d.\n", l, info->src, info->dst); 198 199 return -2; 200 } 201 202 static int 203 feed_rate_set(struct pcm_feeder *f, int what, int value) 204 { 205 struct feed_rate_info *info = f->data; 206 int rvalue; 207 208 if (value < RATEMIN || value > RATEMAX) { 209 return -1; 210 } 211 212 rvalue = (value / ROUNDHZ) * ROUNDHZ; 213 if (value - rvalue > ROUNDHZ / 2) { 214 rvalue += ROUNDHZ; 215 } 216 217 switch(what) { 218 case FEEDRATE_SRC: 219 info->src = rvalue; 220 break; 221 case FEEDRATE_DST: 222 info->dst = rvalue; 223 break; 224 default: 225 return -1; 226 } 227 228 return feed_rate_setup(f); 229 } 230 231 static int 232 feed_rate_get(struct pcm_feeder *f, int what) 233 { 234 struct feed_rate_info *info = f->data; 235 236 switch(what) { 237 case FEEDRATE_SRC: 238 return info->src; 239 case FEEDRATE_DST: 240 return info->dst; 241 default: 242 return -1; 243 } 244 return -1; 245 } 246 247 static int 248 feed_rate_init(struct pcm_feeder *f) 249 { 250 struct feed_rate_info *info; 251 252 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO); 253 if (info == NULL) 254 return ENOMEM; 255 info->src = DSP_DEFAULT_SPEED; 256 info->dst = DSP_DEFAULT_SPEED; 257 info->channels = 2; 258 259 f->data = info; 260 return 0; 261 } 262 263 static int 264 feed_rate_free(struct pcm_feeder *f) 265 { 266 struct feed_rate_info *info = f->data; 267 268 if (info) { 269 free(info, M_RATEFEEDER); 270 } 271 f->data = NULL; 272 return 0; 273 } 274 275 static int 276 convert_stereo_up(struct feed_rate_info *info, 277 uint32_t src_ticks, 278 uint32_t dst_ticks, 279 int16_t *dst) 280 { 281 uint32_t max_dst_ticks; 282 int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o; 283 int16_t *src; 284 285 sp = info->buffer_pos * 2; 286 se = sp + src_ticks * 2; 287 288 src = info->buffer; 289 alpha = info->alpha * info->mscale; 290 dalpha = info->dscale * info->mscale; /* Alpha increment */ 291 malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ 292 mroll = info->mroll; 293 294 /* 295 * For efficiency the main conversion loop should only depend on 296 * one variable. We use the state to work out the maximum number 297 * of output samples that are available and eliminate the checking of 298 * sp from the loop. 299 */ 300 max_dst_ticks = src_ticks * info->dst / info->src - alpha / dalpha; 301 if (max_dst_ticks < dst_ticks) { 302 dst_ticks = max_dst_ticks; 303 } 304 305 dp = 0; 306 de = dst_ticks * 2; 307 /* 308 * Unrolling this loop manually does not help much here because 309 * of the alpha, malpha comparison. 310 */ 311 while (dp < de) { 312 o = malpha - alpha; 313 x = alpha * src[sp + 2] + o * src[sp]; 314 dst[dp++] = x >> mroll; 315 x = alpha * src[sp + 3] + o * src[sp + 1]; 316 dst[dp++] = x >> mroll; 317 alpha += dalpha; 318 if (alpha >= malpha) { 319 alpha -= malpha; 320 sp += 2; 321 } 322 } 323 RATE_ASSERT(sp <= se, ("%s: Source overrun\n", __func__)); 324 325 info->buffer_pos = sp / info->channels; 326 info->alpha = alpha / info->mscale; 327 328 return dp / info->channels; 329 } 330 331 static int 332 convert_stereo_down(struct feed_rate_info *info, 333 uint32_t src_ticks, 334 uint32_t dst_ticks, 335 int16_t *dst) 336 { 337 int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o, m, 338 mdalpha, mstep; 339 int16_t *src; 340 341 sp = info->buffer_pos * 2; 342 se = sp + src_ticks * 2; 343 344 src = info->buffer; 345 alpha = info->alpha * info->mscale; 346 dalpha = info->dscale * info->mscale; /* Alpha increment */ 347 malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ 348 mroll = info->mroll; 349 350 dp = 0; 351 de = dst_ticks * 2; 352 353 m = dalpha / malpha; 354 mstep = m * 2; 355 mdalpha = dalpha - m * malpha; 356 357 /* 358 * TODO: eliminate sp or dp from this loop comparison for a few 359 * extra % performance. 360 */ 361 while (sp < se && dp < de) { 362 o = malpha - alpha; 363 x = alpha * src[sp + 2] + o * src[sp]; 364 dst[dp++] = x >> mroll; 365 x = alpha * src[sp + 3] + o * src[sp + 1]; 366 dst[dp++] = x >> mroll; 367 368 alpha += mdalpha; 369 sp += mstep; 370 if (alpha >= malpha) { 371 alpha -= malpha; 372 sp += 2; 373 } 374 } 375 376 info->buffer_pos = sp / 2; 377 info->alpha = alpha / info->mscale; 378 379 RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, 380 ("%s: Source overrun\n", __func__)); 381 382 return dp / 2; 383 } 384 385 static int 386 feed_rate(struct pcm_feeder *f, 387 struct pcm_channel *c, 388 uint8_t *b, 389 uint32_t count, 390 void *source) 391 { 392 struct feed_rate_info *info = f->data; 393 394 uint32_t done, s_ticks, d_ticks; 395 done = 0; 396 397 RATE_ASSERT(info->channels == 2, 398 ("%s: channels (%d) != 2", __func__, info->channels)); 399 400 while (done < count) { 401 /* Slurp in more data if input buffer is not full */ 402 while (info->buffer_ticks < src_ticks_per_cycle(info)) { 403 uint8_t *u8b; 404 int fetch; 405 fetch = src_bytes_per_cycle(info) - 406 info->buffer_ticks * bytes_per_tick(info); 407 u8b = (uint8_t*)info->buffer + 408 (info->buffer_ticks + 1) * 409 bytes_per_tick(info); 410 fetch = FEEDER_FEED(f->source, c, u8b, fetch, source); 411 RATE_ASSERT(fetch % bytes_per_tick(info) == 0, 412 ("%s: fetched unaligned bytes (%d)", 413 __func__, fetch)); 414 info->buffer_ticks += fetch / bytes_per_tick(info); 415 RATE_ASSERT(src_ticks_per_cycle(info) >= 416 info->buffer_ticks, 417 ("%s: buffer overfilled (%d > %d).", 418 __func__, info->buffer_ticks, 419 src_ticks_per_cycle(info))); 420 if (fetch == 0) 421 break; 422 } 423 424 /* Find amount of input buffer data that should be processed */ 425 d_ticks = (count - done) / bytes_per_tick(info); 426 s_ticks = info->buffer_ticks - info->buffer_pos; 427 if (info->buffer_ticks != src_ticks_per_cycle(info)) { 428 if (s_ticks > 8) 429 s_ticks -= 8; 430 else 431 s_ticks = 0; 432 } 433 434 d_ticks = info->convert(info, s_ticks, d_ticks, 435 (int16_t*)(b + done)); 436 if (d_ticks == 0) 437 break; 438 done += d_ticks * bytes_per_tick(info); 439 440 RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, 441 ("%s: buffer_ticks too big\n", __func__)); 442 RATE_ASSERT(info->buffer_ticks <= src_ticks_per_cycle(info), 443 ("too many ticks %d / %d\n", 444 info->buffer_ticks, src_ticks_per_cycle(info))); 445 RATE_TRACE("%s: ticks %5d / %d pos %d\n", __func__, 446 info->buffer_ticks, src_ticks_per_cycle(info), 447 info->buffer_pos); 448 449 if (src_ticks_per_cycle(info) <= info->buffer_pos) { 450 /* End of cycle reached, copy last samples to start */ 451 uint8_t *u8b; 452 u8b = (uint8_t*)info->buffer; 453 bcopy(u8b + src_bytes_per_cycle(info), u8b, 454 bytes_per_tick(info)); 455 456 RATE_ASSERT(info->alpha == 0, 457 ("%s: completed cycle with " 458 "alpha non-zero", __func__, info->alpha)); 459 460 info->buffer_pos = 0; 461 info->buffer_ticks = 0; 462 } 463 } 464 465 RATE_ASSERT(count >= done, 466 ("%s: generated too many bytes of data (%d > %d).", 467 __func__, done, count)); 468 469 if (done != count) { 470 RATE_TRACE("Only did %d of %d\n", done, count); 471 } 472 473 return done; 474 } 475 476 static struct pcm_feederdesc feeder_rate_desc[] = { 477 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 478 {0, 0, 0, 0}, 479 }; 480 static kobj_method_t feeder_rate_methods[] = { 481 KOBJMETHOD(feeder_init, feed_rate_init), 482 KOBJMETHOD(feeder_free, feed_rate_free), 483 KOBJMETHOD(feeder_set, feed_rate_set), 484 KOBJMETHOD(feeder_get, feed_rate_get), 485 KOBJMETHOD(feeder_feed, feed_rate), 486 {0, 0} 487 }; 488 FEEDER_DECLARE(feeder_rate, 2, NULL); 489 490