Use a 8-th order FIR low-pass filter for resampling

At the expence of slightly more CPU cycles, it almost removes aliasing
noise in most practical use-cases.

2 files changed, 148 insertions, 9 deletions

diff --git a/usr.bin/sndiod/dsp.c b/usr.bin/sndiod/dsp.c
index 0f0c21afc86..5b8ade88be4 100644
--- a/usr.bin/sndiod/dsp.c
+++ b/usr.bin/sndiod/dsp.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dsp.c,v 1.15 2020/12/10 17:30:49 ratchov Exp $	*/
+/*	$OpenBSD: dsp.c,v 1.16 2021/01/11 14:26:00 ratchov Exp $	*/
 /*
  * Copyright (c) 2008-2012 Alexandre Ratchov <alex@caoua.org>
  *
@@ -38,6 +38,75 @@ int aparams_ctltovol[128] = {
 	26008,	27029,	28090,	29193,	30339,	31530,	32768
 };
 
+int resamp_filt[RESAMP_LENGTH / RESAMP_STEP + 1] = {
+	      0,       0,       3,       9,      22,      42,      73,     116,
+	    174,     248,     341,     454,     589,     749,     934,    1148,
+	   1392,    1666,    1974,    2316,    2693,    3107,    3560,    4051,
+	   4582,    5154,    5766,    6420,    7116,    7853,    8632,    9451,
+	  10311,   11210,   12148,   13123,   14133,   15178,   16253,   17359,
+	  18491,   19647,   20824,   22018,   23226,   24443,   25665,   26888,
+	  28106,   29315,   30509,   31681,   32826,   33938,   35009,   36033,
+	  37001,   37908,   38744,   39502,   40174,   40750,   41223,   41582,
+	  41819,   41925,   41890,   41704,   41358,   40842,   40147,   39261,
+	  38176,   36881,   35366,   33623,   31641,   29411,   26923,   24169,
+	  21140,   17827,   14222,   10317,    6105,    1580,   -3267,   -8440,
+	 -13944,  -19785,  -25967,  -32492,  -39364,  -46584,  -54153,  -62072,
+	 -70339,  -78953,  -87911,  -97209, -106843, -116806, -127092, -137692,
+	-148596, -159795, -171276, -183025, -195029, -207271, -219735, -232401,
+	-245249, -258259, -271407, -284670, -298021, -311434, -324880, -338329,
+	-351750, -365111, -378378, -391515, -404485, -417252, -429775, -442015,
+	-453930, -465477, -476613, -487294, -497472, -507102, -516137, -524527,
+	-532225, -539181, -545344, -550664, -555090, -558571, -561055, -562490,
+	-562826, -562010, -559990, -556717, -552139, -546205, -538866, -530074,
+	-519779, -507936, -494496, -479416, -462652, -444160, -423901, -401835,
+	-377923, -352132, -324425, -294772, -263143, -229509, -193847, -156134,
+	-116348,  -74474,  -30494,   15601,   63822,  114174,  166661,  221283,
+	 278037,  336916,  397911,  461009,  526194,  593446,  662741,  734054,
+	 807354,  882608,  959779, 1038826, 1119706, 1202370, 1286768, 1372846,
+	1460546, 1549808, 1640566, 1732753, 1826299, 1921130, 2017169, 2114336,
+	2212550, 2311723, 2411770, 2512598, 2614116, 2716228, 2818836, 2921841,
+	3025142, 3128636, 3232218, 3335782, 3439219, 3542423, 3645282, 3747687,
+	3849526, 3950687, 4051059, 4150530, 4248987, 4346320, 4442415, 4537163,
+	4630453, 4722177, 4812225, 4900493, 4986873, 5071263, 5153561, 5233668,
+	5311485, 5386917, 5459872, 5530259, 5597992, 5662986, 5725160, 5784436,
+	5840739, 5893999, 5944148, 5991122, 6034862, 6075313, 6112422, 6146142,
+	6176430, 6203247, 6226559, 6246335, 6262551, 6275185, 6284220, 6289647,
+	6291456, 6289647, 6284220, 6275185, 6262551, 6246335, 6226559, 6203247,
+	6176430, 6146142, 6112422, 6075313, 6034862, 5991122, 5944148, 5893999,
+	5840739, 5784436, 5725160, 5662986, 5597992, 5530259, 5459872, 5386917,
+	5311485, 5233668, 5153561, 5071263, 4986873, 4900493, 4812225, 4722177,
+	4630453, 4537163, 4442415, 4346320, 4248987, 4150530, 4051059, 3950687,
+	3849526, 3747687, 3645282, 3542423, 3439219, 3335782, 3232218, 3128636,
+	3025142, 2921841, 2818836, 2716228, 2614116, 2512598, 2411770, 2311723,
+	2212550, 2114336, 2017169, 1921130, 1826299, 1732753, 1640566, 1549808,
+	1460546, 1372846, 1286768, 1202370, 1119706, 1038826,  959779,  882608,
+	 807354,  734054,  662741,  593446,  526194,  461009,  397911,  336916,
+	 278037,  221283,  166661,  114174,   63822,   15601,  -30494,  -74474,
+	-116348, -156134, -193847, -229509, -263143, -294772, -324425, -352132,
+	-377923, -401835, -423901, -444160, -462652, -479416, -494496, -507936,
+	-519779, -530074, -538866, -546205, -552139, -556717, -559990, -562010,
+	-562826, -562490, -561055, -558571, -555090, -550664, -545344, -539181,
+	-532225, -524527, -516137, -507102, -497472, -487294, -476613, -465477,
+	-453930, -442015, -429775, -417252, -404485, -391515, -378378, -365111,
+	-351750, -338329, -324880, -311434, -298021, -284670, -271407, -258259,
+	-245249, -232401, -219735, -207271, -195029, -183025, -171276, -159795,
+	-148596, -137692, -127092, -116806, -106843,  -97209,  -87911,  -78953,
+	 -70339,  -62072,  -54153,  -46584,  -39364,  -32492,  -25967,  -19785,
+	 -13944,   -8440,   -3267,    1580,    6105,   10317,   14222,   17827,
+	  21140,   24169,   26923,   29411,   31641,   33623,   35366,   36881,
+	  38176,   39261,   40147,   40842,   41358,   41704,   41890,   41925,
+	  41819,   41582,   41223,   40750,   40174,   39502,   38744,   37908,
+	  37001,   36033,   35009,   33938,   32826,   31681,   30509,   29315,
+	  28106,   26888,   25665,   24443,   23226,   22018,   20824,   19647,
+	  18491,   17359,   16253,   15178,   14133,   13123,   12148,   11210,
+	  10311,    9451,    8632,    7853,    7116,    6420,    5766,    5154,
+	   4582,    4051,    3560,    3107,    2693,    2316,    1974,    1666,
+	   1392,    1148,     934,     749,     589,     454,     341,     248,
+	    174,     116,      73,      42,      22,       9,       3,       0,
+	      0
+};
+
+
 /*
  * Generate a string corresponding to the encoding in par,
  * return the length of the resulting string.
@@ -212,8 +281,10 @@ resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
 	adata_t *odata;
 	unsigned int iblksz;
 	unsigned int c;
+	int64_t f[NCHAN_MAX];
 	adata_t *ctxbuf, *ctx;
 	unsigned int ctx_start;
+	int q, qi, qf, n;
 
 #ifdef DEBUG
 	if (todo % p->iblksz != 0) {
@@ -240,7 +311,7 @@ resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
 		if (diff >= oblksz) {
 			if (todo == 0)
 				break;
-			ctx_start ^= 1;
+			ctx_start = (ctx_start - 1) & (RESAMP_NCTX - 1);
 			ctx = ctxbuf + ctx_start;
 			for (c = nch; c > 0; c--) {
 				*ctx = *idata++;
@@ -249,12 +320,46 @@ resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
 			diff -= oblksz;
 			todo--;
 		} else {
-			ctx = ctxbuf;
+
+			for (c = nch; c > 0; c--)
+				f[c] = 0;
+
+			q = diff * p->filt_step;
+			n = ctx_start;
+
+			while (q < RESAMP_LENGTH) {
+				qi = q >> RESAMP_STEP_BITS;
+				qf = q & (RESAMP_STEP - 1);
+				s = resamp_filt[qi];
+				ds = resamp_filt[qi + 1] - s;
+				s += (int64_t)qf * ds >> RESAMP_STEP_BITS;
+				ctx = ctxbuf;
+				for (c = nch; c > 0; c--) {
+					f[c] += (int64_t)ctx[n] * s;
+					ctx += RESAMP_NCTX;
+				}
+				q += p->filt_cutoff;
+				n = (n + 1) & (RESAMP_NCTX - 1);
+			}
+
 			for (c = nch; c > 0; c--) {
-				s = ctx[ctx_start ^ 1];
-				ds = ctx[ctx_start] - s;
-				ctx += RESAMP_NCTX;
-				*odata++ = s + ADATA_MULDIV(ds, diff, oblksz);
+				s = f[c] >> RESAMP_BITS;
+				s = (int64_t)s * p->filt_cutoff >> RESAMP_BITS;
+#if ADATA_BITS == 16
+				/*
+				 * In 16-bit mode, we've no room for filter
+				 * overshoots, so we need to clip the signal
+				 * to avoid 16-bit integers to wrap around.
+				 * In 24-bit mode, samples may exceed the
+				 * [-1:1] range. Later, cmap_add() will clip
+				 * them, so no need to clip them here as well.
+				 */
+				if (s >= ADATA_UNIT)
+					s = ADATA_UNIT - 1;
+				else if (s < -ADATA_UNIT)
+					s = -ADATA_UNIT;
+#endif
+				*odata++ = s;
 			}
 			diff += iblksz;
 		}
