xref: /openbsd-src/usr.bin/sndiod/dsp.c (revision 7b6392009e6e5a7f8e494c162a4d259ea5e13a62)

/*	$OpenBSD: dsp.c,v 1.22 2024/12/20 07:35:56 ratchov Exp $	*/
/*
 * Copyright (c) 2008-2012 Alexandre Ratchov <alex@caoua.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */
#include <stdio.h>
#include <string.h>
#include "dsp.h"
#include "utils.h"

const int aparams_ctltovol[128] = {
	        0,     65536,     68109,     70783,
	    73562,     76450,     79451,     82570,
	    85812,     89181,     92682,     96321,
	   100102,    104032,    108116,    112361,
	   116772,    121356,    126121,    131072,
	   136218,    141566,    147123,    152899,
	   158902,    165140,    171624,    178361,
	   185364,    192641,    200204,    208064,
	   216232,    224721,    233544,    242713,
	   252241,    262144,    272436,    283131,
	   294247,    305799,    317804,    330281,
	   343247,    356723,    370728,    385282,
	   400408,    416128,    432465,    449443,
	   467088,    485425,    504482,    524288,
	   544871,    566262,    588493,    611597,
	   635608,    660561,    686495,    713446,
	   741455,    770564,    800816,    832255,
	   864929,    898885,    934175,    970850,
	  1008965,   1048576,   1089742,   1132525,
	  1176987,   1223194,   1271216,   1321123,
	  1372989,   1426892,   1482910,   1541128,
	  1601632,   1664511,   1729858,   1797771,
	  1868350,   1941700,   2017930,   2097152,
	  2179485,   2265049,   2353974,   2446389,
	  2542432,   2642246,   2745978,   2853783,
	  2965821,   3082257,   3203264,   3329021,
	  3459716,   3595542,   3736700,   3883400,
	  4035859,   4194304,   4358969,   4530099,
	  4707947,   4892777,   5084864,   5284492,
	  5491957,   5707567,   5931642,   6164513,
	  6406527,   6658043,   6919432,   7191084,
	  7473400,   7766800,   8071719,   8388608
};

const 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.
 */
int
aparams_enctostr(struct aparams *par, char *ostr)
{
	char *p = ostr;

	*p++ = par->sig ? 's' : 'u';
	if (par->bits > 9)
		*p++ = '0' + par->bits / 10;
	*p++ = '0' + par->bits % 10;
	if (par->bps > 1) {
		*p++ = par->le ? 'l' : 'b';
		*p++ = 'e';
		if (par->bps != APARAMS_BPS(par->bits) ||
		    par->bits < par->bps * 8) {
			*p++ = par->bps + '0';
			if (par->bits < par->bps * 8) {
				*p++ = par->msb ? 'm' : 'l';
				*p++ = 's';
				*p++ = 'b';
			}
		}
	}
	*p++ = '\0';
	return p - ostr - 1;
}

/*
 * Parse an encoding string, examples: s8, u8, s16, s16le, s24be ...
 * set *istr to the char following the encoding. Return the number
 * of bytes consumed.
 */
int
aparams_strtoenc(struct aparams *par, char *istr)
{
	char *p = istr;
	int i, sig, bits, le, bps, msb;

#define IS_SEP(c)			\
	(((c) < 'a' || (c) > 'z') &&	\
	 ((c) < 'A' || (c) > 'Z') &&	\
	 ((c) < '0' || (c) > '9'))

	/*
	 * get signedness
	 */
	if (*p == 's') {
		sig = 1;
	} else if (*p == 'u') {
		sig = 0;
	} else
		return 0;
	p++;

	/*
	 * get number of bits per sample
	 */
	bits = 0;
	for (i = 0; i < 2; i++) {
		if (*p < '0' || *p > '9')
			break;
		bits = (bits * 10) + *p - '0';
		p++;
	}
	if (bits < BITS_MIN || bits > BITS_MAX)
		return 0;
	bps = APARAMS_BPS(bits);
	msb = 1;
	le = ADATA_LE;

	/*
	 * get (optional) endianness
	 */
	if (p[0] == 'l' && p[1] == 'e') {
		le = 1;
		p += 2;
	} else if (p[0] == 'b' && p[1] == 'e') {
		le = 0;
		p += 2;
	} else if (IS_SEP(*p)) {
		goto done;
	} else
		return 0;

	/*
	 * get (optional) number of bytes
	 */
	if (*p >= '0' && *p <= '9') {
		bps = *p - '0';
		if (bps < (bits + 7) / 8 ||
		    bps > (BITS_MAX + 7) / 8)
			return 0;
		p++;

		/*
		 * get (optional) alignment
		 */
		if (p[0] == 'm' && p[1] == 's' && p[2] == 'b') {
			msb = 1;
			p += 3;
		} else if (p[0] == 'l' && p[1] == 's' && p[2] == 'b') {
			msb = 0;
			p += 3;
		} else if (IS_SEP(*p)) {
			goto done;
		} else
			return 0;
	} else if (!IS_SEP(*p))
		return 0;

done:
	par->msb = msb;
	par->sig = sig;
	par->bits = bits;
	par->bps = bps;
	par->le = le;
	return p - istr;
}

