/* * Copyright (c) 2003 Orion Hodson <orion@freebsd.org> * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * MAINTAINER: Orion Hodson <orion@freebsd.org> * * This rate conversion code uses linear interpolation without any * pre- or post- interpolation filtering to combat aliasing. This * greatly limits the sound quality and should be addressed at some * stage in the future. * * Since this accuracy of interpolation is sensitive and examination * of the algorithm output is harder from the kernel, th code is * designed to be compiled in the kernel and in a userland test * harness. This is done by selectively including and excluding code * with several portions based on whether _KERNEL is defined. It's a * little ugly, but exceedingly useful. The testsuite and its * revisions can be found at: * http://people.freebsd.org/~orion/feedrate/ * * Special thanks to Ken Marx for exposing flaws in the code and for * testing revisions. */ #ifdef _KERNEL #include <dev/sound/pcm/sound.h> #include "feeder_if.h" SND_DECLARE_FILE("$FreeBSD: src/sys/dev/sound/pcm/feeder_rate.c,v 1.10 2003/04/20 17:08:56 orion Exp $"); #endif /* _KERNEL */ MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); #ifndef RATE_ASSERT #define RATE_ASSERT(x, y) /* KASSERT(x) */ #endif /* RATE_ASSERT */ #ifndef RATE_TRACE #define RATE_TRACE(x...) /* printf(x) */ #endif /*****************************************************************************/ /* The following coefficients are coupled. They are chosen to be * guarantee calculable factors for the interpolation routine. They * have been tested over the range of RATEMIN-RATEMAX Hz. Decreasing * the granularity increases the required buffer size and affects the * gain values at different points in the space. These values were * found by running the test program with -p (probe) and some trial * and error. * * ROUNDHZ the granularity of sample rates (fits n*11025 and n*8000). * FEEDBUFSZ the amount of buffer space. * MINGAIN the minimum acceptable gain in coefficients search. */ #define ROUNDHZ 25 #define FEEDBUFSZ 8192 #define MINGAIN 92 #define RATEMIN 4000 #define RATEMAX 48000 struct feed_rate_info; typedef int (*rate_convert_method)(struct feed_rate_info *, uint32_t, uint32_t, int16_t *); static int convert_stereo_up(struct feed_rate_info *info, uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); static int convert_stereo_down(struct feed_rate_info *info, uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst); struct feed_rate_info { uint32_t src, dst; /* source and destination rates */ uint16_t buffer_ticks; /* number of available samples in buffer */ uint16_t buffer_pos; /* next available sample in buffer */ uint16_t rounds; /* maximum number of cycle rounds w buffer */ uint16_t alpha; /* interpolation distance */ uint16_t sscale; /* src clock scale */ uint16_t dscale; /* dst clock scale */ uint16_t mscale; /* scale factor to avoid divide per sample */ uint16_t mroll; /* roll to again avoid divide per sample */ uint16_t channels; /* 1 = mono, 2 = stereo */ rate_convert_method convert; int16_t buffer[FEEDBUFSZ]; }; #define bytes_per_sample 2 #define src_ticks_per_cycle(info) (info->dscale * info->rounds) #define dst_ticks_per_cycle(info) (info->sscale * info->rounds) #define bytes_per_tick(info) (info->channels * bytes_per_sample) #define src_bytes_per_cycle(info) \ (src_ticks_per_cycle(info) * bytes_per_tick(info)) #define dst_bytes_per_cycle(info) \ (dst_ticks_per_cycle(info) * bytes_per_tick(info)) static uint32_t gcd(uint32_t x, uint32_t y) { uint32_t w; while (y != 0) { w = x % y; x = y; y = w; } return x; } static int feed_rate_setup(struct pcm_feeder *f) { struct feed_rate_info *info = f->data; uint32_t mscale, mroll, l, r, g; /* Beat sample rates down by greatest common divisor */ g = gcd(info->src, info->dst); info->sscale = info->dst / g; info->dscale = info->src / g; info->alpha = 0; info->buffer_ticks = 0; info->buffer_pos = 0; /* Pick suitable conversion routine */ if (info->src > info->dst) { info->convert = convert_stereo_down; } else { info->convert = convert_stereo_up; } /* * Determine number of conversion rounds that will fit into * buffer. NB Must set info->rounds to one before using * src_ticks_per_cycle here since it used by src_ticks_per_cycle. */ info->rounds = 1; r = (FEEDBUFSZ - bytes_per_tick(info)) / (src_ticks_per_cycle(info) * bytes_per_tick(info)); if (r == 0) { RATE_TRACE("Insufficient buffer space for conversion %d -> %d " "(%d < %d)\n", info->src, info->dst, FEEDBUFSZ, src_ticks_per_cycle(info) * bytes_per_tick(info)); return -1; } info->rounds = r; /* * Find scale and roll combination that allows us to trade * costly divide operations in the main loop for multiply-rolls. */ for (l = 96; l >= MINGAIN; l -= 3) { for (mroll = 0; mroll < 16; mroll ++) { mscale = (1 << mroll) / info->sscale; r = (mscale * info->sscale * 100) >> mroll; if (r > l && r <= 100) { info->mscale = mscale; info->mroll = mroll; RATE_TRACE("Converting %d to %d with " "mscale = %d and mroll = %d " "(gain = %d / 100)\n", info->src, info->dst, info->mscale, info->mroll, r); return 0; } } } RATE_TRACE("Failed to find a converter within %d%% gain for " "%d to %d.\n", l, info->src, info->dst); return -2; } static int feed_rate_set(struct pcm_feeder *f, int what, int value) { struct feed_rate_info *info = f->data; int rvalue; if (value < RATEMIN || value > RATEMAX) { return -1; } rvalue = (value / ROUNDHZ) * ROUNDHZ; if (value - rvalue > ROUNDHZ / 2) { rvalue += ROUNDHZ; } switch(what) { case FEEDRATE_SRC: info->src = rvalue; break; case FEEDRATE_DST: info->dst = rvalue; break; default: return -1; } return feed_rate_setup(f); } static int feed_rate_get(struct pcm_feeder *f, int what) { struct feed_rate_info *info = f->data; switch(what) { case FEEDRATE_SRC: return info->src; case FEEDRATE_DST: return info->dst; default: return -1; } return -1; } static int feed_rate_init(struct pcm_feeder *f) { struct feed_rate_info *info; info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO); if (info == NULL) return ENOMEM; info->src = DSP_DEFAULT_SPEED; info->dst = DSP_DEFAULT_SPEED; info->channels = 2; f->data = info; return 0; } static int feed_rate_free(struct pcm_feeder *f) { struct feed_rate_info *info = f->data; if (info) { free(info, M_RATEFEEDER); } f->data = NULL; return 0; } static int convert_stereo_up(struct feed_rate_info *info, uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst) { uint32_t max_dst_ticks; int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o; int16_t *src; sp = info->buffer_pos * 2; se = sp + src_ticks * 2; src = info->buffer; alpha = info->alpha * info->mscale; dalpha = info->dscale * info->mscale; /* Alpha increment */ malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ mroll = info->mroll; /* * For efficiency the main conversion loop should only depend on * one variable. We use the state to work out the maximum number * of output samples that are available and eliminate the checking of * sp from the loop. */ max_dst_ticks = src_ticks * info->dst / info->src - alpha / dalpha; if (max_dst_ticks < dst_ticks) { dst_ticks = max_dst_ticks; } dp = 0; de = dst_ticks * 2; /* * Unrolling this loop manually does not help much here because * of the alpha, malpha comparison. */ while (dp < de) { o = malpha - alpha; x = alpha * src[sp + 2] + o * src[sp]; dst[dp++] = x >> mroll; x = alpha * src[sp + 3] + o * src[sp + 1]; dst[dp++] = x >> mroll; alpha += dalpha; if (alpha >= malpha) { alpha -= malpha; sp += 2; } } RATE_ASSERT(sp <= se, ("%s: Source overrun\n", __func__)); info->buffer_pos = sp / info->channels; info->alpha = alpha / info->mscale; return dp / info->channels; } static int convert_stereo_down(struct feed_rate_info *info, uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst) { int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o, m, mdalpha, mstep; int16_t *src; sp = info->buffer_pos * 2; se = sp + src_ticks * 2; src = info->buffer; alpha = info->alpha * info->mscale; dalpha = info->dscale * info->mscale; /* Alpha increment */ malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */ mroll = info->mroll; dp = 0; de = dst_ticks * 2; m = dalpha / malpha; mstep = m * 2; mdalpha = dalpha - m * malpha; /* * TODO: eliminate sp or dp from this loop comparison for a few * extra % performance. */ while (sp < se && dp < de) { o = malpha - alpha; x = alpha * src[sp + 2] + o * src[sp]; dst[dp++] = x >> mroll; x = alpha * src[sp + 3] + o * src[sp + 1]; dst[dp++] = x >> mroll; alpha += mdalpha; sp += mstep; if (alpha >= malpha) { alpha -= malpha; sp += 2; } } info->buffer_pos = sp / 2; info->alpha = alpha / info->mscale; RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, ("%s: Source overrun\n", __func__)); return dp / 2; } static int feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b, uint32_t count, void *source) { struct feed_rate_info *info = f->data; uint32_t done, s_ticks, d_ticks; done = 0; RATE_ASSERT(info->channels == 2, ("%s: channels (%d) != 2", __func__, info->channels)); while (done < count) { /* Slurp in more data if input buffer is not full */ while (info->buffer_ticks < src_ticks_per_cycle(info)) { uint8_t *u8b; int fetch; fetch = src_bytes_per_cycle(info) - info->buffer_ticks * bytes_per_tick(info); u8b = (uint8_t*)info->buffer + (info->buffer_ticks + 1) * bytes_per_tick(info); fetch = FEEDER_FEED(f->source, c, u8b, fetch, source); RATE_ASSERT(fetch % bytes_per_tick(info) == 0, ("%s: fetched unaligned bytes (%d)", __func__, fetch)); info->buffer_ticks += fetch / bytes_per_tick(info); RATE_ASSERT(src_ticks_per_cycle(info) >= info->buffer_ticks, ("%s: buffer overfilled (%d > %d).", __func__, info->buffer_ticks, src_ticks_per_cycle(info))); if (fetch == 0) break; } /* Find amount of input buffer data that should be processed */ d_ticks = (count - done) / bytes_per_tick(info); s_ticks = info->buffer_ticks - info->buffer_pos; if (info->buffer_ticks != src_ticks_per_cycle(info)) { if (s_ticks > 8) s_ticks -= 8; else s_ticks = 0; } d_ticks = info->convert(info, s_ticks, d_ticks, (int16_t*)(b + done)); if (d_ticks == 0) break; done += d_ticks * bytes_per_tick(info); RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, ("%s: buffer_ticks too big\n", __func__)); RATE_ASSERT(info->buffer_ticks <= src_ticks_per_cycle(info), ("too many ticks %d / %d\n", info->buffer_ticks, src_ticks_per_cycle(info))); RATE_TRACE("%s: ticks %5d / %d pos %d\n", __func__, info->buffer_ticks, src_ticks_per_cycle(info), info->buffer_pos); if (src_ticks_per_cycle(info) <= info->buffer_pos) { /* End of cycle reached, copy last samples to start */ uint8_t *u8b; u8b = (uint8_t*)info->buffer; bcopy(u8b + src_bytes_per_cycle(info), u8b, bytes_per_tick(info)); RATE_ASSERT(info->alpha == 0, ("%s: completed cycle with " "alpha non-zero", __func__, info->alpha)); info->buffer_pos = 0; info->buffer_ticks = 0; } } RATE_ASSERT(count >= done, ("%s: generated too many bytes of data (%d > %d).", __func__, done, count)); if (done != count) { RATE_TRACE("Only did %d of %d\n", done, count); } return done; } static struct pcm_feederdesc feeder_rate_desc[] = { {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, {0, 0, 0, 0}, }; static kobj_method_t feeder_rate_methods[] = { KOBJMETHOD(feeder_init, feed_rate_init), KOBJMETHOD(feeder_free, feed_rate_free), KOBJMETHOD(feeder_set, feed_rate_set), KOBJMETHOD(feeder_get, feed_rate_get), KOBJMETHOD(feeder_feed, feed_rate), {0, 0} }; FEEDER_DECLARE(feeder_rate, 2, NULL);