1 /*-
2 * Copyright (c) 2003 Orion Hodson <orion@freebsd.org>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * MAINTAINER: Orion Hodson <orion@freebsd.org>
27 *
28 * This rate conversion code uses linear interpolation without any
29 * pre- or post- interpolation filtering to combat aliasing. This
30 * greatly limits the sound quality and should be addressed at some
31 * stage in the future.
32 *
33 * Since this accuracy of interpolation is sensitive and examination
34 * of the algorithm output is harder from the kernel, th code is
35 * designed to be compiled in the kernel and in a userland test
36 * harness. This is done by selectively including and excluding code
37 * with several portions based on whether _KERNEL is defined. It's a
38 * little ugly, but exceedingly useful. The testsuite and its
39 * revisions can be found at:
40 * http://people.freebsd.org/~orion/feedrate/
41 *
42 * Special thanks to Ken Marx for exposing flaws in the code and for
43 * testing revisions.
44 */
45
46 #ifdef _KERNEL
47
48 #include <dev/sound/pcm/sound.h>
49 #include "feeder_if.h"
50
51 SND_DECLARE_FILE("$FreeBSD: releng/5.4/sys/dev/sound/pcm/feeder_rate.c 141016 2005-01-30 01:00:13Z imp $");
52
53 #endif /* _KERNEL */
54
55 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
56
57 #ifndef RATE_ASSERT
58 #define RATE_ASSERT(x, y) /* KASSERT(x) */
59 #endif /* RATE_ASSERT */
60
61 #ifndef RATE_TRACE
62 #define RATE_TRACE(x...) /* printf(x) */
63 #endif
64
65 /*****************************************************************************/
66
67 /* The following coefficients are coupled. They are chosen to be
68 * guarantee calculable factors for the interpolation routine. They
69 * have been tested over the range of RATEMIN-RATEMAX Hz. Decreasing
70 * the granularity increases the required buffer size and affects the
71 * gain values at different points in the space. These values were
72 * found by running the test program with -p (probe) and some trial
73 * and error.
74 *
75 * ROUNDHZ the granularity of sample rates (fits n*11025 and n*8000).
76 * FEEDBUFSZ the amount of buffer space.
77 * MINGAIN the minimum acceptable gain in coefficients search.
78 */
79 #define ROUNDHZ 25
80 #define FEEDBUFSZ 8192
81 #define MINGAIN 92
82
83 #define RATEMIN 4000
84 #define RATEMAX 48000
85
86 struct feed_rate_info;
87
88 typedef int (*rate_convert_method)(struct feed_rate_info *,
89 uint32_t, uint32_t, int16_t *);
90
91 static int
92 convert_stereo_up(struct feed_rate_info *info,
93 uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst);
94
95 static int
96 convert_stereo_down(struct feed_rate_info *info,
97 uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst);
98
99 struct feed_rate_info {
100 uint32_t src, dst; /* source and destination rates */
101 uint16_t buffer_ticks; /* number of available samples in buffer */
102 uint16_t buffer_pos; /* next available sample in buffer */
103 uint16_t rounds; /* maximum number of cycle rounds w buffer */
104 uint16_t alpha; /* interpolation distance */
105 uint16_t sscale; /* src clock scale */
106 uint16_t dscale; /* dst clock scale */
107 uint16_t mscale; /* scale factor to avoid divide per sample */
108 uint16_t mroll; /* roll to again avoid divide per sample */
109 uint16_t channels; /* 1 = mono, 2 = stereo */
110
111 rate_convert_method convert;
112 int16_t buffer[FEEDBUFSZ];
113 };
114
115 #define bytes_per_sample 2
116 #define src_ticks_per_cycle(info) (info->dscale * info->rounds)
117 #define dst_ticks_per_cycle(info) (info->sscale * info->rounds)
118 #define bytes_per_tick(info) (info->channels * bytes_per_sample)
119 #define src_bytes_per_cycle(info) \
120 (src_ticks_per_cycle(info) * bytes_per_tick(info))
121 #define dst_bytes_per_cycle(info) \
122 (dst_ticks_per_cycle(info) * bytes_per_tick(info))
123
124 static uint32_t
125 gcd(uint32_t x, uint32_t y)
126 {
127 uint32_t w;
128 while (y != 0) {
129 w = x % y;
130 x = y;
131 y = w;
132 }
133 return x;
134 }
135
136 static int
137 feed_rate_setup(struct pcm_feeder *f)
138 {
139 struct feed_rate_info *info = f->data;
140 uint32_t mscale, mroll, l, r, g;
141
142 /* Beat sample rates down by greatest common divisor */
143 g = gcd(info->src, info->dst);
144 info->sscale = info->dst / g;
145 info->dscale = info->src / g;
146
147 info->alpha = 0;
148 info->buffer_ticks = 0;
149 info->buffer_pos = 0;
150
151 /* Pick suitable conversion routine */
152 if (info->src > info->dst) {
153 info->convert = convert_stereo_down;
154 } else {
155 info->convert = convert_stereo_up;
156 }
157
158 /*
159 * Determine number of conversion rounds that will fit into
160 * buffer. NB Must set info->rounds to one before using
161 * src_ticks_per_cycle here since it used by src_ticks_per_cycle.
162 */
163 info->rounds = 1;
164 r = (FEEDBUFSZ - bytes_per_tick(info)) /
165 (src_ticks_per_cycle(info) * bytes_per_tick(info));
166 if (r == 0) {
167 RATE_TRACE("Insufficient buffer space for conversion %d -> %d "
168 "(%d < %d)\n", info->src, info->dst, FEEDBUFSZ,
169 src_ticks_per_cycle(info) * bytes_per_tick(info));
170 return -1;
171 }
172 info->rounds = r;
173
174 /*
175 * Find scale and roll combination that allows us to trade
176 * costly divide operations in the main loop for multiply-rolls.
177 */
178 for (l = 96; l >= MINGAIN; l -= 3) {
179 for (mroll = 0; mroll < 16; mroll ++) {
180 mscale = (1 << mroll) / info->sscale;
181
182 r = (mscale * info->sscale * 100) >> mroll;
183 if (r > l && r <= 100) {
184 info->mscale = mscale;
185 info->mroll = mroll;
186 RATE_TRACE("Converting %d to %d with "
187 "mscale = %d and mroll = %d "
188 "(gain = %d / 100)\n",
189 info->src, info->dst,
190 info->mscale, info->mroll, r);
191 return 0;
192 }
193 }
194 }
195
196 RATE_TRACE("Failed to find a converter within %d%% gain for "
197 "%d to %d.\n", l, info->src, info->dst);
198
199 return -2;
200 }
201
202 static int
203 feed_rate_set(struct pcm_feeder *f, int what, int value)
204 {
205 struct feed_rate_info *info = f->data;
206 int rvalue;
207
208 if (value < RATEMIN || value > RATEMAX) {
209 return -1;
210 }
211
212 rvalue = (value / ROUNDHZ) * ROUNDHZ;
213 if (value - rvalue > ROUNDHZ / 2) {
214 rvalue += ROUNDHZ;
215 }
216
217 switch(what) {
218 case FEEDRATE_SRC:
219 info->src = rvalue;
220 break;
221 case FEEDRATE_DST:
222 info->dst = rvalue;
223 break;
224 default:
225 return -1;
226 }
227
228 return feed_rate_setup(f);
229 }
230
231 static int
232 feed_rate_get(struct pcm_feeder *f, int what)
233 {
234 struct feed_rate_info *info = f->data;
235
236 switch(what) {
237 case FEEDRATE_SRC:
238 return info->src;
239 case FEEDRATE_DST:
240 return info->dst;
241 default:
242 return -1;
243 }
244 return -1;
245 }
246
247 static int
248 feed_rate_init(struct pcm_feeder *f)
249 {
250 struct feed_rate_info *info;
251
252 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO);
253 if (info == NULL)
254 return ENOMEM;
255 info->src = DSP_DEFAULT_SPEED;
256 info->dst = DSP_DEFAULT_SPEED;
257 info->channels = 2;
258
259 f->data = info;
260 return 0;
261 }
262
263 static int
264 feed_rate_free(struct pcm_feeder *f)
265 {
266 struct feed_rate_info *info = f->data;
267
268 if (info) {
269 free(info, M_RATEFEEDER);
270 }
271 f->data = NULL;
272 return 0;
273 }
274
275 static int
276 convert_stereo_up(struct feed_rate_info *info,
277 uint32_t src_ticks,
278 uint32_t dst_ticks,
279 int16_t *dst)
280 {
281 uint32_t max_dst_ticks;
282 int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o;
283 int16_t *src;
284
285 sp = info->buffer_pos * 2;
286 se = sp + src_ticks * 2;
287
288 src = info->buffer;
289 alpha = info->alpha * info->mscale;
290 dalpha = info->dscale * info->mscale; /* Alpha increment */
291 malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */
292 mroll = info->mroll;
293
294 /*
295 * For efficiency the main conversion loop should only depend on
296 * one variable. We use the state to work out the maximum number
297 * of output samples that are available and eliminate the checking of
298 * sp from the loop.
299 */
300 max_dst_ticks = src_ticks * info->dst / info->src - alpha / dalpha;
301 if (max_dst_ticks < dst_ticks) {
302 dst_ticks = max_dst_ticks;
303 }
304
305 dp = 0;
306 de = dst_ticks * 2;
307 /*
308 * Unrolling this loop manually does not help much here because
309 * of the alpha, malpha comparison.
310 */
311 while (dp < de) {
312 o = malpha - alpha;
313 x = alpha * src[sp + 2] + o * src[sp];
314 dst[dp++] = x >> mroll;
315 x = alpha * src[sp + 3] + o * src[sp + 1];
316 dst[dp++] = x >> mroll;
317 alpha += dalpha;
318 if (alpha >= malpha) {
319 alpha -= malpha;
320 sp += 2;
321 }
322 }
323 RATE_ASSERT(sp <= se, ("%s: Source overrun\n", __func__));
324
325 info->buffer_pos = sp / info->channels;
326 info->alpha = alpha / info->mscale;
327
328 return dp / info->channels;
329 }
330
331 static int
332 convert_stereo_down(struct feed_rate_info *info,
333 uint32_t src_ticks,
334 uint32_t dst_ticks,
335 int16_t *dst)
336 {
337 int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o, m,
338 mdalpha, mstep;
339 int16_t *src;
340
341 sp = info->buffer_pos * 2;
342 se = sp + src_ticks * 2;
343
344 src = info->buffer;
345 alpha = info->alpha * info->mscale;
346 dalpha = info->dscale * info->mscale; /* Alpha increment */
347 malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */
348 mroll = info->mroll;
349
350 dp = 0;
351 de = dst_ticks * 2;
352
353 m = dalpha / malpha;
354 mstep = m * 2;
355 mdalpha = dalpha - m * malpha;
356
357 /*
358 * TODO: eliminate sp or dp from this loop comparison for a few
359 * extra % performance.
360 */
361 while (sp < se && dp < de) {
362 o = malpha - alpha;
363 x = alpha * src[sp + 2] + o * src[sp];
364 dst[dp++] = x >> mroll;
365 x = alpha * src[sp + 3] + o * src[sp + 1];
366 dst[dp++] = x >> mroll;
367
368 alpha += mdalpha;
369 sp += mstep;
370 if (alpha >= malpha) {
371 alpha -= malpha;
372 sp += 2;
373 }
374 }
375
376 info->buffer_pos = sp / 2;
377 info->alpha = alpha / info->mscale;
378
379 RATE_ASSERT(info->buffer_pos <= info->buffer_ticks,
380 ("%s: Source overrun\n", __func__));
381
382 return dp / 2;
383 }
384
385 static int
386 feed_rate(struct pcm_feeder *f,
387 struct pcm_channel *c,
388 uint8_t *b,
389 uint32_t count,
390 void *source)
391 {
392 struct feed_rate_info *info = f->data;
393
394 uint32_t done, s_ticks, d_ticks;
395 done = 0;
396
397 RATE_ASSERT(info->channels == 2,
398 ("%s: channels (%d) != 2", __func__, info->channels));
399
400 while (done < count) {
401 /* Slurp in more data if input buffer is not full */
402 while (info->buffer_ticks < src_ticks_per_cycle(info)) {
403 uint8_t *u8b;
404 int fetch;
405 fetch = src_bytes_per_cycle(info) -
406 info->buffer_ticks * bytes_per_tick(info);
407 u8b = (uint8_t*)info->buffer +
408 (info->buffer_ticks + 1) *
409 bytes_per_tick(info);
410 fetch = FEEDER_FEED(f->source, c, u8b, fetch, source);
411 RATE_ASSERT(fetch % bytes_per_tick(info) == 0,
412 ("%s: fetched unaligned bytes (%d)",
413 __func__, fetch));
414 info->buffer_ticks += fetch / bytes_per_tick(info);
415 RATE_ASSERT(src_ticks_per_cycle(info) >=
416 info->buffer_ticks,
417 ("%s: buffer overfilled (%d > %d).",
418 __func__, info->buffer_ticks,
419 src_ticks_per_cycle(info)));
420 if (fetch == 0)
421 break;
422 }
423
424 /* Find amount of input buffer data that should be processed */
425 d_ticks = (count - done) / bytes_per_tick(info);
426 s_ticks = info->buffer_ticks - info->buffer_pos;
427 if (info->buffer_ticks != src_ticks_per_cycle(info)) {
428 if (s_ticks > 8)
429 s_ticks -= 8;
430 else
431 s_ticks = 0;
432 }
433
434 d_ticks = info->convert(info, s_ticks, d_ticks,
435 (int16_t*)(b + done));
436 if (d_ticks == 0)
437 break;
438 done += d_ticks * bytes_per_tick(info);
439
440 RATE_ASSERT(info->buffer_pos <= info->buffer_ticks,
441 ("%s: buffer_ticks too big\n", __func__));
442 RATE_ASSERT(info->buffer_ticks <= src_ticks_per_cycle(info),
443 ("too many ticks %d / %d\n",
444 info->buffer_ticks, src_ticks_per_cycle(info)));
445 RATE_TRACE("%s: ticks %5d / %d pos %d\n", __func__,
446 info->buffer_ticks, src_ticks_per_cycle(info),
447 info->buffer_pos);
448
449 if (src_ticks_per_cycle(info) <= info->buffer_pos) {
450 /* End of cycle reached, copy last samples to start */
451 uint8_t *u8b;
452 u8b = (uint8_t*)info->buffer;
453 bcopy(u8b + src_bytes_per_cycle(info), u8b,
454 bytes_per_tick(info));
455
456 RATE_ASSERT(info->alpha == 0,
457 ("%s: completed cycle with "
458 "alpha non-zero", __func__, info->alpha));
459
460 info->buffer_pos = 0;
461 info->buffer_ticks = 0;
462 }
463 }
464
465 RATE_ASSERT(count >= done,
466 ("%s: generated too many bytes of data (%d > %d).",
467 __func__, done, count));
468
469 if (done != count) {
470 RATE_TRACE("Only did %d of %d\n", done, count);
471 }
472
473 return done;
474 }
475
476 static struct pcm_feederdesc feeder_rate_desc[] = {
477 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
478 {0, 0, 0, 0},
479 };
480 static kobj_method_t feeder_rate_methods[] = {
481 KOBJMETHOD(feeder_init, feed_rate_init),
482 KOBJMETHOD(feeder_free, feed_rate_free),
483 KOBJMETHOD(feeder_set, feed_rate_set),
484 KOBJMETHOD(feeder_get, feed_rate_get),
485 KOBJMETHOD(feeder_feed, feed_rate),
486 {0, 0}
487 };
488 FEEDER_DECLARE(feeder_rate, 2, NULL);
489
Cache object: 203904a40f7a7384c1eaf99af878ddb5
|