/*
 * Initialise parameters structure with the defaults natively supported
 * by the machine.
 */
void
aparams_init(struct aparams *par)
{
	par->bps = sizeof(adata_t);
	par->bits = ADATA_BITS;
	par->le = ADATA_LE;
	par->sig = 1;
	par->msb = 0;
}

/*
 * return true if encoding corresponds to what we store in adata_t
 */
int
aparams_native(struct aparams *par)
{
	return par->sig &&
	    par->bps == sizeof(adata_t) &&
	    par->bits == ADATA_BITS &&
	    (par->bps == 1 || par->le == ADATA_LE) &&
	    (par->bits == par->bps * 8 || !par->msb);
}

/*
 * Return the number of input and output frame that would be consumed
 * by resamp_do(p, *icnt, *ocnt).
 */
void
resamp_getcnt(struct resamp *p, int *icnt, int *ocnt)
{
	long long idiff, odiff;
	int cdiff;

	cdiff = p->oblksz - p->diff;
	idiff = (long long)*icnt * p->oblksz;
	odiff = (long long)*ocnt * p->iblksz;
	if (odiff - idiff >= cdiff)
		*ocnt = (idiff + cdiff + p->iblksz - 1) / p->iblksz;
	else
		*icnt = (odiff + p->diff) / p->oblksz;
}

/*
 * Resample the given number of frames. The number of output frames
 * must match the corresponding number of input frames. Either always
 * use icnt and ocnt such that:
 *
 *	 icnt * oblksz = ocnt * iblksz
 *
 * or use resamp_getcnt() to calculate the proper numbers.
 */
void
resamp_do(struct resamp *p, adata_t *in, adata_t *out, int icnt, int ocnt)
{
	unsigned int nch;
	adata_t *idata;
	unsigned int oblksz;
	unsigned int ifr;
	int s, ds, diff;
	adata_t *odata;
	unsigned int iblksz;
	unsigned int ofr;
	unsigned int c;
	int64_t f[NCHAN_MAX];
	adata_t *ctxbuf, *ctx;
	unsigned int ctx_start;
	int q, qi, qf, n;

	/*
	 * Partially copy structures into local variables, to avoid
	 * unnecessary indirections; this also allows the compiler to
	 * order local variables more "cache-friendly".
	 */
	idata = in;
	odata = out;
	diff = p->diff;
	iblksz = p->iblksz;
	oblksz = p->oblksz;
	ctxbuf = p->ctx;
	ctx_start = p->ctx_start;
	nch = p->nch;
	ifr = icnt;
	ofr = ocnt;

	/*
	 * Start conversion.
	 */
#ifdef DEBUG
	logx(4, "resamp: copying %d -> %d frames, diff = %d", ifr, ofr, diff);
#endif
	for (;;) {
		if (diff >= oblksz) {
			if (ifr == 0)
				break;
			ctx_start = (ctx_start - 1) & (RESAMP_NCTX - 1);
			ctx = ctxbuf + ctx_start;
			for (c = nch; c > 0; c--) {
				*ctx = *idata++;
				ctx += RESAMP_NCTX;
			}
			diff -= oblksz;
			ifr--;
		} else {
			if (ofr == 0)
				break;

			for (c = 0; c < nch; 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 = 0; c < nch; c++) {
					f[c] += (int64_t)ctx[n] * s;
					ctx += RESAMP_NCTX;
				}
				q += p->filt_cutoff;
				n = (n + 1) & (RESAMP_NCTX - 1);
			}

			for (c = 0; c < nch; c++) {
				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;
			ofr--;
		}
	}
	p->diff = diff;
	p->ctx_start = ctx_start;
#ifdef DEBUG
	if (ifr != 0) {
		logx(0, "resamp_do: %d: too many input frames", ifr);
		panic();
	}
	if (ofr != 0) {
		logx(0, "resamp_do: %d: too many output frames", ofr);
		panic();
	}
#endif
}

static unsigned int
uint_gcd(unsigned int a, unsigned int b)
{
	unsigned int r;

	while (b > 0) {
		r = a % b;
		a = b;
		b = r;
	}
	return a;
}

/*
 * initialize resampler with ibufsz/obufsz factor and "nch" channels
 */