@@ -275,6 +380,13 @@ resamp_init(struct resamp *p, unsigned int iblksz,
 	p->nch = nch;
 	p->ctx_start = 0;
 	memset(p->ctx, 0, sizeof(p->ctx));
+	if (p->iblksz < p->oblksz) {
+		p->filt_cutoff = RESAMP_UNIT;
+		p->filt_step = RESAMP_UNIT / p->oblksz;
+	} else {
+		p->filt_cutoff = (int64_t)RESAMP_UNIT * p->oblksz / p->iblksz;
+		p->filt_step = RESAMP_UNIT / p->iblksz;
+	}
 #ifdef DEBUG
 	if (log_level >= 3) {
 		log_puts("resamp: ");
diff --git a/usr.bin/sndiod/dsp.h b/usr.bin/sndiod/dsp.h
index 3b75a13680f..2fc5a94b922 100644
--- a/usr.bin/sndiod/dsp.h
+++ b/usr.bin/sndiod/dsp.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: dsp.h,v 1.7 2018/06/08 06:21:56 ratchov Exp $	*/
+/*	$OpenBSD: dsp.h,v 1.8 2021/01/11 14:26:00 ratchov Exp $	*/
 /*
  * Copyright (c) 2012 Alexandre Ratchov <alex@caoua.org>
  *
@@ -92,6 +92,32 @@ typedef int adata_t;
 #endif
 
 /*
+ * The FIR is sampled and stored in a table of fixed-point numbers
+ * with 23 fractional bits. For convenience, we use the same fixed-point
+ * numbers to represent time and to walk through the table.
+ */
+#define RESAMP_BITS		23
+#define RESAMP_UNIT		(1 << RESAMP_BITS)
+
+/*
+ * Filter window length (the time unit is RESAMP_UNIT)
+ */
+#define RESAMP_LENGTH		(8 * RESAMP_UNIT)
+
+/*
+ * Time between samples of the FIR (the time unit is RESAMP_UNIT)
+ */
+#define RESAMP_STEP_BITS	(RESAMP_BITS - 6)
+#define RESAMP_STEP		(1 << RESAMP_STEP_BITS)
+
+/*
+ * Maximum downsample/upsample ratio we support, must be a power of two.
+ * The ratio between the max and the min sample rates is 192kHz / 4kHz = 48,
+ * so we can use 64
+ */
+#define RESAMP_RATIO		64
+
+/*
  * Maximum size of the encording string (the longest possible
  * encoding is ``s24le3msb'').
  */
@@ -111,9 +137,10 @@ struct aparams {
 };
 
 struct resamp {
-#define RESAMP_NCTX	2
+#define RESAMP_NCTX	(RESAMP_LENGTH / RESAMP_UNIT * RESAMP_RATIO)
 	unsigned int ctx_start;
 	adata_t ctx[NCHAN_MAX * RESAMP_NCTX];
+	int filt_cutoff, filt_step;
 	unsigned int iblksz, oblksz;
 	int nch;
 };