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