1 /*-
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>
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
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.
15 *
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
26 * SUCH DAMAGE.
27 *
28 * 2005-06-11:
29 * ==========
30 *
31 * *New* and rewritten soft sample rate converter supporting arbitrary sample
32 * rates, fine grained scaling/coefficients and a unified up/down stereo
33 * converter. Most of the disclaimers from orion's notes also applies
34 * here, regarding linear interpolation deficiencies and pre/post
35 * anti-aliasing filtering issues. This version comes with a much simpler and
36 * tighter interface, although it works almost exactly like the older one.
37 *
38 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
39 * *
40 * This new implementation is fully dedicated in memory of Cameron Grant, *
41 * the creator of the magnificent, highly addictive feeder infrastructure. *
42 * *
43 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
44 *
45 * Orion's notes:
46 * =============
47 *
48 * This rate conversion code uses linear interpolation without any
49 * pre- or post- interpolation filtering to combat aliasing. This
50 * greatly limits the sound quality and should be addressed at some
51 * stage in the future.
52 *
53 * Since this accuracy of interpolation is sensitive and examination
54 * of the algorithm output is harder from the kernel, the code is
55 * designed to be compiled in the kernel and in a userland test
56 * harness. This is done by selectively including and excluding code
57 * with several portions based on whether _KERNEL is defined. It's a
58 * little ugly, but exceedingly useful. The testsuite and its
59 * revisions can be found at:
60 * http://people.freebsd.org/~orion/files/feedrate/
61 *
62 * Special thanks to Ken Marx for exposing flaws in the code and for
63 * testing revisions.
64 */
65
66 #include <dev/sound/pcm/sound.h>
67 #include "feeder_if.h"
68
69 SND_DECLARE_FILE("$FreeBSD: releng/6.4/sys/dev/sound/pcm/feeder_rate.c 154970 2006-01-29 02:27:28Z ariff $");
70
71 #define RATE_ASSERT(x, y) /* KASSERT(x,y) */
72 #define RATE_TEST(x, y) /* if (!(x)) printf y */
73 #define RATE_TRACE(x...) /* printf(x) */
74
75 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
76
77 #define FEEDBUFSZ 8192
78 #define ROUNDHZ 25
79 #define RATEMIN 4000
80 /* 8000 * 138 or 11025 * 100 . This is insane, indeed! */
81 #define RATEMAX 1102500
82 #define MINGAIN 92
83 #define MAXGAIN 96
84
85 #define FEEDRATE_CONVERT_64 0
86 #define FEEDRATE_CONVERT_SCALE64 1
87 #define FEEDRATE_CONVERT_SCALE32 2
88 #define FEEDRATE_CONVERT_PLAIN 3
89 #define FEEDRATE_CONVERT_FIXED 4
90 #define FEEDRATE_CONVERT_OPTIMAL 5
91 #define FEEDRATE_CONVERT_WORST 6
92
93 #define FEEDRATE_64_MAXROLL 32
94 #define FEEDRATE_32_MAXROLL 16
95
96 struct feed_rate_info {
97 uint32_t src, dst; /* rounded source / destination rates */
98 uint32_t rsrc, rdst; /* original source / destination rates */
99 uint32_t gx, gy; /* interpolation / decimation ratio */
100 uint32_t alpha; /* interpolation distance */
101 uint32_t pos, bpos; /* current sample / buffer positions */
102 uint32_t bufsz; /* total buffer size */
103 uint32_t stray; /* stray bytes */
104 int32_t scale, roll; /* scale / roll factor */
105 int16_t *buffer;
106 uint32_t (*convert)(struct feed_rate_info *, int16_t *, uint32_t);
107 };
108
109 static uint32_t
110 feed_convert_64(struct feed_rate_info *, int16_t *, uint32_t);
111 static uint32_t
112 feed_convert_scale64(struct feed_rate_info *, int16_t *, uint32_t);
113 static uint32_t
114 feed_convert_scale32(struct feed_rate_info *, int16_t *, uint32_t);
115 static uint32_t
116 feed_convert_plain(struct feed_rate_info *, int16_t *, uint32_t);
117
118 int feeder_rate_ratemin = RATEMIN;
119 int feeder_rate_ratemax = RATEMAX;
120 /*
121 * See 'Feeder Scaling Type' below..
122 */
123 static int feeder_rate_scaling = FEEDRATE_CONVERT_OPTIMAL;
124 static int feeder_rate_buffersize = FEEDBUFSZ & ~1;
125
126 #if 0
127 /*
128 * sysctls.. I love sysctls..
129 */
130 TUNABLE_INT("hw.snd.feeder_rate_ratemin", &feeder_rate_ratemin);
131 TUNABLE_INT("hw.snd.feeder_rate_ratemax", &feeder_rate_ratemin);
132 TUNABLE_INT("hw.snd.feeder_rate_scaling", &feeder_rate_scaling);
133 TUNABLE_INT("hw.snd.feeder_rate_buffersize", &feeder_rate_buffersize);
134
135 static int
136 sysctl_hw_snd_feeder_rate_ratemin(SYSCTL_HANDLER_ARGS)
137 {
138 int err, val;
139
140 val = feeder_rate_ratemin;
141 err = sysctl_handle_int(oidp, &val, sizeof(val), req);
142 if (val < 1 || val >= feeder_rate_ratemax)
143 err = EINVAL;
144 else
145 feeder_rate_ratemin = val;
146 return err;
147 }
148 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemin, CTLTYPE_INT | CTLFLAG_RW,
149 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemin, "I", "");
150
151 static int
152 sysctl_hw_snd_feeder_rate_ratemax(SYSCTL_HANDLER_ARGS)
153 {
154 int err, val;
155
156 val = feeder_rate_ratemax;
157 err = sysctl_handle_int(oidp, &val, sizeof(val), req);
158 if (val <= feeder_rate_ratemin || val > 0x7fffff)
159 err = EINVAL;
160 else
161 feeder_rate_ratemax = val;
162 return err;
163 }
164 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemax, CTLTYPE_INT | CTLFLAG_RW,
165 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemax, "I", "");
166
167 static int
168 sysctl_hw_snd_feeder_rate_scaling(SYSCTL_HANDLER_ARGS)
169 {
170 int err, val;
171
172 val = feeder_rate_scaling;
173 err = sysctl_handle_int(oidp, &val, sizeof(val), req);
174 /*
175 * Feeder Scaling Type
176 * ===================
177 *
178 * 1. Plain 64bit (high precision)
179 * 2. 64bit scaling (high precision, CPU friendly, but can
180 * cause gain up/down).
181 * 3. 32bit scaling (somehow can cause hz roundup, gain
182 * up/down).
183 * 4. Plain copy (default if src == dst. Except if src == dst,
184 * this is the worst / silly conversion method!).
185 *
186 * Sysctl options:-
187 *
188 * 0 - Plain 64bit - no fallback.
189 * 1 - 64bit scaling - no fallback.
190 * 2 - 32bit scaling - no fallback.
191 * 3 - Plain copy - no fallback.
192 * 4 - Fixed rate. Means that, choose optimal conversion method
193 * without causing hz roundup.
194 * 32bit scaling (as long as hz roundup does not occur),
195 * 64bit scaling, Plain 64bit.
196 * 5 - Optimal / CPU friendly (DEFAULT).
197 * 32bit scaling, 64bit scaling, Plain 64bit
198 * 6 - Optimal to worst, no 64bit arithmetic involved.
199 * 32bit scaling, Plain copy.
200 */
201 if (val < FEEDRATE_CONVERT_64 || val > FEEDRATE_CONVERT_WORST)
202 err = EINVAL;
203 else
204 feeder_rate_scaling = val;
205 return err;
206 }
207 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_scaling, CTLTYPE_INT | CTLFLAG_RW,
208 0, sizeof(int), sysctl_hw_snd_feeder_rate_scaling, "I", "");
209
210 static int
211 sysctl_hw_snd_feeder_rate_buffersize(SYSCTL_HANDLER_ARGS)
212 {
213 int err, val;
214
215 val = feeder_rate_buffersize;
216 err = sysctl_handle_int(oidp, &val, sizeof(val), req);
217 /*
218 * Don't waste too much kernel space
219 */
220 if (val < 2 || val > 65536)
221 err = EINVAL;
222 else
223 feeder_rate_buffersize = val & ~1;
224 return err;
225 }
226 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_buffersize, CTLTYPE_INT | CTLFLAG_RW,
227 0, sizeof(int), sysctl_hw_snd_feeder_rate_buffersize, "I", "");
228 #endif
229
230 static void
231 feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy)
232 {
233 uint32_t w, src = x, dst = y;
234
235 while (y != 0) {
236 w = x % y;
237 x = y;
238 y = w;
239 }
240 *gx = src / x;
241 *gy = dst / x;
242 }
243
244 static void
245 feed_scale_roll(uint32_t dst, int32_t *scale, int32_t *roll, int32_t max)
246 {
247 int64_t k, tscale;
248 int32_t j, troll;
249
250 *scale = *roll = -1;
251 for (j = MAXGAIN; j >= MINGAIN; j -= 3) {
252 for (troll = 0; troll < max; troll++) {
253 tscale = (1 << troll) / dst;
254 k = (tscale * dst * 100) >> troll;
255 if (k > j && k <= 100) {
256 *scale = tscale;
257 *roll = troll;
258 return;
259 }
260 }
261 }
262 }
263
264 static int
265 feed_get_best_coef(uint32_t *src, uint32_t *dst, uint32_t *gx, uint32_t *gy,
266 int32_t *scale, int32_t *roll)
267 {
268 uint32_t tsrc, tdst, sscale, dscale;
269 int32_t tscale, troll;
270 int i, j, hzmin, hzmax;
271
272 *scale = *roll = -1;
273 for (i = 0; i < 2; i++) {
274 hzmin = (ROUNDHZ * i) + 1;
275 hzmax = hzmin + ROUNDHZ;
276 for (j = hzmin; j < hzmax; j++) {
277 tsrc = *src - (*src % j);
278 tdst = *dst;
279 if (tsrc < 1 || tdst < 1)
280 goto coef_failed;
281 feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
282 feed_scale_roll(dscale, &tscale, &troll,
283 FEEDRATE_32_MAXROLL);
284 if (tscale != -1 && troll != -1) {
285 *src = tsrc;
286 *gx = sscale;
287 *gy = dscale;
288 *scale = tscale;
289 *roll = troll;
290 return j;
291 }
292 }
293 for (j = hzmin; j < hzmax; j++) {
294 tsrc = *src - (*src % j);
295 tdst = *dst - (*dst % j);
296 if (tsrc < 1 || tdst < 1)
297 goto coef_failed;
298 feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
299 feed_scale_roll(dscale, &tscale, &troll,
300 FEEDRATE_32_MAXROLL);
301 if (tscale != -1 && troll != -1) {
302 *src = tsrc;
303 *dst = tdst;
304 *gx = sscale;
305 *gy = dscale;
306 *scale = tscale;
307 *roll = troll;
308 return j;
309 }
310 }
311 for (j = hzmin; j < hzmax; j++) {
312 tsrc = *src;
313 tdst = *dst - (*dst % j);
314 if (tsrc < 1 || tdst < 1)
315 goto coef_failed;
316 feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
317 feed_scale_roll(dscale, &tscale, &troll,
318 FEEDRATE_32_MAXROLL);
319 if (tscale != -1 && troll != -1) {
320 *src = tsrc;
321 *dst = tdst;
322 *gx = sscale;
323 *gy = dscale;
324 *scale = tscale;
325 *roll = troll;
326 return j;
327 }
328 }
329 }
330 coef_failed:
331 feed_speed_ratio(*src, *dst, gx, gy);
332 feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL);
333 return 0;
334 }
335
336 static void
337 feed_rate_reset(struct feed_rate_info *info)
338 {
339 info->scale = -1;
340 info->roll = -1;
341 info->src = info->rsrc;
342 info->dst = info->rdst;
343 info->gx = 0;
344 info->gy = 0;
345 }
346
347 static int
348 feed_rate_setup(struct pcm_feeder *f)
349 {
350 struct feed_rate_info *info = f->data;
351 int r = 0;
352
353 info->pos = 2;
354 info->bpos = 4;
355 info->alpha = 0;
356 info->stray = 0;
357 feed_rate_reset(info);
358 if (info->src == info->dst) {
359 /*
360 * No conversion ever needed. Just do plain copy.
361 */
362 info->convert = feed_convert_plain;
363 info->gx = 1;
364 info->gy = 1;
365 } else {
366 switch (feeder_rate_scaling) {
367 case FEEDRATE_CONVERT_64:
368 feed_speed_ratio(info->src, info->dst,
369 &info->gx, &info->gy);
370 info->convert = feed_convert_64;
371 break;
372 case FEEDRATE_CONVERT_SCALE64:
373 feed_speed_ratio(info->src, info->dst,
374 &info->gx, &info->gy);
375 feed_scale_roll(info->gy, &info->scale,
376 &info->roll, FEEDRATE_64_MAXROLL);
377 if (info->scale == -1 || info->roll == -1)
378 return -1;
379 info->convert = feed_convert_scale64;
380 break;
381 case FEEDRATE_CONVERT_SCALE32:
382 r = feed_get_best_coef(&info->src, &info->dst,
383 &info->gx, &info->gy, &info->scale,
384 &info->roll);
385 if (r == 0)
386 return -1;
387 info->convert = feed_convert_scale32;
388 break;
389 case FEEDRATE_CONVERT_PLAIN:
390 feed_speed_ratio(info->src, info->dst,
391 &info->gx, &info->gy);
392 info->convert = feed_convert_plain;
393 break;
394 case FEEDRATE_CONVERT_FIXED:
395 r = feed_get_best_coef(&info->src, &info->dst,
396 &info->gx, &info->gy, &info->scale,
397 &info->roll);
398 if (r != 0 && info->src == info->rsrc &&
399 info->dst == info->rdst)
400 info->convert = feed_convert_scale32;
401 else {
402 /* Fallback */
403 feed_rate_reset(info);
404 feed_speed_ratio(info->src, info->dst,
405 &info->gx, &info->gy);
406 feed_scale_roll(info->gy, &info->scale,
407 &info->roll, FEEDRATE_64_MAXROLL);
408 if (info->scale != -1 && info->roll != -1)
409 info->convert = feed_convert_scale64;
410 else
411 info->convert = feed_convert_64;
412 }
413 break;
414 case FEEDRATE_CONVERT_OPTIMAL:
415 r = feed_get_best_coef(&info->src, &info->dst,
416 &info->gx, &info->gy, &info->scale,
417 &info->roll);
418 if (r != 0)
419 info->convert = feed_convert_scale32;
420 else {
421 /* Fallback */
422 feed_rate_reset(info);
423 feed_speed_ratio(info->src, info->dst,
424 &info->gx, &info->gy);
425 feed_scale_roll(info->gy, &info->scale,
426 &info->roll, FEEDRATE_64_MAXROLL);
427 if (info->scale != -1 && info->roll != -1)
428 info->convert = feed_convert_scale64;
429 else
430 info->convert = feed_convert_64;
431 }
432 break;
433 case FEEDRATE_CONVERT_WORST:
434 r = feed_get_best_coef(&info->src, &info->dst,
435 &info->gx, &info->gy, &info->scale,
436 &info->roll);
437 if (r != 0)
438 info->convert = feed_convert_scale32;
439 else {
440 /* Fallback */
441 feed_rate_reset(info);
442 feed_speed_ratio(info->src, info->dst,
443 &info->gx, &info->gy);
444 info->convert = feed_convert_plain;
445 }
446 break;
447 default:
448 return -1;
449 break;
450 }
451 /* No way! */
452 if (info->gx == 0 || info->gy == 0)
453 return -1;
454 /*
455 * No need to interpolate/decimate, just do plain copy.
456 * This probably caused by Hz roundup.
457 */
458 if (info->gx == info->gy)
459 info->convert = feed_convert_plain;
460 }
461 return 0;
462 }
463
464 static int
465 feed_rate_set(struct pcm_feeder *f, int what, int value)
466 {
467 struct feed_rate_info *info = f->data;
468
469 if (value < feeder_rate_ratemin || value > feeder_rate_ratemax)
470 return -1;
471
472 switch (what) {
473 case FEEDRATE_SRC:
474 info->rsrc = value;
475 break;
476 case FEEDRATE_DST:
477 info->rdst = value;
478 break;
479 default:
480 return -1;
481 }
482 return feed_rate_setup(f);
483 }
484
485 static int
486 feed_rate_get(struct pcm_feeder *f, int what)
487 {
488 struct feed_rate_info *info = f->data;
489
490 /*
491 * Return *real* src/dst rate.
492 */
493 switch (what) {
494 case FEEDRATE_SRC:
495 return info->rsrc;
496 case FEEDRATE_DST:
497 return info->rdst;
498 default:
499 return -1;
500 }
501 return -1;
502 }
503
504 static int
505 feed_rate_init(struct pcm_feeder *f)
506 {
507 struct feed_rate_info *info;
508
509 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO);
510 if (info == NULL)
511 return ENOMEM;
512 /*
513 * bufsz = sample from last cycle + conversion space
514 */
515 info->bufsz = 2 + feeder_rate_buffersize;
516 info->buffer = malloc(sizeof(*info->buffer) * info->bufsz,
517 M_RATEFEEDER, M_NOWAIT | M_ZERO);
518 if (info->buffer == NULL) {
519 free(info, M_RATEFEEDER);
520 return ENOMEM;
521 }
522 info->rsrc = DSP_DEFAULT_SPEED;
523 info->rdst = DSP_DEFAULT_SPEED;
524 f->data = info;
525 return feed_rate_setup(f);
526 }
527
528 static int
529 feed_rate_free(struct pcm_feeder *f)
530 {
531 struct feed_rate_info *info = f->data;
532
533 if (info) {
534 if (info->buffer)
535 free(info->buffer, M_RATEFEEDER);
536 free(info, M_RATEFEEDER);
537 }
538 f->data = NULL;
539 return 0;
540 }
541
542 static uint32_t
543 feed_convert_64(struct feed_rate_info *info, int16_t *dst, uint32_t max)
544 {
545 int64_t x, alpha, distance;
546 uint32_t ret;
547 int32_t pos, bpos, gx, gy;
548 int16_t *src;
549 /*
550 * Plain, straight forward 64bit arith. No bit-magic applied here.
551 */
552 ret = 0;
553 alpha = info->alpha;
554 gx = info->gx;
555 gy = info->gy;
556 pos = info->pos;
557 bpos = info->bpos;
558 src = info->buffer;
559 for (;;) {
560 if (alpha < gx) {
561 alpha += gy;
562 pos += 2;
563 if (pos == bpos)
564 break;
565 } else {
566 alpha -= gx;
567 distance = gy - alpha;
568 x = (alpha * src[pos - 2]) + (distance * src[pos]);
569 dst[ret++] = x / gy;
570 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
571 dst[ret++] = x / gy;
572 if (ret == max)
573 break;
574 }
575 }
576 info->alpha = alpha;
577 info->pos = pos;
578 return ret;
579 }
580
581 static uint32_t
582 feed_convert_scale64(struct feed_rate_info *info, int16_t *dst, uint32_t max)
583 {
584 int64_t x, alpha, distance;
585 uint32_t ret;
586 int32_t pos, bpos, gx, gy, roll;
587 int16_t *src;
588 /*
589 * 64bit scaling.
590 */
591 ret = 0;
592 roll = info->roll;
593 alpha = info->alpha * info->scale;
594 gx = info->gx * info->scale;
595 gy = info->gy * info->scale;
596 pos = info->pos;
597 bpos = info->bpos;
598 src = info->buffer;
599 for (;;) {
600 if (alpha < gx) {
601 alpha += gy;
602 pos += 2;
603 if (pos == bpos)
604 break;
605 } else {
606 alpha -= gx;
607 distance = gy - alpha;
608 x = (alpha * src[pos - 2]) + (distance * src[pos]);
609 dst[ret++] = x >> roll;
610 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
611 dst[ret++] = x >> roll;
612 if (ret == max)
613 break;
614 }
615 }
616 info->alpha = alpha / info->scale;
617 info->pos = pos;
618 return ret;
619 }
620
621 static uint32_t
622 feed_convert_scale32(struct feed_rate_info *info, int16_t *dst, uint32_t max)
623 {
624 uint32_t ret;
625 int32_t x, pos, bpos, gx, gy, alpha, roll, distance;
626 int16_t *src;
627 /*
628 * 32bit scaling.
629 */
630 ret = 0;
631 roll = info->roll;
632 alpha = info->alpha * info->scale;
633 gx = info->gx * info->scale;
634 gy = info->gy * info->scale;
635 pos = info->pos;
636 bpos = info->bpos;
637 src = info->buffer;
638 for (;;) {
639 if (alpha < gx) {
640 alpha += gy;
641 pos += 2;
642 if (pos == bpos)
643 break;
644 } else {
645 alpha -= gx;
646 distance = gy - alpha;
647 x = (alpha * src[pos - 2]) + (distance * src[pos]);
648 dst[ret++] = x >> roll;
649 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
650 dst[ret++] = x >> roll;
651 if (ret == max)
652 break;
653 }
654 }
655 info->alpha = alpha / info->scale;
656 info->pos = pos;
657 return ret;
658 }
659
660 static uint32_t
661 feed_convert_plain(struct feed_rate_info *info, int16_t *dst, uint32_t max)
662 {
663 uint32_t ret;
664 int32_t pos, bpos, gx, gy, alpha;
665 int16_t *src;
666 /*
667 * Plain copy.
668 */
669 ret = 0;
670 gx = info->gx;
671 gy = info->gy;
672 alpha = info->alpha;
673 pos = info->pos;
674 bpos = info->bpos;
675 src = info->buffer;
676 for (;;) {
677 if (alpha < gx) {
678 alpha += gy;
679 pos += 2;
680 if (pos == bpos)
681 break;
682 } else {
683 alpha -= gx;
684 dst[ret++] = src[pos];
685 dst[ret++] = src[pos + 1];
686 if (ret == max)
687 break;
688 }
689 }
690 info->pos = pos;
691 info->alpha = alpha;
692 return ret;
693 }
694
695 static int32_t
696 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
697 uint32_t count, void *source)
698 {
699 struct feed_rate_info *info = f->data;
700 uint32_t i;
701 int32_t fetch, slot;
702 int16_t *dst = (int16_t *)b;
703 /*
704 * This loop has been optimized to generalize both up / down
705 * sampling without causing missing samples or excessive buffer
706 * feeding.
707 */
708 RATE_TEST(count >= 4 && (count & 3) == 0,
709 ("%s: Count size not byte integral (%d)\n", __func__, count));
710 if (count < 4)
711 return 0;
712 count >>= 1;
713 count &= ~1;
714 slot = (((info->gx * (count >> 1)) + info->gy - info->alpha - 1) / info->gy) << 1;
715 RATE_TEST((slot & 1) == 0, ("%s: Slot count not sample integral (%d)\n",
716 __func__, slot));
717 /*
718 * Optimize buffer feeding aggressively to ensure calculated slot
719 * can be fitted nicely into available buffer free space, hence
720 * avoiding multiple feeding.
721 */
722 RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n",
723 __func__,info->stray));
724 if (info->pos != 2 && info->bpos - info->pos == 2 &&
725 info->bpos + slot > info->bufsz) {
726 /*
727 * Copy last unit sample and its previous to
728 * beginning of buffer.
729 */
730 info->buffer[0] = info->buffer[info->pos - 2];
731 info->buffer[1] = info->buffer[info->pos - 1];
732 info->buffer[2] = info->buffer[info->pos];
733 info->buffer[3] = info->buffer[info->pos + 1];
734 info->pos = 2;
735 info->bpos = 4;
736 }
737 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n",
738 __func__, slot));
739 i = 0;
740 for (;;) {
741 for (;;) {
742 fetch = (info->bufsz - info->bpos) << 1;
743 fetch -= info->stray;
744 RATE_ASSERT(fetch >= 0,
745 ("%s: [1] Buffer overrun: %d > %d\n",
746 __func__, info->bpos, info->bufsz));
747 if ((slot << 1) < fetch)
748 fetch = slot << 1;
749 if (fetch > 0) {
750 RATE_ASSERT(((info->bpos << 1) - info->stray) >= 0 &&
751 ((info->bpos << 1) - info->stray) < (info->bufsz << 1),
752 ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n", __func__,
753 info->bufsz << 1, (info->bpos << 1) - info->stray));
754 fetch = FEEDER_FEED(f->source, c,
755 (uint8_t *)(info->buffer) + (info->bpos << 1) - info->stray,
756 fetch, source);
757 info->stray = 0;
758 if (fetch == 0)
759 break;
760 RATE_TEST((fetch & 3) == 0,
761 ("%s: Fetch size not byte integral (%d)\n",
762 __func__, fetch));
763 info->stray += fetch & 3;
764 RATE_TEST(info->stray == 0,
765 ("%s: Stray bytes detected (%d)\n",
766 __func__, info->stray));
767 fetch >>= 1;
768 fetch &= ~1;
769 info->bpos += fetch;
770 slot -= fetch;
771 RATE_ASSERT(slot >= 0,
772 ("%s: Negative Slot: %d\n", __func__,
773 slot));
774 if (slot == 0)
775 break;
776 if (info->bpos == info->bufsz)
777 break;
778 } else
779 break;
780 }
781 if (info->pos == info->bpos) {
782 RATE_TEST(info->pos == 2,
783 ("%s: EOF while in progress\n", __func__));
784 break;
785 }
786 RATE_ASSERT(info->pos <= info->bpos,
787 ("%s: [2] Buffer overrun: %d > %d\n", __func__,
788 info->pos, info->bpos));
789 RATE_ASSERT(info->pos < info->bpos,
790 ("%s: Zero buffer!\n", __func__));
791 RATE_ASSERT(((info->bpos - info->pos) & 1) == 0,
792 ("%s: Buffer not sample integral (%d)\n",
793 __func__, info->bpos - info->pos));
794 i += info->convert(info, dst + i, count - i);
795 RATE_ASSERT(info->pos <= info->bpos,
796 ("%s: [3] Buffer overrun: %d > %d\n",
797 __func__, info->pos, info->bpos));
798 if (info->pos == info->bpos) {
799 /*
800 * End of buffer cycle. Copy last unit sample
801 * to beginning of buffer so next cycle can
802 * interpolate using it.
803 */
804 RATE_TEST(info->stray == 0, ("%s: [2] Stray bytes: %u\n", __func__, info->stray));
805 info->buffer[0] = info->buffer[info->pos - 2];
806 info->buffer[1] = info->buffer[info->pos - 1];
807 info->bpos = 2;
808 info->pos = 2;
809 }
810 if (i == count)
811 break;
812 }
813 #if 0
814 RATE_TEST(count == i, ("Expect: %u , Got: %u\n", count << 1, i << 1));
815 #endif
816 RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__, info->stray));
817 return i << 1;
818 }
819
820 static struct pcm_feederdesc feeder_rate_desc[] = {
821 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
822 {0, 0, 0, 0},
823 };
824 static kobj_method_t feeder_rate_methods[] = {
825 KOBJMETHOD(feeder_init, feed_rate_init),
826 KOBJMETHOD(feeder_free, feed_rate_free),
827 KOBJMETHOD(feeder_set, feed_rate_set),
828 KOBJMETHOD(feeder_get, feed_rate_get),
829 KOBJMETHOD(feeder_feed, feed_rate),
830 {0, 0}
831 };
832 FEEDER_DECLARE(feeder_rate, 2, NULL);
Cache object: b68c10e0393414ba0bdbe2247fec64e0
|