2 * Copyright (c) 1999 Cameron Grant <cg@FreeBSD.org>
3 * Copyright (c) 2003 Orion Hodson <orion@FreeBSD.org>
4 * Copyright (c) 2005 Ariff Abdullah <ariff@FreeBSD.org>
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
33 * Major cleanup and overhaul to remove much redundant codes.
35 * 1) Support for signed / unsigned 16, 24 and 32 bit,
36 * big / little endian,
37 * 2) Unlimited channels.
42 * *New* and rewritten soft sample rate converter supporting arbitrary sample
43 * rates, fine grained scaling/coefficients and a unified up/down stereo
44 * converter. Most of the disclaimers from orion's notes also applies
45 * here, regarding linear interpolation deficiencies and pre/post
46 * anti-aliasing filtering issues. This version comes with a much simpler and
47 * tighter interface, although it works almost exactly like the older one.
49 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
51 * This new implementation is fully dedicated in memory of Cameron Grant, *
52 * the creator of the magnificent, highly addictive feeder infrastructure. *
54 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
59 * This rate conversion code uses linear interpolation without any
60 * pre- or post- interpolation filtering to combat aliasing. This
61 * greatly limits the sound quality and should be addressed at some
62 * stage in the future.
64 * Since this accuracy of interpolation is sensitive and examination
65 * of the algorithm output is harder from the kernel, the code is
66 * designed to be compiled in the kernel and in a userland test
67 * harness. This is done by selectively including and excluding code
68 * with several portions based on whether _KERNEL is defined. It's a
69 * little ugly, but exceedingly useful. The testsuite and its
70 * revisions can be found at:
71 * http://people.freebsd.org/~orion/files/feedrate/
73 * Special thanks to Ken Marx for exposing flaws in the code and for
77 #include <dev/sound/pcm/sound.h>
78 #include "feeder_if.h"
80 SND_DECLARE_FILE("$FreeBSD$");
82 #define RATE_ASSERT(x, y) /* KASSERT(x,y) */
83 #define RATE_TEST(x, y) /* if (!(x)) printf y */
84 #define RATE_TRACE(x...) /* printf(x) */
86 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
89 * Don't overflow 32bit integer, since everything is done
90 * within 32bit arithmetic.
92 #define RATE_FACTOR_MIN 1
93 #define RATE_FACTOR_MAX PCM_S24_MAX
94 #define RATE_FACTOR_SAFE(val) (!((val) < RATE_FACTOR_MIN || \
95 (val) > RATE_FACTOR_MAX))
97 struct feed_rate_info;
99 typedef uint32_t (*feed_rate_converter)(struct feed_rate_info *,
100 uint8_t *, uint32_t);
102 struct feed_rate_info {
103 uint32_t src, dst; /* rounded source / destination rates */
104 uint32_t rsrc, rdst; /* original source / destination rates */
105 uint32_t gx, gy; /* interpolation / decimation ratio */
106 uint32_t alpha; /* interpolation distance */
107 uint32_t pos, bpos; /* current sample / buffer positions */
108 uint32_t bufsz; /* total buffer size limit */
109 uint32_t bufsz_init; /* allocated buffer size */
110 uint32_t channels; /* total channels */
111 uint32_t bps; /* bytes-per-sample */
112 #ifdef FEEDRATE_STRAY
113 uint32_t stray; /* stray bytes */
116 feed_rate_converter convert;
119 int feeder_rate_min = FEEDRATE_RATEMIN;
120 int feeder_rate_max = FEEDRATE_RATEMAX;
121 int feeder_rate_round = FEEDRATE_ROUNDHZ;
123 TUNABLE_INT("hw.snd.feeder_rate_min", &feeder_rate_min);
124 TUNABLE_INT("hw.snd.feeder_rate_max", &feeder_rate_max);
125 TUNABLE_INT("hw.snd.feeder_rate_round", &feeder_rate_round);
128 sysctl_hw_snd_feeder_rate_min(SYSCTL_HANDLER_ARGS)
132 val = feeder_rate_min;
133 err = sysctl_handle_int(oidp, &val, 0, req);
134 if (err != 0 || req->newptr == NULL)
136 if (RATE_FACTOR_SAFE(val) && val < feeder_rate_max)
137 feeder_rate_min = val;
142 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_min, CTLTYPE_INT | CTLFLAG_RW,
143 0, sizeof(int), sysctl_hw_snd_feeder_rate_min, "I",
144 "minimum allowable rate");
147 sysctl_hw_snd_feeder_rate_max(SYSCTL_HANDLER_ARGS)
151 val = feeder_rate_max;
152 err = sysctl_handle_int(oidp, &val, 0, req);
153 if (err != 0 || req->newptr == NULL)
155 if (RATE_FACTOR_SAFE(val) && val > feeder_rate_min)
156 feeder_rate_max = val;
161 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_max, CTLTYPE_INT | CTLFLAG_RW,
162 0, sizeof(int), sysctl_hw_snd_feeder_rate_max, "I",
163 "maximum allowable rate");
166 sysctl_hw_snd_feeder_rate_round(SYSCTL_HANDLER_ARGS)
170 val = feeder_rate_round;
171 err = sysctl_handle_int(oidp, &val, 0, req);
172 if (err != 0 || req->newptr == NULL)
174 if (val < FEEDRATE_ROUNDHZ_MIN || val > FEEDRATE_ROUNDHZ_MAX)
177 feeder_rate_round = val - (val % FEEDRATE_ROUNDHZ);
180 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_round, CTLTYPE_INT | CTLFLAG_RW,
181 0, sizeof(int), sysctl_hw_snd_feeder_rate_round, "I",
182 "sample rate converter rounding threshold");
184 #define FEEDER_RATE_CONVERT(FMTBIT, RATE_INTCAST, SIGN, SIGNS, ENDIAN, ENDIANS) \
186 feed_convert_##SIGNS##FMTBIT##ENDIANS(struct feed_rate_info *info, \
187 uint8_t *dst, uint32_t max) \
189 uint32_t ret, smpsz, ch, pos, bpos, gx, gy, alpha, d1, d2; \
192 uint8_t *src, *sx, *sy; \
195 alpha = info->alpha; \
200 src = info->buffer + pos; \
201 ch = info->channels; \
202 smpsz = PCM_##FMTBIT##_BPS * ch; \
212 d1 = (alpha << PCM_FXSHIFT) / gy; \
213 d2 = (1U << PCM_FXSHIFT) - d1; \
218 x = PCM_READ_##SIGN##FMTBIT##_##ENDIAN(sx); \
219 y = PCM_READ_##SIGN##FMTBIT##_##ENDIAN(sy); \
220 x = (((RATE_INTCAST)x * d1) + \
221 ((RATE_INTCAST)y * d2)) >> PCM_FXSHIFT; \
222 PCM_WRITE_##SIGN##FMTBIT##_##ENDIAN(dst, x); \
223 dst += PCM_##FMTBIT##_BPS; \
224 sx += PCM_##FMTBIT##_BPS; \
225 sy += PCM_##FMTBIT##_BPS; \
226 ret += PCM_##FMTBIT##_BPS; \
227 } while (--i != 0); \
232 info->alpha = alpha; \
237 FEEDER_RATE_CONVERT(8, int32_t, S, s, NE, ne)
238 FEEDER_RATE_CONVERT(16, int32_t, S, s, LE, le)
239 FEEDER_RATE_CONVERT(24, int32_t, S, s, LE, le)
240 FEEDER_RATE_CONVERT(32, intpcm_t, S, s, LE, le)
241 FEEDER_RATE_CONVERT(16, int32_t, S, s, BE, be)
242 FEEDER_RATE_CONVERT(24, int32_t, S, s, BE, be)
243 FEEDER_RATE_CONVERT(32, intpcm_t, S, s, BE, be)
244 FEEDER_RATE_CONVERT(8, int32_t, U, u, NE, ne)
245 FEEDER_RATE_CONVERT(16, int32_t, U, u, LE, le)
246 FEEDER_RATE_CONVERT(24, int32_t, U, u, LE, le)
247 FEEDER_RATE_CONVERT(32, intpcm_t, U, u, LE, le)
248 FEEDER_RATE_CONVERT(16, int32_t, U, u, BE, be)
249 FEEDER_RATE_CONVERT(24, int32_t, U, u, BE, be)
250 FEEDER_RATE_CONVERT(32, intpcm_t, U, u, BE, be)
253 feed_speed_ratio(uint32_t src, uint32_t dst, uint32_t *gx, uint32_t *gy)
255 uint32_t w, x = src, y = dst;
267 feed_rate_reset(struct feed_rate_info *info)
269 info->src = info->rsrc - (info->rsrc %
270 ((feeder_rate_round > 0) ? feeder_rate_round : 1));
271 info->dst = info->rdst - (info->rdst %
272 ((feeder_rate_round > 0) ? feeder_rate_round : 1));
277 info->bps = PCM_8_BPS;
278 info->convert = NULL;
279 info->bufsz = info->bufsz_init;
282 #ifdef FEEDRATE_STRAY
288 feed_rate_setup(struct pcm_feeder *f)
290 struct feed_rate_info *info = f->data;
291 static const struct {
292 uint32_t format; /* pcm / audio format */
293 uint32_t bps; /* bytes-per-sample, regardless of
295 feed_rate_converter convert;
297 { AFMT_S8, PCM_8_BPS, feed_convert_s8ne },
298 { AFMT_S16_LE, PCM_16_BPS, feed_convert_s16le },
299 { AFMT_S24_LE, PCM_24_BPS, feed_convert_s24le },
300 { AFMT_S32_LE, PCM_32_BPS, feed_convert_s32le },
301 { AFMT_S16_BE, PCM_16_BPS, feed_convert_s16be },
302 { AFMT_S24_BE, PCM_24_BPS, feed_convert_s24be },
303 { AFMT_S32_BE, PCM_32_BPS, feed_convert_s32be },
304 { AFMT_U8, PCM_8_BPS, feed_convert_u8ne },
305 { AFMT_U16_LE, PCM_16_BPS, feed_convert_u16le },
306 { AFMT_U24_LE, PCM_24_BPS, feed_convert_u24le },
307 { AFMT_U32_LE, PCM_32_BPS, feed_convert_u32le },
308 { AFMT_U16_BE, PCM_16_BPS, feed_convert_u16be },
309 { AFMT_U24_BE, PCM_24_BPS, feed_convert_u24be },
310 { AFMT_U32_BE, PCM_32_BPS, feed_convert_u32be },
315 feed_rate_reset(info);
317 if (info->src != info->dst)
318 feed_speed_ratio(info->src, info->dst, &info->gx, &info->gy);
320 if (!(RATE_FACTOR_SAFE(info->gx) && RATE_FACTOR_SAFE(info->gy)))
323 for (i = 0; i < sizeof(convtbl) / sizeof(convtbl[0]); i++) {
324 if (convtbl[i].format == 0)
326 if ((f->desc->out & ~AFMT_STEREO) == convtbl[i].format) {
327 info->bps = convtbl[i].bps;
328 info->convert = convtbl[i].convert;
334 * No need to interpolate/decimate, just do plain copy.
336 if (info->gx == info->gy)
337 info->convert = NULL;
339 info->channels = (f->desc->out & AFMT_STEREO) ? 2 : 1;
340 info->pos = info->bps * info->channels;
341 info->bpos = info->pos << 1;
342 info->bufsz -= info->bufsz % info->pos;
344 memset(info->buffer, sndbuf_zerodata(f->desc->out), info->bpos);
346 RATE_TRACE("%s: %u (%u) -> %u (%u) [%u/%u] , "
347 "format=0x%08x, channels=%u, bufsz=%u\n",
348 __func__, info->src, info->rsrc, info->dst, info->rdst,
349 info->gx, info->gy, f->desc->out, info->channels,
350 info->bufsz - info->pos);
356 feed_rate_set(struct pcm_feeder *f, int what, int32_t value)
358 struct feed_rate_info *info = f->data;
360 if (value < feeder_rate_min || value > feeder_rate_max)
373 return (feed_rate_setup(f));
377 feed_rate_get(struct pcm_feeder *f, int what)
379 struct feed_rate_info *info = f->data;
393 feed_rate_init(struct pcm_feeder *f)
395 struct feed_rate_info *info;
397 if (f->desc->out != f->desc->in)
400 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO);
404 * bufsz = sample from last cycle + conversion space
406 info->bufsz_init = 8 + feeder_buffersize;
407 info->buffer = malloc(info->bufsz_init, M_RATEFEEDER,
409 if (info->buffer == NULL) {
410 free(info, M_RATEFEEDER);
413 info->rsrc = DSP_DEFAULT_SPEED;
414 info->rdst = DSP_DEFAULT_SPEED;
416 return (feed_rate_setup(f));
420 feed_rate_free(struct pcm_feeder *f)
422 struct feed_rate_info *info = f->data;
425 if (info->buffer != NULL)
426 free(info->buffer, M_RATEFEEDER);
427 free(info, M_RATEFEEDER);
434 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
435 uint32_t count, void *source)
437 struct feed_rate_info *info = f->data;
441 if (info->convert == NULL)
442 return (FEEDER_FEED(f->source, c, b, count, source));
445 * This loop has been optimized to generalize both up / down
446 * sampling without causing missing samples or excessive buffer
447 * feeding. The tricky part is to calculate *precise* (slot) value
448 * needed for the entire conversion space since we are bound to
449 * return and fill up the buffer according to the requested 'count'.
450 * Too much feeding will cause the extra buffer stay within temporary
451 * circular buffer forever and always manifest itself as a truncated
452 * sound during end of playback / recording. Too few, and we end up
453 * with possible underruns and waste of cpu cycles.
455 * 'Stray' management exist to combat with possible unaligned
456 * buffering by the caller.
458 smpsz = info->bps * info->channels;
459 RATE_TEST(count >= smpsz && (count % smpsz) == 0,
460 ("%s: Count size not sample integral (%d)\n", __func__, count));
463 count -= count % smpsz;
465 * This slot count formula will stay here for the next million years
466 * to come. This is the key of our circular buffering precision.
468 slot = (((info->gx * (count / smpsz)) + info->gy - info->alpha - 1) /
470 RATE_TEST((slot % smpsz) == 0,
471 ("%s: Slot count not sample integral (%d)\n", __func__, slot));
472 #ifdef FEEDRATE_STRAY
473 RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n", __func__,
476 if (info->pos != smpsz && info->bpos - info->pos == smpsz &&
477 info->bpos + slot > info->bufsz) {
479 * Copy last unit sample and its previous to
480 * beginning of buffer.
482 bcopy(info->buffer + info->pos - smpsz, info->buffer,
485 info->bpos = smpsz << 1;
487 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n", __func__, slot));
491 fetch = info->bufsz - info->bpos;
492 #ifdef FEEDRATE_STRAY
493 fetch -= info->stray;
495 RATE_ASSERT(fetch >= 0,
496 ("%s: [1] Buffer overrun: %d > %d\n", __func__,
497 info->bpos, info->bufsz));
500 #ifdef FEEDRATE_STRAY
506 RATE_ASSERT((int)(info->bpos
507 #ifdef FEEDRATE_STRAY
511 (info->bpos - info->stray) < info->bufsz,
512 ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n",
513 __func__, info->bufsz, info->bpos
514 #ifdef FEEDRATE_STRAY
518 fetch = FEEDER_FEED(f->source, c,
519 info->buffer + info->bpos
520 #ifdef FEEDRATE_STRAY
524 #ifdef FEEDRATE_STRAY
531 RATE_TEST((fetch % smpsz) == 0,
532 ("%s: Fetch size not sample integral (%d)\n",
534 #ifdef FEEDRATE_STRAY
535 info->stray += fetch % smpsz;
536 RATE_TEST(info->stray == 0,
537 ("%s: Stray bytes detected (%d)\n", __func__,
540 fetch -= fetch % smpsz;
543 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n",
545 if (slot == 0 || info->bpos == info->bufsz)
548 if (info->pos == info->bpos) {
549 RATE_TEST(info->pos == smpsz,
550 ("%s: EOF while in progress\n", __func__));
553 RATE_ASSERT(info->pos <= info->bpos,
554 ("%s: [2] Buffer overrun: %d > %d\n", __func__, info->pos,
556 RATE_ASSERT(info->pos < info->bpos,
557 ("%s: Zero buffer!\n", __func__));
558 RATE_ASSERT(((info->bpos - info->pos) % smpsz) == 0,
559 ("%s: Buffer not sample integral (%d)\n", __func__,
560 info->bpos - info->pos));
561 i += info->convert(info, b + i, count - i);
562 RATE_ASSERT(info->pos <= info->bpos,
563 ("%s: [3] Buffer overrun: %d > %d\n", __func__, info->pos,
565 if (info->pos == info->bpos) {
567 * End of buffer cycle. Copy last unit sample
568 * to beginning of buffer so next cycle can
569 * interpolate using it.
571 #ifdef FEEDRATE_STRAY
572 RATE_TEST(info->stray == 0,
573 ("%s: [2] Stray bytes: %u\n", __func__,
576 bcopy(info->buffer + info->pos - smpsz, info->buffer,
585 RATE_TEST((slot == 0 && count == i) || (slot > 0 && count > i &&
586 info->pos == info->bpos && info->pos == smpsz),
587 ("%s: Inconsistent slot/count! "
588 "Count Expect: %u , Got: %u, Slot Left: %d\n", __func__, count, i,
591 #ifdef FEEDRATE_STRAY
592 RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__,
599 static struct pcm_feederdesc feeder_rate_desc[] = {
600 {FEEDER_RATE, AFMT_S8, AFMT_S8, 0},
601 {FEEDER_RATE, AFMT_S16_LE, AFMT_S16_LE, 0},
602 {FEEDER_RATE, AFMT_S24_LE, AFMT_S24_LE, 0},
603 {FEEDER_RATE, AFMT_S32_LE, AFMT_S32_LE, 0},
604 {FEEDER_RATE, AFMT_S16_BE, AFMT_S16_BE, 0},
605 {FEEDER_RATE, AFMT_S24_BE, AFMT_S24_BE, 0},
606 {FEEDER_RATE, AFMT_S32_BE, AFMT_S32_BE, 0},
607 {FEEDER_RATE, AFMT_S8 | AFMT_STEREO, AFMT_S8 | AFMT_STEREO, 0},
608 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
609 {FEEDER_RATE, AFMT_S24_LE | AFMT_STEREO, AFMT_S24_LE | AFMT_STEREO, 0},
610 {FEEDER_RATE, AFMT_S32_LE | AFMT_STEREO, AFMT_S32_LE | AFMT_STEREO, 0},
611 {FEEDER_RATE, AFMT_S16_BE | AFMT_STEREO, AFMT_S16_BE | AFMT_STEREO, 0},
612 {FEEDER_RATE, AFMT_S24_BE | AFMT_STEREO, AFMT_S24_BE | AFMT_STEREO, 0},
613 {FEEDER_RATE, AFMT_S32_BE | AFMT_STEREO, AFMT_S32_BE | AFMT_STEREO, 0},
614 {FEEDER_RATE, AFMT_U8, AFMT_U8, 0},
615 {FEEDER_RATE, AFMT_U16_LE, AFMT_U16_LE, 0},
616 {FEEDER_RATE, AFMT_U24_LE, AFMT_U24_LE, 0},
617 {FEEDER_RATE, AFMT_U32_LE, AFMT_U32_LE, 0},
618 {FEEDER_RATE, AFMT_U16_BE, AFMT_U16_BE, 0},
619 {FEEDER_RATE, AFMT_U24_BE, AFMT_U24_BE, 0},
620 {FEEDER_RATE, AFMT_U32_BE, AFMT_U32_BE, 0},
621 {FEEDER_RATE, AFMT_U8 | AFMT_STEREO, AFMT_U8 | AFMT_STEREO, 0},
622 {FEEDER_RATE, AFMT_U16_LE | AFMT_STEREO, AFMT_U16_LE | AFMT_STEREO, 0},
623 {FEEDER_RATE, AFMT_U24_LE | AFMT_STEREO, AFMT_U24_LE | AFMT_STEREO, 0},
624 {FEEDER_RATE, AFMT_U32_LE | AFMT_STEREO, AFMT_U32_LE | AFMT_STEREO, 0},
625 {FEEDER_RATE, AFMT_U16_BE | AFMT_STEREO, AFMT_U16_BE | AFMT_STEREO, 0},
626 {FEEDER_RATE, AFMT_U24_BE | AFMT_STEREO, AFMT_U24_BE | AFMT_STEREO, 0},
627 {FEEDER_RATE, AFMT_U32_BE | AFMT_STEREO, AFMT_U32_BE | AFMT_STEREO, 0},
631 static 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),
640 FEEDER_DECLARE(feeder_rate, 2, NULL);