Fix errors and warnings.
[kugel-rb.git] / apps / dsp.c
blob3cff1918d77e7b0576ee9543b27bf2c05d985a26
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2005 Miika Pekkarinen
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.
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
20 ****************************************************************************/
21 #include "config.h"
22 #include <stdbool.h>
23 #include <inttypes.h>
24 #include <string.h>
25 #include <sound.h>
26 #include "dsp.h"
27 #include "eq.h"
28 #include "kernel.h"
29 #include "system.h"
30 #include "settings.h"
31 #include "replaygain.h"
32 #include "tdspeed.h"
33 #include "buffer.h"
34 #include "fixedpoint.h"
35 #include "fracmul.h"
37 /* Define LOGF_ENABLE to enable logf output in this file */
38 /*#define LOGF_ENABLE*/
39 #include "logf.h"
41 /* 16-bit samples are scaled based on these constants. The shift should be
42 * no more than 15.
44 #define WORD_SHIFT 12
45 #define WORD_FRACBITS 27
47 #define NATIVE_DEPTH 16
48 /* If the small buffer size changes, check the assembly code! */
49 #define SMALL_SAMPLE_BUF_COUNT 256
50 #define DEFAULT_GAIN 0x01000000
52 /* enums to index conversion properly with stereo mode and other settings */
53 enum
55 SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED,
56 SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED,
57 SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO,
58 SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES,
59 SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES,
60 SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES,
61 SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES
64 enum
66 SAMPLE_OUTPUT_MONO = 0,
67 SAMPLE_OUTPUT_STEREO,
68 SAMPLE_OUTPUT_DITHERED_MONO,
69 SAMPLE_OUTPUT_DITHERED_STEREO
72 /****************************************************************************
73 * NOTE: Any assembly routines that use these structures must be updated
74 * if current data members are moved or changed.
76 struct resample_data
78 uint32_t delta; /* 00h */
79 uint32_t phase; /* 04h */
80 int32_t last_sample[2]; /* 08h */
81 /* 10h */
84 /* This is for passing needed data to assembly dsp routines. If another
85 * dsp parameter needs to be passed, add to the end of the structure
86 * and remove from dsp_config.
87 * If another function type becomes assembly optimized and requires dsp
88 * config info, add a pointer paramter of type "struct dsp_data *".
89 * If removing something from other than the end, reserve the spot or
90 * else update every implementation for every target.
91 * Be sure to add the offset of the new member for easy viewing as well. :)
92 * It is the first member of dsp_config and all members can be accessesed
93 * through the main aggregate but this is intended to make a safe haven
94 * for these items whereas the c part can be rearranged at will. dsp_data
95 * could even moved within dsp_config without disurbing the order.
97 struct dsp_data
99 int output_scale; /* 00h */
100 int num_channels; /* 04h */
101 struct resample_data resample_data; /* 08h */
102 int32_t clip_min; /* 18h */
103 int32_t clip_max; /* 1ch */
104 int32_t gain; /* 20h - Note that this is in S8.23 format. */
105 /* 24h */
108 /* No asm...yet */
109 struct dither_data
111 long error[3]; /* 00h */
112 long random; /* 0ch */
113 /* 10h */
116 struct crossfeed_data
118 int32_t gain; /* 00h - Direct path gain */
119 int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */
120 int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */
121 int32_t delay[13][2]; /* 20h */
122 int32_t *index; /* 88h - Current pointer into the delay line */
123 /* 8ch */
126 /* Current setup is one lowshelf filters three peaking filters and one
127 * highshelf filter. Varying the number of shelving filters make no sense,
128 * but adding peaking filters is possible.
130 struct eq_state
132 char enabled[5]; /* 00h - Flags for active filters */
133 struct eqfilter filters[5]; /* 08h - packing is 4? */
134 /* 10ch */
137 struct compressor_menu
139 int threshold; /* dB - from menu */
140 bool auto_gain; /* 0 = off, 1 = auto */
141 int ratio; /* from menu */
142 bool soft_knee; /* 0 = hard knee, 1 = soft knee */
143 int release; /* samples - from menu */
146 /* Include header with defines which functions are implemented in assembly
147 code for the target */
148 #include <dsp_asm.h>
150 /* Typedefs keep things much neater in this case */
151 typedef void (*sample_input_fn_type)(int count, const char *src[],
152 int32_t *dst[]);
153 typedef int (*resample_fn_type)(int count, struct dsp_data *data,
154 const int32_t *src[], int32_t *dst[]);
155 typedef void (*sample_output_fn_type)(int count, struct dsp_data *data,
156 const int32_t *src[], int16_t *dst);
158 /* Single-DSP channel processing in place */
159 typedef void (*channels_process_fn_type)(int count, int32_t *buf[]);
160 /* DSP local channel processing in place */
161 typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data,
162 int32_t *buf[]);
165 ***************************************************************************/
167 struct dsp_config
169 struct dsp_data data; /* Config members for use in asm routines */
170 long codec_frequency; /* Sample rate of data coming from the codec */
171 long frequency; /* Effective sample rate after pitch shift (if any) */
172 int sample_depth;
173 int sample_bytes;
174 int stereo_mode;
175 int32_t tdspeed_percent; /* Speed% * PITCH_SPEED_PRECISION */
176 #ifdef HAVE_PITCHSCREEN
177 bool tdspeed_active; /* Timestretch is in use */
178 #endif
179 int frac_bits;
180 #ifdef HAVE_SW_TONE_CONTROLS
181 /* Filter struct for software bass/treble controls */
182 struct eqfilter tone_filter;
183 #endif
184 /* Functions that change depending upon settings - NULL if stage is
185 disabled */
186 sample_input_fn_type input_samples;
187 resample_fn_type resample;
188 sample_output_fn_type output_samples;
189 /* These will be NULL for the voice codec and is more economical that
190 way */
191 channels_process_dsp_fn_type apply_gain;
192 channels_process_fn_type apply_crossfeed;
193 channels_process_fn_type eq_process;
194 channels_process_fn_type channels_process;
195 channels_process_fn_type compressor_process;
198 /* General DSP config */
199 static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */
200 /* Dithering */
201 static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */
202 static long dither_mask IBSS_ATTR;
203 static long dither_bias IBSS_ATTR;
204 /* Crossfeed */
205 struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */
207 .index = (int32_t *)crossfeed_data.delay
210 /* Equalizer */
211 static struct eq_state eq_data; /* A */
213 /* Software tone controls */
214 #ifdef HAVE_SW_TONE_CONTROLS
215 static int prescale; /* A/V */
216 static int bass; /* A/V */
217 static int treble; /* A/V */
218 #endif
220 /* Settings applicable to audio codec only */
221 #ifdef HAVE_PITCHSCREEN
222 static int32_t pitch_ratio = PITCH_SPEED_100;
223 #endif
224 static int channels_mode;
225 long dsp_sw_gain;
226 long dsp_sw_cross;
227 static bool dither_enabled;
228 static long eq_precut;
229 static long track_gain;
230 static bool new_gain;
231 static long album_gain;
232 static long track_peak;
233 static long album_peak;
234 static long replaygain;
235 static bool crossfeed_enabled;
237 #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
238 #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
240 /* The internal format is 32-bit samples, non-interleaved, stereo. This
241 * format is similar to the raw output from several codecs, so the amount
242 * of copying needed is minimized for that case.
245 #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
247 static int32_t small_sample_buf[SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR;
248 static int32_t small_resample_buf[SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO] IBSS_ATTR;
250 #ifdef HAVE_PITCHSCREEN
251 static int32_t *big_sample_buf = NULL;
252 static int32_t *big_resample_buf = NULL;
253 static int big_sample_buf_count = -1; /* -1=unknown, 0=not available */
254 #endif
256 static int sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
257 static int32_t *sample_buf = small_sample_buf;
258 static int32_t *resample_buf = small_resample_buf;
260 #define SAMPLE_BUF_LEFT_CHANNEL 0
261 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2)
262 #define RESAMPLE_BUF_LEFT_CHANNEL 0
263 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO)
265 /* compressor */
266 static struct compressor_menu c_menu;
267 static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */
268 static int32_t comp_makeup_gain IBSS_ATTR; /* S7.24 format */
269 static int32_t comp_curve[66] IBSS_ATTR; /* S7.24 format */
270 static int32_t release_gain IBSS_ATTR; /* S7.24 format */
271 #define UNITY (1L << 24) /* unity gain in S7.24 format */
272 static void compressor_process(int count, int32_t *buf[]);
275 /* Clip sample to signed 16 bit range */
276 static inline int32_t clip_sample_16(int32_t sample)
278 if ((int16_t)sample != sample)
279 sample = 0x7fff ^ (sample >> 31);
280 return sample;
283 #ifdef HAVE_PITCHSCREEN
284 int32_t sound_get_pitch(void)
286 return pitch_ratio;
289 void sound_set_pitch(int32_t percent)
291 pitch_ratio = percent;
292 dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY,
293 AUDIO_DSP.codec_frequency);
296 static void tdspeed_setup(struct dsp_config *dspc)
298 /* Assume timestretch will not be used */
299 dspc->tdspeed_active = false;
300 sample_buf = small_sample_buf;
301 resample_buf = small_resample_buf;
302 sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
304 if(!dsp_timestretch_available())
305 return; /* Timestretch not enabled or buffer not allocated */
306 if (dspc->tdspeed_percent == 0)
307 dspc->tdspeed_percent = PITCH_SPEED_100;
308 if (!tdspeed_config(
309 dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency,
310 dspc->stereo_mode != STEREO_MONO,
311 dspc->tdspeed_percent))
312 return; /* Timestretch not possible or needed with these parameters */
314 /* Timestretch is to be used */
315 dspc->tdspeed_active = true;
316 sample_buf = big_sample_buf;
317 sample_buf_count = big_sample_buf_count;
318 resample_buf = big_resample_buf;
321 void dsp_timestretch_enable(bool enabled)
323 /* Hook to set up timestretch buffer on first call to settings_apply() */
324 if (big_sample_buf_count < 0) /* Only do something on first call */
326 if (enabled)
328 /* Set up timestretch buffers */
329 big_sample_buf_count = SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO;
330 big_sample_buf = small_resample_buf;
331 big_resample_buf = (int32_t *) buffer_alloc(big_sample_buf_count * RESAMPLE_RATIO * sizeof(int32_t));
333 else
335 /* Not enabled at startup, "big" buffers will never be available */
336 big_sample_buf_count = 0;
338 tdspeed_setup(&AUDIO_DSP);
342 void dsp_set_timestretch(int32_t percent)
344 AUDIO_DSP.tdspeed_percent = percent;
345 tdspeed_setup(&AUDIO_DSP);
348 int32_t dsp_get_timestretch()
350 return AUDIO_DSP.tdspeed_percent;
353 bool dsp_timestretch_available()
355 return (global_settings.timestretch_enabled && big_sample_buf_count > 0);
357 #endif
359 /* Convert count samples to the internal format, if needed. Updates src
360 * to point past the samples "consumed" and dst is set to point to the
361 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
362 * is no point in processing the same data twice.
365 /* convert count 16-bit mono to 32-bit mono */
366 static void sample_input_lte_native_mono(
367 int count, const char *src[], int32_t *dst[])
369 const int16_t *s = (int16_t *) src[0];
370 const int16_t * const send = s + count;
371 int32_t *d = dst[0] = dst[1] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
372 int scale = WORD_SHIFT;
374 while (s < send)
376 *d++ = *s++ << scale;
379 src[0] = (char *)s;
382 /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
383 static void sample_input_lte_native_i_stereo(
384 int count, const char *src[], int32_t *dst[])
386 const int32_t *s = (int32_t *) src[0];
387 const int32_t * const send = s + count;
388 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
389 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
390 int scale = WORD_SHIFT;
392 while (s < send)
394 int32_t slr = *s++;
395 #ifdef ROCKBOX_LITTLE_ENDIAN
396 *dl++ = (slr >> 16) << scale;
397 *dr++ = (int32_t)(int16_t)slr << scale;
398 #else /* ROCKBOX_BIG_ENDIAN */
399 *dl++ = (int32_t)(int16_t)slr << scale;
400 *dr++ = (slr >> 16) << scale;
401 #endif
404 src[0] = (char *)s;
407 /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
408 static void sample_input_lte_native_ni_stereo(
409 int count, const char *src[], int32_t *dst[])
411 const int16_t *sl = (int16_t *) src[0];
412 const int16_t *sr = (int16_t *) src[1];
413 const int16_t * const slend = sl + count;
414 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
415 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
416 int scale = WORD_SHIFT;
418 while (sl < slend)
420 *dl++ = *sl++ << scale;
421 *dr++ = *sr++ << scale;
424 src[0] = (char *)sl;
425 src[1] = (char *)sr;
428 /* convert count 32-bit mono to 32-bit mono */
429 static void sample_input_gt_native_mono(
430 int count, const char *src[], int32_t *dst[])
432 dst[0] = dst[1] = (int32_t *)src[0];
433 src[0] = (char *)(dst[0] + count);
436 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
437 static void sample_input_gt_native_i_stereo(
438 int count, const char *src[], int32_t *dst[])
440 const int32_t *s = (int32_t *)src[0];
441 const int32_t * const send = s + 2*count;
442 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
443 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
445 while (s < send)
447 *dl++ = *s++;
448 *dr++ = *s++;
451 src[0] = (char *)send;
454 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
455 static void sample_input_gt_native_ni_stereo(
456 int count, const char *src[], int32_t *dst[])
458 dst[0] = (int32_t *)src[0];
459 dst[1] = (int32_t *)src[1];
460 src[0] = (char *)(dst[0] + count);
461 src[1] = (char *)(dst[1] + count);
465 * sample_input_new_format()
467 * set the to-native sample conversion function based on dsp sample parameters
469 * !DSPPARAMSYNC
470 * needs syncing with changes to the following dsp parameters:
471 * * dsp->stereo_mode (A/V)
472 * * dsp->sample_depth (A/V)
474 static void sample_input_new_format(struct dsp_config *dsp)
476 static const sample_input_fn_type sample_input_functions[] =
478 [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo,
479 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo,
480 [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono,
481 [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo,
482 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo,
483 [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono,
486 int convert = dsp->stereo_mode;
488 if (dsp->sample_depth > NATIVE_DEPTH)
489 convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX;
491 dsp->input_samples = sample_input_functions[convert];
495 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
496 /* write mono internal format to output format */
497 static void sample_output_mono(int count, struct dsp_data *data,
498 const int32_t *src[], int16_t *dst)
500 const int32_t *s0 = src[0];
501 const int scale = data->output_scale;
502 const int dc_bias = 1 << (scale - 1);
504 while (count-- > 0)
506 int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale);
507 *dst++ = lr;
508 *dst++ = lr;
511 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
513 /* write stereo internal format to output format */
514 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
515 static void sample_output_stereo(int count, struct dsp_data *data,
516 const int32_t *src[], int16_t *dst)
518 const int32_t *s0 = src[0];
519 const int32_t *s1 = src[1];
520 const int scale = data->output_scale;
521 const int dc_bias = 1 << (scale - 1);
523 while (count-- > 0)
525 *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale);
526 *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale);
529 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
532 * The "dither" code to convert the 24-bit samples produced by libmad was
533 * taken from the coolplayer project - coolplayer.sourceforge.net
535 * This function handles mono and stereo outputs.
537 static void sample_output_dithered(int count, struct dsp_data *data,
538 const int32_t *src[], int16_t *dst)
540 const int32_t mask = dither_mask;
541 const int32_t bias = dither_bias;
542 const int scale = data->output_scale;
543 const int32_t min = data->clip_min;
544 const int32_t max = data->clip_max;
545 const int32_t range = max - min;
546 int ch;
547 int16_t *d;
549 for (ch = 0; ch < data->num_channels; ch++)
551 struct dither_data * const dither = &dither_data[ch];
552 const int32_t *s = src[ch];
553 int i;
555 for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2)
557 int32_t output, sample;
558 int32_t random;
560 /* Noise shape and bias (for correct rounding later) */
561 sample = *s;
562 sample += dither->error[0] - dither->error[1] + dither->error[2];
563 dither->error[2] = dither->error[1];
564 dither->error[1] = dither->error[0]/2;
566 output = sample + bias;
568 /* Dither, highpass triangle PDF */
569 random = dither->random*0x0019660dL + 0x3c6ef35fL;
570 output += (random & mask) - (dither->random & mask);
571 dither->random = random;
573 /* Round sample to output range */
574 output &= ~mask;
576 /* Error feedback */
577 dither->error[0] = sample - output;
579 /* Clip */
580 if ((uint32_t)(output - min) > (uint32_t)range)
582 int32_t c = min;
583 if (output > min)
584 c += range;
585 output = c;
588 /* Quantize and store */
589 *d = output >> scale;
593 if (data->num_channels == 2)
594 return;
596 /* Have to duplicate left samples into the right channel since
597 pcm buffer and hardware is interleaved stereo */
598 d = &dst[0];
600 while (count-- > 0)
602 int16_t s = *d++;
603 *d++ = s;
608 * sample_output_new_format()
610 * set the from-native to ouput sample conversion routine
612 * !DSPPARAMSYNC
613 * needs syncing with changes to the following dsp parameters:
614 * * dsp->stereo_mode (A/V)
615 * * dither_enabled (A)
617 static void sample_output_new_format(struct dsp_config *dsp)
619 static const sample_output_fn_type sample_output_functions[] =
621 sample_output_mono,
622 sample_output_stereo,
623 sample_output_dithered,
624 sample_output_dithered
627 int out = dsp->data.num_channels - 1;
629 if (dsp == &AUDIO_DSP && dither_enabled)
630 out += 2;
632 dsp->output_samples = sample_output_functions[out];
636 * Linear interpolation resampling that introduces a one sample delay because
637 * of our inability to look into the future at the end of a frame.
639 #ifndef DSP_HAVE_ASM_RESAMPLING
640 static int dsp_downsample(int count, struct dsp_data *data,
641 const int32_t *src[], int32_t *dst[])
643 int ch = data->num_channels - 1;
644 uint32_t delta = data->resample_data.delta;
645 uint32_t phase, pos;
646 int32_t *d;
648 /* Rolled channel loop actually showed slightly faster. */
651 /* Just initialize things and not worry too much about the relatively
652 * uncommon case of not being able to spit out a sample for the frame.
654 const int32_t *s = src[ch];
655 int32_t last = data->resample_data.last_sample[ch];
657 data->resample_data.last_sample[ch] = s[count - 1];
658 d = dst[ch];
659 phase = data->resample_data.phase;
660 pos = phase >> 16;
662 /* Do we need last sample of previous frame for interpolation? */
663 if (pos > 0)
664 last = s[pos - 1];
666 while (pos < (uint32_t)count)
668 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
669 phase += delta;
670 pos = phase >> 16;
671 last = s[pos - 1];
674 while (--ch >= 0);
676 /* Wrap phase accumulator back to start of next frame. */
677 data->resample_data.phase = phase - (count << 16);
678 return d - dst[0];
681 static int dsp_upsample(int count, struct dsp_data *data,
682 const int32_t *src[], int32_t *dst[])
684 int ch = data->num_channels - 1;
685 uint32_t delta = data->resample_data.delta;
686 uint32_t phase, pos;
687 int32_t *d;
689 /* Rolled channel loop actually showed slightly faster. */
692 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
693 const int32_t *s = src[ch];
694 int32_t last = data->resample_data.last_sample[ch];
696 data->resample_data.last_sample[ch] = s[count - 1];
697 d = dst[ch];
698 phase = data->resample_data.phase;
699 pos = phase >> 16;
701 while (pos == 0)
703 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last);
704 phase += delta;
705 pos = phase >> 16;
708 while (pos < (uint32_t)count)
710 last = s[pos - 1];
711 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
712 phase += delta;
713 pos = phase >> 16;
716 while (--ch >= 0);
718 /* Wrap phase accumulator back to start of next frame. */
719 data->resample_data.phase = phase & 0xffff;
720 return d - dst[0];
722 #endif /* DSP_HAVE_ASM_RESAMPLING */
724 static void resampler_new_delta(struct dsp_config *dsp)
726 dsp->data.resample_data.delta = (unsigned long)
727 dsp->frequency * 65536LL / NATIVE_FREQUENCY;
729 if (dsp->frequency == NATIVE_FREQUENCY)
731 /* NOTE: If fully glitch-free transistions from no resampling to
732 resampling are desired, last_sample history should be maintained
733 even when not resampling. */
734 dsp->resample = NULL;
735 dsp->data.resample_data.phase = 0;
736 dsp->data.resample_data.last_sample[0] = 0;
737 dsp->data.resample_data.last_sample[1] = 0;
739 else if (dsp->frequency < NATIVE_FREQUENCY)
740 dsp->resample = dsp_upsample;
741 else
742 dsp->resample = dsp_downsample;
745 /* Resample count stereo samples. Updates the src array, if resampling is
746 * done, to refer to the resampled data. Returns number of stereo samples
747 * for further processing.
749 static inline int resample(struct dsp_config *dsp, int count, int32_t *src[])
751 int32_t *dst[2] =
753 &resample_buf[RESAMPLE_BUF_LEFT_CHANNEL],
754 &resample_buf[RESAMPLE_BUF_RIGHT_CHANNEL],
757 count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst);
759 src[0] = dst[0];
760 src[1] = dst[dsp->data.num_channels - 1];
762 return count;
765 static void dither_init(struct dsp_config *dsp)
767 memset(dither_data, 0, sizeof (dither_data));
768 dither_bias = (1L << (dsp->frac_bits - NATIVE_DEPTH));
769 dither_mask = (1L << (dsp->frac_bits + 1 - NATIVE_DEPTH)) - 1;
772 void dsp_dither_enable(bool enable)
774 struct dsp_config *dsp = &AUDIO_DSP;
775 dither_enabled = enable;
776 sample_output_new_format(dsp);
779 /* Applies crossfeed to the stereo signal in src.
780 * Crossfeed is a process where listening over speakers is simulated. This
781 * is good for old hard panned stereo records, which might be quite fatiguing
782 * to listen to on headphones with no crossfeed.
784 #ifndef DSP_HAVE_ASM_CROSSFEED
785 static void apply_crossfeed(int count, int32_t *buf[])
787 int32_t *hist_l = &crossfeed_data.history[0];
788 int32_t *hist_r = &crossfeed_data.history[2];
789 int32_t *delay = &crossfeed_data.delay[0][0];
790 int32_t *coefs = &crossfeed_data.coefs[0];
791 int32_t gain = crossfeed_data.gain;
792 int32_t *di = crossfeed_data.index;
794 int32_t acc;
795 int32_t left, right;
796 int i;
798 for (i = 0; i < count; i++)
800 left = buf[0][i];
801 right = buf[1][i];
803 /* Filter delayed sample from left speaker */
804 acc = FRACMUL(*di, coefs[0]);
805 acc += FRACMUL(hist_l[0], coefs[1]);
806 acc += FRACMUL(hist_l[1], coefs[2]);
807 /* Save filter history for left speaker */
808 hist_l[1] = acc;
809 hist_l[0] = *di;
810 *di++ = left;
811 /* Filter delayed sample from right speaker */
812 acc = FRACMUL(*di, coefs[0]);
813 acc += FRACMUL(hist_r[0], coefs[1]);
814 acc += FRACMUL(hist_r[1], coefs[2]);
815 /* Save filter history for right speaker */
816 hist_r[1] = acc;
817 hist_r[0] = *di;
818 *di++ = right;
819 /* Now add the attenuated direct sound and write to outputs */
820 buf[0][i] = FRACMUL(left, gain) + hist_r[1];
821 buf[1][i] = FRACMUL(right, gain) + hist_l[1];
823 /* Wrap delay line index if bigger than delay line size */
824 if (di >= delay + 13*2)
825 di = delay;
827 /* Write back local copies of data we've modified */
828 crossfeed_data.index = di;
830 #endif /* DSP_HAVE_ASM_CROSSFEED */
833 * dsp_set_crossfeed(bool enable)
835 * !DSPPARAMSYNC
836 * needs syncing with changes to the following dsp parameters:
837 * * dsp->stereo_mode (A)
839 void dsp_set_crossfeed(bool enable)
841 crossfeed_enabled = enable;
842 AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1)
843 ? apply_crossfeed : NULL;
846 void dsp_set_crossfeed_direct_gain(int gain)
848 crossfeed_data.gain = get_replaygain_int(gain * 10) << 7;
849 /* If gain is negative, the calculation overflowed and we need to clamp */
850 if (crossfeed_data.gain < 0)
851 crossfeed_data.gain = 0x7fffffff;
854 /* Both gains should be below 0 dB */
855 void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
857 int32_t *c = crossfeed_data.coefs;
858 long scaler = get_replaygain_int(lf_gain * 10) << 7;
860 cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff;
861 hf_gain -= lf_gain;
862 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
863 * point instead of shelf midpoint. This is for compatibility with the old
864 * crossfeed shelf filter and should be removed if crossfeed settings are
865 * ever made incompatible for any other good reason.
867 cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
868 filter_shelf_coefs(cutoff, hf_gain, false, c);
869 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
870 * over 1 and can do this safely
872 c[0] = FRACMUL_SHL(c[0], scaler, 4);
873 c[1] = FRACMUL_SHL(c[1], scaler, 4);
874 c[2] <<= 4;
877 /* Apply a constant gain to the samples (e.g., for ReplayGain).
878 * Note that this must be called before the resampler.
880 #ifndef DSP_HAVE_ASM_APPLY_GAIN
881 static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
883 const int32_t gain = data->gain;
884 int ch;
886 for (ch = 0; ch < data->num_channels; ch++)
888 int32_t *d = buf[ch];
889 int i;
891 for (i = 0; i < count; i++)
892 d[i] = FRACMUL_SHL(d[i], gain, 8);
895 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
897 /* Combine all gains to a global gain. */
898 static void set_gain(struct dsp_config *dsp)
900 /* gains are in S7.24 format */
901 dsp->data.gain = DEFAULT_GAIN;
903 /* Replay gain not relevant to voice */
904 if (dsp == &AUDIO_DSP && replaygain)
906 dsp->data.gain = replaygain;
909 if (dsp->eq_process && eq_precut)
911 dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24);
914 #ifdef HAVE_SW_VOLUME_CONTROL
915 if (global_settings.volume < SW_VOLUME_MAX ||
916 global_settings.volume > SW_VOLUME_MIN)
918 int vol_gain = get_replaygain_int(global_settings.volume * 100);
919 dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24);
921 #endif
923 if (dsp->data.gain == DEFAULT_GAIN)
925 dsp->data.gain = 0;
927 else
929 dsp->data.gain >>= 1; /* convert gain to S8.23 format */
932 dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL;
936 * Update the amount to cut the audio before applying the equalizer.
938 * @param precut to apply in decibels (multiplied by 10)
940 void dsp_set_eq_precut(int precut)
942 eq_precut = get_replaygain_int(precut * -10);
943 set_gain(&AUDIO_DSP);
947 * Synchronize the equalizer filter coefficients with the global settings.
949 * @param band the equalizer band to synchronize
951 void dsp_set_eq_coefs(int band)
953 const int *setting;
954 long gain;
955 unsigned long cutoff, q;
957 /* Adjust setting pointer to the band we actually want to change */
958 setting = &global_settings.eq_band0_cutoff + (band * 3);
960 /* Convert user settings to format required by coef generator functions */
961 cutoff = 0xffffffff / NATIVE_FREQUENCY * (*setting++);
962 q = *setting++;
963 gain = *setting++;
965 if (q == 0)
966 q = 1;
968 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
969 which it should be, since we're executed from the main thread. */
971 /* Assume a band is disabled if the gain is zero */
972 if (gain == 0)
974 eq_data.enabled[band] = 0;
976 else
978 if (band == 0)
979 eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
980 else if (band == 4)
981 eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
982 else
983 eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
985 eq_data.enabled[band] = 1;
989 /* Apply EQ filters to those bands that have got it switched on. */
990 static void eq_process(int count, int32_t *buf[])
992 static const int shifts[] =
994 EQ_SHELF_SHIFT, /* low shelf */
995 EQ_PEAK_SHIFT, /* peaking */
996 EQ_PEAK_SHIFT, /* peaking */
997 EQ_PEAK_SHIFT, /* peaking */
998 EQ_SHELF_SHIFT, /* high shelf */
1000 unsigned int channels = AUDIO_DSP.data.num_channels;
1001 int i;
1003 /* filter configuration currently is 1 low shelf filter, 3 band peaking
1004 filters and 1 high shelf filter, in that order. we need to know this
1005 so we can choose the correct shift factor.
1007 for (i = 0; i < 5; i++)
1009 if (!eq_data.enabled[i])
1010 continue;
1011 eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]);
1016 * Use to enable the equalizer.
1018 * @param enable true to enable the equalizer
1020 void dsp_set_eq(bool enable)
1022 AUDIO_DSP.eq_process = enable ? eq_process : NULL;
1023 set_gain(&AUDIO_DSP);
1026 static void dsp_set_stereo_width(int value)
1028 long width, straight, cross;
1030 width = value * 0x7fffff / 100;
1032 if (value <= 100)
1034 straight = (0x7fffff + width) / 2;
1035 cross = straight - width;
1037 else
1039 /* straight = (1 + width) / (2 * width) */
1040 straight = ((int64_t)(0x7fffff + width) << 22) / width;
1041 cross = straight - 0x7fffff;
1044 dsp_sw_gain = straight << 8;
1045 dsp_sw_cross = cross << 8;
1049 * Implements the different channel configurations and stereo width.
1052 /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1053 * completeness. */
1054 #if 0
1055 static void channels_process_sound_chan_stereo(int count, int32_t *buf[])
1057 /* The channels are each just themselves */
1058 (void)count; (void)buf;
1060 #endif
1062 #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1063 static void channels_process_sound_chan_mono(int count, int32_t *buf[])
1065 int32_t *sl = buf[0], *sr = buf[1];
1067 while (count-- > 0)
1069 int32_t lr = *sl/2 + *sr/2;
1070 *sl++ = lr;
1071 *sr++ = lr;
1074 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1076 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1077 static void channels_process_sound_chan_custom(int count, int32_t *buf[])
1079 const int32_t gain = dsp_sw_gain;
1080 const int32_t cross = dsp_sw_cross;
1081 int32_t *sl = buf[0], *sr = buf[1];
1083 while (count-- > 0)
1085 int32_t l = *sl;
1086 int32_t r = *sr;
1087 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
1088 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
1091 #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1093 static void channels_process_sound_chan_mono_left(int count, int32_t *buf[])
1095 /* Just copy over the other channel */
1096 memcpy(buf[1], buf[0], count * sizeof (*buf));
1099 static void channels_process_sound_chan_mono_right(int count, int32_t *buf[])
1101 /* Just copy over the other channel */
1102 memcpy(buf[0], buf[1], count * sizeof (*buf));
1105 #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1106 static void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
1108 int32_t *sl = buf[0], *sr = buf[1];
1110 while (count-- > 0)
1112 int32_t ch = *sl/2 - *sr/2;
1113 *sl++ = ch;
1114 *sr++ = -ch;
1117 #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1119 static void dsp_set_channel_config(int value)
1121 static const channels_process_fn_type channels_process_functions[] =
1123 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1124 [SOUND_CHAN_STEREO] = NULL,
1125 [SOUND_CHAN_MONO] = channels_process_sound_chan_mono,
1126 [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom,
1127 [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left,
1128 [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right,
1129 [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke,
1132 if ((unsigned)value >= ARRAYLEN(channels_process_functions) ||
1133 AUDIO_DSP.stereo_mode == STEREO_MONO)
1135 value = SOUND_CHAN_STEREO;
1138 /* This doesn't apply to voice */
1139 channels_mode = value;
1140 AUDIO_DSP.channels_process = channels_process_functions[value];
1143 #if CONFIG_CODEC == SWCODEC
1145 #ifdef HAVE_SW_TONE_CONTROLS
1146 static void set_tone_controls(void)
1148 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
1149 0xffffffff/NATIVE_FREQUENCY*3500,
1150 bass, treble, -prescale,
1151 AUDIO_DSP.tone_filter.coefs);
1152 /* Sync the voice dsp coefficients */
1153 memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs,
1154 sizeof (VOICE_DSP.tone_filter.coefs));
1156 #endif
1158 /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1159 * code directly.
1161 int dsp_callback(int msg, intptr_t param)
1163 switch (msg)
1165 #ifdef HAVE_SW_TONE_CONTROLS
1166 case DSP_CALLBACK_SET_PRESCALE:
1167 prescale = param;
1168 set_tone_controls();
1169 break;
1170 /* prescaler is always set after calling any of these, so we wait with
1171 * calculating coefs until the above case is hit.
1173 case DSP_CALLBACK_SET_BASS:
1174 bass = param;
1175 break;
1176 case DSP_CALLBACK_SET_TREBLE:
1177 treble = param;
1178 break;
1179 #ifdef HAVE_SW_VOLUME_CONTROL
1180 case DSP_CALLBACK_SET_SW_VOLUME:
1181 set_gain(&AUDIO_DSP);
1182 break;
1183 #endif
1184 #endif
1185 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
1186 dsp_set_channel_config(param);
1187 break;
1188 case DSP_CALLBACK_SET_STEREO_WIDTH:
1189 dsp_set_stereo_width(param);
1190 break;
1191 default:
1192 break;
1194 return 0;
1196 #endif
1198 /* Process and convert src audio to dst based on the DSP configuration,
1199 * reading count number of audio samples. dst is assumed to be large
1200 * enough; use dsp_output_count() to get the required number. src is an
1201 * array of pointers; for mono and interleaved stereo, it contains one
1202 * pointer to the start of the audio data and the other is ignored; for
1203 * non-interleaved stereo, it contains two pointers, one for each audio
1204 * channel. Returns number of bytes written to dst.
1206 int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count)
1208 int32_t *tmp[2];
1209 static long last_yield;
1210 long tick;
1211 int written = 0;
1213 #if defined(CPU_COLDFIRE)
1214 /* set emac unit for dsp processing, and save old macsr, we're running in
1215 codec thread context at this point, so can't clobber it */
1216 unsigned long old_macsr = coldfire_get_macsr();
1217 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
1218 #endif
1220 if (new_gain)
1221 dsp_set_replaygain(); /* Gain has changed */
1223 /* Perform at least one yield before starting */
1224 last_yield = current_tick;
1225 yield();
1227 /* Testing function pointers for NULL is preferred since the pointer
1228 will be preloaded to be used for the call if not. */
1229 while (count > 0)
1231 int samples = MIN(sample_buf_count/2, count);
1232 count -= samples;
1234 dsp->input_samples(samples, src, tmp);
1236 #ifdef HAVE_PITCHSCREEN
1237 if (dsp->tdspeed_active)
1238 samples = tdspeed_doit(tmp, samples);
1239 #endif
1241 int chunk_offset = 0;
1242 while (samples > 0)
1244 int32_t *t2[2];
1245 t2[0] = tmp[0]+chunk_offset;
1246 t2[1] = tmp[1]+chunk_offset;
1248 int chunk = MIN(sample_buf_count/2, samples);
1249 chunk_offset += chunk;
1250 samples -= chunk;
1252 if (dsp->apply_gain)
1253 dsp->apply_gain(chunk, &dsp->data, t2);
1255 if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0)
1256 break; /* I'm pretty sure we're downsampling here */
1258 if (dsp->apply_crossfeed)
1259 dsp->apply_crossfeed(chunk, t2);
1261 if (dsp->eq_process)
1262 dsp->eq_process(chunk, t2);
1264 #ifdef HAVE_SW_TONE_CONTROLS
1265 if ((bass | treble) != 0)
1266 eq_filter(t2, &dsp->tone_filter, chunk,
1267 dsp->data.num_channels, FILTER_BISHELF_SHIFT);
1268 #endif
1270 if (dsp->channels_process)
1271 dsp->channels_process(chunk, t2);
1273 if (dsp->compressor_process)
1274 dsp->compressor_process(chunk, t2);
1276 dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst);
1278 written += chunk;
1279 dst += chunk * sizeof (int16_t) * 2;
1281 /* yield at least once each tick */
1282 tick = current_tick;
1283 if (TIME_AFTER(tick, last_yield))
1285 last_yield = tick;
1286 yield();
1291 #if defined(CPU_COLDFIRE)
1292 /* set old macsr again */
1293 coldfire_set_macsr(old_macsr);
1294 #endif
1295 return written;
1298 /* Given count number of input samples, calculate the maximum number of
1299 * samples of output data that would be generated (the calculation is not
1300 * entirely exact and rounds upwards to be on the safe side; during
1301 * resampling, the number of samples generated depends on the current state
1302 * of the resampler).
1304 /* dsp_input_size MUST be called afterwards */
1305 int dsp_output_count(struct dsp_config *dsp, int count)
1307 #ifdef HAVE_PITCHSCREEN
1308 if (dsp->tdspeed_active)
1309 count = tdspeed_est_output_size();
1310 #endif
1311 if (dsp->resample)
1313 count = (int)(((unsigned long)count * NATIVE_FREQUENCY
1314 + (dsp->frequency - 1)) / dsp->frequency);
1317 /* Now we have the resampled sample count which must not exceed
1318 * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One
1319 * must call dsp_input_count() to get the correct input sample
1320 * count.
1322 if (count > RESAMPLE_BUF_RIGHT_CHANNEL)
1323 count = RESAMPLE_BUF_RIGHT_CHANNEL;
1325 return count;
1328 /* Given count output samples, calculate number of input samples
1329 * that would be consumed in order to fill the output buffer.
1331 int dsp_input_count(struct dsp_config *dsp, int count)
1333 /* count is now the number of resampled input samples. Convert to
1334 original input samples. */
1335 if (dsp->resample)
1337 /* Use the real resampling delta =
1338 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1339 * round towards zero to avoid buffer overflows. */
1340 count = (int)(((unsigned long)count *
1341 dsp->data.resample_data.delta) >> 16);
1344 #ifdef HAVE_PITCHSCREEN
1345 if (dsp->tdspeed_active)
1346 count = tdspeed_est_input_size(count);
1347 #endif
1349 return count;
1352 static void dsp_set_gain_var(long *var, long value)
1354 *var = value;
1355 new_gain = true;
1358 static void dsp_update_functions(struct dsp_config *dsp)
1360 sample_input_new_format(dsp);
1361 sample_output_new_format(dsp);
1362 if (dsp == &AUDIO_DSP)
1363 dsp_set_crossfeed(crossfeed_enabled);
1366 intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value)
1368 switch (setting)
1370 case DSP_MYDSP:
1371 switch (value)
1373 case CODEC_IDX_AUDIO:
1374 return (intptr_t)&AUDIO_DSP;
1375 case CODEC_IDX_VOICE:
1376 return (intptr_t)&VOICE_DSP;
1377 default:
1378 return (intptr_t)NULL;
1381 case DSP_SET_FREQUENCY:
1382 memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data));
1383 /* Fall through!!! */
1384 case DSP_SWITCH_FREQUENCY:
1385 dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value;
1386 /* Account for playback speed adjustment when setting dsp->frequency
1387 if we're called from the main audio thread. Voice UI thread should
1388 not need this feature.
1390 #ifdef HAVE_PITCHSCREEN
1391 if (dsp == &AUDIO_DSP)
1392 dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100;
1393 else
1394 #endif
1395 dsp->frequency = dsp->codec_frequency;
1397 resampler_new_delta(dsp);
1398 #ifdef HAVE_PITCHSCREEN
1399 tdspeed_setup(dsp);
1400 #endif
1401 break;
1403 case DSP_SET_SAMPLE_DEPTH:
1404 dsp->sample_depth = value;
1406 if (dsp->sample_depth <= NATIVE_DEPTH)
1408 dsp->frac_bits = WORD_FRACBITS;
1409 dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */
1410 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1411 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1413 else
1415 dsp->frac_bits = value;
1416 dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */
1417 dsp->data.clip_max = (1 << value) - 1;
1418 dsp->data.clip_min = -(1 << value);
1421 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1422 sample_input_new_format(dsp);
1423 dither_init(dsp);
1424 break;
1426 case DSP_SET_STEREO_MODE:
1427 dsp->stereo_mode = value;
1428 dsp->data.num_channels = value == STEREO_MONO ? 1 : 2;
1429 dsp_update_functions(dsp);
1430 #ifdef HAVE_PITCHSCREEN
1431 tdspeed_setup(dsp);
1432 #endif
1433 break;
1435 case DSP_RESET:
1436 dsp->stereo_mode = STEREO_NONINTERLEAVED;
1437 dsp->data.num_channels = 2;
1438 dsp->sample_depth = NATIVE_DEPTH;
1439 dsp->frac_bits = WORD_FRACBITS;
1440 dsp->sample_bytes = sizeof (int16_t);
1441 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1442 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1443 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1444 dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY;
1446 if (dsp == &AUDIO_DSP)
1448 track_gain = 0;
1449 album_gain = 0;
1450 track_peak = 0;
1451 album_peak = 0;
1452 new_gain = true;
1455 dsp_update_functions(dsp);
1456 resampler_new_delta(dsp);
1457 #ifdef HAVE_PITCHSCREEN
1458 tdspeed_setup(dsp);
1459 #endif
1460 if (dsp == &AUDIO_DSP)
1461 release_gain = UNITY;
1462 break;
1464 case DSP_FLUSH:
1465 memset(&dsp->data.resample_data, 0,
1466 sizeof (dsp->data.resample_data));
1467 resampler_new_delta(dsp);
1468 dither_init(dsp);
1469 #ifdef HAVE_PITCHSCREEN
1470 tdspeed_setup(dsp);
1471 #endif
1472 if (dsp == &AUDIO_DSP)
1473 release_gain = UNITY;
1474 break;
1476 case DSP_SET_TRACK_GAIN:
1477 if (dsp == &AUDIO_DSP)
1478 dsp_set_gain_var(&track_gain, value);
1479 break;
1481 case DSP_SET_ALBUM_GAIN:
1482 if (dsp == &AUDIO_DSP)
1483 dsp_set_gain_var(&album_gain, value);
1484 break;
1486 case DSP_SET_TRACK_PEAK:
1487 if (dsp == &AUDIO_DSP)
1488 dsp_set_gain_var(&track_peak, value);
1489 break;
1491 case DSP_SET_ALBUM_PEAK:
1492 if (dsp == &AUDIO_DSP)
1493 dsp_set_gain_var(&album_peak, value);
1494 break;
1496 default:
1497 return 0;
1500 return 1;
1503 int get_replaygain_mode(bool have_track_gain, bool have_album_gain)
1505 int type;
1507 bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK)
1508 || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE)
1509 && global_settings.playlist_shuffle));
1511 type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM
1512 : have_track_gain ? REPLAYGAIN_TRACK : -1;
1514 return type;
1517 void dsp_set_replaygain(void)
1519 long gain = 0;
1521 new_gain = false;
1523 if ((global_settings.replaygain_type != REPLAYGAIN_OFF) ||
1524 global_settings.replaygain_noclip)
1526 bool track_mode = get_replaygain_mode(track_gain != 0,
1527 album_gain != 0) == REPLAYGAIN_TRACK;
1528 long peak = (track_mode || !album_peak) ? track_peak : album_peak;
1530 if (global_settings.replaygain_type != REPLAYGAIN_OFF)
1532 gain = (track_mode || !album_gain) ? track_gain : album_gain;
1534 if (global_settings.replaygain_preamp)
1536 long preamp = get_replaygain_int(
1537 global_settings.replaygain_preamp * 10);
1539 gain = (long) (((int64_t) gain * preamp) >> 24);
1543 if (gain == 0)
1545 /* So that noclip can work even with no gain information. */
1546 gain = DEFAULT_GAIN;
1549 if (global_settings.replaygain_noclip && (peak != 0)
1550 && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN))
1552 gain = (((int64_t) DEFAULT_GAIN << 24) / peak);
1555 if (gain == DEFAULT_GAIN)
1557 /* Nothing to do, disable processing. */
1558 gain = 0;
1562 /* Store in S7.24 format to simplify calculations. */
1563 replaygain = gain;
1564 set_gain(&AUDIO_DSP);
1567 /** SET COMPRESSOR
1568 * Called by the menu system to configure the compressor process */
1569 void dsp_set_compressor(int c_threshold, int c_gain, int c_ratio,
1570 int c_knee, int c_release)
1572 bool changed = false;
1573 bool active = (c_threshold < 0);
1574 bool new_auto_gain = (c_gain == 1);
1575 const int comp_ratio[] = {2, 4, 6, 10, 0};
1576 int new_ratio = comp_ratio[c_ratio];
1577 bool new_knee = (c_knee == 1);
1578 int new_release = c_release * NATIVE_FREQUENCY / 1000;
1580 if (c_menu.threshold != c_threshold)
1582 changed = true;
1583 c_menu.threshold = c_threshold;
1584 logf(" Compressor Threshold: %d dB\tEnabled: %s",
1585 c_menu.threshold, active ? "Yes" : "No");
1588 if (c_menu.auto_gain != new_auto_gain)
1590 changed = true;
1591 c_menu.auto_gain = new_auto_gain;
1592 logf(" Compressor Makeup Gain: %s",
1593 c_menu.auto_gain ? "Auto" : "Off");
1596 if (c_menu.ratio != new_ratio)
1598 changed = true;
1599 c_menu.ratio = new_ratio;
1600 if (c_menu.ratio)
1601 { logf(" Compressor Ratio: %d:1", c_menu.ratio); }
1602 else
1603 { logf(" Compressor Ratio: Limit"); }
1606 if (c_menu.soft_knee != new_knee)
1608 changed = true;
1609 c_menu.soft_knee = new_knee;
1610 logf(" Compressor Knee: %s", c_menu.soft_knee==1?"Soft":"Hard");
1613 if (c_menu.release != new_release)
1615 changed = true;
1616 c_menu.release = new_release;
1617 logf(" Compressor Release: %d", c_menu.release);
1620 if (changed && active)
1622 /* configure variables for compressor operation */
1623 int i;
1624 const int32_t db[] ={0x000000, /* positive db equivalents in S15.16 format */
1625 0x241FA4, 0x1E1A5E, 0x1A94C8, 0x181518, 0x1624EA, 0x148F82, 0x1338BD, 0x120FD2,
1626 0x1109EB, 0x101FA4, 0x0F4BB6, 0x0E8A3C, 0x0DD840, 0x0D3377, 0x0C9A0E, 0x0C0A8C,
1627 0x0B83BE, 0x0B04A5, 0x0A8C6C, 0x0A1A5E, 0x09ADE1, 0x094670, 0x08E398, 0x0884F6,
1628 0x082A30, 0x07D2FA, 0x077F0F, 0x072E31, 0x06E02A, 0x0694C8, 0x064BDF, 0x060546,
1629 0x05C0DA, 0x057E78, 0x053E03, 0x04FF5F, 0x04C273, 0x048726, 0x044D64, 0x041518,
1630 0x03DE30, 0x03A89B, 0x037448, 0x03412A, 0x030F32, 0x02DE52, 0x02AE80, 0x027FB0,
1631 0x0251D6, 0x0224EA, 0x01F8E2, 0x01CDB4, 0x01A359, 0x0179C9, 0x0150FC, 0x0128EB,
1632 0x010190, 0x00DAE4, 0x00B4E1, 0x008F82, 0x006AC1, 0x004699, 0x002305};
1634 struct curve_point
1636 int32_t db; /* S15.16 format */
1637 int32_t offset; /* S15.16 format */
1638 } db_curve[5];
1640 /** Set up the shape of the compression curve first as decibel values*/
1641 /* db_curve[0] = bottom of knee
1642 [1] = threshold
1643 [2] = top of knee
1644 [3] = 0 db input
1645 [4] = ~+12db input (2 bits clipping overhead) */
1647 db_curve[1].db = c_menu.threshold << 16;
1648 if (c_menu.soft_knee)
1650 /* bottom of knee is 3dB below the threshold for soft knee*/
1651 db_curve[0].db = db_curve[1].db - (3 << 16);
1652 /* top of knee is 3dB above the threshold for soft knee */
1653 db_curve[2].db = db_curve[1].db + (3 << 16);
1654 if (c_menu.ratio)
1655 /* offset = -3db * (ratio - 1) / ratio */
1656 db_curve[2].offset = (int32_t)((long long)(-3 << 16)
1657 * (c_menu.ratio - 1) / c_menu.ratio);
1658 else
1659 /* offset = -3db for hard limit */
1660 db_curve[2].offset = (-3 << 16);
1662 else
1664 /* bottom of knee is at the threshold for hard knee */
1665 db_curve[0].db = c_menu.threshold << 16;
1666 /* top of knee is at the threshold for hard knee */
1667 db_curve[2].db = c_menu.threshold << 16;
1668 db_curve[2].offset = 0;
1671 /* Calculate 0db and ~+12db offsets */
1672 db_curve[4].db = 0xC0A8C; /* db of 2 bits clipping */
1673 if (c_menu.ratio)
1675 /* offset = threshold * (ratio - 1) / ratio */
1676 db_curve[3].offset = (int32_t)((long long)(c_menu.threshold << 16)
1677 * (c_menu.ratio - 1) / c_menu.ratio);
1678 db_curve[4].offset = (int32_t)((long long)-db_curve[4].db
1679 * (c_menu.ratio - 1) / c_menu.ratio) + db_curve[3].offset;
1681 else
1683 /* offset = threshold for hard limit */
1684 db_curve[3].offset = (c_menu.threshold << 16);
1685 db_curve[4].offset = -db_curve[4].db + db_curve[3].offset;
1688 /** Now set up the comp_curve table with compression offsets in the form
1689 of gain factors in S7.24 format */
1690 /* comp_curve[0] is 0 (-infinity db) input */
1691 comp_curve[0] = UNITY;
1692 /* comp_curve[1 to 63] are intermediate compression values corresponding
1693 to the 6 MSB of the input values of a non-clipped signal */
1694 for (i = 1; i < 64; i++)
1696 /* db constants are stored as positive numbers;
1697 make them negative here */
1698 int32_t this_db = -db[i];
1700 /* no compression below the knee */
1701 if (this_db <= db_curve[0].db)
1702 comp_curve[i] = UNITY;
1704 /* if soft knee and below top of knee,
1705 interpolate along soft knee slope */
1706 else if (c_menu.soft_knee && (this_db <= db_curve[2].db))
1707 comp_curve[i] = fp_factor(fp_mul(
1708 ((this_db - db_curve[0].db) / 6),
1709 db_curve[2].offset, 16), 16) << 8;
1711 /* interpolate along ratio slope above the knee */
1712 else
1713 comp_curve[i] = fp_factor(fp_mul(
1714 fp_div((db_curve[1].db - this_db), db_curve[1].db, 16),
1715 db_curve[3].offset, 16), 16) << 8;
1717 /* comp_curve[64] is the compression level of a maximum level,
1718 non-clipped signal */
1719 comp_curve[64] = fp_factor(db_curve[3].offset, 16) << 8;
1721 /* comp_curve[65] is the compression level of a maximum level,
1722 clipped signal */
1723 comp_curve[65] = fp_factor(db_curve[4].offset, 16) << 8;
1725 #if defined(ROCKBOX_HAS_LOGF) && defined(LOGF_ENABLE)
1726 logf("\n *** Compression Offsets ***");
1727 /* some settings for display only, not used in calculations */
1728 db_curve[0].offset = 0;
1729 db_curve[1].offset = 0;
1730 db_curve[3].db = 0;
1732 for (i = 0; i <= 4; i++)
1734 logf("Curve[%d]: db: % 6.2f\toffset: % 6.2f", i,
1735 (float)db_curve[i].db / (1 << 16),
1736 (float)db_curve[i].offset / (1 << 16));
1739 logf("\nGain factors:");
1740 for (i = 1; i <= 65; i++)
1742 debugf("%02d: %.6f ", i, (float)comp_curve[i] / UNITY);
1743 if (i % 4 == 0) debugf("\n");
1745 debugf("\n");
1746 #endif
1748 /* if using auto peak, then makeup gain is max offset - .1dB headroom */
1749 comp_makeup_gain = c_menu.auto_gain ?
1750 fp_factor(-(db_curve[3].offset) - 0x199A, 16) << 8 : UNITY;
1751 logf("Makeup gain:\t%.6f", (float)comp_makeup_gain / UNITY);
1753 /* calculate per-sample gain change a rate of 10db over release time */
1754 comp_rel_slope = 0xAF0BB2 / c_menu.release;
1755 logf("Release slope:\t%.6f", (float)comp_rel_slope / UNITY);
1757 release_gain = UNITY;
1760 /* enable/disable the compressor */
1761 AUDIO_DSP.compressor_process = active ? compressor_process : NULL;
1764 /** GET COMPRESSION GAIN
1765 * Returns the required gain factor in S7.24 format in order to compress the
1766 * sample in accordance with the compression curve. Always 1 or less.
1768 static inline int32_t get_compression_gain(int32_t sample)
1770 const int frac_bits_offset = AUDIO_DSP.frac_bits - 15;
1772 /* sample must be positive */
1773 if (sample < 0)
1774 sample = -(sample + 1);
1776 /* shift sample into 15 frac bit range */
1777 if (frac_bits_offset > 0)
1778 sample >>= frac_bits_offset;
1779 if (frac_bits_offset < 0)
1780 sample <<= -frac_bits_offset;
1782 /* normal case: sample isn't clipped */
1783 if (sample < (1 << 15))
1785 /* index is 6 MSB, rem is 9 LSB */
1786 int index = sample >> 9;
1787 int32_t rem = (sample & 0x1FF) << 22;
1789 /* interpolate from the compression curve:
1790 higher gain - ((rem / (1 << 31)) * (higher gain - lower gain)) */
1791 return comp_curve[index] - (FRACMUL(rem,
1792 (comp_curve[index] - comp_curve[index + 1])));
1794 /* sample is somewhat clipped, up to 2 bits of overhead */
1795 if (sample < (1 << 17))
1797 /* straight interpolation:
1798 higher gain - ((clipped portion of sample * 4/3
1799 / (1 << 31)) * (higher gain - lower gain)) */
1800 return comp_curve[64] - (FRACMUL(((sample - (1 << 15)) / 3) << 16,
1801 (comp_curve[64] - comp_curve[65])));
1804 /* sample is too clipped, return invalid value */
1805 return -1;
1808 /** COMPRESSOR PROCESS
1809 * Changes the gain of the samples according to the compressor curve
1811 static void compressor_process(int count, int32_t *buf[])
1813 const int num_chan = AUDIO_DSP.data.num_channels;
1814 int32_t *in_buf[2] = {buf[0], buf[1]};
1816 while (count-- > 0)
1818 int ch;
1819 /* use lowest (most compressed) gain factor of the output buffer
1820 sample pair for both samples (mono is also handled correctly here) */
1821 int32_t sample_gain = UNITY;
1822 for (ch = 0; ch < num_chan; ch++)
1824 int32_t this_gain = get_compression_gain(*in_buf[ch]);
1825 if (this_gain < sample_gain)
1826 sample_gain = this_gain;
1829 /* perform release slope; skip if no compression and no release slope */
1830 if ((sample_gain != UNITY) || (release_gain != UNITY))
1832 /* if larger offset than previous slope, start new release slope */
1833 if ((sample_gain <= release_gain) && (sample_gain > 0))
1835 release_gain = sample_gain;
1837 else
1838 /* keep sloping towards unity gain (and ignore invalid value) */
1840 release_gain += comp_rel_slope;
1841 if (release_gain > UNITY)
1843 release_gain = UNITY;
1848 /* total gain factor is the product of release gain and makeup gain,
1849 but avoid computation if possible */
1850 int32_t total_gain = ((release_gain == UNITY) ? comp_makeup_gain :
1851 (comp_makeup_gain == UNITY) ? release_gain :
1852 FRACMUL_SHL(release_gain, comp_makeup_gain, 7));
1854 /* Implement the compressor: apply total gain factor (if any) to the
1855 output buffer sample pair/mono sample */
1856 if (total_gain != UNITY)
1858 for (ch = 0; ch < num_chan; ch++)
1860 *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7);
1863 in_buf[0]++;
1864 in_buf[1]++;