1 files changed, 308 insertions, 53 deletions

diff --git a/sys/dev/sound/pcm/feeder_rate.c b/sys/dev/sound/pcm/feeder_rate.c
index fca795c..70f1058 100644
--- a/sys/dev/sound/pcm/feeder_rate.c
+++ b/sys/dev/sound/pcm/feeder_rate.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 1999 Cameron Grant <gandalf@vilnya.demon.co.uk>
+ * Copyright (c) 2003 Orion Hodson <orion@freebsd.org>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -22,35 +22,149 @@
  * 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 very useful.  The testsuite and its revisions can
+ * be found at http://people.freebsd.org/~orion/feedrate/ 
  */
 
-#include <dev/sound/pcm/sound.h>
+#ifdef _KERNEL
 
+#include <dev/sound/pcm/sound.h>
 #include "feeder_if.h"
 
 SND_DECLARE_FILE("$FreeBSD$");
 
 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
 
+#define RATE_ASSERT(x, y) /* KASSERT(x,y) */
+
+#define RATE_TRACE(x...) /* printf(x) */
+
+#else /* _KERNEL */
+
+/* #define RATE_DEBUG */
+
+#include "test_rate_head.h"
+
+#endif /* _KERNEL */
+
 #define FEEDBUFSZ	8192
-#undef FEEDER_DEBUG
+
+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 {
-	u_int32_t src, dst;
-	int srcpos, srcinc;
-	int16_t *buffer;
-	u_int16_t alpha;
+	uint32_t src, dst;	/* source and destination rates */
+	int16_t *buffer;	/* input buffer */
+	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;
 };
 
+#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 * 2)
+
+#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 n1, uint32_t n2)
+{
+	return n2 ? gcd(n2, n1 % n2) : n1;
+}
+
 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->srcinc = (info->src << 16) / info->dst;
-	/* srcinc is 16.16 fixed point increment for srcpos for each dstpos */
-	info->srcpos = 0;
-	return 0;
+	info->alpha = 0;
+	info->channels = 2;
+	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;	
+	info->rounds = FEEDBUFSZ / 
+		(src_ticks_per_cycle(info) * bytes_per_tick(info)) - 1;
+
+	/*
+	 * Find scale and roll combination that allows us to trade 
+	 * costly divide operations in the main loop for multiply-rolls.
+	 */
+        for (l = 99; l > 90; l -= 3) {
+		for (mroll = 2; 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 90%% gain.");
+
+        return -1;
 }
 
 static int
@@ -68,6 +182,7 @@ feed_rate_set(struct pcm_feeder *f, int what, int value)
 	default:
 		return -1;
 	}
+
 	return feed_rate_setup(f);
 }
 
@@ -102,7 +217,7 @@ feed_rate_init(struct pcm_feeder *f)
 	}
 	info->src = DSP_DEFAULT_SPEED;
 	info->dst = DSP_DEFAULT_SPEED;
-	info->alpha = 0;
+
 	f->data = info;
 	return feed_rate_setup(f);
 }
@@ -122,59 +237,195 @@ feed_rate_free(struct pcm_feeder *f)
 }
 
 static int
-feed_rate(struct pcm_feeder *f, struct pcm_channel *c, u_int8_t *b, u_int32_t count, void *source)
+convert_stereo_up(struct feed_rate_info *info, 
+		  uint32_t src_ticks, 
+		  uint32_t dst_ticks, 
+		  int16_t *dst)
 {
-	struct feed_rate_info *info = f->data;
-	int16_t *destbuf = (int16_t *)b;
-	int fetch, v, alpha, hidelta, spos, dpos;
+	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;
 
 	/*
-	 * at this point:
-	 * info->srcpos is 24.8 fixed offset into the fetchbuffer.  0 <= srcpos <= 0xff
-	 *
-	 * our input and output are always AFMT_S16LE stereo.  this simplifies things.
+	 * 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;
 	/*
-	 * we start by fetching enough source data into our buffer to generate
-	 * about as much as was requested.  we put it at offset 2 in the
-	 * buffer so that we can interpolate from the last samples in the
-	 * previous iteration- when we finish we will move our last samples
-	 * to the start of the buffer.
+	 * FYI: unrolling this loop manually does not help much here because
+	 * of the alpha, malpha comparison.
 	 */
-	spos = 0;
-	dpos = 0;
-
-	/* fetch is in bytes */
-	fetch = (count * info->srcinc) >> 16;
-	fetch = min(fetch, FEEDBUFSZ - 4) & ~3;
-	if (fetch == 0)
-		return 0;
-	fetch = FEEDER_FEED(f->source, c, ((u_int8_t *)info->buffer) + 4, fetch, source);
-	fetch /= 2;
-
-	alpha = info->alpha;
-	hidelta = min(info->srcinc >> 16, 1) * 2;
-	while ((spos + hidelta + 1) < fetch) {
-		v = (info->buffer[spos] * (0xffff - alpha)) + (info->buffer[spos + hidelta] * alpha);
-		destbuf[dpos++] = v >> 16;
-
-		v = (info->buffer[spos + 1] * (0xffff - alpha)) + (info->buffer[spos + hidelta + 1] * alpha);
-		destbuf[dpos++] = v >> 16;
-
-		alpha += info->srcinc;
-		spos += (alpha >> 16) * 2;
-		alpha &= 0xffff;
+	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 / info->channels;
+	info->alpha = alpha / info->mscale;
+
+	RATE_ASSERT(info->buffer_pos <= info->buffer_ticks, 
+		    ("%s: Source overrun\n", __func__)); 
+
+	return dp / info->channels;
+}
+
+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 */
+		if (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 && info->buffer_pos == info->buffer_ticks)
+				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;
+
+		d_ticks = info->convert(info, s_ticks, d_ticks,
+					(uint16_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_TRACE("%s: ticks %5d pos %d\n",
+		      __func__, info->buffer_ticks, 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;
+		}
 	}
-	info->alpha = alpha & 0xffff;
-	info->buffer[0] = info->buffer[spos - hidelta];
-	info->buffer[1] = info->buffer[spos - hidelta + 1];
+	
+	RATE_ASSERT(count >= done, 
+		    ("%s: generated too many bytes of data (%d > %d).",
+		     __func__, done, count));
 
-	count = dpos * 2;
-	return count;
+	return done;
 }
 
+#ifdef _KERNEL
+
 static struct pcm_feederdesc feeder_rate_desc[] = {
 	{FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
 	{0},
@@ -189,4 +440,8 @@ static kobj_method_t feeder_rate_methods[] = {
 };
 FEEDER_DECLARE(feeder_rate, 2, NULL);
 
+#else /* _KERNEL */
+
+#include "test_rate_tail.h"
 
+#endif /* _KERNEL */