diff options
Diffstat (limited to 'lib/rbcodec/dsp/dsp.c')
-rw-r--r-- | lib/rbcodec/dsp/dsp.c | 1568 |
1 files changed, 0 insertions, 1568 deletions
diff --git a/lib/rbcodec/dsp/dsp.c b/lib/rbcodec/dsp/dsp.c deleted file mode 100644 index de647dc0dd..0000000000 --- a/lib/rbcodec/dsp/dsp.c +++ /dev/null | |||
@@ -1,1568 +0,0 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include "config.h" | ||
22 | #include "system.h" | ||
23 | #include <sound.h> | ||
24 | #include "dsp.h" | ||
25 | #include "dsp-util.h" | ||
26 | #include "eq.h" | ||
27 | #include "compressor.h" | ||
28 | #include "kernel.h" | ||
29 | #include "settings.h" | ||
30 | #include "replaygain.h" | ||
31 | #include "tdspeed.h" | ||
32 | #include "core_alloc.h" | ||
33 | #include "fixedpoint.h" | ||
34 | #include "fracmul.h" | ||
35 | |||
36 | /* Define LOGF_ENABLE to enable logf output in this file */ | ||
37 | /*#define LOGF_ENABLE*/ | ||
38 | #include "logf.h" | ||
39 | |||
40 | /* 16-bit samples are scaled based on these constants. The shift should be | ||
41 | * no more than 15. | ||
42 | */ | ||
43 | #define WORD_SHIFT 12 | ||
44 | #define WORD_FRACBITS 27 | ||
45 | |||
46 | #define NATIVE_DEPTH 16 | ||
47 | #define SMALL_SAMPLE_BUF_COUNT 128 /* Per channel */ | ||
48 | #define DEFAULT_GAIN 0x01000000 | ||
49 | |||
50 | /* enums to index conversion properly with stereo mode and other settings */ | ||
51 | enum | ||
52 | { | ||
53 | SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED, | ||
54 | SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED, | ||
55 | SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO, | ||
56 | SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES, | ||
57 | SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES, | ||
58 | SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES, | ||
59 | SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES | ||
60 | }; | ||
61 | |||
62 | enum | ||
63 | { | ||
64 | SAMPLE_OUTPUT_MONO = 0, | ||
65 | SAMPLE_OUTPUT_STEREO, | ||
66 | SAMPLE_OUTPUT_DITHERED_MONO, | ||
67 | SAMPLE_OUTPUT_DITHERED_STEREO | ||
68 | }; | ||
69 | |||
70 | /* No asm...yet */ | ||
71 | struct dither_data | ||
72 | { | ||
73 | long error[3]; /* 00h */ | ||
74 | long random; /* 0ch */ | ||
75 | /* 10h */ | ||
76 | }; | ||
77 | |||
78 | struct crossfeed_data | ||
79 | { | ||
80 | int32_t gain; /* 00h - Direct path gain */ | ||
81 | int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */ | ||
82 | int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */ | ||
83 | int32_t delay[13][2]; /* 20h */ | ||
84 | int32_t *index; /* 88h - Current pointer into the delay line */ | ||
85 | /* 8ch */ | ||
86 | }; | ||
87 | |||
88 | /* Current setup is one lowshelf filters three peaking filters and one | ||
89 | * highshelf filter. Varying the number of shelving filters make no sense, | ||
90 | * but adding peaking filters is possible. | ||
91 | */ | ||
92 | struct eq_state | ||
93 | { | ||
94 | char enabled[5]; /* 00h - Flags for active filters */ | ||
95 | struct eqfilter filters[5]; /* 08h - packing is 4? */ | ||
96 | /* 10ch */ | ||
97 | }; | ||
98 | |||
99 | /* Include header with defines which functions are implemented in assembly | ||
100 | code for the target */ | ||
101 | #include <dsp_asm.h> | ||
102 | |||
103 | /* Typedefs keep things much neater in this case */ | ||
104 | typedef void (*sample_input_fn_type)(int count, const char *src[], | ||
105 | int32_t *dst[]); | ||
106 | typedef int (*resample_fn_type)(int count, struct dsp_data *data, | ||
107 | const int32_t *src[], int32_t *dst[]); | ||
108 | typedef void (*sample_output_fn_type)(int count, struct dsp_data *data, | ||
109 | const int32_t *src[], int16_t *dst); | ||
110 | |||
111 | /* Single-DSP channel processing in place */ | ||
112 | typedef void (*channels_process_fn_type)(int count, int32_t *buf[]); | ||
113 | /* DSP local channel processing in place */ | ||
114 | typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data, | ||
115 | int32_t *buf[]); | ||
116 | |||
117 | /* | ||
118 | ***************************************************************************/ | ||
119 | |||
120 | struct dsp_config | ||
121 | { | ||
122 | struct dsp_data data; /* Config members for use in external routines */ | ||
123 | long codec_frequency; /* Sample rate of data coming from the codec */ | ||
124 | long frequency; /* Effective sample rate after pitch shift (if any) */ | ||
125 | int sample_depth; | ||
126 | int sample_bytes; | ||
127 | int stereo_mode; | ||
128 | int32_t tdspeed_percent; /* Speed% * PITCH_SPEED_PRECISION */ | ||
129 | #ifdef HAVE_PITCHSCREEN | ||
130 | bool tdspeed_active; /* Timestretch is in use */ | ||
131 | #endif | ||
132 | #ifdef HAVE_SW_TONE_CONTROLS | ||
133 | /* Filter struct for software bass/treble controls */ | ||
134 | struct eqfilter tone_filter; | ||
135 | #endif | ||
136 | /* Functions that change depending upon settings - NULL if stage is | ||
137 | disabled */ | ||
138 | sample_input_fn_type input_samples; | ||
139 | resample_fn_type resample; | ||
140 | sample_output_fn_type output_samples; | ||
141 | /* These will be NULL for the voice codec and is more economical that | ||
142 | way */ | ||
143 | channels_process_dsp_fn_type apply_gain; | ||
144 | channels_process_fn_type apply_crossfeed; | ||
145 | channels_process_fn_type eq_process; | ||
146 | channels_process_fn_type channels_process; | ||
147 | channels_process_dsp_fn_type compressor_process; | ||
148 | }; | ||
149 | |||
150 | /* General DSP config */ | ||
151 | static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */ | ||
152 | /* Dithering */ | ||
153 | static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */ | ||
154 | static long dither_mask IBSS_ATTR; | ||
155 | static long dither_bias IBSS_ATTR; | ||
156 | /* Crossfeed */ | ||
157 | struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */ | ||
158 | { | ||
159 | .index = (int32_t *)crossfeed_data.delay | ||
160 | }; | ||
161 | |||
162 | /* Equalizer */ | ||
163 | static struct eq_state eq_data; /* A */ | ||
164 | |||
165 | /* Software tone controls */ | ||
166 | #ifdef HAVE_SW_TONE_CONTROLS | ||
167 | static int prescale; /* A/V */ | ||
168 | static int bass; /* A/V */ | ||
169 | static int treble; /* A/V */ | ||
170 | #endif | ||
171 | |||
172 | /* Settings applicable to audio codec only */ | ||
173 | #ifdef HAVE_PITCHSCREEN | ||
174 | static int32_t pitch_ratio = PITCH_SPEED_100; | ||
175 | static int big_sample_locks; | ||
176 | #endif | ||
177 | static int channels_mode; | ||
178 | long dsp_sw_gain; | ||
179 | long dsp_sw_cross; | ||
180 | static bool dither_enabled; | ||
181 | static long eq_precut; | ||
182 | static long track_gain; | ||
183 | static bool new_gain; | ||
184 | static long album_gain; | ||
185 | static long track_peak; | ||
186 | static long album_peak; | ||
187 | static long replaygain; | ||
188 | static bool crossfeed_enabled; | ||
189 | |||
190 | #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO]) | ||
191 | #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE]) | ||
192 | |||
193 | /* The internal format is 32-bit samples, non-interleaved, stereo. This | ||
194 | * format is similar to the raw output from several codecs, so the amount | ||
195 | * of copying needed is minimized for that case. | ||
196 | */ | ||
197 | |||
198 | #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */ | ||
199 | #define SMALL_RESAMPLE_BUF_COUNT (SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO) | ||
200 | #define BIG_SAMPLE_BUF_COUNT SMALL_RESAMPLE_BUF_COUNT | ||
201 | #define BIG_RESAMPLE_BUF_COUNT (BIG_SAMPLE_BUF_COUNT * RESAMPLE_RATIO) | ||
202 | |||
203 | static int32_t small_sample_buf[2][SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR; | ||
204 | static int32_t small_resample_buf[2][SMALL_RESAMPLE_BUF_COUNT] IBSS_ATTR; | ||
205 | |||
206 | #ifdef HAVE_PITCHSCREEN | ||
207 | static int32_t (* big_sample_buf)[BIG_SAMPLE_BUF_COUNT] = NULL; | ||
208 | static int32_t (* big_resample_buf)[BIG_RESAMPLE_BUF_COUNT] = NULL; | ||
209 | #endif | ||
210 | |||
211 | static int sample_buf_count = SMALL_SAMPLE_BUF_COUNT; | ||
212 | static int32_t *sample_buf[2] = { small_sample_buf[0], small_sample_buf[1] }; | ||
213 | static int resample_buf_count = SMALL_RESAMPLE_BUF_COUNT; | ||
214 | static int32_t *resample_buf[2] = { small_resample_buf[0], small_resample_buf[1] }; | ||
215 | |||
216 | #ifdef HAVE_PITCHSCREEN | ||
217 | int32_t sound_get_pitch(void) | ||
218 | { | ||
219 | return pitch_ratio; | ||
220 | } | ||
221 | |||
222 | void sound_set_pitch(int32_t percent) | ||
223 | { | ||
224 | pitch_ratio = percent; | ||
225 | dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY, | ||
226 | AUDIO_DSP.codec_frequency); | ||
227 | } | ||
228 | |||
229 | static void tdspeed_set_pointers( bool time_stretch_active ) | ||
230 | { | ||
231 | if( time_stretch_active ) | ||
232 | { | ||
233 | sample_buf_count = BIG_SAMPLE_BUF_COUNT; | ||
234 | resample_buf_count = BIG_RESAMPLE_BUF_COUNT; | ||
235 | sample_buf[0] = big_sample_buf[0]; | ||
236 | sample_buf[1] = big_sample_buf[1]; | ||
237 | resample_buf[0] = big_resample_buf[0]; | ||
238 | resample_buf[1] = big_resample_buf[1]; | ||
239 | } | ||
240 | else | ||
241 | { | ||
242 | sample_buf_count = SMALL_SAMPLE_BUF_COUNT; | ||
243 | resample_buf_count = SMALL_RESAMPLE_BUF_COUNT; | ||
244 | sample_buf[0] = small_sample_buf[0]; | ||
245 | sample_buf[1] = small_sample_buf[1]; | ||
246 | resample_buf[0] = small_resample_buf[0]; | ||
247 | resample_buf[1] = small_resample_buf[1]; | ||
248 | } | ||
249 | } | ||
250 | |||
251 | static void tdspeed_setup(struct dsp_config *dspc) | ||
252 | { | ||
253 | /* Assume timestretch will not be used */ | ||
254 | dspc->tdspeed_active = false; | ||
255 | |||
256 | tdspeed_set_pointers( false ); | ||
257 | |||
258 | if (!dsp_timestretch_available()) | ||
259 | return; /* Timestretch not enabled or buffer not allocated */ | ||
260 | |||
261 | if (dspc->tdspeed_percent == 0) | ||
262 | dspc->tdspeed_percent = PITCH_SPEED_100; | ||
263 | |||
264 | if (!tdspeed_config( | ||
265 | dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency, | ||
266 | dspc->stereo_mode != STEREO_MONO, | ||
267 | dspc->tdspeed_percent)) | ||
268 | return; /* Timestretch not possible or needed with these parameters */ | ||
269 | |||
270 | /* Timestretch is to be used */ | ||
271 | dspc->tdspeed_active = true; | ||
272 | |||
273 | tdspeed_set_pointers( true ); | ||
274 | } | ||
275 | |||
276 | |||
277 | static int move_callback(int handle, void* current, void* new) | ||
278 | { | ||
279 | (void)handle;(void)current; | ||
280 | |||
281 | if ( big_sample_locks > 0 ) | ||
282 | return BUFLIB_CB_CANNOT_MOVE; | ||
283 | |||
284 | big_sample_buf = new; | ||
285 | |||
286 | /* no allocation without timestretch enabled */ | ||
287 | tdspeed_set_pointers( true ); | ||
288 | return BUFLIB_CB_OK; | ||
289 | } | ||
290 | |||
291 | static void lock_sample_buf( bool lock ) | ||
292 | { | ||
293 | if ( lock ) | ||
294 | big_sample_locks++; | ||
295 | else | ||
296 | big_sample_locks--; | ||
297 | } | ||
298 | |||
299 | static struct buflib_callbacks ops = { | ||
300 | .move_callback = move_callback, | ||
301 | .shrink_callback = NULL, | ||
302 | }; | ||
303 | |||
304 | |||
305 | void dsp_timestretch_enable(bool enabled) | ||
306 | { | ||
307 | /* Hook to set up timestretch buffer on first call to settings_apply() */ | ||
308 | static int handle = -1; | ||
309 | if (enabled) | ||
310 | { | ||
311 | if (big_sample_buf) | ||
312 | return; /* already allocated and enabled */ | ||
313 | |||
314 | /* Set up timestretch buffers */ | ||
315 | big_sample_buf = &small_resample_buf[0]; | ||
316 | handle = core_alloc_ex("resample buf", | ||
317 | 2 * BIG_RESAMPLE_BUF_COUNT * sizeof(int32_t), | ||
318 | &ops); | ||
319 | big_sample_locks = 0; | ||
320 | enabled = handle >= 0; | ||
321 | |||
322 | if (enabled) | ||
323 | { | ||
324 | /* success, now setup tdspeed */ | ||
325 | big_resample_buf = core_get_data(handle); | ||
326 | |||
327 | tdspeed_init(); | ||
328 | tdspeed_setup(&AUDIO_DSP); | ||
329 | } | ||
330 | } | ||
331 | |||
332 | if (!enabled) | ||
333 | { | ||
334 | dsp_set_timestretch(PITCH_SPEED_100); | ||
335 | tdspeed_finish(); | ||
336 | |||
337 | if (handle >= 0) | ||
338 | core_free(handle); | ||
339 | |||
340 | handle = -1; | ||
341 | big_sample_buf = NULL; | ||
342 | } | ||
343 | } | ||
344 | |||
345 | void dsp_set_timestretch(int32_t percent) | ||
346 | { | ||
347 | AUDIO_DSP.tdspeed_percent = percent; | ||
348 | tdspeed_setup(&AUDIO_DSP); | ||
349 | } | ||
350 | |||
351 | int32_t dsp_get_timestretch() | ||
352 | { | ||
353 | return AUDIO_DSP.tdspeed_percent; | ||
354 | } | ||
355 | |||
356 | bool dsp_timestretch_available() | ||
357 | { | ||
358 | return (global_settings.timestretch_enabled && big_sample_buf); | ||
359 | } | ||
360 | #endif /* HAVE_PITCHSCREEN */ | ||
361 | |||
362 | /* Convert count samples to the internal format, if needed. Updates src | ||
363 | * to point past the samples "consumed" and dst is set to point to the | ||
364 | * samples to consume. Note that for mono, dst[0] equals dst[1], as there | ||
365 | * is no point in processing the same data twice. | ||
366 | */ | ||
367 | |||
368 | /* convert count 16-bit mono to 32-bit mono */ | ||
369 | static void sample_input_lte_native_mono( | ||
370 | int count, const char *src[], int32_t *dst[]) | ||
371 | { | ||
372 | const int16_t *s = (int16_t *) src[0]; | ||
373 | const int16_t * const send = s + count; | ||
374 | int32_t *d = dst[0] = dst[1] = sample_buf[0]; | ||
375 | int scale = WORD_SHIFT; | ||
376 | |||
377 | while (s < send) | ||
378 | { | ||
379 | *d++ = *s++ << scale; | ||
380 | } | ||
381 | |||
382 | src[0] = (char *)s; | ||
383 | } | ||
384 | |||
385 | /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */ | ||
386 | static void sample_input_lte_native_i_stereo( | ||
387 | int count, const char *src[], int32_t *dst[]) | ||
388 | { | ||
389 | const int32_t *s = (int32_t *) src[0]; | ||
390 | const int32_t * const send = s + count; | ||
391 | int32_t *dl = dst[0] = sample_buf[0]; | ||
392 | int32_t *dr = dst[1] = sample_buf[1]; | ||
393 | int scale = WORD_SHIFT; | ||
394 | |||
395 | while (s < send) | ||
396 | { | ||
397 | int32_t slr = *s++; | ||
398 | #ifdef ROCKBOX_LITTLE_ENDIAN | ||
399 | *dl++ = (slr >> 16) << scale; | ||
400 | *dr++ = (int32_t)(int16_t)slr << scale; | ||
401 | #else /* ROCKBOX_BIG_ENDIAN */ | ||
402 | *dl++ = (int32_t)(int16_t)slr << scale; | ||
403 | *dr++ = (slr >> 16) << scale; | ||
404 | #endif | ||
405 | } | ||
406 | |||
407 | src[0] = (char *)s; | ||
408 | } | ||
409 | |||
410 | /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */ | ||
411 | static void sample_input_lte_native_ni_stereo( | ||
412 | int count, const char *src[], int32_t *dst[]) | ||
413 | { | ||
414 | const int16_t *sl = (int16_t *) src[0]; | ||
415 | const int16_t *sr = (int16_t *) src[1]; | ||
416 | const int16_t * const slend = sl + count; | ||
417 | int32_t *dl = dst[0] = sample_buf[0]; | ||
418 | int32_t *dr = dst[1] = sample_buf[1]; | ||
419 | int scale = WORD_SHIFT; | ||
420 | |||
421 | while (sl < slend) | ||
422 | { | ||
423 | *dl++ = *sl++ << scale; | ||
424 | *dr++ = *sr++ << scale; | ||
425 | } | ||
426 | |||
427 | src[0] = (char *)sl; | ||
428 | src[1] = (char *)sr; | ||
429 | } | ||
430 | |||
431 | /* convert count 32-bit mono to 32-bit mono */ | ||
432 | static void sample_input_gt_native_mono( | ||
433 | int count, const char *src[], int32_t *dst[]) | ||
434 | { | ||
435 | dst[0] = dst[1] = (int32_t *)src[0]; | ||
436 | src[0] = (char *)(dst[0] + count); | ||
437 | } | ||
438 | |||
439 | /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */ | ||
440 | static void sample_input_gt_native_i_stereo( | ||
441 | int count, const char *src[], int32_t *dst[]) | ||
442 | { | ||
443 | const int32_t *s = (int32_t *)src[0]; | ||
444 | const int32_t * const send = s + 2*count; | ||
445 | int32_t *dl = dst[0] = sample_buf[0]; | ||
446 | int32_t *dr = dst[1] = sample_buf[1]; | ||
447 | |||
448 | while (s < send) | ||
449 | { | ||
450 | *dl++ = *s++; | ||
451 | *dr++ = *s++; | ||
452 | } | ||
453 | |||
454 | src[0] = (char *)send; | ||
455 | } | ||
456 | |||
457 | /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */ | ||
458 | static void sample_input_gt_native_ni_stereo( | ||
459 | int count, const char *src[], int32_t *dst[]) | ||
460 | { | ||
461 | dst[0] = (int32_t *)src[0]; | ||
462 | dst[1] = (int32_t *)src[1]; | ||
463 | src[0] = (char *)(dst[0] + count); | ||
464 | src[1] = (char *)(dst[1] + count); | ||
465 | } | ||
466 | |||
467 | /** | ||
468 | * sample_input_new_format() | ||
469 | * | ||
470 | * set the to-native sample conversion function based on dsp sample parameters | ||
471 | * | ||
472 | * !DSPPARAMSYNC | ||
473 | * needs syncing with changes to the following dsp parameters: | ||
474 | * * dsp->stereo_mode (A/V) | ||
475 | * * dsp->sample_depth (A/V) | ||
476 | */ | ||
477 | static void sample_input_new_format(struct dsp_config *dsp) | ||
478 | { | ||
479 | static const sample_input_fn_type sample_input_functions[] = | ||
480 | { | ||
481 | [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo, | ||
482 | [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo, | ||
483 | [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono, | ||
484 | [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo, | ||
485 | [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo, | ||
486 | [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono, | ||
487 | }; | ||
488 | |||
489 | int convert = dsp->stereo_mode; | ||
490 | |||
491 | if (dsp->sample_depth > NATIVE_DEPTH) | ||
492 | convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX; | ||
493 | |||
494 | dsp->input_samples = sample_input_functions[convert]; | ||
495 | } | ||
496 | |||
497 | |||
498 | #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO | ||
499 | /* write mono internal format to output format */ | ||
500 | static void sample_output_mono(int count, struct dsp_data *data, | ||
501 | const int32_t *src[], int16_t *dst) | ||
502 | { | ||
503 | const int32_t *s0 = src[0]; | ||
504 | const int scale = data->output_scale; | ||
505 | const int dc_bias = 1 << (scale - 1); | ||
506 | |||
507 | while (count-- > 0) | ||
508 | { | ||
509 | int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
510 | *dst++ = lr; | ||
511 | *dst++ = lr; | ||
512 | } | ||
513 | } | ||
514 | #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */ | ||
515 | |||
516 | /* write stereo internal format to output format */ | ||
517 | #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO | ||
518 | static void sample_output_stereo(int count, struct dsp_data *data, | ||
519 | const int32_t *src[], int16_t *dst) | ||
520 | { | ||
521 | const int32_t *s0 = src[0]; | ||
522 | const int32_t *s1 = src[1]; | ||
523 | const int scale = data->output_scale; | ||
524 | const int dc_bias = 1 << (scale - 1); | ||
525 | |||
526 | while (count-- > 0) | ||
527 | { | ||
528 | *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
529 | *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale); | ||
530 | } | ||
531 | } | ||
532 | #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */ | ||
533 | |||
534 | /** | ||
535 | * The "dither" code to convert the 24-bit samples produced by libmad was | ||
536 | * taken from the coolplayer project - coolplayer.sourceforge.net | ||
537 | * | ||
538 | * This function handles mono and stereo outputs. | ||
539 | */ | ||
540 | static void sample_output_dithered(int count, struct dsp_data *data, | ||
541 | const int32_t *src[], int16_t *dst) | ||
542 | { | ||
543 | const int32_t mask = dither_mask; | ||
544 | const int32_t bias = dither_bias; | ||
545 | const int scale = data->output_scale; | ||
546 | const int32_t min = data->clip_min; | ||
547 | const int32_t max = data->clip_max; | ||
548 | const int32_t range = max - min; | ||
549 | int ch; | ||
550 | int16_t *d; | ||
551 | |||
552 | for (ch = 0; ch < data->num_channels; ch++) | ||
553 | { | ||
554 | struct dither_data * const dither = &dither_data[ch]; | ||
555 | const int32_t *s = src[ch]; | ||
556 | int i; | ||
557 | |||
558 | for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2) | ||
559 | { | ||
560 | int32_t output, sample; | ||
561 | int32_t random; | ||
562 | |||
563 | /* Noise shape and bias (for correct rounding later) */ | ||
564 | sample = *s; | ||
565 | sample += dither->error[0] - dither->error[1] + dither->error[2]; | ||
566 | dither->error[2] = dither->error[1]; | ||
567 | dither->error[1] = dither->error[0]/2; | ||
568 | |||
569 | output = sample + bias; | ||
570 | |||
571 | /* Dither, highpass triangle PDF */ | ||
572 | random = dither->random*0x0019660dL + 0x3c6ef35fL; | ||
573 | output += (random & mask) - (dither->random & mask); | ||
574 | dither->random = random; | ||
575 | |||
576 | /* Round sample to output range */ | ||
577 | output &= ~mask; | ||
578 | |||
579 | /* Error feedback */ | ||
580 | dither->error[0] = sample - output; | ||
581 | |||
582 | /* Clip */ | ||
583 | if ((uint32_t)(output - min) > (uint32_t)range) | ||
584 | { | ||
585 | int32_t c = min; | ||
586 | if (output > min) | ||
587 | c += range; | ||
588 | output = c; | ||
589 | } | ||
590 | |||
591 | /* Quantize and store */ | ||
592 | *d = output >> scale; | ||
593 | } | ||
594 | } | ||
595 | |||
596 | if (data->num_channels == 2) | ||
597 | return; | ||
598 | |||
599 | /* Have to duplicate left samples into the right channel since | ||
600 | pcm buffer and hardware is interleaved stereo */ | ||
601 | d = &dst[0]; | ||
602 | |||
603 | while (count-- > 0) | ||
604 | { | ||
605 | int16_t s = *d++; | ||
606 | *d++ = s; | ||
607 | } | ||
608 | } | ||
609 | |||
610 | /** | ||
611 | * sample_output_new_format() | ||
612 | * | ||
613 | * set the from-native to ouput sample conversion routine | ||
614 | * | ||
615 | * !DSPPARAMSYNC | ||
616 | * needs syncing with changes to the following dsp parameters: | ||
617 | * * dsp->stereo_mode (A/V) | ||
618 | * * dither_enabled (A) | ||
619 | */ | ||
620 | static void sample_output_new_format(struct dsp_config *dsp) | ||
621 | { | ||
622 | static const sample_output_fn_type sample_output_functions[] = | ||
623 | { | ||
624 | sample_output_mono, | ||
625 | sample_output_stereo, | ||
626 | sample_output_dithered, | ||
627 | sample_output_dithered | ||
628 | }; | ||
629 | |||
630 | int out = dsp->data.num_channels - 1; | ||
631 | |||
632 | if (dsp == &AUDIO_DSP && dither_enabled) | ||
633 | out += 2; | ||
634 | |||
635 | dsp->output_samples = sample_output_functions[out]; | ||
636 | } | ||
637 | |||
638 | /** | ||
639 | * Linear interpolation resampling that introduces a one sample delay because | ||
640 | * of our inability to look into the future at the end of a frame. | ||
641 | */ | ||
642 | #ifndef DSP_HAVE_ASM_RESAMPLING | ||
643 | static int dsp_downsample(int count, struct dsp_data *data, | ||
644 | const int32_t *src[], int32_t *dst[]) | ||
645 | { | ||
646 | int ch = data->num_channels - 1; | ||
647 | uint32_t delta = data->resample_data.delta; | ||
648 | uint32_t phase, pos; | ||
649 | int32_t *d; | ||
650 | |||
651 | /* Rolled channel loop actually showed slightly faster. */ | ||
652 | do | ||
653 | { | ||
654 | /* Just initialize things and not worry too much about the relatively | ||
655 | * uncommon case of not being able to spit out a sample for the frame. | ||
656 | */ | ||
657 | const int32_t *s = src[ch]; | ||
658 | int32_t last = data->resample_data.last_sample[ch]; | ||
659 | |||
660 | data->resample_data.last_sample[ch] = s[count - 1]; | ||
661 | d = dst[ch]; | ||
662 | phase = data->resample_data.phase; | ||
663 | pos = phase >> 16; | ||
664 | |||
665 | /* Do we need last sample of previous frame for interpolation? */ | ||
666 | if (pos > 0) | ||
667 | last = s[pos - 1]; | ||
668 | |||
669 | while (pos < (uint32_t)count) | ||
670 | { | ||
671 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); | ||
672 | phase += delta; | ||
673 | pos = phase >> 16; | ||
674 | last = s[pos - 1]; | ||
675 | } | ||
676 | } | ||
677 | while (--ch >= 0); | ||
678 | |||
679 | /* Wrap phase accumulator back to start of next frame. */ | ||
680 | data->resample_data.phase = phase - (count << 16); | ||
681 | return d - dst[0]; | ||
682 | } | ||
683 | |||
684 | static int dsp_upsample(int count, struct dsp_data *data, | ||
685 | const int32_t *src[], int32_t *dst[]) | ||
686 | { | ||
687 | int ch = data->num_channels - 1; | ||
688 | uint32_t delta = data->resample_data.delta; | ||
689 | uint32_t phase, pos; | ||
690 | int32_t *d; | ||
691 | |||
692 | /* Rolled channel loop actually showed slightly faster. */ | ||
693 | do | ||
694 | { | ||
695 | /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */ | ||
696 | const int32_t *s = src[ch]; | ||
697 | int32_t last = data->resample_data.last_sample[ch]; | ||
698 | |||
699 | data->resample_data.last_sample[ch] = s[count - 1]; | ||
700 | d = dst[ch]; | ||
701 | phase = data->resample_data.phase; | ||
702 | pos = phase >> 16; | ||
703 | |||
704 | while (pos == 0) | ||
705 | { | ||
706 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last); | ||
707 | phase += delta; | ||
708 | pos = phase >> 16; | ||
709 | } | ||
710 | |||
711 | while (pos < (uint32_t)count) | ||
712 | { | ||
713 | last = s[pos - 1]; | ||
714 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); | ||
715 | phase += delta; | ||
716 | pos = phase >> 16; | ||
717 | } | ||
718 | } | ||
719 | while (--ch >= 0); | ||
720 | |||
721 | /* Wrap phase accumulator back to start of next frame. */ | ||
722 | data->resample_data.phase = phase & 0xffff; | ||
723 | return d - dst[0]; | ||
724 | } | ||
725 | #endif /* DSP_HAVE_ASM_RESAMPLING */ | ||
726 | |||
727 | static void resampler_new_delta(struct dsp_config *dsp) | ||
728 | { | ||
729 | dsp->data.resample_data.delta = (unsigned long) | ||
730 | dsp->frequency * 65536LL / NATIVE_FREQUENCY; | ||
731 | |||
732 | if (dsp->frequency == NATIVE_FREQUENCY) | ||
733 | { | ||
734 | /* NOTE: If fully glitch-free transistions from no resampling to | ||
735 | resampling are desired, last_sample history should be maintained | ||
736 | even when not resampling. */ | ||
737 | dsp->resample = NULL; | ||
738 | dsp->data.resample_data.phase = 0; | ||
739 | dsp->data.resample_data.last_sample[0] = 0; | ||
740 | dsp->data.resample_data.last_sample[1] = 0; | ||
741 | } | ||
742 | else if (dsp->frequency < NATIVE_FREQUENCY) | ||
743 | dsp->resample = dsp_upsample; | ||
744 | else | ||
745 | dsp->resample = dsp_downsample; | ||
746 | } | ||
747 | |||
748 | /* Resample count stereo samples. Updates the src array, if resampling is | ||
749 | * done, to refer to the resampled data. Returns number of stereo samples | ||
750 | * for further processing. | ||
751 | */ | ||
752 | static inline int resample(struct dsp_config *dsp, int count, int32_t *src[]) | ||
753 | { | ||
754 | int32_t *dst[2] = | ||
755 | { | ||
756 | resample_buf[0], | ||
757 | resample_buf[1] | ||
758 | }; | ||
759 | lock_sample_buf( true ); | ||
760 | count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst); | ||
761 | |||
762 | src[0] = dst[0]; | ||
763 | src[1] = dst[dsp->data.num_channels - 1]; | ||
764 | lock_sample_buf( false ); | ||
765 | return count; | ||
766 | } | ||
767 | |||
768 | static void dither_init(struct dsp_config *dsp) | ||
769 | { | ||
770 | memset(dither_data, 0, sizeof (dither_data)); | ||
771 | dither_bias = (1L << (dsp->data.frac_bits - NATIVE_DEPTH)); | ||
772 | dither_mask = (1L << (dsp->data.frac_bits + 1 - NATIVE_DEPTH)) - 1; | ||
773 | } | ||
774 | |||
775 | void dsp_dither_enable(bool enable) | ||
776 | { | ||
777 | struct dsp_config *dsp = &AUDIO_DSP; | ||
778 | dither_enabled = enable; | ||
779 | sample_output_new_format(dsp); | ||
780 | } | ||
781 | |||
782 | /* Applies crossfeed to the stereo signal in src. | ||
783 | * Crossfeed is a process where listening over speakers is simulated. This | ||
784 | * is good for old hard panned stereo records, which might be quite fatiguing | ||
785 | * to listen to on headphones with no crossfeed. | ||
786 | */ | ||
787 | #ifndef DSP_HAVE_ASM_CROSSFEED | ||
788 | static void apply_crossfeed(int count, int32_t *buf[]) | ||
789 | { | ||
790 | int32_t *hist_l = &crossfeed_data.history[0]; | ||
791 | int32_t *hist_r = &crossfeed_data.history[2]; | ||
792 | int32_t *delay = &crossfeed_data.delay[0][0]; | ||
793 | int32_t *coefs = &crossfeed_data.coefs[0]; | ||
794 | int32_t gain = crossfeed_data.gain; | ||
795 | int32_t *di = crossfeed_data.index; | ||
796 | |||
797 | int32_t acc; | ||
798 | int32_t left, right; | ||
799 | int i; | ||
800 | |||
801 | for (i = 0; i < count; i++) | ||
802 | { | ||
803 | left = buf[0][i]; | ||
804 | right = buf[1][i]; | ||
805 | |||
806 | /* Filter delayed sample from left speaker */ | ||
807 | acc = FRACMUL(*di, coefs[0]); | ||
808 | acc += FRACMUL(hist_l[0], coefs[1]); | ||
809 | acc += FRACMUL(hist_l[1], coefs[2]); | ||
810 | /* Save filter history for left speaker */ | ||
811 | hist_l[1] = acc; | ||
812 | hist_l[0] = *di; | ||
813 | *di++ = left; | ||
814 | /* Filter delayed sample from right speaker */ | ||
815 | acc = FRACMUL(*di, coefs[0]); | ||
816 | acc += FRACMUL(hist_r[0], coefs[1]); | ||
817 | acc += FRACMUL(hist_r[1], coefs[2]); | ||
818 | /* Save filter history for right speaker */ | ||
819 | hist_r[1] = acc; | ||
820 | hist_r[0] = *di; | ||
821 | *di++ = right; | ||
822 | /* Now add the attenuated direct sound and write to outputs */ | ||
823 | buf[0][i] = FRACMUL(left, gain) + hist_r[1]; | ||
824 | buf[1][i] = FRACMUL(right, gain) + hist_l[1]; | ||
825 | |||
826 | /* Wrap delay line index if bigger than delay line size */ | ||
827 | if (di >= delay + 13*2) | ||
828 | di = delay; | ||
829 | } | ||
830 | /* Write back local copies of data we've modified */ | ||
831 | crossfeed_data.index = di; | ||
832 | } | ||
833 | #endif /* DSP_HAVE_ASM_CROSSFEED */ | ||
834 | |||
835 | /** | ||
836 | * dsp_set_crossfeed(bool enable) | ||
837 | * | ||
838 | * !DSPPARAMSYNC | ||
839 | * needs syncing with changes to the following dsp parameters: | ||
840 | * * dsp->stereo_mode (A) | ||
841 | */ | ||
842 | void dsp_set_crossfeed(bool enable) | ||
843 | { | ||
844 | crossfeed_enabled = enable; | ||
845 | AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1) | ||
846 | ? apply_crossfeed : NULL; | ||
847 | } | ||
848 | |||
849 | void dsp_set_crossfeed_direct_gain(int gain) | ||
850 | { | ||
851 | crossfeed_data.gain = get_replaygain_int(gain * 10) << 7; | ||
852 | /* If gain is negative, the calculation overflowed and we need to clamp */ | ||
853 | if (crossfeed_data.gain < 0) | ||
854 | crossfeed_data.gain = 0x7fffffff; | ||
855 | } | ||
856 | |||
857 | /* Both gains should be below 0 dB */ | ||
858 | void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) | ||
859 | { | ||
860 | int32_t *c = crossfeed_data.coefs; | ||
861 | long scaler = get_replaygain_int(lf_gain * 10) << 7; | ||
862 | |||
863 | cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff; | ||
864 | hf_gain -= lf_gain; | ||
865 | /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB | ||
866 | * point instead of shelf midpoint. This is for compatibility with the old | ||
867 | * crossfeed shelf filter and should be removed if crossfeed settings are | ||
868 | * ever made incompatible for any other good reason. | ||
869 | */ | ||
870 | cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24); | ||
871 | filter_shelf_coefs(cutoff, hf_gain, false, c); | ||
872 | /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains | ||
873 | * over 1 and can do this safely | ||
874 | */ | ||
875 | c[0] = FRACMUL_SHL(c[0], scaler, 4); | ||
876 | c[1] = FRACMUL_SHL(c[1], scaler, 4); | ||
877 | c[2] <<= 4; | ||
878 | } | ||
879 | |||
880 | /* Apply a constant gain to the samples (e.g., for ReplayGain). | ||
881 | * Note that this must be called before the resampler. | ||
882 | */ | ||
883 | #ifndef DSP_HAVE_ASM_APPLY_GAIN | ||
884 | static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) | ||
885 | { | ||
886 | const int32_t gain = data->gain; | ||
887 | int ch; | ||
888 | |||
889 | for (ch = 0; ch < data->num_channels; ch++) | ||
890 | { | ||
891 | int32_t *d = buf[ch]; | ||
892 | int i; | ||
893 | |||
894 | for (i = 0; i < count; i++) | ||
895 | d[i] = FRACMUL_SHL(d[i], gain, 8); | ||
896 | } | ||
897 | } | ||
898 | #endif /* DSP_HAVE_ASM_APPLY_GAIN */ | ||
899 | |||
900 | /* Combine all gains to a global gain. */ | ||
901 | static void set_gain(struct dsp_config *dsp) | ||
902 | { | ||
903 | /* gains are in S7.24 format */ | ||
904 | dsp->data.gain = DEFAULT_GAIN; | ||
905 | |||
906 | /* Replay gain not relevant to voice */ | ||
907 | if (dsp == &AUDIO_DSP && replaygain) | ||
908 | { | ||
909 | dsp->data.gain = replaygain; | ||
910 | } | ||
911 | |||
912 | if (dsp->eq_process && eq_precut) | ||
913 | { | ||
914 | dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24); | ||
915 | } | ||
916 | |||
917 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
918 | if (global_settings.volume < SW_VOLUME_MAX || | ||
919 | global_settings.volume > SW_VOLUME_MIN) | ||
920 | { | ||
921 | int vol_gain = get_replaygain_int(global_settings.volume * 100); | ||
922 | dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24); | ||
923 | } | ||
924 | #endif | ||
925 | |||
926 | if (dsp->data.gain == DEFAULT_GAIN) | ||
927 | { | ||
928 | dsp->data.gain = 0; | ||
929 | } | ||
930 | else | ||
931 | { | ||
932 | dsp->data.gain >>= 1; /* convert gain to S8.23 format */ | ||
933 | } | ||
934 | |||
935 | dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL; | ||
936 | } | ||
937 | |||
938 | /** | ||
939 | * Update the amount to cut the audio before applying the equalizer. | ||
940 | * | ||
941 | * @param precut to apply in decibels (multiplied by 10) | ||
942 | */ | ||
943 | void dsp_set_eq_precut(int precut) | ||
944 | { | ||
945 | eq_precut = get_replaygain_int(precut * -10); | ||
946 | set_gain(&AUDIO_DSP); | ||
947 | } | ||
948 | |||
949 | /** | ||
950 | * Synchronize the equalizer filter coefficients with the global settings. | ||
951 | * | ||
952 | * @param band the equalizer band to synchronize | ||
953 | */ | ||
954 | void dsp_set_eq_coefs(int band, int cutoff, int q, int gain) | ||
955 | { | ||
956 | /* Convert user settings to format required by coef generator functions */ | ||
957 | cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff; | ||
958 | |||
959 | if (q == 0) | ||
960 | q = 1; | ||
961 | |||
962 | /* NOTE: The coef functions assume the EMAC unit is in fractional mode, | ||
963 | which it should be, since we're executed from the main thread. */ | ||
964 | |||
965 | /* Assume a band is disabled if the gain is zero */ | ||
966 | if (gain == 0) | ||
967 | { | ||
968 | eq_data.enabled[band] = 0; | ||
969 | } | ||
970 | else | ||
971 | { | ||
972 | if (band == 0) | ||
973 | eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
974 | else if (band == 4) | ||
975 | eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
976 | else | ||
977 | eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
978 | |||
979 | eq_data.enabled[band] = 1; | ||
980 | } | ||
981 | } | ||
982 | |||
983 | /* Apply EQ filters to those bands that have got it switched on. */ | ||
984 | static void eq_process(int count, int32_t *buf[]) | ||
985 | { | ||
986 | static const int shifts[] = | ||
987 | { | ||
988 | EQ_SHELF_SHIFT, /* low shelf */ | ||
989 | EQ_PEAK_SHIFT, /* peaking */ | ||
990 | EQ_PEAK_SHIFT, /* peaking */ | ||
991 | EQ_PEAK_SHIFT, /* peaking */ | ||
992 | EQ_SHELF_SHIFT, /* high shelf */ | ||
993 | }; | ||
994 | unsigned int channels = AUDIO_DSP.data.num_channels; | ||
995 | int i; | ||
996 | |||
997 | /* filter configuration currently is 1 low shelf filter, 3 band peaking | ||
998 | filters and 1 high shelf filter, in that order. we need to know this | ||
999 | so we can choose the correct shift factor. | ||
1000 | */ | ||
1001 | for (i = 0; i < 5; i++) | ||
1002 | { | ||
1003 | if (!eq_data.enabled[i]) | ||
1004 | continue; | ||
1005 | eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]); | ||
1006 | } | ||
1007 | } | ||
1008 | |||
1009 | /** | ||
1010 | * Use to enable the equalizer. | ||
1011 | * | ||
1012 | * @param enable true to enable the equalizer | ||
1013 | */ | ||
1014 | void dsp_set_eq(bool enable) | ||
1015 | { | ||
1016 | AUDIO_DSP.eq_process = enable ? eq_process : NULL; | ||
1017 | set_gain(&AUDIO_DSP); | ||
1018 | } | ||
1019 | |||
1020 | static void dsp_set_stereo_width(int value) | ||
1021 | { | ||
1022 | long width, straight, cross; | ||
1023 | |||
1024 | width = value * 0x7fffff / 100; | ||
1025 | |||
1026 | if (value <= 100) | ||
1027 | { | ||
1028 | straight = (0x7fffff + width) / 2; | ||
1029 | cross = straight - width; | ||
1030 | } | ||
1031 | else | ||
1032 | { | ||
1033 | /* straight = (1 + width) / (2 * width) */ | ||
1034 | straight = ((int64_t)(0x7fffff + width) << 22) / width; | ||
1035 | cross = straight - 0x7fffff; | ||
1036 | } | ||
1037 | |||
1038 | dsp_sw_gain = straight << 8; | ||
1039 | dsp_sw_cross = cross << 8; | ||
1040 | } | ||
1041 | |||
1042 | /** | ||
1043 | * Implements the different channel configurations and stereo width. | ||
1044 | */ | ||
1045 | |||
1046 | /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for | ||
1047 | * completeness. */ | ||
1048 | #if 0 | ||
1049 | static void channels_process_sound_chan_stereo(int count, int32_t *buf[]) | ||
1050 | { | ||
1051 | /* The channels are each just themselves */ | ||
1052 | (void)count; (void)buf; | ||
1053 | } | ||
1054 | #endif | ||
1055 | |||
1056 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO | ||
1057 | static void channels_process_sound_chan_mono(int count, int32_t *buf[]) | ||
1058 | { | ||
1059 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1060 | |||
1061 | while (count-- > 0) | ||
1062 | { | ||
1063 | int32_t lr = *sl/2 + *sr/2; | ||
1064 | *sl++ = lr; | ||
1065 | *sr++ = lr; | ||
1066 | } | ||
1067 | } | ||
1068 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */ | ||
1069 | |||
1070 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM | ||
1071 | static void channels_process_sound_chan_custom(int count, int32_t *buf[]) | ||
1072 | { | ||
1073 | const int32_t gain = dsp_sw_gain; | ||
1074 | const int32_t cross = dsp_sw_cross; | ||
1075 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1076 | |||
1077 | while (count-- > 0) | ||
1078 | { | ||
1079 | int32_t l = *sl; | ||
1080 | int32_t r = *sr; | ||
1081 | *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross); | ||
1082 | *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross); | ||
1083 | } | ||
1084 | } | ||
1085 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */ | ||
1086 | |||
1087 | static void channels_process_sound_chan_mono_left(int count, int32_t *buf[]) | ||
1088 | { | ||
1089 | /* Just copy over the other channel */ | ||
1090 | memcpy(buf[1], buf[0], count * sizeof (*buf)); | ||
1091 | } | ||
1092 | |||
1093 | static void channels_process_sound_chan_mono_right(int count, int32_t *buf[]) | ||
1094 | { | ||
1095 | /* Just copy over the other channel */ | ||
1096 | memcpy(buf[0], buf[1], count * sizeof (*buf)); | ||
1097 | } | ||
1098 | |||
1099 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE | ||
1100 | static void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) | ||
1101 | { | ||
1102 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1103 | |||
1104 | while (count-- > 0) | ||
1105 | { | ||
1106 | int32_t ch = *sl/2 - *sr/2; | ||
1107 | *sl++ = ch; | ||
1108 | *sr++ = -ch; | ||
1109 | } | ||
1110 | } | ||
1111 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */ | ||
1112 | |||
1113 | static void dsp_set_channel_config(int value) | ||
1114 | { | ||
1115 | static const channels_process_fn_type channels_process_functions[] = | ||
1116 | { | ||
1117 | /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */ | ||
1118 | [SOUND_CHAN_STEREO] = NULL, | ||
1119 | [SOUND_CHAN_MONO] = channels_process_sound_chan_mono, | ||
1120 | [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom, | ||
1121 | [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left, | ||
1122 | [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right, | ||
1123 | [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke, | ||
1124 | }; | ||
1125 | |||
1126 | if ((unsigned)value >= ARRAYLEN(channels_process_functions) || | ||
1127 | AUDIO_DSP.stereo_mode == STEREO_MONO) | ||
1128 | { | ||
1129 | value = SOUND_CHAN_STEREO; | ||
1130 | } | ||
1131 | |||
1132 | /* This doesn't apply to voice */ | ||
1133 | channels_mode = value; | ||
1134 | AUDIO_DSP.channels_process = channels_process_functions[value]; | ||
1135 | } | ||
1136 | |||
1137 | #if CONFIG_CODEC == SWCODEC | ||
1138 | |||
1139 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1140 | static void set_tone_controls(void) | ||
1141 | { | ||
1142 | filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200, | ||
1143 | 0xffffffff/NATIVE_FREQUENCY*3500, | ||
1144 | bass, treble, -prescale, | ||
1145 | AUDIO_DSP.tone_filter.coefs); | ||
1146 | /* Sync the voice dsp coefficients */ | ||
1147 | memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs, | ||
1148 | sizeof (VOICE_DSP.tone_filter.coefs)); | ||
1149 | } | ||
1150 | #endif | ||
1151 | |||
1152 | /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/ | ||
1153 | * code directly. | ||
1154 | */ | ||
1155 | int dsp_callback(int msg, intptr_t param) | ||
1156 | { | ||
1157 | switch (msg) | ||
1158 | { | ||
1159 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1160 | case DSP_CALLBACK_SET_PRESCALE: | ||
1161 | prescale = param; | ||
1162 | set_tone_controls(); | ||
1163 | break; | ||
1164 | /* prescaler is always set after calling any of these, so we wait with | ||
1165 | * calculating coefs until the above case is hit. | ||
1166 | */ | ||
1167 | case DSP_CALLBACK_SET_BASS: | ||
1168 | bass = param; | ||
1169 | break; | ||
1170 | case DSP_CALLBACK_SET_TREBLE: | ||
1171 | treble = param; | ||
1172 | break; | ||
1173 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
1174 | case DSP_CALLBACK_SET_SW_VOLUME: | ||
1175 | set_gain(&AUDIO_DSP); | ||
1176 | break; | ||
1177 | #endif | ||
1178 | #endif | ||
1179 | case DSP_CALLBACK_SET_CHANNEL_CONFIG: | ||
1180 | dsp_set_channel_config(param); | ||
1181 | break; | ||
1182 | case DSP_CALLBACK_SET_STEREO_WIDTH: | ||
1183 | dsp_set_stereo_width(param); | ||
1184 | break; | ||
1185 | default: | ||
1186 | break; | ||
1187 | } | ||
1188 | return 0; | ||
1189 | } | ||
1190 | #endif | ||
1191 | |||
1192 | /* Process and convert src audio to dst based on the DSP configuration, | ||
1193 | * reading count number of audio samples. dst is assumed to be large | ||
1194 | * enough; use dsp_output_count() to get the required number. src is an | ||
1195 | * array of pointers; for mono and interleaved stereo, it contains one | ||
1196 | * pointer to the start of the audio data and the other is ignored; for | ||
1197 | * non-interleaved stereo, it contains two pointers, one for each audio | ||
1198 | * channel. Returns number of bytes written to dst. | ||
1199 | */ | ||
1200 | int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count) | ||
1201 | { | ||
1202 | static int32_t *tmp[2]; /* tdspeed_doit() needs it static */ | ||
1203 | static long last_yield; | ||
1204 | long tick; | ||
1205 | int written = 0; | ||
1206 | |||
1207 | #if defined(CPU_COLDFIRE) | ||
1208 | /* set emac unit for dsp processing, and save old macsr, we're running in | ||
1209 | codec thread context at this point, so can't clobber it */ | ||
1210 | unsigned long old_macsr = coldfire_get_macsr(); | ||
1211 | coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE); | ||
1212 | #endif | ||
1213 | |||
1214 | if (new_gain) | ||
1215 | dsp_set_replaygain(); /* Gain has changed */ | ||
1216 | |||
1217 | /* Perform at least one yield before starting */ | ||
1218 | last_yield = current_tick; | ||
1219 | yield(); | ||
1220 | |||
1221 | /* Testing function pointers for NULL is preferred since the pointer | ||
1222 | will be preloaded to be used for the call if not. */ | ||
1223 | while (count > 0) | ||
1224 | { | ||
1225 | int samples = MIN(sample_buf_count, count); | ||
1226 | count -= samples; | ||
1227 | |||
1228 | dsp->input_samples(samples, src, tmp); | ||
1229 | |||
1230 | #ifdef HAVE_PITCHSCREEN | ||
1231 | if (dsp->tdspeed_active) | ||
1232 | samples = tdspeed_doit(tmp, samples); | ||
1233 | #endif | ||
1234 | |||
1235 | int chunk_offset = 0; | ||
1236 | while (samples > 0) | ||
1237 | { | ||
1238 | int32_t *t2[2]; | ||
1239 | t2[0] = tmp[0]+chunk_offset; | ||
1240 | t2[1] = tmp[1]+chunk_offset; | ||
1241 | |||
1242 | int chunk = MIN(sample_buf_count, samples); | ||
1243 | chunk_offset += chunk; | ||
1244 | samples -= chunk; | ||
1245 | |||
1246 | if (dsp->apply_gain) | ||
1247 | dsp->apply_gain(chunk, &dsp->data, t2); | ||
1248 | |||
1249 | if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0) | ||
1250 | break; /* I'm pretty sure we're downsampling here */ | ||
1251 | |||
1252 | if (dsp->apply_crossfeed) | ||
1253 | dsp->apply_crossfeed(chunk, t2); | ||
1254 | |||
1255 | if (dsp->eq_process) | ||
1256 | dsp->eq_process(chunk, t2); | ||
1257 | |||
1258 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1259 | if ((bass | treble) != 0) | ||
1260 | eq_filter(t2, &dsp->tone_filter, chunk, | ||
1261 | dsp->data.num_channels, FILTER_BISHELF_SHIFT); | ||
1262 | #endif | ||
1263 | |||
1264 | if (dsp->channels_process) | ||
1265 | dsp->channels_process(chunk, t2); | ||
1266 | |||
1267 | if (dsp->compressor_process) | ||
1268 | dsp->compressor_process(chunk, &dsp->data, t2); | ||
1269 | |||
1270 | dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst); | ||
1271 | |||
1272 | written += chunk; | ||
1273 | dst += chunk * sizeof (int16_t) * 2; | ||
1274 | |||
1275 | /* yield at least once each tick */ | ||
1276 | tick = current_tick; | ||
1277 | if (TIME_AFTER(tick, last_yield)) | ||
1278 | { | ||
1279 | last_yield = tick; | ||
1280 | yield(); | ||
1281 | } | ||
1282 | } | ||
1283 | } | ||
1284 | |||
1285 | #if defined(CPU_COLDFIRE) | ||
1286 | /* set old macsr again */ | ||
1287 | coldfire_set_macsr(old_macsr); | ||
1288 | #endif | ||
1289 | return written; | ||
1290 | } | ||
1291 | |||
1292 | /* Given count number of input samples, calculate the maximum number of | ||
1293 | * samples of output data that would be generated (the calculation is not | ||
1294 | * entirely exact and rounds upwards to be on the safe side; during | ||
1295 | * resampling, the number of samples generated depends on the current state | ||
1296 | * of the resampler). | ||
1297 | */ | ||
1298 | /* dsp_input_size MUST be called afterwards */ | ||
1299 | int dsp_output_count(struct dsp_config *dsp, int count) | ||
1300 | { | ||
1301 | #ifdef HAVE_PITCHSCREEN | ||
1302 | if (dsp->tdspeed_active) | ||
1303 | count = tdspeed_est_output_size(); | ||
1304 | #endif | ||
1305 | if (dsp->resample) | ||
1306 | { | ||
1307 | count = (int)(((unsigned long)count * NATIVE_FREQUENCY | ||
1308 | + (dsp->frequency - 1)) / dsp->frequency); | ||
1309 | } | ||
1310 | |||
1311 | /* Now we have the resampled sample count which must not exceed | ||
1312 | * resample_buf_count to avoid resample buffer overflow. One | ||
1313 | * must call dsp_input_count() to get the correct input sample | ||
1314 | * count. | ||
1315 | */ | ||
1316 | if (count > resample_buf_count) | ||
1317 | count = resample_buf_count; | ||
1318 | |||
1319 | return count; | ||
1320 | } | ||
1321 | |||
1322 | /* Given count output samples, calculate number of input samples | ||
1323 | * that would be consumed in order to fill the output buffer. | ||
1324 | */ | ||
1325 | int dsp_input_count(struct dsp_config *dsp, int count) | ||
1326 | { | ||
1327 | /* count is now the number of resampled input samples. Convert to | ||
1328 | original input samples. */ | ||
1329 | if (dsp->resample) | ||
1330 | { | ||
1331 | /* Use the real resampling delta = | ||
1332 | * dsp->frequency * 65536 / NATIVE_FREQUENCY, and | ||
1333 | * round towards zero to avoid buffer overflows. */ | ||
1334 | count = (int)(((unsigned long)count * | ||
1335 | dsp->data.resample_data.delta) >> 16); | ||
1336 | } | ||
1337 | |||
1338 | #ifdef HAVE_PITCHSCREEN | ||
1339 | if (dsp->tdspeed_active) | ||
1340 | count = tdspeed_est_input_size(count); | ||
1341 | #endif | ||
1342 | |||
1343 | return count; | ||
1344 | } | ||
1345 | |||
1346 | static void dsp_set_gain_var(long *var, long value) | ||
1347 | { | ||
1348 | *var = value; | ||
1349 | new_gain = true; | ||
1350 | } | ||
1351 | |||
1352 | static void dsp_update_functions(struct dsp_config *dsp) | ||
1353 | { | ||
1354 | sample_input_new_format(dsp); | ||
1355 | sample_output_new_format(dsp); | ||
1356 | if (dsp == &AUDIO_DSP) | ||
1357 | dsp_set_crossfeed(crossfeed_enabled); | ||
1358 | } | ||
1359 | |||
1360 | intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value) | ||
1361 | { | ||
1362 | switch (setting) | ||
1363 | { | ||
1364 | case DSP_MYDSP: | ||
1365 | switch (value) | ||
1366 | { | ||
1367 | case CODEC_IDX_AUDIO: | ||
1368 | return (intptr_t)&AUDIO_DSP; | ||
1369 | case CODEC_IDX_VOICE: | ||
1370 | return (intptr_t)&VOICE_DSP; | ||
1371 | default: | ||
1372 | return (intptr_t)NULL; | ||
1373 | } | ||
1374 | |||
1375 | case DSP_SET_FREQUENCY: | ||
1376 | memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data)); | ||
1377 | /* Fall through!!! */ | ||
1378 | case DSP_SWITCH_FREQUENCY: | ||
1379 | dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value; | ||
1380 | /* Account for playback speed adjustment when setting dsp->frequency | ||
1381 | if we're called from the main audio thread. Voice UI thread should | ||
1382 | not need this feature. | ||
1383 | */ | ||
1384 | #ifdef HAVE_PITCHSCREEN | ||
1385 | if (dsp == &AUDIO_DSP) | ||
1386 | dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100; | ||
1387 | else | ||
1388 | #endif | ||
1389 | dsp->frequency = dsp->codec_frequency; | ||
1390 | |||
1391 | resampler_new_delta(dsp); | ||
1392 | #ifdef HAVE_PITCHSCREEN | ||
1393 | tdspeed_setup(dsp); | ||
1394 | #endif | ||
1395 | break; | ||
1396 | |||
1397 | case DSP_SET_SAMPLE_DEPTH: | ||
1398 | dsp->sample_depth = value; | ||
1399 | |||
1400 | if (dsp->sample_depth <= NATIVE_DEPTH) | ||
1401 | { | ||
1402 | dsp->data.frac_bits = WORD_FRACBITS; | ||
1403 | dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */ | ||
1404 | dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); | ||
1405 | dsp->data.clip_min = -((1 << WORD_FRACBITS)); | ||
1406 | } | ||
1407 | else | ||
1408 | { | ||
1409 | dsp->data.frac_bits = value; | ||
1410 | dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */ | ||
1411 | dsp->data.clip_max = (1 << value) - 1; | ||
1412 | dsp->data.clip_min = -(1 << value); | ||
1413 | } | ||
1414 | |||
1415 | dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH; | ||
1416 | sample_input_new_format(dsp); | ||
1417 | dither_init(dsp); | ||
1418 | break; | ||
1419 | |||
1420 | case DSP_SET_STEREO_MODE: | ||
1421 | dsp->stereo_mode = value; | ||
1422 | dsp->data.num_channels = value == STEREO_MONO ? 1 : 2; | ||
1423 | dsp_update_functions(dsp); | ||
1424 | #ifdef HAVE_PITCHSCREEN | ||
1425 | tdspeed_setup(dsp); | ||
1426 | #endif | ||
1427 | break; | ||
1428 | |||
1429 | case DSP_RESET: | ||
1430 | dsp->stereo_mode = STEREO_NONINTERLEAVED; | ||
1431 | dsp->data.num_channels = 2; | ||
1432 | dsp->sample_depth = NATIVE_DEPTH; | ||
1433 | dsp->data.frac_bits = WORD_FRACBITS; | ||
1434 | dsp->sample_bytes = sizeof (int16_t); | ||
1435 | dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH; | ||
1436 | dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); | ||
1437 | dsp->data.clip_min = -((1 << WORD_FRACBITS)); | ||
1438 | dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY; | ||
1439 | |||
1440 | if (dsp == &AUDIO_DSP) | ||
1441 | { | ||
1442 | track_gain = 0; | ||
1443 | album_gain = 0; | ||
1444 | track_peak = 0; | ||
1445 | album_peak = 0; | ||
1446 | new_gain = true; | ||
1447 | } | ||
1448 | |||
1449 | dsp_update_functions(dsp); | ||
1450 | resampler_new_delta(dsp); | ||
1451 | #ifdef HAVE_PITCHSCREEN | ||
1452 | tdspeed_setup(dsp); | ||
1453 | #endif | ||
1454 | if (dsp == &AUDIO_DSP) | ||
1455 | compressor_reset(); | ||
1456 | break; | ||
1457 | |||
1458 | case DSP_FLUSH: | ||
1459 | memset(&dsp->data.resample_data, 0, | ||
1460 | sizeof (dsp->data.resample_data)); | ||
1461 | resampler_new_delta(dsp); | ||
1462 | dither_init(dsp); | ||
1463 | #ifdef HAVE_PITCHSCREEN | ||
1464 | tdspeed_setup(dsp); | ||
1465 | #endif | ||
1466 | if (dsp == &AUDIO_DSP) | ||
1467 | compressor_reset(); | ||
1468 | break; | ||
1469 | |||
1470 | case DSP_SET_TRACK_GAIN: | ||
1471 | if (dsp == &AUDIO_DSP) | ||
1472 | dsp_set_gain_var(&track_gain, value); | ||
1473 | break; | ||
1474 | |||
1475 | case DSP_SET_ALBUM_GAIN: | ||
1476 | if (dsp == &AUDIO_DSP) | ||
1477 | dsp_set_gain_var(&album_gain, value); | ||
1478 | break; | ||
1479 | |||
1480 | case DSP_SET_TRACK_PEAK: | ||
1481 | if (dsp == &AUDIO_DSP) | ||
1482 | dsp_set_gain_var(&track_peak, value); | ||
1483 | break; | ||
1484 | |||
1485 | case DSP_SET_ALBUM_PEAK: | ||
1486 | if (dsp == &AUDIO_DSP) | ||
1487 | dsp_set_gain_var(&album_peak, value); | ||
1488 | break; | ||
1489 | |||
1490 | default: | ||
1491 | return 0; | ||
1492 | } | ||
1493 | |||
1494 | return 1; | ||
1495 | } | ||
1496 | |||
1497 | int get_replaygain_mode(bool have_track_gain, bool have_album_gain) | ||
1498 | { | ||
1499 | int type; | ||
1500 | |||
1501 | bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK) | ||
1502 | || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE) | ||
1503 | && global_settings.playlist_shuffle)); | ||
1504 | |||
1505 | type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM | ||
1506 | : have_track_gain ? REPLAYGAIN_TRACK : -1; | ||
1507 | |||
1508 | return type; | ||
1509 | } | ||
1510 | |||
1511 | void dsp_set_replaygain(void) | ||
1512 | { | ||
1513 | long gain = 0; | ||
1514 | |||
1515 | new_gain = false; | ||
1516 | |||
1517 | if ((global_settings.replaygain_type != REPLAYGAIN_OFF) || | ||
1518 | global_settings.replaygain_noclip) | ||
1519 | { | ||
1520 | bool track_mode = get_replaygain_mode(track_gain != 0, | ||
1521 | album_gain != 0) == REPLAYGAIN_TRACK; | ||
1522 | long peak = (track_mode || !album_peak) ? track_peak : album_peak; | ||
1523 | |||
1524 | if (global_settings.replaygain_type != REPLAYGAIN_OFF) | ||
1525 | { | ||
1526 | gain = (track_mode || !album_gain) ? track_gain : album_gain; | ||
1527 | |||
1528 | if (global_settings.replaygain_preamp) | ||
1529 | { | ||
1530 | long preamp = get_replaygain_int( | ||
1531 | global_settings.replaygain_preamp * 10); | ||
1532 | |||
1533 | gain = (long) (((int64_t) gain * preamp) >> 24); | ||
1534 | } | ||
1535 | } | ||
1536 | |||
1537 | if (gain == 0) | ||
1538 | { | ||
1539 | /* So that noclip can work even with no gain information. */ | ||
1540 | gain = DEFAULT_GAIN; | ||
1541 | } | ||
1542 | |||
1543 | if (global_settings.replaygain_noclip && (peak != 0) | ||
1544 | && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN)) | ||
1545 | { | ||
1546 | gain = (((int64_t) DEFAULT_GAIN << 24) / peak); | ||
1547 | } | ||
1548 | |||
1549 | if (gain == DEFAULT_GAIN) | ||
1550 | { | ||
1551 | /* Nothing to do, disable processing. */ | ||
1552 | gain = 0; | ||
1553 | } | ||
1554 | } | ||
1555 | |||
1556 | /* Store in S7.24 format to simplify calculations. */ | ||
1557 | replaygain = gain; | ||
1558 | set_gain(&AUDIO_DSP); | ||
1559 | } | ||
1560 | |||
1561 | /** SET COMPRESSOR | ||
1562 | * Called by the menu system to configure the compressor process */ | ||
1563 | void dsp_set_compressor(const struct compressor_settings *settings) | ||
1564 | { | ||
1565 | /* enable/disable the compressor */ | ||
1566 | AUDIO_DSP.compressor_process = compressor_update(settings) ? | ||
1567 | compressor_process : NULL; | ||
1568 | } | ||