1 /***************************************************************************
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
10 * Copyright (C) 2005 Miika Pekkarinen
12 * All files in this archive are subject to the GNU General Public License.
13 * See the file COPYING in the source tree root for full license agreement.
15 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
16 * KIND, either express or implied.
18 ****************************************************************************/
25 /* The "dither" code to convert the 24-bit samples produced by libmad was
26 * taken from the coolplayer project - coolplayer.sourceforge.net
29 /* 16-bit samples are scaled based on these constants. The shift should be
33 #define WORD_FRACBITS 27
35 #define NATIVE_DEPTH 16
36 #define SAMPLE_BUF_SIZE 256
37 #define RESAMPLE_BUF_SIZE (256 * 4) /* Enough for 11,025 Hz -> 44,100 Hz*/
39 #if CONFIG_CPU == MCF5249 && !defined(SIMULATOR)
41 #define INIT() asm volatile ("move.l #0xb0, %macsr") /* frac, round, clip */
42 /* Multiply 2 32-bit integers and return the 32 most significant bits of the
45 #define FRACMUL(x, y) \
48 asm volatile ("mac.l %[a], %[b], %%acc0\n\t" \
49 "movclr.l %%acc0, %[t]\n\t" \
50 : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \
57 #define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 32))
86 static struct dsp_config dsp
;
87 static struct dither_data dither_data
[2] IDATA_ATTR
;
88 static struct resample_data resample_data
[2] IDATA_ATTR
;
90 /* The internal format is 32-bit samples, non-interleaved, stereo. This
91 * format is similar to the raw output from several codecs, so the amount
92 * of copying needed is minimized for that case.
95 static long sample_buf
[SAMPLE_BUF_SIZE
] IDATA_ATTR
;
96 static long resample_buf
[RESAMPLE_BUF_SIZE
] IDATA_ATTR
;
99 /* Convert at most count samples to the internal format, if needed. Returns
100 * number of samples ready for further processing. Updates src to point
101 * past the samples "consumed" and dst is set to point to the samples to
102 * consume. Note that for mono, dst[0] equals dst[1], as there is no point
103 * in processing the same data twice.
105 static int convert_to_internal(char* src
[], int count
, long* dst
[])
107 count
= MIN(SAMPLE_BUF_SIZE
/ 2, count
);
109 if ((dsp
.sample_depth
<= NATIVE_DEPTH
)
110 || (dsp
.stereo_mode
== STEREO_INTERLEAVED
))
112 dst
[0] = &sample_buf
[0];
113 dst
[1] = (dsp
.stereo_mode
== STEREO_MONO
)
114 ? dst
[0] : &sample_buf
[SAMPLE_BUF_SIZE
/ 2];
118 dst
[0] = (long*) src
[0];
119 dst
[1] = (long*) ((dsp
.stereo_mode
== STEREO_MONO
) ? src
[0] : src
[1]);
122 if (dsp
.sample_depth
<= NATIVE_DEPTH
)
124 short* s0
= (short*) src
[0];
127 int scale
= WORD_SHIFT
;
130 if (dsp
.stereo_mode
== STEREO_INTERLEAVED
)
132 for (i
= 0; i
< count
; i
++)
134 *d0
++ = *s0
++ << scale
;
135 *d1
++ = *s0
++ << scale
;
138 else if (dsp
.stereo_mode
== STEREO_NONINTERLEAVED
)
140 short* s1
= (short*) src
[1];
142 for (i
= 0; i
< count
; i
++)
144 *d0
++ = *s0
++ << scale
;
145 *d1
++ = *s1
++ << scale
;
150 for (i
= 0; i
< count
; i
++)
152 *d0
++ = *s0
++ << scale
;
156 else if (dsp
.stereo_mode
== STEREO_INTERLEAVED
)
158 long* s0
= (long*) src
[0];
163 for (i
= 0; i
< count
; i
++)
170 if (dsp
.stereo_mode
== STEREO_NONINTERLEAVED
)
172 src
[0] += count
* dsp
.sample_bytes
;
173 src
[1] += count
* dsp
.sample_bytes
;
175 else if (dsp
.stereo_mode
== STEREO_INTERLEAVED
)
177 src
[0] += count
* dsp
.sample_bytes
* 2;
181 src
[0] += count
* dsp
.sample_bytes
;
187 /* Linear resampling that introduces a one sample delay, because of our
188 * inability to look into the future at the end of a frame.
191 static long downsample(long *dst
, long *src
, int count
,
192 struct resample_data
*r
)
194 long phase
= r
->phase
;
195 long delta
= r
->delta
;
196 long last_sample
= r
->last_sample
;
197 int pos
= phase
>> 16;
202 /* Do we need last sample of previous frame for interpolation? */
205 last_sample
= src
[pos
- 1];
208 *dst
++ = last_sample
+ FRACMUL((phase
& 0xffff) << 15,
209 src
[pos
] - last_sample
);
212 while ((pos
= phase
>> 16) < count
)
214 *dst
++ = src
[pos
- 1] + FRACMUL((phase
& 0xffff) << 15,
215 src
[pos
] - src
[pos
- 1]);
220 /* Wrap phase accumulator back to start of next frame. */
221 r
->phase
= phase
- (count
<< 16);
223 r
->last_sample
= src
[count
- 1];
227 static long upsample(long *dst
, long *src
, int count
, struct resample_data
*r
)
229 long phase
= r
->phase
;
230 long delta
= r
->delta
;
231 long last_sample
= r
->last_sample
;
237 while ((pos
= phase
>> 16) == 0)
239 *dst
++ = last_sample
+ FRACMUL((phase
& 0xffff) << 15,
240 src
[pos
] - last_sample
);
245 while ((pos
= phase
>> 16) < count
)
247 *dst
++ = src
[pos
- 1] + FRACMUL((phase
& 0xffff) << 15,
248 src
[pos
] - src
[pos
- 1]);
253 /* Wrap phase accumulator back to start of next frame. */
254 r
->phase
= phase
- (count
<< 16);
256 r
->last_sample
= src
[count
- 1];
260 /* Resample count stereo samples. Updates the src array, if resampling is
261 * done, to refer to the resampled data. Returns number of stereo samples
262 * for further processing.
264 static inline int resample(long* src
[], int count
)
268 if (dsp
.frequency
!= NATIVE_FREQUENCY
)
270 long* d0
= &resample_buf
[0];
271 /* Only process the second channel if needed. */
272 long* d1
= (src
[0] == src
[1]) ? d0
273 : &resample_buf
[RESAMPLE_BUF_SIZE
/ 2];
275 if (dsp
.frequency
< NATIVE_FREQUENCY
)
277 new_count
= upsample(d0
, src
[0], count
, &resample_data
[0]);
281 upsample(d1
, src
[1], count
, &resample_data
[1]);
286 new_count
= downsample(d0
, src
[0], count
, &resample_data
[0]);
290 downsample(d1
, src
[1], count
, &resample_data
[1]);
305 static inline long clip_sample(long sample
)
307 if (sample
> dsp
.clip_max
)
309 sample
= dsp
.clip_max
;
311 else if (sample
< dsp
.clip_min
)
313 sample
= dsp
.clip_min
;
319 /* The "dither" code to convert the 24-bit samples produced by libmad was
320 * taken from the coolplayer project - coolplayer.sourceforge.net
323 static long dither_sample(long sample
, long bias
, long mask
,
324 struct dither_data
* dither
)
329 /* Noise shape and bias */
331 sample
+= dither
->error
[0] - dither
->error
[1] + dither
->error
[2];
332 dither
->error
[2] = dither
->error
[1];
333 dither
->error
[1] = dither
->error
[0] / 2;
335 output
= sample
+ bias
;
339 random
= dither
->random
* 0x0019660dL
+ 0x3c6ef35fL
;
340 sample
+= (random
& mask
) - (dither
->random
& mask
);
341 dither
->random
= random
;
343 /* Clip and quantize */
345 sample
= clip_sample(sample
);
346 output
= clip_sample(output
) & ~mask
;
350 dither
->error
[0] = sample
- output
;
355 static void write_samples(short* dst
, long* src
[], int count
)
359 int scale
= dsp
.frac_bits
+ 1 - NATIVE_DEPTH
;
361 if (dsp
.dither_enabled
)
363 long bias
= (1L << (dsp
.frac_bits
- NATIVE_DEPTH
));
364 long mask
= (1L << scale
) - 1;
368 *dst
++ = (short) (dither_sample(*s0
++, bias
, mask
, &dither_data
[0])
370 *dst
++ = (short) (dither_sample(*s1
++, bias
, mask
, &dither_data
[1])
378 *dst
++ = (short) (clip_sample(*s0
++) >> scale
);
379 *dst
++ = (short) (clip_sample(*s1
++) >> scale
);
384 /* Process and convert src audio to dst based on the DSP configuration,
385 * reading size bytes of audio data. dst is assumed to be large enough; use
386 * dst_get_dest_size() to get the required size. src is an array of
387 * pointers; for mono and interleaved stereo, it contains one pointer to the
388 * start of the audio data; for non-interleaved stereo, it contains two
389 * pointers, one for each audio channel. Returns number of bytes written to
392 long dsp_process(char* dst
, char* src
[], long size
)
396 long factor
= (dsp
.stereo_mode
!= STEREO_MONO
) ? 2 : 1;
399 size
/= dsp
.sample_bytes
* factor
;
403 samples
= convert_to_internal(src
, size
, tmp
);
405 samples
= resample(tmp
, samples
);
406 write_samples((short*) dst
, tmp
, samples
);
408 dst
+= samples
* sizeof(short) * 2;
412 return written
* sizeof(short) * 2;
415 /* Given size bytes of input data, calculate the maximum number of bytes of
416 * output data that would be generated (the calculation is not entirely
417 * exact and rounds upwards to be on the safe side; during resampling,
418 * the number of samples generated depends on the current state of the
421 long dsp_output_size(long size
)
423 if (dsp
.stereo_mode
== STEREO_MONO
)
428 if (dsp
.sample_depth
> NATIVE_DEPTH
)
433 if (dsp
.frequency
!= NATIVE_FREQUENCY
)
435 size
= (long) ((((unsigned long) size
* NATIVE_FREQUENCY
)
436 + (dsp
.frequency
- 1)) / dsp
.frequency
);
439 return (size
+ 3) & ~3;
442 /* Given size bytes of output buffer, calculate number of bytes of input
443 * data that would be consumed in order to fill the output buffer.
445 long dsp_input_size(long size
)
447 if (dsp
.stereo_mode
== STEREO_MONO
)
452 if (dsp
.sample_depth
> NATIVE_DEPTH
)
457 if (dsp
.frequency
!= NATIVE_FREQUENCY
)
459 size
= (long) ((((unsigned long) size
* dsp
.frequency
)
460 + (NATIVE_FREQUENCY
- 1)) / NATIVE_FREQUENCY
);
466 int dsp_stereo_mode(void)
468 return dsp
.stereo_mode
;
471 bool dsp_configure(int setting
, void *value
)
475 case DSP_SET_FREQUENCY
:
476 dsp
.frequency
= ((int) value
== 0) ? NATIVE_FREQUENCY
: (int) value
;
477 memset(resample_data
, 0, sizeof(resample_data
));
478 resample_data
[0].delta
= resample_data
[1].delta
=
479 (unsigned long) dsp
.frequency
* 65536 / NATIVE_FREQUENCY
;
482 case DSP_SET_CLIP_MIN
:
483 dsp
.clip_min
= (long) value
;
486 case DSP_SET_CLIP_MAX
:
487 dsp
.clip_max
= (long) value
;
490 case DSP_SET_SAMPLE_DEPTH
:
491 dsp
.sample_depth
= (long) value
;
493 if (dsp
.sample_depth
<= NATIVE_DEPTH
)
495 dsp
.frac_bits
= WORD_FRACBITS
;
496 dsp
.sample_bytes
= sizeof(short);
497 dsp
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
498 dsp
.clip_min
= -((1 << WORD_FRACBITS
));
502 dsp
.frac_bits
= (long) value
;
503 dsp
.sample_bytes
= sizeof(long);
508 case DSP_SET_STEREO_MODE
:
509 dsp
.stereo_mode
= (long) value
;
513 dsp
.dither_enabled
= false;
514 dsp
.stereo_mode
= STEREO_NONINTERLEAVED
;
515 dsp
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
516 dsp
.clip_min
= -((1 << WORD_FRACBITS
));
517 dsp
.frequency
= NATIVE_FREQUENCY
;
518 dsp
.sample_depth
= NATIVE_DEPTH
;
519 dsp
.frac_bits
= WORD_FRACBITS
;
523 memset(dither_data
, 0, sizeof(dither_data
));
524 dsp
.dither_enabled
= (bool) value
;