void
resamp_init(struct resamp *p, unsigned int iblksz,
    unsigned int oblksz, int nch)
{
	unsigned int g;

	/*
	 * reduce iblksz/oblksz fraction
	 */
	g = uint_gcd(iblksz, oblksz);
	iblksz /= g;
	oblksz /= g;

	/*
	 * ensure weird rates don't cause integer overflow
	 */
	while (iblksz > ADATA_UNIT || oblksz > ADATA_UNIT) {
		iblksz >>= 1;
		oblksz >>= 1;
	}

	p->iblksz = iblksz;
	p->oblksz = oblksz;
	p->diff = 0;
	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
	logx(3, "resamp_init: %u/%u", iblksz, oblksz);
#endif
}

/*
 * encode "todo" frames from native to foreign encoding
 */
void
enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
{
	unsigned int f;
	adata_t *idata;
	unsigned int s;
	unsigned int oshift;
	unsigned int obias;
	unsigned int obps;
	unsigned int i;
	unsigned char *odata;
	int obnext;
	int osnext;

#ifdef DEBUG
	logx(4, "enc: copying %u frames", todo);
#endif
	/*
	 * Partially copy structures into local variables, to avoid
	 * unnecessary indirections; this also allows the compiler to
	 * order local variables more "cache-friendly".
	 */
	idata = (adata_t *)in;
	odata = out;
	oshift = p->shift;
	obias = p->bias;
	obps = p->bps;
	obnext = p->bnext;
	osnext = p->snext;

	/*
	 * Start conversion.
	 */
	odata += p->bfirst;
	for (f = todo * p->nch; f > 0; f--) {
		/* convert adata to u32 */
		s = (int)*idata++ + ADATA_UNIT;
		s <<= 32 - ADATA_BITS;
		/* convert u32 to uN */
		s >>= oshift;
		/* convert uN to sN */
		s -= obias;
		/* packetize sN */
		for (i = obps; i > 0; i--) {
			*odata = (unsigned char)s;
			s >>= 8;
			odata += obnext;
		}
		odata += osnext;
	}
}

/*
 * store "todo" frames of silence in foreign encoding
 */
void
enc_sil_do(struct conv *p, unsigned char *out, int todo)
{
	unsigned int f;
	unsigned int s;
	unsigned int oshift;
	int obias;
	unsigned int obps;
	unsigned int i;
	unsigned char *odata;
	int obnext;
	int osnext;

#ifdef DEBUG
	logx(4, "enc: silence %u frames", todo);
#endif
	/*
	 * Partially copy structures into local variables, to avoid
	 * unnecessary indirections; this also allows the compiler to
	 * order local variables more "cache-friendly".
	 */
	odata = out;
	oshift = p->shift;
	obias = p->bias;
	obps = p->bps;
	obnext = p->bnext;
	osnext = p->snext;

	/*
	 * Start conversion.
	 */
	odata += p->bfirst;
	for (f = todo * p->nch; f > 0; f--) {
		s = ((1U << 31) >> oshift) - obias;
		for (i = obps; i > 0; i--) {
			*odata = (unsigned char)s;
			s >>= 8;
			odata += obnext;
		}
		odata += osnext;
	}
}

/*
 * initialize encoder from native to foreign encoding
 */
void
enc_init(struct conv *p, struct aparams *par, int nch)
{
#ifdef DEBUG
	char enc_str[ENCMAX];
#endif

	p->nch = nch;
	p->bps = par->bps;
	if (par->msb) {
		p->shift = 32 - par->bps * 8;
	} else {
		p->shift = 32 - par->bits;
	}
	if (par->sig) {
		p->bias = (1U << 31) >> p->shift;
	} else {
		p->bias = 0;
	}
	if (!par->le) {
		p->bfirst = par->bps - 1;
		p->bnext = -1;
		p->snext = 2 * par->bps;
	} else {
		p->bfirst = 0;
		p->bnext = 1;
		p->snext = 0;
	}
#ifdef DEBUG
	logx(3, "enc: %s, %d channels",
	    (aparams_enctostr(par, enc_str), enc_str), p->nch);
#endif
}

/*
 * decode "todo" frames from foreign to native encoding
 */
