kernel: Remove some unneeded NULL checks after kmalloc() with M_WAITOK.
[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  */
67
68 #include <dev/sound/pcm/sound.h>
69 #include "feeder_if.h"
70
71 SND_DECLARE_FILE("$DragonFly: src/sys/dev/sound/pcm/feeder_rate.c,v 1.5 2007/01/04 21:47:03 corecode Exp $");
72
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) */
76
77 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
78
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
97
98 struct 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 };
110
111 static uint32_t
112 feed_convert_64(struct feed_rate_info *, int16_t *, uint32_t);
113 static uint32_t
114 feed_convert_scale64(struct feed_rate_info *, int16_t *, uint32_t);
115 static uint32_t
116 feed_convert_scale32(struct feed_rate_info *, int16_t *, uint32_t);
117 static uint32_t
118 feed_convert_plain(struct feed_rate_info *, int16_t *, uint32_t);
119
120 int feeder_rate_ratemin = RATEMIN;
121 int feeder_rate_ratemax = RATEMAX;
122 /*
123  * See 'Feeder Scaling Type' below..
124  */
125 static int feeder_rate_scaling = FEEDRATE_CONVERT_OPTIMAL;
126 static int feeder_rate_buffersize = FEEDBUFSZ & ~1;
127
128 #if 0
129 /* 
130  * sysctls.. I love sysctls..
131  */
132 TUNABLE_INT("hw.snd.feeder_rate_ratemin", &feeder_rate_ratemin);
133 TUNABLE_INT("hw.snd.feeder_rate_ratemax", &feeder_rate_ratemin);
134 TUNABLE_INT("hw.snd.feeder_rate_scaling", &feeder_rate_scaling);
135 TUNABLE_INT("hw.snd.feeder_rate_buffersize", &feeder_rate_buffersize);
136
137 static int
138 sysctl_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 }
150 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemin, CTLTYPE_INT | CTLFLAG_RW,
151         0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemin, "I", "");
152
153 static int
154 sysctl_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 }
166 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemax, CTLTYPE_INT | CTLFLAG_RW,
167         0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemax, "I", "");
168
169 static int
170 sysctl_hw_snd_feeder_rate_scaling(SYSCTL_HANDLER_ARGS)
171 {
172         int err, val;
173
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 }
209 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_scaling, CTLTYPE_INT | CTLFLAG_RW,
210         0, sizeof(int), sysctl_hw_snd_feeder_rate_scaling, "I", "");
211
212 static int
213 sysctl_hw_snd_feeder_rate_buffersize(SYSCTL_HANDLER_ARGS)
214 {
215         int err, val;
216
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 }
228 SYSCTL_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
232 static void
233 feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy)
234 {
235         uint32_t w, src = x, dst = y;
236
237         while (y != 0) {
238                 w = x % y;
239                 x = y;
240                 y = w;
241         }
242         *gx = src / x;
243         *gy = dst / x;
244 }
245
246 static void
247 feed_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
266 static int
267 feed_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         }
332 coef_failed:
333         feed_speed_ratio(*src, *dst, gx, gy);
334         feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL);
335         return 0;
336 }
337
338 static void
339 feed_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;
347 }
348
349 static int
350 feed_rate_setup(struct pcm_feeder *f)
351 {
352         struct feed_rate_info *info = f->data;
353         int r = 0;
354
355         info->pos = 2;
356         info->bpos = 4;
357         info->alpha = 0;
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;
367         } else {
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;
462         }
463         return 0;
464 }
465
466 static int
467 feed_rate_set(struct pcm_feeder *f, int what, int value)
468 {
469         struct feed_rate_info *info = f->data;
470
471         if (value < feeder_rate_ratemin || value > feeder_rate_ratemax)
472                 return -1;
473         
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;
483         }
484         return feed_rate_setup(f);
485 }
486
487 static int
488 feed_rate_get(struct pcm_feeder *f, int what)
489 {
490         struct feed_rate_info *info = f->data;
491
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;
502         }
503         return -1;
504 }
505
506 static int
507 feed_rate_init(struct pcm_feeder *f)
508 {
509         struct feed_rate_info *info;
510
511         info = kmalloc(sizeof(*info), M_RATEFEEDER, M_WAITOK | M_ZERO);
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,
517                                         M_RATEFEEDER, M_WAITOK | M_ZERO);
518         info->rsrc = DSP_DEFAULT_SPEED;
519         info->rdst = DSP_DEFAULT_SPEED;
520         f->data = info;
521         return feed_rate_setup(f);
522 }
523
524 static int
525 feed_rate_free(struct pcm_feeder *f)
526 {
527         struct feed_rate_info *info = f->data;
528
529         if (info) {
530                 if (info->buffer)
531                         kfree(info->buffer, M_RATEFEEDER);
532                 kfree(info, M_RATEFEEDER);
533         }
534         f->data = NULL;
535         return 0;
536 }
537
538 static uint32_t
539 feed_convert_64(struct feed_rate_info *info, int16_t *dst, uint32_t max)
540 {
541         int64_t x, alpha, distance;
542         uint32_t ret;
543         int32_t pos, bpos, gx, gy;
544         int16_t *src;
545         /*
546          * Plain, straight forward 64bit arith. No bit-magic applied here.
547          */
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                 }
571         }
572         info->alpha = alpha;
573         info->pos = pos;
574         return ret;
575 }
576
577 static uint32_t
578 feed_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;
584         /*
585          * 64bit scaling.
586          */
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;
610                 }
611         }
612         info->alpha = alpha / info->scale;
613         info->pos = pos;
614         return ret;
615 }
616
617 static uint32_t
618 feed_convert_scale32(struct feed_rate_info *info, int16_t *dst, uint32_t max)
619 {
620         uint32_t ret;
621         int32_t x, pos, bpos, gx, gy, alpha, roll, distance;
622         int16_t *src;
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;
633         src = info->buffer;
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 }
655
656 static uint32_t
657 feed_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;
662         /*
663          * Plain copy.
664          */
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;
684                 }
685         }
686         info->pos = pos;
687         info->alpha = alpha;
688         return ret;
689 }
690
691 static int32_t
692 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
693                         uint32_t count, void *source)
694 {
695         struct feed_rate_info *info = f->data;
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
775                                 break;
776                 }
777                 if (info->pos == info->bpos) {
778                         RATE_TEST(info->pos == 2,
779                                 ("%s: EOF while in progress\n", __func__));
780                         break;
781                 }
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;
808         }
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;
814 }
815
816 static 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 };
820 static 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 };
828 FEEDER_DECLARE(feeder_rate, 2, NULL);