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