void
dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
{
	unsigned int f;
	unsigned int ibps;
	unsigned int i;
	unsigned int s = 0xdeadbeef;
	unsigned char *idata;
	int ibnext;
	int isnext;
	unsigned int ibias;
	unsigned int ishift;
	adata_t *odata;

#ifdef DEBUG
	logx(4, "dec: copying %u frames", todo);
#endif
	/*
	 * Partially copy structures into local variables, to avoid
	 * unnecessary indirections; this also allows the compiler to
	 * order local variables more "cache-friendly".
	 */
	idata = in;
	odata = (adata_t *)out;
	ibps = p->bps;
	ibnext = p->bnext;
	ibias = p->bias;
	ishift = p->shift;
	isnext = p->snext;

	/*
	 * Start conversion.
	 */
	idata += p->bfirst;
	for (f = todo * p->nch; f > 0; f--) {
		for (i = ibps; i > 0; i--) {
			s <<= 8;
			s |= *idata;
			idata += ibnext;
		}
		idata += isnext;
		s += ibias;
		s <<= ishift;
		s >>= 32 - ADATA_BITS;
		*odata++ = s - ADATA_UNIT;
	}
}

/*
 * initialize decoder from foreign to native encoding
 */
void
dec_init(struct conv *p, struct aparams *par, int nch)
{
#ifdef DEBUG
	char enc_str[ENCMAX];
#endif

	p->bps = par->bps;
	p->nch = nch;
	if (par->msb) {
		p->shift = 32 - par->bps * 8;
	} else {
		p->shift = 32 - par->bits;
	}
	if (par->sig) {
		p->bias = (1U << 31) >> p->shift;
	} else {
		p->bias = 0;
	}
	if (par->le) {
		p->bfirst = par->bps - 1;
		p->bnext = -1;
		p->snext = 2 * par->bps;
	} else {
		p->bfirst = 0;
		p->bnext = 1;
		p->snext = 0;
	}
#ifdef DEBUG
	logx(3, "dec: %s, %d channels",
	    (aparams_enctostr(par, enc_str), enc_str), p->nch);
#endif
}

/*
 * mix "todo" input frames on the output with the given volume
 */
void
cmap_add(struct cmap *p, void *in, void *out, int vol, int todo)
{
	adata_t *idata, *odata;
	int i, j, nch, istart, inext, onext, ostart, y, v;

#ifdef DEBUG
	logx(4, "cmap: adding %d frames", todo);
#endif
	idata = in;
	odata = out;
	ostart = p->ostart;
	onext = p->onext;
	istart = p->istart;
	inext = p->inext;
	nch = p->nch;
	v = vol;

	/*
	 * map/mix input on the output
	 */
	for (i = todo; i > 0; i--) {
		odata += ostart;
		idata += istart;
		for (j = nch; j > 0; j--) {
			y = *odata + ADATA_MUL(*idata, v);
			if (y >= ADATA_UNIT)
				y = ADATA_UNIT - 1;
			else if (y < -ADATA_UNIT)
				y = -ADATA_UNIT;
			*odata = y;
			idata++;
			odata++;
		}
		odata += onext;
		idata += inext;
	}
}

/*
 * overwrite output with "todo" input frames with the given volume
 */
void
cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo)
{
	adata_t *idata, *odata;
	int i, j, nch, istart, inext, onext, ostart, v;

#ifdef DEBUG
	logx(4, "cmap: copying %d frames", todo);
#endif
	idata = in;
	odata = out;
	ostart = p->ostart;
	onext = p->onext;
	istart = p->istart;
	inext = p->inext;
	nch = p->nch;
	v = vol;

	/*
	 * copy to the output buffer
	 */
	for (i = todo; i > 0; i--) {
		idata += istart;
		odata += ostart;
		for (j = nch; j > 0; j--) {
			*odata = ADATA_MUL(*idata, v);
			odata++;
			idata++;
		}
		odata += onext;
		idata += inext;
	}
}

/*
 * initialize channel mapper, to map a subset of input channel range
 * into a subset of the output channel range
 */
void
cmap_init(struct cmap *p,
    int imin, int imax, int isubmin, int isubmax,
    int omin, int omax, int osubmin, int osubmax)
{
	int inch, onch, nch;

	/*
	 * Ignore channels outside of the available sets
	 */
	if (isubmin < imin)
		isubmin = imin;
	if (isubmax > imax)
		isubmax = imax;
	if (osubmin < omin)
		osubmin = omin;
	if (osubmax > omax)
		osubmax = omax;

	/*
	 * Shrink the input or the output subset to make both subsets of
	 * the same size
	 */
	inch = isubmax - isubmin + 1;
	onch = osubmax - osubmin + 1;
	nch = (inch < onch) ? inch : onch;
	isubmax = isubmin + nch - 1;
	osubmax = osubmin + nch - 1;

	p->ostart = osubmin - omin;
	p->onext = omax - osubmax;
	p->istart = isubmin - imin;
	p->inext = imax - isubmax;
	p->nch = nch;
#ifdef DEBUG
	logx(3, "%s: nch = %d, ostart = %d, onext = %d, istart = %d, inext = %d",
	    __func__, p->nch, p->ostart, p->onext, p->istart, p->inext);
#endif
}