Re-implemention of the interpolation code used for sample rate

conversion.  The new version has improved interpolation accuracy and
maintains the timing relationship between the input and output signals
exactly.

Approved by:	cg

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 */