feeder_rate.c revision 113752
175320Scg/* 2109547Sorion * Copyright (c) 2003 Orion Hodson <orion@freebsd.org> 375320Scg * All rights reserved. 475320Scg * 575320Scg * Redistribution and use in source and binary forms, with or without 675320Scg * modification, are permitted provided that the following conditions 775320Scg * are met: 875320Scg * 1. Redistributions of source code must retain the above copyright 975320Scg * notice, this list of conditions and the following disclaimer. 1075320Scg * 2. Redistributions in binary form must reproduce the above copyright 1175320Scg * notice, this list of conditions and the following disclaimer in the 1275320Scg * documentation and/or other materials provided with the distribution. 1375320Scg * 1475320Scg * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 1575320Scg * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1675320Scg * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 1775320Scg * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 1875320Scg * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 1975320Scg * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 2075320Scg * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 2175320Scg * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 2275320Scg * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 2375320Scg * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 2475320Scg * SUCH DAMAGE. 25109547Sorion * 26109547Sorion * MAINTAINER: Orion Hodson <orion@freebsd.org> 27109547Sorion * 28109547Sorion * This rate conversion code uses linear interpolation without any 29109547Sorion * pre- or post- interpolation filtering to combat aliasing. This 30109547Sorion * greatly limits the sound quality and should be addressed at some 31109547Sorion * stage in the future. 32109547Sorion * 33109547Sorion * Since this accuracy of interpolation is sensitive and examination 34109547Sorion * of the algorithm output is harder from the kernel, th code is 35109547Sorion * designed to be compiled in the kernel and in a userland test 36109547Sorion * harness. This is done by selectively including and excluding code 37109547Sorion * with several portions based on whether _KERNEL is defined. It's a 38110108Sorion * little ugly, but exceedingly useful. The testsuite and its 39110108Sorion * revisions can be found at: 40110108Sorion * http://people.freebsd.org/~orion/feedrate/ 41110108Sorion * 42110108Sorion * Special thanks to Ken Marx for exposing flaws in the code and for 43110108Sorion * testing revisions. 4475320Scg */ 4575320Scg 46109547Sorion#ifdef _KERNEL 47109547Sorion 4875320Scg#include <dev/sound/pcm/sound.h> 4975320Scg#include "feeder_if.h" 5075320Scg 5182180ScgSND_DECLARE_FILE("$FreeBSD: head/sys/dev/sound/pcm/feeder_rate.c 113752 2003-04-20 17:08:56Z orion $"); 52110466Sorion 53110466Sorion#endif /* _KERNEL */ 54110466Sorion 5575320ScgMALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); 5675320Scg 57110466Sorion#ifndef RATE_ASSERT 58110466Sorion#define RATE_ASSERT(x, y) /* KASSERT(x) */ 59110466Sorion#endif /* RATE_ASSERT */ 60109547Sorion 61110466Sorion#ifndef RATE_TRACE 62110466Sorion#define RATE_TRACE(x...) /* printf(x) */ 63110466Sorion#endif 64109547Sorion 65110108Sorion/*****************************************************************************/ 66110466Sorion 67110466Sorion/* The following coefficients are coupled. They are chosen to be 68110466Sorion * guarantee calculable factors for the interpolation routine. They 69110466Sorion * have been tested over the range of RATEMIN-RATEMAX Hz. Decreasing 70110466Sorion * the granularity increases the required buffer size and affects the 71110466Sorion * gain values at different points in the space. These values were 72110466Sorion * found by running the test program with -p (probe) and some trial 73110466Sorion * and error. 74110108Sorion * 75110108Sorion * ROUNDHZ the granularity of sample rates (fits n*11025 and n*8000). 76110108Sorion * FEEDBUFSZ the amount of buffer space. 77110108Sorion * MINGAIN the minimum acceptable gain in coefficients search. 78110108Sorion */ 79110108Sorion#define ROUNDHZ 25 80110466Sorion#define FEEDBUFSZ 8192 81110108Sorion#define MINGAIN 92 82109547Sorion 83110466Sorion#define RATEMIN 4000 84110466Sorion#define RATEMAX 48000 85109547Sorion 86109547Sorionstruct feed_rate_info; 87109547Sorion 88109547Soriontypedef int (*rate_convert_method)(struct feed_rate_info *, 89109547Sorion uint32_t, uint32_t, int16_t *); 90109547Sorion 91109547Sorionstatic int 92109547Sorionconvert_stereo_up(struct feed_rate_info *info, 93109547Sorion uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); 94109547Sorion 95109547Sorionstatic int 96109547Sorionconvert_stereo_down(struct feed_rate_info *info, 97109547Sorion uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); 98109547Sorion 9975320Scgstruct feed_rate_info { 100109547Sorion uint32_t src, dst; /* source and destination rates */ 101109547Sorion uint16_t buffer_ticks; /* number of available samples in buffer */ 102109547Sorion uint16_t buffer_pos; /* next available sample in buffer */ 103109547Sorion uint16_t rounds; /* maximum number of cycle rounds w buffer */ 104109547Sorion uint16_t alpha; /* interpolation distance */ 105109547Sorion uint16_t sscale; /* src clock scale */ 106109547Sorion uint16_t dscale; /* dst clock scale */ 107109547Sorion uint16_t mscale; /* scale factor to avoid divide per sample */ 108109547Sorion uint16_t mroll; /* roll to again avoid divide per sample */ 109109547Sorion uint16_t channels; /* 1 = mono, 2 = stereo */ 110109547Sorion 111109547Sorion rate_convert_method convert; 112110108Sorion int16_t buffer[FEEDBUFSZ]; 11375320Scg}; 11475320Scg 115110108Sorion#define bytes_per_sample 2 116110108Sorion#define src_ticks_per_cycle(info) (info->dscale * info->rounds) 117110108Sorion#define dst_ticks_per_cycle(info) (info->sscale * info->rounds) 118110108Sorion#define bytes_per_tick(info) (info->channels * bytes_per_sample) 119109547Sorion#define src_bytes_per_cycle(info) \ 120110108Sorion (src_ticks_per_cycle(info) * bytes_per_tick(info)) 121109547Sorion#define dst_bytes_per_cycle(info) \ 122110108Sorion (dst_ticks_per_cycle(info) * bytes_per_tick(info)) 123109547Sorion 124109547Sorionstatic uint32_t 125110108Soriongcd(uint32_t x, uint32_t y) 126109547Sorion{ 127110108Sorion uint32_t w; 128110108Sorion while (y != 0) { 129110108Sorion w = x % y; 130110108Sorion x = y; 131110108Sorion y = w; 132110108Sorion } 133110108Sorion return x; 134109547Sorion} 135109547Sorion 13675320Scgstatic int 13775320Scgfeed_rate_setup(struct pcm_feeder *f) 13875320Scg{ 13975320Scg struct feed_rate_info *info = f->data; 140109547Sorion uint32_t mscale, mroll, l, r, g; 141109547Sorion 142109547Sorion /* Beat sample rates down by greatest common divisor */ 143109547Sorion g = gcd(info->src, info->dst); 144109547Sorion info->sscale = info->dst / g; 145109547Sorion info->dscale = info->src / g; 14675320Scg 147109547Sorion info->alpha = 0; 148109547Sorion info->buffer_ticks = 0; 149109547Sorion info->buffer_pos = 0; 150109547Sorion 151109547Sorion /* Pick suitable conversion routine */ 152109547Sorion if (info->src > info->dst) { 153109547Sorion info->convert = convert_stereo_down; 154109547Sorion } else { 155109547Sorion info->convert = convert_stereo_up; 156109547Sorion } 157109547Sorion 158109547Sorion /* 159109547Sorion * Determine number of conversion rounds that will fit into 160109547Sorion * buffer. NB Must set info->rounds to one before using 161109547Sorion * src_ticks_per_cycle here since it used by src_ticks_per_cycle. 162109547Sorion */ 163109547Sorion info->rounds = 1; 164110108Sorion r = (FEEDBUFSZ - bytes_per_tick(info)) / 165110108Sorion (src_ticks_per_cycle(info) * bytes_per_tick(info)); 166110108Sorion if (r == 0) { 167110108Sorion RATE_TRACE("Insufficient buffer space for conversion %d -> %d " 168110108Sorion "(%d < %d)\n", info->src, info->dst, FEEDBUFSZ, 169110108Sorion src_ticks_per_cycle(info) * bytes_per_tick(info)); 170110108Sorion return -1; 171110108Sorion } 172110108Sorion info->rounds = r; 173110108Sorion 174109547Sorion /* 175109547Sorion * Find scale and roll combination that allows us to trade 176109547Sorion * costly divide operations in the main loop for multiply-rolls. 177109547Sorion */ 178110108Sorion for (l = 96; l >= MINGAIN; l -= 3) { 179110108Sorion for (mroll = 0; mroll < 16; mroll ++) { 180109547Sorion mscale = (1 << mroll) / info->sscale; 181110108Sorion 182109547Sorion r = (mscale * info->sscale * 100) >> mroll; 183109547Sorion if (r > l && r <= 100) { 184109547Sorion info->mscale = mscale; 185109547Sorion info->mroll = mroll; 186109547Sorion RATE_TRACE("Converting %d to %d with " 187109547Sorion "mscale = %d and mroll = %d " 188109547Sorion "(gain = %d / 100)\n", 189109547Sorion info->src, info->dst, 190109547Sorion info->mscale, info->mroll, r); 191109547Sorion return 0; 192109547Sorion } 193109547Sorion } 194109547Sorion } 195110108Sorion 196110108Sorion RATE_TRACE("Failed to find a converter within %d%% gain for " 197110108Sorion "%d to %d.\n", l, info->src, info->dst); 198109547Sorion 199110108Sorion return -2; 20075320Scg} 20175320Scg 20275320Scgstatic int 20375320Scgfeed_rate_set(struct pcm_feeder *f, int what, int value) 20475320Scg{ 20575320Scg struct feed_rate_info *info = f->data; 206110108Sorion int rvalue; 207110108Sorion 208110108Sorion if (value < RATEMIN || value > RATEMAX) { 209110108Sorion return -1; 210110108Sorion } 211110108Sorion 212110108Sorion rvalue = (value / ROUNDHZ) * ROUNDHZ; 213110108Sorion if (value - rvalue > ROUNDHZ / 2) { 214110108Sorion rvalue += ROUNDHZ; 215110108Sorion } 216110108Sorion 21775320Scg switch(what) { 21875320Scg case FEEDRATE_SRC: 219110108Sorion info->src = rvalue; 22075320Scg break; 22175320Scg case FEEDRATE_DST: 222110108Sorion info->dst = rvalue; 22375320Scg break; 22475320Scg default: 22575320Scg return -1; 22675320Scg } 227109547Sorion 22875320Scg return feed_rate_setup(f); 22975320Scg} 23075320Scg 23175320Scgstatic int 23277266Scgfeed_rate_get(struct pcm_feeder *f, int what) 23377266Scg{ 23477266Scg struct feed_rate_info *info = f->data; 23577266Scg 23677266Scg switch(what) { 23777266Scg case FEEDRATE_SRC: 23877266Scg return info->src; 23977266Scg case FEEDRATE_DST: 24077266Scg return info->dst; 24177266Scg default: 24277266Scg return -1; 24377266Scg } 24477266Scg return -1; 24577266Scg} 24677266Scg 24777266Scgstatic int 24875320Scgfeed_rate_init(struct pcm_feeder *f) 24975320Scg{ 25075320Scg struct feed_rate_info *info; 25175320Scg 252111909Sorion info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO); 253113752Sorion if (info == NULL) 254113752Sorion return ENOMEM; 25575320Scg info->src = DSP_DEFAULT_SPEED; 25675320Scg info->dst = DSP_DEFAULT_SPEED; 257110108Sorion info->channels = 2; 258109547Sorion 25975320Scg f->data = info; 260110108Sorion return 0; 26175320Scg} 26275320Scg 26375320Scgstatic int 26475320Scgfeed_rate_free(struct pcm_feeder *f) 26575320Scg{ 26675320Scg struct feed_rate_info *info = f->data; 26775320Scg 26875320Scg if (info) { 26975320Scg free(info, M_RATEFEEDER); 27075320Scg } 27175320Scg f->data = NULL; 27275320Scg return 0; 27375320Scg} 27475320Scg 27575320Scgstatic int 276109547Sorionconvert_stereo_up(struct feed_rate_info *info, 277110108Sorion uint32_t src_ticks, 278110108Sorion uint32_t dst_ticks, 279110108Sorion int16_t *dst) 28075320Scg{ 281109547Sorion uint32_t max_dst_ticks; 282109547Sorion int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o; 283109547Sorion int16_t *src; 28475320Scg 285109547Sorion sp = info->buffer_pos * 2; 286109547Sorion se = sp + src_ticks * 2; 287109547Sorion 288109547Sorion src = info->buffer; 289109547Sorion alpha = info->alpha * info->mscale; 290109547Sorion dalpha = info->dscale * info->mscale; /* Alpha increment */ 291109547Sorion malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ 292109547Sorion mroll = info->mroll; 293109547Sorion 29475320Scg /* 295109547Sorion * For efficiency the main conversion loop should only depend on 296109547Sorion * one variable. We use the state to work out the maximum number 297109547Sorion * of output samples that are available and eliminate the checking of 298109547Sorion * sp from the loop. 29975320Scg */ 300109547Sorion max_dst_ticks = src_ticks * info->dst / info->src - alpha / dalpha; 301109547Sorion if (max_dst_ticks < dst_ticks) { 302109547Sorion dst_ticks = max_dst_ticks; 303109547Sorion } 30475320Scg 305109547Sorion dp = 0; 306109547Sorion de = dst_ticks * 2; 30775320Scg /* 308110108Sorion * Unrolling this loop manually does not help much here because 309109547Sorion * of the alpha, malpha comparison. 31075320Scg */ 311109547Sorion while (dp < de) { 312109547Sorion o = malpha - alpha; 313109547Sorion x = alpha * src[sp + 2] + o * src[sp]; 314109547Sorion dst[dp++] = x >> mroll; 315109547Sorion x = alpha * src[sp + 3] + o * src[sp + 1]; 316109547Sorion dst[dp++] = x >> mroll; 317109547Sorion alpha += dalpha; 318109547Sorion if (alpha >= malpha) { 319109547Sorion alpha -= malpha; 320109547Sorion sp += 2; 321109547Sorion } 322109547Sorion } 323109547Sorion RATE_ASSERT(sp <= se, ("%s: Source overrun\n", __func__)); 32475320Scg 325109547Sorion info->buffer_pos = sp / info->channels; 326109547Sorion info->alpha = alpha / info->mscale; 32775320Scg 328109547Sorion return dp / info->channels; 329109547Sorion} 33075320Scg 331109547Sorionstatic int 332109547Sorionconvert_stereo_down(struct feed_rate_info *info, 333110108Sorion uint32_t src_ticks, 334110108Sorion uint32_t dst_ticks, 335110108Sorion int16_t *dst) 336109547Sorion{ 337109547Sorion int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o, m, 338109547Sorion mdalpha, mstep; 339109547Sorion int16_t *src; 34075320Scg 341109547Sorion sp = info->buffer_pos * 2; 342109547Sorion se = sp + src_ticks * 2; 34375320Scg 344109547Sorion src = info->buffer; 345109547Sorion alpha = info->alpha * info->mscale; 346109547Sorion dalpha = info->dscale * info->mscale; /* Alpha increment */ 347109547Sorion malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ 348109547Sorion mroll = info->mroll; 349109547Sorion 350109547Sorion dp = 0; 351109547Sorion de = dst_ticks * 2; 352109547Sorion 353109547Sorion m = dalpha / malpha; 354109547Sorion mstep = m * 2; 355109547Sorion mdalpha = dalpha - m * malpha; 356109547Sorion 357109547Sorion /* 358109547Sorion * TODO: eliminate sp or dp from this loop comparison for a few 359109547Sorion * extra % performance. 360109547Sorion */ 361109547Sorion while (sp < se && dp < de) { 362109547Sorion o = malpha - alpha; 363109547Sorion x = alpha * src[sp + 2] + o * src[sp]; 364109547Sorion dst[dp++] = x >> mroll; 365109547Sorion x = alpha * src[sp + 3] + o * src[sp + 1]; 366109547Sorion dst[dp++] = x >> mroll; 367109547Sorion 368109547Sorion alpha += mdalpha; 369109547Sorion sp += mstep; 370109547Sorion if (alpha >= malpha) { 371109547Sorion alpha -= malpha; 372109547Sorion sp += 2; 373109547Sorion } 37475320Scg } 37575320Scg 376110108Sorion info->buffer_pos = sp / 2; 377109547Sorion info->alpha = alpha / info->mscale; 378109547Sorion 379109547Sorion RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, 380109547Sorion ("%s: Source overrun\n", __func__)); 381109547Sorion 382110108Sorion return dp / 2; 38375320Scg} 38475320Scg 385109547Sorionstatic int 386109547Sorionfeed_rate(struct pcm_feeder *f, 387109547Sorion struct pcm_channel *c, 388109547Sorion uint8_t *b, 389109547Sorion uint32_t count, 390109547Sorion void *source) 391109547Sorion{ 392109547Sorion struct feed_rate_info *info = f->data; 393109547Sorion 394109547Sorion uint32_t done, s_ticks, d_ticks; 395109547Sorion done = 0; 396109547Sorion 397109547Sorion RATE_ASSERT(info->channels == 2, 398109547Sorion ("%s: channels (%d) != 2", __func__, info->channels)); 399109547Sorion 400109547Sorion while (done < count) { 401109547Sorion /* Slurp in more data if input buffer is not full */ 402110108Sorion while (info->buffer_ticks < src_ticks_per_cycle(info)) { 403109547Sorion uint8_t *u8b; 404109547Sorion int fetch; 405109547Sorion fetch = src_bytes_per_cycle(info) - 406109547Sorion info->buffer_ticks * bytes_per_tick(info); 407109547Sorion u8b = (uint8_t*)info->buffer + 408109547Sorion (info->buffer_ticks + 1) * 409109547Sorion bytes_per_tick(info); 410109547Sorion fetch = FEEDER_FEED(f->source, c, u8b, fetch, source); 411109547Sorion RATE_ASSERT(fetch % bytes_per_tick(info) == 0, 412109547Sorion ("%s: fetched unaligned bytes (%d)", 413109547Sorion __func__, fetch)); 414109547Sorion info->buffer_ticks += fetch / bytes_per_tick(info); 415109547Sorion RATE_ASSERT(src_ticks_per_cycle(info) >= 416109547Sorion info->buffer_ticks, 417109547Sorion ("%s: buffer overfilled (%d > %d).", 418109547Sorion __func__, info->buffer_ticks, 419109547Sorion src_ticks_per_cycle(info))); 420110108Sorion if (fetch == 0) 421109547Sorion break; 422109547Sorion } 423109547Sorion 424109547Sorion /* Find amount of input buffer data that should be processed */ 425109547Sorion d_ticks = (count - done) / bytes_per_tick(info); 426109547Sorion s_ticks = info->buffer_ticks - info->buffer_pos; 427110108Sorion if (info->buffer_ticks != src_ticks_per_cycle(info)) { 428110108Sorion if (s_ticks > 8) 429110108Sorion s_ticks -= 8; 430110108Sorion else 431110108Sorion s_ticks = 0; 432110108Sorion } 433109547Sorion 434109547Sorion d_ticks = info->convert(info, s_ticks, d_ticks, 435110108Sorion (int16_t*)(b + done)); 436109547Sorion if (d_ticks == 0) 437109547Sorion break; 438109547Sorion done += d_ticks * bytes_per_tick(info); 439109547Sorion 440109547Sorion RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, 441109547Sorion ("%s: buffer_ticks too big\n", __func__)); 442110466Sorion RATE_ASSERT(info->buffer_ticks <= src_ticks_per_cycle(info), 443110466Sorion ("too many ticks %d / %d\n", 444110466Sorion info->buffer_ticks, src_ticks_per_cycle(info))); 445110466Sorion RATE_TRACE("%s: ticks %5d / %d pos %d\n", __func__, 446110466Sorion info->buffer_ticks, src_ticks_per_cycle(info), 447110466Sorion info->buffer_pos); 448109547Sorion 449109547Sorion if (src_ticks_per_cycle(info) <= info->buffer_pos) { 450109547Sorion /* End of cycle reached, copy last samples to start */ 451109547Sorion uint8_t *u8b; 452109547Sorion u8b = (uint8_t*)info->buffer; 453109547Sorion bcopy(u8b + src_bytes_per_cycle(info), u8b, 454109547Sorion bytes_per_tick(info)); 455109547Sorion 456109547Sorion RATE_ASSERT(info->alpha == 0, 457110108Sorion ("%s: completed cycle with " 458110108Sorion "alpha non-zero", __func__, info->alpha)); 459109547Sorion 460109547Sorion info->buffer_pos = 0; 461109547Sorion info->buffer_ticks = 0; 462109547Sorion } 463109547Sorion } 464109547Sorion 465109547Sorion RATE_ASSERT(count >= done, 466109547Sorion ("%s: generated too many bytes of data (%d > %d).", 467109547Sorion __func__, done, count)); 468109547Sorion 469110108Sorion if (done != count) { 470110108Sorion RATE_TRACE("Only did %d of %d\n", done, count); 471110108Sorion } 472110108Sorion 473109547Sorion return done; 474109547Sorion} 475109547Sorion 47675320Scgstatic struct pcm_feederdesc feeder_rate_desc[] = { 47775320Scg {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 478110466Sorion {0, 0, 0, 0}, 47975320Scg}; 48075320Scgstatic kobj_method_t feeder_rate_methods[] = { 48175320Scg KOBJMETHOD(feeder_init, feed_rate_init), 48275320Scg KOBJMETHOD(feeder_free, feed_rate_free), 48375320Scg KOBJMETHOD(feeder_set, feed_rate_set), 48477266Scg KOBJMETHOD(feeder_get, feed_rate_get), 48575320Scg KOBJMETHOD(feeder_feed, feed_rate), 486110466Sorion {0, 0} 48775320Scg}; 48875320ScgFEEDER_DECLARE(feeder_rate, 2, NULL); 48975320Scg 490