feeder_rate.c revision 170289
1154133Sharti/*-
2154133Sharti * Copyright (c) 1999 Cameron Grant <cg@FreeBSD.org>
3154133Sharti * Copyright (c) 2003 Orion Hodson <orion@FreeBSD.org>
4154133Sharti * Copyright (c) 2005 Ariff Abdullah <ariff@FreeBSD.org>
5154133Sharti * All rights reserved.
6154133Sharti *
7154133Sharti * Redistribution and use in source and binary forms, with or without
8154133Sharti * modification, are permitted provided that the following conditions
9154133Sharti * are met:
10154133Sharti * 1. Redistributions of source code must retain the above copyright
11154133Sharti *    notice, this list of conditions and the following disclaimer.
12154133Sharti * 2. Redistributions in binary form must reproduce the above copyright
13154133Sharti *    notice, this list of conditions and the following disclaimer in the
14154133Sharti *    documentation and/or other materials provided with the distribution.
15154133Sharti *
16154133Sharti * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17154133Sharti * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18154133Sharti * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19154133Sharti * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20154133Sharti * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21154133Sharti * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22154133Sharti * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23154133Sharti * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24154133Sharti * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25154133Sharti * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26154133Sharti * SUCH DAMAGE.
27154133Sharti */
28154133Sharti
29154133Sharti/*
30154133Sharti * 2006-02-21:
31154133Sharti * ==========
32154133Sharti *
33154133Sharti * Major cleanup and overhaul to remove much redundant codes.
34154133Sharti * Highlights:
35154133Sharti *	1) Support for signed / unsigned 16, 24 and 32 bit,
36154133Sharti *	   big / little endian,
37154133Sharti *	2) Unlimited channels.
38154133Sharti *
39154133Sharti * 2005-06-11:
40154133Sharti * ==========
41154856Sharti *
42154856Sharti * *New* and rewritten soft sample rate converter supporting arbitrary sample
43154133Sharti * rates, fine grained scaling/coefficients and a unified up/down stereo
44154133Sharti * converter. Most of the disclaimers from orion's notes also applies
45154133Sharti * here, regarding linear interpolation deficiencies and pre/post
46154133Sharti * anti-aliasing filtering issues. This version comes with a much simpler and
47160341Sharti * tighter interface, although it works almost exactly like the older one.
48154133Sharti *
49154133Sharti * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
50154133Sharti *                                                                         *
51154133Sharti * This new implementation is fully dedicated in memory of Cameron Grant,  *
52154133Sharti * the creator of the magnificent, highly addictive feeder infrastructure. *
53154856Sharti *                                                                         *
54160341Sharti * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
55154856Sharti *
56154856Sharti * Orion's notes:
57154856Sharti * =============
58154856Sharti *
59160341Sharti * This rate conversion code uses linear interpolation without any
60160341Sharti * pre- or post- interpolation filtering to combat aliasing.  This
61160341Sharti * greatly limits the sound quality and should be addressed at some
62154133Sharti * stage in the future.
63154133Sharti *
64154133Sharti * Since this accuracy of interpolation is sensitive and examination
65154133Sharti * of the algorithm output is harder from the kernel, the code is
66160341Sharti * designed to be compiled in the kernel and in a userland test
67160341Sharti * harness.  This is done by selectively including and excluding code
68160341Sharti * with several portions based on whether _KERNEL is defined.  It's a
69154133Sharti * little ugly, but exceedingly useful.  The testsuite and its
70154133Sharti * revisions can be found at:
71154133Sharti *		http://people.freebsd.org/~orion/files/feedrate/
72160341Sharti *
73154133Sharti * Special thanks to Ken Marx for exposing flaws in the code and for
74154133Sharti * testing revisions.
75154133Sharti */
76154133Sharti
77154133Sharti#include <dev/sound/pcm/sound.h>
78154133Sharti#include "feeder_if.h"
79154133Sharti
80154133ShartiSND_DECLARE_FILE("$FreeBSD: head/sys/dev/sound/pcm/feeder_rate.c 170289 2007-06-04 18:25:08Z dwmalone $");
81154133Sharti
82160341Sharti#define RATE_ASSERT(x, y)	/* KASSERT(x,y) */
83160341Sharti#define RATE_TEST(x, y)		/* if (!(x)) printf y */
84154133Sharti#define RATE_TRACE(x...)	/* printf(x) */
85154133Sharti
86154133ShartiMALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
87154133Sharti
88154133Sharti/*
89154133Sharti * Don't overflow 32bit integer, since everything is done
90154133Sharti * within 32bit arithmetic.
91154133Sharti */
92154133Sharti#define RATE_FACTOR_MIN		1
93154133Sharti#define RATE_FACTOR_MAX		PCM_S24_MAX
94154133Sharti#define RATE_FACTOR_SAFE(val)	(!((val) < RATE_FACTOR_MIN || \
95154133Sharti				(val) > RATE_FACTOR_MAX))
96154133Sharti
97154133Shartistruct feed_rate_info;
98154133Sharti
99154133Shartitypedef uint32_t (*feed_rate_converter)(struct feed_rate_info *,
100154133Sharti							uint8_t *, uint32_t);
101154133Sharti
102154133Shartistruct feed_rate_info {
103154133Sharti	uint32_t src, dst;	/* rounded source / destination rates */
104154133Sharti	uint32_t rsrc, rdst;	/* original source / destination rates */
105160341Sharti	uint32_t gx, gy;	/* interpolation / decimation ratio */
106160341Sharti	uint32_t alpha;		/* interpolation distance */
107160341Sharti	uint32_t pos, bpos;	/* current sample / buffer positions */
108160341Sharti	uint32_t bufsz;		/* total buffer size limit */
109160341Sharti	uint32_t bufsz_init;	/* allocated buffer size */
110160341Sharti	uint32_t channels;	/* total channels */
111160341Sharti	uint32_t bps;		/* bytes-per-sample */
112160341Sharti#ifdef FEEDRATE_STRAY
113160341Sharti	uint32_t stray;		/* stray bytes */
114160341Sharti#endif
115160341Sharti	uint8_t  *buffer;
116160341Sharti	feed_rate_converter convert;
117160341Sharti};
118160341Sharti
119160341Shartiint feeder_rate_min = FEEDRATE_RATEMIN;
120160341Shartiint feeder_rate_max = FEEDRATE_RATEMAX;
121160341Shartiint feeder_rate_round = FEEDRATE_ROUNDHZ;
122160341Sharti
123160341ShartiTUNABLE_INT("hw.snd.feeder_rate_min", &feeder_rate_min);
124160341ShartiTUNABLE_INT("hw.snd.feeder_rate_max", &feeder_rate_max);
125160341ShartiTUNABLE_INT("hw.snd.feeder_rate_round", &feeder_rate_round);
126160341Sharti
127160341Shartistatic int
128160341Shartisysctl_hw_snd_feeder_rate_min(SYSCTL_HANDLER_ARGS)
129160341Sharti{
130160341Sharti	int err, val;
131160341Sharti
132160341Sharti	val = feeder_rate_min;
133160341Sharti	err = sysctl_handle_int(oidp, &val, 0, req);
134160341Sharti	if (err != 0 || req->newptr == NULL)
135160341Sharti		return (err);
136160341Sharti	if (RATE_FACTOR_SAFE(val) && val < feeder_rate_max)
137160341Sharti		feeder_rate_min = val;
138160341Sharti	else
139160341Sharti		err = EINVAL;
140160341Sharti	return (err);
141160341Sharti}
142160341ShartiSYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_min, CTLTYPE_INT | CTLFLAG_RW,
143160341Sharti	0, sizeof(int), sysctl_hw_snd_feeder_rate_min, "I",
144160341Sharti	"minimum allowable rate");
145160341Sharti
146160341Shartistatic int
147160341Shartisysctl_hw_snd_feeder_rate_max(SYSCTL_HANDLER_ARGS)
148160341Sharti{
149160341Sharti	int err, val;
150160341Sharti
151160341Sharti	val = feeder_rate_max;
152160341Sharti	err = sysctl_handle_int(oidp, &val, 0, req);
153160341Sharti	if (err != 0 || req->newptr == NULL)
154160341Sharti		return (err);
155154133Sharti	if (RATE_FACTOR_SAFE(val) && val > feeder_rate_min)
156154133Sharti		feeder_rate_max = val;
157154133Sharti	else
158154133Sharti		err = EINVAL;
159154856Sharti	return (err);
160154133Sharti}
161154133ShartiSYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_max, CTLTYPE_INT | CTLFLAG_RW,
162160341Sharti	0, sizeof(int), sysctl_hw_snd_feeder_rate_max, "I",
163160341Sharti	"maximum allowable rate");
164154133Sharti
165154133Shartistatic int
166154856Shartisysctl_hw_snd_feeder_rate_round(SYSCTL_HANDLER_ARGS)
167154856Sharti{
168154133Sharti	int err, val;
169154133Sharti
170154133Sharti	val = feeder_rate_round;
171154133Sharti	err = sysctl_handle_int(oidp, &val, 0, req);
172160341Sharti	if (err != 0 || req->newptr == NULL)
173154133Sharti		return (err);
174154133Sharti	if (val < FEEDRATE_ROUNDHZ_MIN || val > FEEDRATE_ROUNDHZ_MAX)
175154133Sharti		err = EINVAL;
176154133Sharti	else
177160341Sharti		feeder_rate_round = val - (val % FEEDRATE_ROUNDHZ);
178154133Sharti	return (err);
179160341Sharti}
180154133ShartiSYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_round, CTLTYPE_INT | CTLFLAG_RW,
181154133Sharti	0, sizeof(int), sysctl_hw_snd_feeder_rate_round, "I",
182160341Sharti	"sample rate converter rounding threshold");
183154133Sharti
184154133Sharti#define FEEDER_RATE_CONVERT(FMTBIT, RATE_INTCAST, SIGN, SIGNS, ENDIAN, ENDIANS)	\
185154133Shartistatic uint32_t									\
186154133Shartifeed_convert_##SIGNS##FMTBIT##ENDIANS(struct feed_rate_info *info,		\
187154133Sharti						uint8_t *dst, uint32_t max)	\
188154133Sharti{										\
189154133Sharti	uint32_t ret, smpsz, ch, pos, bpos, gx, gy, alpha, d1, d2;		\
190160341Sharti	int32_t x, y;								\
191160341Sharti	int i;									\
192160341Sharti	uint8_t *src, *sx, *sy;							\
193160341Sharti										\
194160341Sharti	ret = 0;								\
195160341Sharti	alpha = info->alpha;							\
196160341Sharti	gx = info->gx;								\
197160341Sharti	gy = info->gy;								\
198160341Sharti	pos = info->pos;							\
199154133Sharti	bpos = info->bpos;							\
200154133Sharti	src = info->buffer + pos;						\
201160341Sharti	ch = info->channels;							\
202154133Sharti	smpsz = PCM_##FMTBIT##_BPS * ch;					\
203160341Sharti	for (;;) {								\
204160341Sharti		if (alpha < gx) {						\
205154133Sharti			alpha += gy;						\
206154133Sharti			pos += smpsz;						\
207154133Sharti			if (pos == bpos)					\
208154856Sharti				break;						\
209154133Sharti			src += smpsz;						\
210154133Sharti		} else {							\
211154133Sharti			alpha -= gx;						\
212154856Sharti			d1 = (alpha << PCM_FXSHIFT) / gy;			\
213154133Sharti			d2 = (1U << PCM_FXSHIFT) - d1;				\
214154133Sharti			sx = src - smpsz;					\
215160341Sharti			sy = src;						\
216160341Sharti			i = ch;							\
217160341Sharti			do {							\
218160341Sharti				x = PCM_READ_##SIGN##FMTBIT##_##ENDIAN(sx);	\
219160341Sharti				y = PCM_READ_##SIGN##FMTBIT##_##ENDIAN(sy);	\
220160341Sharti				x = (((RATE_INTCAST)x * d1) +			\
221154133Sharti				    ((RATE_INTCAST)y * d2)) >> PCM_FXSHIFT;	\
222160341Sharti				PCM_WRITE_##SIGN##FMTBIT##_##ENDIAN(dst, x);	\
223160341Sharti				dst += PCM_##FMTBIT##_BPS;			\
224154133Sharti				sx += PCM_##FMTBIT##_BPS;			\
225160341Sharti				sy += PCM_##FMTBIT##_BPS;			\
226154133Sharti				ret += PCM_##FMTBIT##_BPS;			\
227160341Sharti			} while (--i != 0);					\
228160341Sharti			if (ret == max)						\
229160341Sharti				break;						\
230160341Sharti		}								\
231154133Sharti	}									\
232160341Sharti	info->alpha = alpha;							\
233160341Sharti	info->pos = pos;							\
234160341Sharti	return (ret);								\
235160341Sharti}
236160341Sharti
237154133ShartiFEEDER_RATE_CONVERT(8, int32_t, S, s, NE, ne)
238160341ShartiFEEDER_RATE_CONVERT(16, int32_t, S, s, LE, le)
239160341ShartiFEEDER_RATE_CONVERT(24, int32_t, S, s, LE, le)
240160341ShartiFEEDER_RATE_CONVERT(32, intpcm_t, S, s, LE, le)
241160341ShartiFEEDER_RATE_CONVERT(16, int32_t, S, s, BE, be)
242160341ShartiFEEDER_RATE_CONVERT(24, int32_t, S, s, BE, be)
243160341ShartiFEEDER_RATE_CONVERT(32, intpcm_t, S, s, BE, be)
244160341ShartiFEEDER_RATE_CONVERT(8, int32_t, U, u, NE, ne)
245160341ShartiFEEDER_RATE_CONVERT(16, int32_t, U, u, LE, le)
246160341ShartiFEEDER_RATE_CONVERT(24, int32_t, U, u, LE, le)
247160341ShartiFEEDER_RATE_CONVERT(32, intpcm_t, U, u, LE, le)
248160341ShartiFEEDER_RATE_CONVERT(16, int32_t, U, u, BE, be)
249160341ShartiFEEDER_RATE_CONVERT(24, int32_t, U, u, BE, be)
250160341ShartiFEEDER_RATE_CONVERT(32, intpcm_t, U, u, BE, be)
251160341Sharti
252154133Shartistatic void
253154133Shartifeed_speed_ratio(uint32_t src, uint32_t dst, uint32_t *gx, uint32_t *gy)
254154133Sharti{
255154133Sharti	uint32_t w, x = src, y = dst;
256154133Sharti
257154133Sharti	while (y != 0) {
258154133Sharti		w = x % y;
259154133Sharti		x = y;
260154133Sharti		y = w;
261154133Sharti	}
262154133Sharti	*gx = src / x;
263154133Sharti	*gy = dst / x;
264154133Sharti}
265154133Sharti
266154133Shartistatic void
267154133Shartifeed_rate_reset(struct feed_rate_info *info)
268154133Sharti{
269154133Sharti	info->src = info->rsrc - (info->rsrc %
270154133Sharti	    ((feeder_rate_round > 0) ? feeder_rate_round : 1));
271160341Sharti	info->dst = info->rdst - (info->rdst %
272160341Sharti	    ((feeder_rate_round > 0) ? feeder_rate_round : 1));
273154133Sharti	info->gx = 1;
274154133Sharti	info->gy = 1;
275154133Sharti	info->alpha = 0;
276154133Sharti	info->channels = 1;
277154133Sharti	info->bps = PCM_8_BPS;
278154133Sharti	info->convert = NULL;
279154133Sharti	info->bufsz = info->bufsz_init;
280154133Sharti	info->pos = 1;
281154133Sharti	info->bpos = 2;
282154133Sharti#ifdef FEEDRATE_STRAY
283154133Sharti	info->stray = 0;
284154133Sharti#endif
285154133Sharti}
286154133Sharti
287154133Shartistatic int
288154133Shartifeed_rate_setup(struct pcm_feeder *f)
289154133Sharti{
290154133Sharti	struct feed_rate_info *info = f->data;
291154133Sharti	static const struct {
292154133Sharti		uint32_t format;	/* pcm / audio format */
293154133Sharti		uint32_t bps;		/* bytes-per-sample, regardless of
294154133Sharti					   total channels */
295154133Sharti		feed_rate_converter convert;
296154133Sharti	} convtbl[] = {
297154133Sharti		{ AFMT_S8,     PCM_8_BPS,  feed_convert_s8ne  },
298154133Sharti		{ AFMT_S16_LE, PCM_16_BPS, feed_convert_s16le },
299154133Sharti		{ AFMT_S24_LE, PCM_24_BPS, feed_convert_s24le },
300154133Sharti		{ AFMT_S32_LE, PCM_32_BPS, feed_convert_s32le },
301154133Sharti		{ AFMT_S16_BE, PCM_16_BPS, feed_convert_s16be },
302154133Sharti		{ AFMT_S24_BE, PCM_24_BPS, feed_convert_s24be },
303154133Sharti		{ AFMT_S32_BE, PCM_32_BPS, feed_convert_s32be },
304154133Sharti		{ AFMT_U8,     PCM_8_BPS,  feed_convert_u8ne  },
305154133Sharti		{ AFMT_U16_LE, PCM_16_BPS, feed_convert_u16le },
306154133Sharti		{ AFMT_U24_LE, PCM_24_BPS, feed_convert_u24le },
307154856Sharti		{ AFMT_U32_LE, PCM_32_BPS, feed_convert_u32le },
308154133Sharti		{ AFMT_U16_BE, PCM_16_BPS, feed_convert_u16be },
309154133Sharti		{ AFMT_U24_BE, PCM_24_BPS, feed_convert_u24be },
310154133Sharti		{ AFMT_U32_BE, PCM_32_BPS, feed_convert_u32be },
311154133Sharti		{ 0, 0, NULL },
312154856Sharti	};
313154133Sharti	uint32_t i;
314160341Sharti
315154133Sharti	feed_rate_reset(info);
316154133Sharti
317154856Sharti	if (info->src != info->dst)
318154856Sharti		feed_speed_ratio(info->src, info->dst, &info->gx, &info->gy);
319154856Sharti
320154133Sharti	if (!(RATE_FACTOR_SAFE(info->gx) && RATE_FACTOR_SAFE(info->gy)))
321154133Sharti		return (-1);
322154856Sharti
323154133Sharti	for (i = 0; i < sizeof(convtbl) / sizeof(*convtbl); i++) {
324154856Sharti		if (convtbl[i].format == 0)
325154856Sharti			return (-1);
326154856Sharti		if ((f->desc->out & ~AFMT_STEREO) == convtbl[i].format) {
327154133Sharti			info->bps = convtbl[i].bps;
328154133Sharti			info->convert = convtbl[i].convert;
329154133Sharti			break;
330154133Sharti		}
331154133Sharti	}
332154856Sharti
333154856Sharti	/*
334154133Sharti	 * No need to interpolate/decimate, just do plain copy.
335154133Sharti	 */
336154133Sharti	if (info->gx == info->gy)
337154133Sharti		info->convert = NULL;
338154133Sharti
339154133Sharti	info->channels = (f->desc->out & AFMT_STEREO) ? 2 : 1;
340154133Sharti	info->pos = info->bps * info->channels;
341154133Sharti	info->bpos = info->pos << 1;
342154133Sharti	info->bufsz -= info->bufsz % info->pos;
343154133Sharti
344160341Sharti	memset(info->buffer, sndbuf_zerodata(f->desc->out), info->bpos);
345154133Sharti
346154133Sharti	RATE_TRACE("%s: %u (%u) -> %u (%u) [%u/%u] , "
347154133Sharti	    "format=0x%08x, channels=%u, bufsz=%u\n",
348154133Sharti	    __func__, info->src, info->rsrc, info->dst, info->rdst,
349154133Sharti	    info->gx, info->gy, f->desc->out, info->channels,
350154133Sharti	    info->bufsz - info->pos);
351154133Sharti
352154856Sharti	return (0);
353154856Sharti}
354154133Sharti
355154856Shartistatic int
356154856Shartifeed_rate_set(struct pcm_feeder *f, int what, int32_t value)
357154856Sharti{
358154856Sharti	struct feed_rate_info *info = f->data;
359154856Sharti
360154856Sharti	if (value < feeder_rate_min || value > feeder_rate_max)
361154856Sharti		return (-1);
362154856Sharti
363154856Sharti	switch (what) {
364154856Sharti	case FEEDRATE_SRC:
365154856Sharti		info->rsrc = value;
366154856Sharti		break;
367154856Sharti	case FEEDRATE_DST:
368154856Sharti		info->rdst = value;
369154856Sharti		break;
370154856Sharti	default:
371154856Sharti		return (-1);
372154856Sharti	}
373154856Sharti	return (feed_rate_setup(f));
374154856Sharti}
375154856Sharti
376154856Shartistatic int
377154856Shartifeed_rate_get(struct pcm_feeder *f, int what)
378154856Sharti{
379154856Sharti	struct feed_rate_info *info = f->data;
380154856Sharti
381154856Sharti	switch (what) {
382154856Sharti	case FEEDRATE_SRC:
383154856Sharti		return (info->rsrc);
384154856Sharti	case FEEDRATE_DST:
385154856Sharti		return (info->rdst);
386154856Sharti	default:
387154856Sharti		return (-1);
388154856Sharti	}
389154856Sharti	return (-1);
390154856Sharti}
391154856Sharti
392154856Shartistatic int
393154856Shartifeed_rate_init(struct pcm_feeder *f)
394154856Sharti{
395154856Sharti	struct feed_rate_info *info;
396154856Sharti
397154856Sharti	if (f->desc->out != f->desc->in)
398154856Sharti		return (EINVAL);
399154856Sharti
400154856Sharti	info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO);
401154856Sharti	if (info == NULL)
402154856Sharti		return (ENOMEM);
403154856Sharti	/*
404154856Sharti	 * bufsz = sample from last cycle + conversion space
405154856Sharti	 */
406154856Sharti	info->bufsz_init = 8 + feeder_buffersize;
407154856Sharti	info->buffer = malloc(sizeof(*info->buffer) * info->bufsz_init,
408154856Sharti	    M_RATEFEEDER, M_NOWAIT | M_ZERO);
409154856Sharti	if (info->buffer == NULL) {
410154856Sharti		free(info, M_RATEFEEDER);
411154856Sharti		return (ENOMEM);
412154856Sharti	}
413154856Sharti	info->rsrc = DSP_DEFAULT_SPEED;
414154856Sharti	info->rdst = DSP_DEFAULT_SPEED;
415154856Sharti	f->data = info;
416154856Sharti	return (feed_rate_setup(f));
417154856Sharti}
418154856Sharti
419154856Shartistatic int
420154856Shartifeed_rate_free(struct pcm_feeder *f)
421154856Sharti{
422154856Sharti	struct feed_rate_info *info = f->data;
423154856Sharti
424154856Sharti	if (info != NULL) {
425154856Sharti		if (info->buffer != NULL)
426154856Sharti			free(info->buffer, M_RATEFEEDER);
427154856Sharti		free(info, M_RATEFEEDER);
428154856Sharti	}
429154856Sharti	f->data = NULL;
430154856Sharti	return (0);
431154856Sharti}
432154856Sharti
433154856Shartistatic int
434154856Shartifeed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
435154856Sharti						uint32_t count, void *source)
436154856Sharti{
437154856Sharti	struct feed_rate_info *info = f->data;
438154856Sharti	uint32_t i, smpsz;
439154856Sharti	int32_t fetch, slot;
440154856Sharti
441154856Sharti	if (info->convert == NULL)
442154856Sharti		return (FEEDER_FEED(f->source, c, b, count, source));
443154856Sharti
444154856Sharti	/*
445154856Sharti	 * This loop has been optimized to generalize both up / down
446154856Sharti	 * sampling without causing missing samples or excessive buffer
447154856Sharti	 * feeding. The tricky part is to calculate *precise* (slot) value
448154856Sharti	 * needed for the entire conversion space since we are bound to
449154133Sharti	 * return and fill up the buffer according to the requested 'count'.
450154133Sharti	 * Too much feeding will cause the extra buffer stay within temporary
451154133Sharti	 * circular buffer forever and always manifest itself as a truncated
452154856Sharti	 * sound during end of playback / recording. Too few, and we end up
453154856Sharti	 * with possible underruns and waste of cpu cycles.
454154856Sharti	 *
455154133Sharti	 * 'Stray' management exist to combat with possible unaligned
456154133Sharti	 * buffering by the caller.
457154133Sharti	 */
458154133Sharti	smpsz = info->bps * info->channels;
459160341Sharti	RATE_TEST(count >= smpsz && (count % smpsz) == 0,
460154856Sharti	    ("%s: Count size not sample integral (%d)\n", __func__, count));
461154856Sharti	if (count < smpsz)
462154856Sharti		return (0);
463154856Sharti	count -= count % smpsz;
464154133Sharti	/*
465154133Sharti	 * This slot count formula will stay here for the next million years
466154133Sharti	 * to come. This is the key of our circular buffering precision.
467154856Sharti	 */
468154856Sharti	slot = (((info->gx * (count / smpsz)) + info->gy - info->alpha - 1) /
469154856Sharti	    info->gy) * smpsz;
470154856Sharti	RATE_TEST((slot % smpsz) == 0,
471154856Sharti	    ("%s: Slot count not sample integral (%d)\n", __func__, slot));
472154856Sharti#ifdef FEEDRATE_STRAY
473154856Sharti	RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n", __func__,
474154856Sharti	    info->stray));
475154856Sharti#endif
476154856Sharti	if (info->pos != smpsz && info->bpos - info->pos == smpsz &&
477154856Sharti	    info->bpos + slot > info->bufsz) {
478154856Sharti		/*
479154856Sharti		 * Copy last unit sample and its previous to
480154856Sharti		 * beginning of buffer.
481154856Sharti		 */
482154856Sharti		bcopy(info->buffer + info->pos - smpsz, info->buffer,
483154856Sharti		    sizeof(*info->buffer) * (smpsz << 1));
484154856Sharti		info->pos = smpsz;
485154856Sharti		info->bpos = smpsz << 1;
486154856Sharti	}
487154856Sharti	RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n", __func__, slot));
488154856Sharti	i = 0;
489154856Sharti	for (;;) {
490154856Sharti		for (;;) {
491154856Sharti			fetch = info->bufsz - info->bpos;
492154856Sharti#ifdef FEEDRATE_STRAY
493154856Sharti			fetch -= info->stray;
494154856Sharti#endif
495154856Sharti			RATE_ASSERT(fetch >= 0,
496154856Sharti			    ("%s: [1] Buffer overrun: %d > %d\n", __func__,
497154856Sharti			    info->bpos, info->bufsz));
498154856Sharti			if (slot < fetch)
499154856Sharti				fetch = slot;
500154856Sharti#ifdef FEEDRATE_STRAY
501154133Sharti			if (fetch < 1)
502154133Sharti#else
503154133Sharti			if (fetch < smpsz)
504154133Sharti#endif
505154133Sharti				break;
506154133Sharti			RATE_ASSERT((int)(info->bpos
507154133Sharti#ifdef FEEDRATE_STRAY
508154133Sharti			    - info->stray
509154133Sharti#endif
510154133Sharti			    ) >= 0 &&
511154133Sharti			    (info->bpos  - info->stray) < info->bufsz,
512154133Sharti			    ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n",
513154133Sharti			    __func__, info->bufsz, info->bpos
514154133Sharti#ifdef FEEDRATE_STRAY
515154133Sharti			    - info->stray
516154133Sharti#endif
517154133Sharti			    ));
518154133Sharti			fetch = FEEDER_FEED(f->source, c,
519154133Sharti			    info->buffer + info->bpos
520154133Sharti#ifdef FEEDRATE_STRAY
521154133Sharti			    - info->stray
522154133Sharti#endif
523154133Sharti			    , fetch, source);
524154133Sharti#ifdef FEEDRATE_STRAY
525154133Sharti			info->stray = 0;
526154133Sharti			if (fetch == 0)
527154133Sharti#else
528160341Sharti			if (fetch < smpsz)
529154133Sharti#endif
530154133Sharti				break;
531154133Sharti			RATE_TEST((fetch % smpsz) == 0,
532160341Sharti			    ("%s: Fetch size not sample integral (%d)\n",
533160341Sharti			    __func__, fetch));
534154133Sharti#ifdef FEEDRATE_STRAY
535154133Sharti			info->stray += fetch % smpsz;
536160341Sharti			RATE_TEST(info->stray == 0,
537154133Sharti			    ("%s: Stray bytes detected (%d)\n", __func__,
538160341Sharti			    info->stray));
539154133Sharti#endif
540154133Sharti			fetch -= fetch % smpsz;
541154133Sharti			info->bpos += fetch;
542154133Sharti			slot -= fetch;
543154133Sharti			RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n",
544154133Sharti			    __func__, slot));
545154133Sharti			if (slot == 0 || info->bpos == info->bufsz)
546154133Sharti				break;
547154133Sharti		}
548154133Sharti		if (info->pos == info->bpos) {
549154133Sharti			RATE_TEST(info->pos == smpsz,
550154133Sharti			    ("%s: EOF while in progress\n", __func__));
551154133Sharti			break;
552154133Sharti		}
553154133Sharti		RATE_ASSERT(info->pos <= info->bpos,
554154133Sharti		    ("%s: [2] Buffer overrun: %d > %d\n", __func__, info->pos,
555154133Sharti		    info->bpos));
556154133Sharti		RATE_ASSERT(info->pos < info->bpos,
557154133Sharti		    ("%s: Zero buffer!\n", __func__));
558154133Sharti		RATE_ASSERT(((info->bpos - info->pos) % smpsz) == 0,
559154133Sharti		    ("%s: Buffer not sample integral (%d)\n", __func__,
560154133Sharti		    info->bpos - info->pos));
561154133Sharti		i += info->convert(info, b + i, count - i);
562154133Sharti		RATE_ASSERT(info->pos <= info->bpos,
563154133Sharti		    ("%s: [3] Buffer overrun: %d > %d\n", __func__, info->pos,
564154133Sharti		    info->bpos));
565154133Sharti		if (info->pos == info->bpos) {
566154133Sharti			/*
567154133Sharti			 * End of buffer cycle. Copy last unit sample
568154133Sharti			 * to beginning of buffer so next cycle can
569154133Sharti			 * interpolate using it.
570154133Sharti			 */
571154133Sharti#ifdef FEEDRATE_STRAY
572154133Sharti			RATE_TEST(info->stray == 0,
573154133Sharti			    ("%s: [2] Stray bytes: %u\n", __func__,
574154133Sharti			    info->stray));
575154133Sharti#endif
576154133Sharti			bcopy(info->buffer + info->pos - smpsz, info->buffer,
577154133Sharti			    sizeof(*info->buffer) * smpsz);
578154133Sharti			info->bpos = smpsz;
579154133Sharti			info->pos = smpsz;
580160341Sharti		}
581160341Sharti		if (i == count)
582154133Sharti			break;
583154133Sharti	}
584160341Sharti
585160341Sharti	RATE_TEST((slot == 0 && count == i) || (slot > 0 && count > i &&
586160341Sharti	    info->pos == info->bpos && info->pos == smpsz),
587160341Sharti	    ("%s: Inconsistent slot/count! "
588154133Sharti	    "Count Expect: %u , Got: %u, Slot Left: %d\n", __func__, count, i,
589154133Sharti	    slot));
590154133Sharti
591160341Sharti#ifdef FEEDRATE_STRAY
592160341Sharti	RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__,
593154133Sharti	    info->stray));
594154133Sharti#endif
595154133Sharti
596154133Sharti	return (i);
597160341Sharti}
598160341Sharti
599154133Shartistatic struct pcm_feederdesc feeder_rate_desc[] = {
600154133Sharti	{FEEDER_RATE, AFMT_S8, AFMT_S8, 0},
601154133Sharti	{FEEDER_RATE, AFMT_S16_LE, AFMT_S16_LE, 0},
602154133Sharti	{FEEDER_RATE, AFMT_S24_LE, AFMT_S24_LE, 0},
603154133Sharti	{FEEDER_RATE, AFMT_S32_LE, AFMT_S32_LE, 0},
604154133Sharti	{FEEDER_RATE, AFMT_S16_BE, AFMT_S16_BE, 0},
605154133Sharti	{FEEDER_RATE, AFMT_S24_BE, AFMT_S24_BE, 0},
606154133Sharti	{FEEDER_RATE, AFMT_S32_BE, AFMT_S32_BE, 0},
607154133Sharti	{FEEDER_RATE, AFMT_S8 | AFMT_STEREO, AFMT_S8 | AFMT_STEREO, 0},
608154133Sharti	{FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
609154133Sharti	{FEEDER_RATE, AFMT_S24_LE | AFMT_STEREO, AFMT_S24_LE | AFMT_STEREO, 0},
610154133Sharti	{FEEDER_RATE, AFMT_S32_LE | AFMT_STEREO, AFMT_S32_LE | AFMT_STEREO, 0},
611154133Sharti	{FEEDER_RATE, AFMT_S16_BE | AFMT_STEREO, AFMT_S16_BE | AFMT_STEREO, 0},
612160341Sharti	{FEEDER_RATE, AFMT_S24_BE | AFMT_STEREO, AFMT_S24_BE | AFMT_STEREO, 0},
613154133Sharti	{FEEDER_RATE, AFMT_S32_BE | AFMT_STEREO, AFMT_S32_BE | AFMT_STEREO, 0},
614154133Sharti	{FEEDER_RATE, AFMT_U8, AFMT_U8, 0},
615154133Sharti	{FEEDER_RATE, AFMT_U16_LE, AFMT_U16_LE, 0},
616154133Sharti	{FEEDER_RATE, AFMT_U24_LE, AFMT_U24_LE, 0},
617154133Sharti	{FEEDER_RATE, AFMT_U32_LE, AFMT_U32_LE, 0},
618154133Sharti	{FEEDER_RATE, AFMT_U16_BE, AFMT_U16_BE, 0},
619154133Sharti	{FEEDER_RATE, AFMT_U24_BE, AFMT_U24_BE, 0},
620154133Sharti	{FEEDER_RATE, AFMT_U32_BE, AFMT_U32_BE, 0},
621154133Sharti	{FEEDER_RATE, AFMT_U8 | AFMT_STEREO, AFMT_U8 | AFMT_STEREO, 0},
622154133Sharti	{FEEDER_RATE, AFMT_U16_LE | AFMT_STEREO, AFMT_U16_LE | AFMT_STEREO, 0},
623154133Sharti	{FEEDER_RATE, AFMT_U24_LE | AFMT_STEREO, AFMT_U24_LE | AFMT_STEREO, 0},
624154133Sharti	{FEEDER_RATE, AFMT_U32_LE | AFMT_STEREO, AFMT_U32_LE | AFMT_STEREO, 0},
625154133Sharti	{FEEDER_RATE, AFMT_U16_BE | AFMT_STEREO, AFMT_U16_BE | AFMT_STEREO, 0},
626154133Sharti	{FEEDER_RATE, AFMT_U24_BE | AFMT_STEREO, AFMT_U24_BE | AFMT_STEREO, 0},
627154133Sharti	{FEEDER_RATE, AFMT_U32_BE | AFMT_STEREO, AFMT_U32_BE | AFMT_STEREO, 0},
628154133Sharti	{0, 0, 0, 0},
629154133Sharti};
630154133Sharti
631static kobj_method_t feeder_rate_methods[] = {
632	KOBJMETHOD(feeder_init,		feed_rate_init),
633	KOBJMETHOD(feeder_free,		feed_rate_free),
634	KOBJMETHOD(feeder_set,		feed_rate_set),
635	KOBJMETHOD(feeder_get,		feed_rate_get),
636	KOBJMETHOD(feeder_feed,		feed_rate),
637	{0, 0}
638};
639
640FEEDER_DECLARE(feeder_rate, 2, NULL);
641