fix yellow
[kugel-rb.git] / apps / dsp.c
blob72923282122a1ec383b3e57372748f0e75c36d15
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 bool tdspeed_active; /* Timestretch is in use */
177 int frac_bits;
178 #ifdef HAVE_SW_TONE_CONTROLS
179 /* Filter struct for software bass/treble controls */
180 struct eqfilter tone_filter;
181 #endif
182 /* Functions that change depending upon settings - NULL if stage is
183 disabled */
184 sample_input_fn_type input_samples;
185 resample_fn_type resample;
186 sample_output_fn_type output_samples;
187 /* These will be NULL for the voice codec and is more economical that
188 way */
189 channels_process_dsp_fn_type apply_gain;
190 channels_process_fn_type apply_crossfeed;
191 channels_process_fn_type eq_process;
192 channels_process_fn_type channels_process;
193 channels_process_fn_type compressor_process;
196 /* General DSP config */
197 static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */
198 /* Dithering */
199 static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */
200 static long dither_mask IBSS_ATTR;
201 static long dither_bias IBSS_ATTR;
202 /* Crossfeed */
203 struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */
205 .index = (int32_t *)crossfeed_data.delay
208 /* Equalizer */
209 static struct eq_state eq_data; /* A */
211 /* Software tone controls */
212 #ifdef HAVE_SW_TONE_CONTROLS
213 static int prescale; /* A/V */
214 static int bass; /* A/V */
215 static int treble; /* A/V */
216 #endif
218 /* Settings applicable to audio codec only */
219 static int32_t pitch_ratio = PITCH_SPEED_100;
220 static int channels_mode;
221 long dsp_sw_gain;
222 long dsp_sw_cross;
223 static bool dither_enabled;
224 static long eq_precut;
225 static long track_gain;
226 static bool new_gain;
227 static long album_gain;
228 static long track_peak;
229 static long album_peak;
230 static long replaygain;
231 static bool crossfeed_enabled;
233 #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
234 #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
236 /* The internal format is 32-bit samples, non-interleaved, stereo. This
237 * format is similar to the raw output from several codecs, so the amount
238 * of copying needed is minimized for that case.
241 #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
243 static int32_t small_sample_buf[SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR;
244 static int32_t small_resample_buf[SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO] IBSS_ATTR;
246 static int32_t *big_sample_buf = NULL;
247 static int32_t *big_resample_buf = NULL;
248 static int big_sample_buf_count = -1; /* -1=unknown, 0=not available */
250 static int sample_buf_count;
251 static int32_t *sample_buf;
252 static int32_t *resample_buf;
254 #define SAMPLE_BUF_LEFT_CHANNEL 0
255 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2)
256 #define RESAMPLE_BUF_LEFT_CHANNEL 0
257 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO)
259 /* compressor */
260 static struct compressor_menu c_menu;
261 static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */
262 static int32_t comp_makeup_gain IBSS_ATTR; /* S7.24 format */
263 static int32_t comp_curve[66] IBSS_ATTR; /* S7.24 format */
264 static int32_t release_gain IBSS_ATTR; /* S7.24 format */
265 #define UNITY (1L << 24) /* unity gain in S7.24 format */
266 static void compressor_process(int count, int32_t *buf[]);
269 /* Clip sample to signed 16 bit range */
270 static inline int32_t clip_sample_16(int32_t sample)
272 if ((int16_t)sample != sample)
273 sample = 0x7fff ^ (sample >> 31);
274 return sample;
277 int32_t sound_get_pitch(void)
279 return pitch_ratio;
282 void sound_set_pitch(int32_t percent)
284 pitch_ratio = percent;
285 dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY,
286 AUDIO_DSP.codec_frequency);
289 static void tdspeed_setup(struct dsp_config *dspc)
291 /* Assume timestretch will not be used */
292 dspc->tdspeed_active = false;
293 sample_buf = small_sample_buf;
294 resample_buf = small_resample_buf;
295 sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
297 if(!dsp_timestretch_available())
298 return; /* Timestretch not enabled or buffer not allocated */
299 if (dspc->tdspeed_percent == 0)
300 dspc->tdspeed_percent = PITCH_SPEED_100;
301 if (!tdspeed_config(
302 dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency,
303 dspc->stereo_mode != STEREO_MONO,
304 dspc->tdspeed_percent))
305 return; /* Timestretch not possible or needed with these parameters */
307 /* Timestretch is to be used */
308 dspc->tdspeed_active = true;
309 sample_buf = big_sample_buf;
310 sample_buf_count = big_sample_buf_count;
311 resample_buf = big_resample_buf;
314 void dsp_timestretch_enable(bool enabled)
316 /* Hook to set up timestretch buffer on first call to settings_apply() */
317 if (big_sample_buf_count < 0) /* Only do something on first call */
319 if (enabled)
321 /* Set up timestretch buffers */
322 big_sample_buf_count = SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO;
323 big_sample_buf = small_resample_buf;
324 big_resample_buf = (int32_t *) buffer_alloc(big_sample_buf_count * RESAMPLE_RATIO * sizeof(int32_t));
326 else
328 /* Not enabled at startup, "big" buffers will never be available */
329 big_sample_buf_count = 0;
331 tdspeed_setup(&AUDIO_DSP);
335 void dsp_set_timestretch(int32_t percent)
337 AUDIO_DSP.tdspeed_percent = percent;
338 tdspeed_setup(&AUDIO_DSP);
341 int32_t dsp_get_timestretch()
343 return AUDIO_DSP.tdspeed_percent;
346 bool dsp_timestretch_available()
348 return (global_settings.timestretch_enabled && big_sample_buf_count > 0);
351 /* Convert count samples to the internal format, if needed. Updates src
352 * to point past the samples "consumed" and dst is set to point to the
353 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
354 * is no point in processing the same data twice.
357 /* convert count 16-bit mono to 32-bit mono */
358 static void sample_input_lte_native_mono(
359 int count, const char *src[], int32_t *dst[])
361 const int16_t *s = (int16_t *) src[0];
362 const int16_t * const send = s + count;
363 int32_t *d = dst[0] = dst[1] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
364 int scale = WORD_SHIFT;
366 while (s < send)
368 *d++ = *s++ << scale;
371 src[0] = (char *)s;
374 /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
375 static void sample_input_lte_native_i_stereo(
376 int count, const char *src[], int32_t *dst[])
378 const int32_t *s = (int32_t *) src[0];
379 const int32_t * const send = s + count;
380 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
381 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
382 int scale = WORD_SHIFT;
384 while (s < send)
386 int32_t slr = *s++;
387 #ifdef ROCKBOX_LITTLE_ENDIAN
388 *dl++ = (slr >> 16) << scale;
389 *dr++ = (int32_t)(int16_t)slr << scale;
390 #else /* ROCKBOX_BIG_ENDIAN */
391 *dl++ = (int32_t)(int16_t)slr << scale;
392 *dr++ = (slr >> 16) << scale;
393 #endif
396 src[0] = (char *)s;
399 /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
400 static void sample_input_lte_native_ni_stereo(
401 int count, const char *src[], int32_t *dst[])
403 const int16_t *sl = (int16_t *) src[0];
404 const int16_t *sr = (int16_t *) src[1];
405 const int16_t * const slend = sl + count;
406 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
407 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
408 int scale = WORD_SHIFT;
410 while (sl < slend)
412 *dl++ = *sl++ << scale;
413 *dr++ = *sr++ << scale;
416 src[0] = (char *)sl;
417 src[1] = (char *)sr;
420 /* convert count 32-bit mono to 32-bit mono */
421 static void sample_input_gt_native_mono(
422 int count, const char *src[], int32_t *dst[])
424 dst[0] = dst[1] = (int32_t *)src[0];
425 src[0] = (char *)(dst[0] + count);
428 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
429 static void sample_input_gt_native_i_stereo(
430 int count, const char *src[], int32_t *dst[])
432 const int32_t *s = (int32_t *)src[0];
433 const int32_t * const send = s + 2*count;
434 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
435 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
437 while (s < send)
439 *dl++ = *s++;
440 *dr++ = *s++;
443 src[0] = (char *)send;
446 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
447 static void sample_input_gt_native_ni_stereo(
448 int count, const char *src[], int32_t *dst[])
450 dst[0] = (int32_t *)src[0];
451 dst[1] = (int32_t *)src[1];
452 src[0] = (char *)(dst[0] + count);
453 src[1] = (char *)(dst[1] + count);
457 * sample_input_new_format()
459 * set the to-native sample conversion function based on dsp sample parameters
461 * !DSPPARAMSYNC
462 * needs syncing with changes to the following dsp parameters:
463 * * dsp->stereo_mode (A/V)
464 * * dsp->sample_depth (A/V)
466 static void sample_input_new_format(struct dsp_config *dsp)
468 static const sample_input_fn_type sample_input_functions[] =
470 [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo,
471 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo,
472 [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono,
473 [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo,
474 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo,
475 [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono,
478 int convert = dsp->stereo_mode;
480 if (dsp->sample_depth > NATIVE_DEPTH)
481 convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX;
483 dsp->input_samples = sample_input_functions[convert];
487 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
488 /* write mono internal format to output format */
489 static void sample_output_mono(int count, struct dsp_data *data,
490 const int32_t *src[], int16_t *dst)
492 const int32_t *s0 = src[0];
493 const int scale = data->output_scale;
494 const int dc_bias = 1 << (scale - 1);
496 while (count-- > 0)
498 int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale);
499 *dst++ = lr;
500 *dst++ = lr;
503 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
505 /* write stereo internal format to output format */
506 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
507 static void sample_output_stereo(int count, struct dsp_data *data,
508 const int32_t *src[], int16_t *dst)
510 const int32_t *s0 = src[0];
511 const int32_t *s1 = src[1];
512 const int scale = data->output_scale;
513 const int dc_bias = 1 << (scale - 1);
515 while (count-- > 0)
517 *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale);
518 *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale);
521 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
524 * The "dither" code to convert the 24-bit samples produced by libmad was
525 * taken from the coolplayer project - coolplayer.sourceforge.net
527 * This function handles mono and stereo outputs.
529 static void sample_output_dithered(int count, struct dsp_data *data,
530 const int32_t *src[], int16_t *dst)
532 const int32_t mask = dither_mask;
533 const int32_t bias = dither_bias;
534 const int scale = data->output_scale;
535 const int32_t min = data->clip_min;
536 const int32_t max = data->clip_max;
537 const int32_t range = max - min;
538 int ch;
539 int16_t *d;
541 for (ch = 0; ch < data->num_channels; ch++)
543 struct dither_data * const dither = &dither_data[ch];
544 const int32_t *s = src[ch];
545 int i;
547 for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2)
549 int32_t output, sample;
550 int32_t random;
552 /* Noise shape and bias (for correct rounding later) */
553 sample = *s;
554 sample += dither->error[0] - dither->error[1] + dither->error[2];
555 dither->error[2] = dither->error[1];
556 dither->error[1] = dither->error[0]/2;
558 output = sample + bias;
560 /* Dither, highpass triangle PDF */
561 random = dither->random*0x0019660dL + 0x3c6ef35fL;
562 output += (random & mask) - (dither->random & mask);
563 dither->random = random;
565 /* Round sample to output range */
566 output &= ~mask;
568 /* Error feedback */
569 dither->error[0] = sample - output;
571 /* Clip */
572 if ((uint32_t)(output - min) > (uint32_t)range)
574 int32_t c = min;
575 if (output > min)
576 c += range;
577 output = c;
580 /* Quantize and store */
581 *d = output >> scale;
585 if (data->num_channels == 2)
586 return;
588 /* Have to duplicate left samples into the right channel since
589 pcm buffer and hardware is interleaved stereo */
590 d = &dst[0];
592 while (count-- > 0)
594 int16_t s = *d++;
595 *d++ = s;
600 * sample_output_new_format()
602 * set the from-native to ouput sample conversion routine
604 * !DSPPARAMSYNC
605 * needs syncing with changes to the following dsp parameters:
606 * * dsp->stereo_mode (A/V)
607 * * dither_enabled (A)
609 static void sample_output_new_format(struct dsp_config *dsp)
611 static const sample_output_fn_type sample_output_functions[] =
613 sample_output_mono,
614 sample_output_stereo,
615 sample_output_dithered,
616 sample_output_dithered
619 int out = dsp->data.num_channels - 1;
621 if (dsp == &AUDIO_DSP && dither_enabled)
622 out += 2;
624 dsp->output_samples = sample_output_functions[out];
628 * Linear interpolation resampling that introduces a one sample delay because
629 * of our inability to look into the future at the end of a frame.
631 #ifndef DSP_HAVE_ASM_RESAMPLING
632 static int dsp_downsample(int count, struct dsp_data *data,
633 const int32_t *src[], int32_t *dst[])
635 int ch = data->num_channels - 1;
636 uint32_t delta = data->resample_data.delta;
637 uint32_t phase, pos;
638 int32_t *d;
640 /* Rolled channel loop actually showed slightly faster. */
643 /* Just initialize things and not worry too much about the relatively
644 * uncommon case of not being able to spit out a sample for the frame.
646 const int32_t *s = src[ch];
647 int32_t last = data->resample_data.last_sample[ch];
649 data->resample_data.last_sample[ch] = s[count - 1];
650 d = dst[ch];
651 phase = data->resample_data.phase;
652 pos = phase >> 16;
654 /* Do we need last sample of previous frame for interpolation? */
655 if (pos > 0)
656 last = s[pos - 1];
658 while (pos < (uint32_t)count)
660 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
661 phase += delta;
662 pos = phase >> 16;
663 last = s[pos - 1];
666 while (--ch >= 0);
668 /* Wrap phase accumulator back to start of next frame. */
669 data->resample_data.phase = phase - (count << 16);
670 return d - dst[0];
673 static int dsp_upsample(int count, struct dsp_data *data,
674 const int32_t *src[], int32_t *dst[])
676 int ch = data->num_channels - 1;
677 uint32_t delta = data->resample_data.delta;
678 uint32_t phase, pos;
679 int32_t *d;
681 /* Rolled channel loop actually showed slightly faster. */
684 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
685 const int32_t *s = src[ch];
686 int32_t last = data->resample_data.last_sample[ch];
688 data->resample_data.last_sample[ch] = s[count - 1];
689 d = dst[ch];
690 phase = data->resample_data.phase;
691 pos = phase >> 16;
693 while (pos == 0)
695 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last);
696 phase += delta;
697 pos = phase >> 16;
700 while (pos < (uint32_t)count)
702 last = s[pos - 1];
703 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
704 phase += delta;
705 pos = phase >> 16;
708 while (--ch >= 0);
710 /* Wrap phase accumulator back to start of next frame. */
711 data->resample_data.phase = phase & 0xffff;
712 return d - dst[0];
714 #endif /* DSP_HAVE_ASM_RESAMPLING */
716 static void resampler_new_delta(struct dsp_config *dsp)
718 dsp->data.resample_data.delta = (unsigned long)
719 dsp->frequency * 65536LL / NATIVE_FREQUENCY;
721 if (dsp->frequency == NATIVE_FREQUENCY)
723 /* NOTE: If fully glitch-free transistions from no resampling to
724 resampling are desired, last_sample history should be maintained
725 even when not resampling. */
726 dsp->resample = NULL;
727 dsp->data.resample_data.phase = 0;
728 dsp->data.resample_data.last_sample[0] = 0;
729 dsp->data.resample_data.last_sample[1] = 0;
731 else if (dsp->frequency < NATIVE_FREQUENCY)
732 dsp->resample = dsp_upsample;
733 else
734 dsp->resample = dsp_downsample;
737 /* Resample count stereo samples. Updates the src array, if resampling is
738 * done, to refer to the resampled data. Returns number of stereo samples
739 * for further processing.
741 static inline int resample(struct dsp_config *dsp, int count, int32_t *src[])
743 int32_t *dst[2] =
745 &resample_buf[RESAMPLE_BUF_LEFT_CHANNEL],
746 &resample_buf[RESAMPLE_BUF_RIGHT_CHANNEL],
749 count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst);
751 src[0] = dst[0];
752 src[1] = dst[dsp->data.num_channels - 1];
754 return count;
757 static void dither_init(struct dsp_config *dsp)
759 memset(dither_data, 0, sizeof (dither_data));
760 dither_bias = (1L << (dsp->frac_bits - NATIVE_DEPTH));
761 dither_mask = (1L << (dsp->frac_bits + 1 - NATIVE_DEPTH)) - 1;
764 void dsp_dither_enable(bool enable)
766 struct dsp_config *dsp = &AUDIO_DSP;
767 dither_enabled = enable;
768 sample_output_new_format(dsp);
771 /* Applies crossfeed to the stereo signal in src.
772 * Crossfeed is a process where listening over speakers is simulated. This
773 * is good for old hard panned stereo records, which might be quite fatiguing
774 * to listen to on headphones with no crossfeed.
776 #ifndef DSP_HAVE_ASM_CROSSFEED
777 static void apply_crossfeed(int count, int32_t *buf[])
779 int32_t *hist_l = &crossfeed_data.history[0];
780 int32_t *hist_r = &crossfeed_data.history[2];
781 int32_t *delay = &crossfeed_data.delay[0][0];
782 int32_t *coefs = &crossfeed_data.coefs[0];
783 int32_t gain = crossfeed_data.gain;
784 int32_t *di = crossfeed_data.index;
786 int32_t acc;
787 int32_t left, right;
788 int i;
790 for (i = 0; i < count; i++)
792 left = buf[0][i];
793 right = buf[1][i];
795 /* Filter delayed sample from left speaker */
796 acc = FRACMUL(*di, coefs[0]);
797 acc += FRACMUL(hist_l[0], coefs[1]);
798 acc += FRACMUL(hist_l[1], coefs[2]);
799 /* Save filter history for left speaker */
800 hist_l[1] = acc;
801 hist_l[0] = *di;
802 *di++ = left;
803 /* Filter delayed sample from right speaker */
804 acc = FRACMUL(*di, coefs[0]);
805 acc += FRACMUL(hist_r[0], coefs[1]);
806 acc += FRACMUL(hist_r[1], coefs[2]);
807 /* Save filter history for right speaker */
808 hist_r[1] = acc;
809 hist_r[0] = *di;
810 *di++ = right;
811 /* Now add the attenuated direct sound and write to outputs */
812 buf[0][i] = FRACMUL(left, gain) + hist_r[1];
813 buf[1][i] = FRACMUL(right, gain) + hist_l[1];
815 /* Wrap delay line index if bigger than delay line size */
816 if (di >= delay + 13*2)
817 di = delay;
819 /* Write back local copies of data we've modified */
820 crossfeed_data.index = di;
822 #endif /* DSP_HAVE_ASM_CROSSFEED */
825 * dsp_set_crossfeed(bool enable)
827 * !DSPPARAMSYNC
828 * needs syncing with changes to the following dsp parameters:
829 * * dsp->stereo_mode (A)
831 void dsp_set_crossfeed(bool enable)
833 crossfeed_enabled = enable;
834 AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1)
835 ? apply_crossfeed : NULL;
838 void dsp_set_crossfeed_direct_gain(int gain)
840 crossfeed_data.gain = get_replaygain_int(gain * 10) << 7;
841 /* If gain is negative, the calculation overflowed and we need to clamp */
842 if (crossfeed_data.gain < 0)
843 crossfeed_data.gain = 0x7fffffff;
846 /* Both gains should be below 0 dB */
847 void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
849 int32_t *c = crossfeed_data.coefs;
850 long scaler = get_replaygain_int(lf_gain * 10) << 7;
852 cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff;
853 hf_gain -= lf_gain;
854 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
855 * point instead of shelf midpoint. This is for compatibility with the old
856 * crossfeed shelf filter and should be removed if crossfeed settings are
857 * ever made incompatible for any other good reason.
859 cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);
860 filter_shelf_coefs(cutoff, hf_gain, false, c);
861 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
862 * over 1 and can do this safely
864 c[0] = FRACMUL_SHL(c[0], scaler, 4);
865 c[1] = FRACMUL_SHL(c[1], scaler, 4);
866 c[2] <<= 4;
869 /* Apply a constant gain to the samples (e.g., for ReplayGain).
870 * Note that this must be called before the resampler.
872 #ifndef DSP_HAVE_ASM_APPLY_GAIN
873 static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
875 const int32_t gain = data->gain;
876 int ch;
878 for (ch = 0; ch < data->num_channels; ch++)
880 int32_t *d = buf[ch];
881 int i;
883 for (i = 0; i < count; i++)
884 d[i] = FRACMUL_SHL(d[i], gain, 8);
887 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
889 /* Combine all gains to a global gain. */
890 static void set_gain(struct dsp_config *dsp)
892 /* gains are in S7.24 format */
893 dsp->data.gain = DEFAULT_GAIN;
895 /* Replay gain not relevant to voice */
896 if (dsp == &AUDIO_DSP && replaygain)
898 dsp->data.gain = replaygain;
901 if (dsp->eq_process && eq_precut)
903 dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24);
906 #ifdef HAVE_SW_VOLUME_CONTROL
907 if (global_settings.volume < SW_VOLUME_MAX ||
908 global_settings.volume > SW_VOLUME_MIN)
910 int vol_gain = get_replaygain_int(global_settings.volume * 100);
911 dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24);
913 #endif
915 if (dsp->data.gain == DEFAULT_GAIN)
917 dsp->data.gain = 0;
919 else
921 dsp->data.gain >>= 1; /* convert gain to S8.23 format */
924 dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL;
928 * Update the amount to cut the audio before applying the equalizer.
930 * @param precut to apply in decibels (multiplied by 10)
932 void dsp_set_eq_precut(int precut)
934 eq_precut = get_replaygain_int(precut * -10);
935 set_gain(&AUDIO_DSP);
939 * Synchronize the equalizer filter coefficients with the global settings.
941 * @param band the equalizer band to synchronize
943 void dsp_set_eq_coefs(int band)
945 const int *setting;
946 long gain;
947 unsigned long cutoff, q;
949 /* Adjust setting pointer to the band we actually want to change */
950 setting = &global_settings.eq_band0_cutoff + (band * 3);
952 /* Convert user settings to format required by coef generator functions */
953 cutoff = 0xffffffff / NATIVE_FREQUENCY * (*setting++);
954 q = *setting++;
955 gain = *setting++;
957 if (q == 0)
958 q = 1;
960 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
961 which it should be, since we're executed from the main thread. */
963 /* Assume a band is disabled if the gain is zero */
964 if (gain == 0)
966 eq_data.enabled[band] = 0;
968 else
970 if (band == 0)
971 eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
972 else if (band == 4)
973 eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
974 else
975 eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
977 eq_data.enabled[band] = 1;
981 /* Apply EQ filters to those bands that have got it switched on. */
982 static void eq_process(int count, int32_t *buf[])
984 static const int shifts[] =
986 EQ_SHELF_SHIFT, /* low shelf */
987 EQ_PEAK_SHIFT, /* peaking */
988 EQ_PEAK_SHIFT, /* peaking */
989 EQ_PEAK_SHIFT, /* peaking */
990 EQ_SHELF_SHIFT, /* high shelf */
992 unsigned int channels = AUDIO_DSP.data.num_channels;
993 int i;
995 /* filter configuration currently is 1 low shelf filter, 3 band peaking
996 filters and 1 high shelf filter, in that order. we need to know this
997 so we can choose the correct shift factor.
999 for (i = 0; i < 5; i++)
1001 if (!eq_data.enabled[i])
1002 continue;
1003 eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]);
1008 * Use to enable the equalizer.
1010 * @param enable true to enable the equalizer
1012 void dsp_set_eq(bool enable)
1014 AUDIO_DSP.eq_process = enable ? eq_process : NULL;
1015 set_gain(&AUDIO_DSP);
1018 static void dsp_set_stereo_width(int value)
1020 long width, straight, cross;
1022 width = value * 0x7fffff / 100;
1024 if (value <= 100)
1026 straight = (0x7fffff + width) / 2;
1027 cross = straight - width;
1029 else
1031 /* straight = (1 + width) / (2 * width) */
1032 straight = ((int64_t)(0x7fffff + width) << 22) / width;
1033 cross = straight - 0x7fffff;
1036 dsp_sw_gain = straight << 8;
1037 dsp_sw_cross = cross << 8;
1041 * Implements the different channel configurations and stereo width.
1044 /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1045 * completeness. */
1046 #if 0
1047 static void channels_process_sound_chan_stereo(int count, int32_t *buf[])
1049 /* The channels are each just themselves */
1050 (void)count; (void)buf;
1052 #endif
1054 #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1055 static void channels_process_sound_chan_mono(int count, int32_t *buf[])
1057 int32_t *sl = buf[0], *sr = buf[1];
1059 while (count-- > 0)
1061 int32_t lr = *sl/2 + *sr/2;
1062 *sl++ = lr;
1063 *sr++ = lr;
1066 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1068 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1069 static void channels_process_sound_chan_custom(int count, int32_t *buf[])
1071 const int32_t gain = dsp_sw_gain;
1072 const int32_t cross = dsp_sw_cross;
1073 int32_t *sl = buf[0], *sr = buf[1];
1075 while (count-- > 0)
1077 int32_t l = *sl;
1078 int32_t r = *sr;
1079 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
1080 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
1083 #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1085 static void channels_process_sound_chan_mono_left(int count, int32_t *buf[])
1087 /* Just copy over the other channel */
1088 memcpy(buf[1], buf[0], count * sizeof (*buf));
1091 static void channels_process_sound_chan_mono_right(int count, int32_t *buf[])
1093 /* Just copy over the other channel */
1094 memcpy(buf[0], buf[1], count * sizeof (*buf));
1097 #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1098 static void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
1100 int32_t *sl = buf[0], *sr = buf[1];
1102 while (count-- > 0)
1104 int32_t ch = *sl/2 - *sr/2;
1105 *sl++ = ch;
1106 *sr++ = -ch;
1109 #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1111 static void dsp_set_channel_config(int value)
1113 static const channels_process_fn_type channels_process_functions[] =
1115 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1116 [SOUND_CHAN_STEREO] = NULL,
1117 [SOUND_CHAN_MONO] = channels_process_sound_chan_mono,
1118 [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom,
1119 [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left,
1120 [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right,
1121 [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke,
1124 if ((unsigned)value >= ARRAYLEN(channels_process_functions) ||
1125 AUDIO_DSP.stereo_mode == STEREO_MONO)
1127 value = SOUND_CHAN_STEREO;
1130 /* This doesn't apply to voice */
1131 channels_mode = value;
1132 AUDIO_DSP.channels_process = channels_process_functions[value];
1135 #if CONFIG_CODEC == SWCODEC
1137 #ifdef HAVE_SW_TONE_CONTROLS
1138 static void set_tone_controls(void)
1140 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
1141 0xffffffff/NATIVE_FREQUENCY*3500,
1142 bass, treble, -prescale,
1143 AUDIO_DSP.tone_filter.coefs);
1144 /* Sync the voice dsp coefficients */
1145 memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs,
1146 sizeof (VOICE_DSP.tone_filter.coefs));
1148 #endif
1150 /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1151 * code directly.
1153 int dsp_callback(int msg, intptr_t param)
1155 switch (msg)
1157 #ifdef HAVE_SW_TONE_CONTROLS
1158 case DSP_CALLBACK_SET_PRESCALE:
1159 prescale = param;
1160 set_tone_controls();
1161 break;
1162 /* prescaler is always set after calling any of these, so we wait with
1163 * calculating coefs until the above case is hit.
1165 case DSP_CALLBACK_SET_BASS:
1166 bass = param;
1167 break;
1168 case DSP_CALLBACK_SET_TREBLE:
1169 treble = param;
1170 break;
1171 #ifdef HAVE_SW_VOLUME_CONTROL
1172 case DSP_CALLBACK_SET_SW_VOLUME:
1173 set_gain(&AUDIO_DSP);
1174 break;
1175 #endif
1176 #endif
1177 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
1178 dsp_set_channel_config(param);
1179 break;
1180 case DSP_CALLBACK_SET_STEREO_WIDTH:
1181 dsp_set_stereo_width(param);
1182 break;
1183 default:
1184 break;
1186 return 0;
1188 #endif
1190 /* Process and convert src audio to dst based on the DSP configuration,
1191 * reading count number of audio samples. dst is assumed to be large
1192 * enough; use dsp_output_count() to get the required number. src is an
1193 * array of pointers; for mono and interleaved stereo, it contains one
1194 * pointer to the start of the audio data and the other is ignored; for
1195 * non-interleaved stereo, it contains two pointers, one for each audio
1196 * channel. Returns number of bytes written to dst.
1198 int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count)
1200 int32_t *tmp[2];
1201 static long last_yield;
1202 long tick;
1203 int written = 0;
1205 #if defined(CPU_COLDFIRE)
1206 /* set emac unit for dsp processing, and save old macsr, we're running in
1207 codec thread context at this point, so can't clobber it */
1208 unsigned long old_macsr = coldfire_get_macsr();
1209 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
1210 #endif
1212 if (new_gain)
1213 dsp_set_replaygain(); /* Gain has changed */
1215 /* Perform at least one yield before starting */
1216 last_yield = current_tick;
1217 yield();
1219 /* Testing function pointers for NULL is preferred since the pointer
1220 will be preloaded to be used for the call if not. */
1221 while (count > 0)
1223 int samples = MIN(sample_buf_count/2, count);
1224 count -= samples;
1226 dsp->input_samples(samples, src, tmp);
1228 if (dsp->tdspeed_active)
1229 samples = tdspeed_doit(tmp, samples);
1231 int chunk_offset = 0;
1232 while (samples > 0)
1234 int32_t *t2[2];
1235 t2[0] = tmp[0]+chunk_offset;
1236 t2[1] = tmp[1]+chunk_offset;
1238 int chunk = MIN(sample_buf_count/2, samples);
1239 chunk_offset += chunk;
1240 samples -= chunk;
1242 if (dsp->apply_gain)
1243 dsp->apply_gain(chunk, &dsp->data, t2);
1245 if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0)
1246 break; /* I'm pretty sure we're downsampling here */
1248 if (dsp->apply_crossfeed)
1249 dsp->apply_crossfeed(chunk, t2);
1251 if (dsp->eq_process)
1252 dsp->eq_process(chunk, t2);
1254 #ifdef HAVE_SW_TONE_CONTROLS
1255 if ((bass | treble) != 0)
1256 eq_filter(t2, &dsp->tone_filter, chunk,
1257 dsp->data.num_channels, FILTER_BISHELF_SHIFT);
1258 #endif
1260 if (dsp->channels_process)
1261 dsp->channels_process(chunk, t2);
1263 if (dsp->compressor_process)
1264 dsp->compressor_process(chunk, t2);
1266 dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst);
1268 written += chunk;
1269 dst += chunk * sizeof (int16_t) * 2;
1271 /* yield at least once each tick */
1272 tick = current_tick;
1273 if (TIME_AFTER(tick, last_yield))
1275 last_yield = tick;
1276 yield();
1281 #if defined(CPU_COLDFIRE)
1282 /* set old macsr again */
1283 coldfire_set_macsr(old_macsr);
1284 #endif
1285 return written;
1288 /* Given count number of input samples, calculate the maximum number of
1289 * samples of output data that would be generated (the calculation is not
1290 * entirely exact and rounds upwards to be on the safe side; during
1291 * resampling, the number of samples generated depends on the current state
1292 * of the resampler).
1294 /* dsp_input_size MUST be called afterwards */
1295 int dsp_output_count(struct dsp_config *dsp, int count)
1297 if (dsp->tdspeed_active)
1298 count = tdspeed_est_output_size();
1299 if (dsp->resample)
1301 count = (int)(((unsigned long)count * NATIVE_FREQUENCY
1302 + (dsp->frequency - 1)) / dsp->frequency);
1305 /* Now we have the resampled sample count which must not exceed
1306 * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One
1307 * must call dsp_input_count() to get the correct input sample
1308 * count.
1310 if (count > RESAMPLE_BUF_RIGHT_CHANNEL)
1311 count = RESAMPLE_BUF_RIGHT_CHANNEL;
1313 return count;
1316 /* Given count output samples, calculate number of input samples
1317 * that would be consumed in order to fill the output buffer.
1319 int dsp_input_count(struct dsp_config *dsp, int count)
1321 /* count is now the number of resampled input samples. Convert to
1322 original input samples. */
1323 if (dsp->resample)
1325 /* Use the real resampling delta =
1326 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1327 * round towards zero to avoid buffer overflows. */
1328 count = (int)(((unsigned long)count *
1329 dsp->data.resample_data.delta) >> 16);
1332 if (dsp->tdspeed_active)
1333 count = tdspeed_est_input_size(count);
1335 return count;
1338 static void dsp_set_gain_var(long *var, long value)
1340 *var = value;
1341 new_gain = true;
1344 static void dsp_update_functions(struct dsp_config *dsp)
1346 sample_input_new_format(dsp);
1347 sample_output_new_format(dsp);
1348 if (dsp == &AUDIO_DSP)
1349 dsp_set_crossfeed(crossfeed_enabled);
1352 intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value)
1354 switch (setting)
1356 case DSP_MYDSP:
1357 switch (value)
1359 case CODEC_IDX_AUDIO:
1360 return (intptr_t)&AUDIO_DSP;
1361 case CODEC_IDX_VOICE:
1362 return (intptr_t)&VOICE_DSP;
1363 default:
1364 return (intptr_t)NULL;
1367 case DSP_SET_FREQUENCY:
1368 memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data));
1369 /* Fall through!!! */
1370 case DSP_SWITCH_FREQUENCY:
1371 dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value;
1372 /* Account for playback speed adjustment when setting dsp->frequency
1373 if we're called from the main audio thread. Voice UI thread should
1374 not need this feature.
1376 if (dsp == &AUDIO_DSP)
1377 dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100;
1378 else
1379 dsp->frequency = dsp->codec_frequency;
1381 resampler_new_delta(dsp);
1382 tdspeed_setup(dsp);
1383 break;
1385 case DSP_SET_SAMPLE_DEPTH:
1386 dsp->sample_depth = value;
1388 if (dsp->sample_depth <= NATIVE_DEPTH)
1390 dsp->frac_bits = WORD_FRACBITS;
1391 dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */
1392 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1393 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1395 else
1397 dsp->frac_bits = value;
1398 dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */
1399 dsp->data.clip_max = (1 << value) - 1;
1400 dsp->data.clip_min = -(1 << value);
1403 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1404 sample_input_new_format(dsp);
1405 dither_init(dsp);
1406 break;
1408 case DSP_SET_STEREO_MODE:
1409 dsp->stereo_mode = value;
1410 dsp->data.num_channels = value == STEREO_MONO ? 1 : 2;
1411 dsp_update_functions(dsp);
1412 tdspeed_setup(dsp);
1413 break;
1415 case DSP_RESET:
1416 dsp->stereo_mode = STEREO_NONINTERLEAVED;
1417 dsp->data.num_channels = 2;
1418 dsp->sample_depth = NATIVE_DEPTH;
1419 dsp->frac_bits = WORD_FRACBITS;
1420 dsp->sample_bytes = sizeof (int16_t);
1421 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1422 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1423 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1424 dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY;
1426 if (dsp == &AUDIO_DSP)
1428 track_gain = 0;
1429 album_gain = 0;
1430 track_peak = 0;
1431 album_peak = 0;
1432 new_gain = true;
1435 dsp_update_functions(dsp);
1436 resampler_new_delta(dsp);
1437 tdspeed_setup(dsp);
1438 if (dsp == &AUDIO_DSP)
1439 release_gain = UNITY;
1440 break;
1442 case DSP_FLUSH:
1443 memset(&dsp->data.resample_data, 0,
1444 sizeof (dsp->data.resample_data));
1445 resampler_new_delta(dsp);
1446 dither_init(dsp);
1447 tdspeed_setup(dsp);
1448 if (dsp == &AUDIO_DSP)
1449 release_gain = UNITY;
1450 break;
1452 case DSP_SET_TRACK_GAIN:
1453 if (dsp == &AUDIO_DSP)
1454 dsp_set_gain_var(&track_gain, value);
1455 break;
1457 case DSP_SET_ALBUM_GAIN:
1458 if (dsp == &AUDIO_DSP)
1459 dsp_set_gain_var(&album_gain, value);
1460 break;
1462 case DSP_SET_TRACK_PEAK:
1463 if (dsp == &AUDIO_DSP)
1464 dsp_set_gain_var(&track_peak, value);
1465 break;
1467 case DSP_SET_ALBUM_PEAK:
1468 if (dsp == &AUDIO_DSP)
1469 dsp_set_gain_var(&album_peak, value);
1470 break;
1472 default:
1473 return 0;
1476 return 1;
1479 int get_replaygain_mode(bool have_track_gain, bool have_album_gain)
1481 int type;
1483 bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK)
1484 || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE)
1485 && global_settings.playlist_shuffle));
1487 type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM
1488 : have_track_gain ? REPLAYGAIN_TRACK : -1;
1490 return type;
1493 void dsp_set_replaygain(void)
1495 long gain = 0;
1497 new_gain = false;
1499 if ((global_settings.replaygain_type != REPLAYGAIN_OFF) ||
1500 global_settings.replaygain_noclip)
1502 bool track_mode = get_replaygain_mode(track_gain != 0,
1503 album_gain != 0) == REPLAYGAIN_TRACK;
1504 long peak = (track_mode || !album_peak) ? track_peak : album_peak;
1506 if (global_settings.replaygain_type != REPLAYGAIN_OFF)
1508 gain = (track_mode || !album_gain) ? track_gain : album_gain;
1510 if (global_settings.replaygain_preamp)
1512 long preamp = get_replaygain_int(
1513 global_settings.replaygain_preamp * 10);
1515 gain = (long) (((int64_t) gain * preamp) >> 24);
1519 if (gain == 0)
1521 /* So that noclip can work even with no gain information. */
1522 gain = DEFAULT_GAIN;
1525 if (global_settings.replaygain_noclip && (peak != 0)
1526 && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN))
1528 gain = (((int64_t) DEFAULT_GAIN << 24) / peak);
1531 if (gain == DEFAULT_GAIN)
1533 /* Nothing to do, disable processing. */
1534 gain = 0;
1538 /* Store in S7.24 format to simplify calculations. */
1539 replaygain = gain;
1540 set_gain(&AUDIO_DSP);
1543 /** SET COMPRESSOR
1544 * Called by the menu system to configure the compressor process */
1545 void dsp_set_compressor(int c_threshold, int c_gain, int c_ratio,
1546 int c_knee, int c_release)
1548 bool changed = false;
1549 bool active = (c_threshold < 0);
1550 bool new_auto_gain = (c_gain == 1);
1551 const int comp_ratio[] = {2, 4, 6, 10, 0};
1552 int new_ratio = comp_ratio[c_ratio];
1553 bool new_knee = (c_knee == 1);
1554 int new_release = c_release * NATIVE_FREQUENCY / 1000;
1556 if (c_menu.threshold != c_threshold)
1558 changed = true;
1559 c_menu.threshold = c_threshold;
1560 logf(" Compressor Threshold: %d dB\tEnabled: %s",
1561 c_menu.threshold, active ? "Yes" : "No");
1564 if (c_menu.auto_gain != new_auto_gain)
1566 changed = true;
1567 c_menu.auto_gain = new_auto_gain;
1568 logf(" Compressor Makeup Gain: %s",
1569 c_menu.auto_gain ? "Auto" : "Off");
1572 if (c_menu.ratio != new_ratio)
1574 changed = true;
1575 c_menu.ratio = new_ratio;
1576 if (c_menu.ratio)
1577 { logf(" Compressor Ratio: %d:1", c_menu.ratio); }
1578 else
1579 { logf(" Compressor Ratio: Limit"); }
1582 if (c_menu.soft_knee != new_knee)
1584 changed = true;
1585 c_menu.soft_knee = new_knee;
1586 logf(" Compressor Knee: %s", c_menu.soft_knee==1?"Soft":"Hard");
1589 if (c_menu.release != new_release)
1591 changed = true;
1592 c_menu.release = new_release;
1593 logf(" Compressor Release: %d", c_menu.release);
1596 if (changed && active)
1598 /* configure variables for compressor operation */
1599 int i;
1600 const int32_t db[] ={0x000000, /* positive db equivalents in S15.16 format */
1601 0x241FA4, 0x1E1A5E, 0x1A94C8, 0x181518, 0x1624EA, 0x148F82, 0x1338BD, 0x120FD2,
1602 0x1109EB, 0x101FA4, 0x0F4BB6, 0x0E8A3C, 0x0DD840, 0x0D3377, 0x0C9A0E, 0x0C0A8C,
1603 0x0B83BE, 0x0B04A5, 0x0A8C6C, 0x0A1A5E, 0x09ADE1, 0x094670, 0x08E398, 0x0884F6,
1604 0x082A30, 0x07D2FA, 0x077F0F, 0x072E31, 0x06E02A, 0x0694C8, 0x064BDF, 0x060546,
1605 0x05C0DA, 0x057E78, 0x053E03, 0x04FF5F, 0x04C273, 0x048726, 0x044D64, 0x041518,
1606 0x03DE30, 0x03A89B, 0x037448, 0x03412A, 0x030F32, 0x02DE52, 0x02AE80, 0x027FB0,
1607 0x0251D6, 0x0224EA, 0x01F8E2, 0x01CDB4, 0x01A359, 0x0179C9, 0x0150FC, 0x0128EB,
1608 0x010190, 0x00DAE4, 0x00B4E1, 0x008F82, 0x006AC1, 0x004699, 0x002305};
1610 struct curve_point
1612 int32_t db; /* S15.16 format */
1613 int32_t offset; /* S15.16 format */
1614 } db_curve[5];
1616 /** Set up the shape of the compression curve first as decibel values*/
1617 /* db_curve[0] = bottom of knee
1618 [1] = threshold
1619 [2] = top of knee
1620 [3] = 0 db input
1621 [4] = ~+12db input (2 bits clipping overhead) */
1623 db_curve[1].db = c_menu.threshold << 16;
1624 if (c_menu.soft_knee)
1626 /* bottom of knee is 3dB below the threshold for soft knee*/
1627 db_curve[0].db = db_curve[1].db - (3 << 16);
1628 /* top of knee is 3dB above the threshold for soft knee */
1629 db_curve[2].db = db_curve[1].db + (3 << 16);
1630 if (c_menu.ratio)
1631 /* offset = -3db * (ratio - 1) / ratio */
1632 db_curve[2].offset = (int32_t)((long long)(-3 << 16)
1633 * (c_menu.ratio - 1) / c_menu.ratio);
1634 else
1635 /* offset = -3db for hard limit */
1636 db_curve[2].offset = (-3 << 16);
1638 else
1640 /* bottom of knee is at the threshold for hard knee */
1641 db_curve[0].db = c_menu.threshold << 16;
1642 /* top of knee is at the threshold for hard knee */
1643 db_curve[2].db = c_menu.threshold << 16;
1644 db_curve[2].offset = 0;
1647 /* Calculate 0db and ~+12db offsets */
1648 db_curve[4].db = 0xC0A8C; /* db of 2 bits clipping */
1649 if (c_menu.ratio)
1651 /* offset = threshold * (ratio - 1) / ratio */
1652 db_curve[3].offset = (int32_t)((long long)(c_menu.threshold << 16)
1653 * (c_menu.ratio - 1) / c_menu.ratio);
1654 db_curve[4].offset = (int32_t)((long long)-db_curve[4].db
1655 * (c_menu.ratio - 1) / c_menu.ratio) + db_curve[3].offset;
1657 else
1659 /* offset = threshold for hard limit */
1660 db_curve[3].offset = (c_menu.threshold << 16);
1661 db_curve[4].offset = -db_curve[4].db + db_curve[3].offset;
1664 /** Now set up the comp_curve table with compression offsets in the form
1665 of gain factors in S7.24 format */
1666 /* comp_curve[0] is 0 (-infinity db) input */
1667 comp_curve[0] = UNITY;
1668 /* comp_curve[1 to 63] are intermediate compression values corresponding
1669 to the 6 MSB of the input values of a non-clipped signal */
1670 for (i = 1; i < 64; i++)
1672 /* db constants are stored as positive numbers;
1673 make them negative here */
1674 int32_t this_db = -db[i];
1676 /* no compression below the knee */
1677 if (this_db <= db_curve[0].db)
1678 comp_curve[i] = UNITY;
1680 /* if soft knee and below top of knee,
1681 interpolate along soft knee slope */
1682 else if (c_menu.soft_knee && (this_db <= db_curve[2].db))
1683 comp_curve[i] = fp_factor(fp_mul(
1684 ((this_db - db_curve[0].db) / 6),
1685 db_curve[2].offset, 16), 16) << 8;
1687 /* interpolate along ratio slope above the knee */
1688 else
1689 comp_curve[i] = fp_factor(fp_mul(
1690 fp_div((db_curve[1].db - this_db), db_curve[1].db, 16),
1691 db_curve[3].offset, 16), 16) << 8;
1693 /* comp_curve[64] is the compression level of a maximum level,
1694 non-clipped signal */
1695 comp_curve[64] = fp_factor(db_curve[3].offset, 16) << 8;
1697 /* comp_curve[65] is the compression level of a maximum level,
1698 clipped signal */
1699 comp_curve[65] = fp_factor(db_curve[4].offset, 16) << 8;
1701 #if defined(ROCKBOX_HAS_LOGF) && defined(LOGF_ENABLE)
1702 logf("\n *** Compression Offsets ***");
1703 /* some settings for display only, not used in calculations */
1704 db_curve[0].offset = 0;
1705 db_curve[1].offset = 0;
1706 db_curve[3].db = 0;
1708 for (i = 0; i <= 4; i++)
1710 logf("Curve[%d]: db: % 6.2f\toffset: % 6.2f", i,
1711 (float)db_curve[i].db / (1 << 16),
1712 (float)db_curve[i].offset / (1 << 16));
1715 logf("\nGain factors:");
1716 for (i = 1; i <= 65; i++)
1718 debugf("%02d: %.6f ", i, (float)comp_curve[i] / UNITY);
1719 if (i % 4 == 0) debugf("\n");
1721 debugf("\n");
1722 #endif
1724 /* if using auto peak, then makeup gain is max offset - .1dB headroom */
1725 comp_makeup_gain = c_menu.auto_gain ?
1726 fp_factor(-(db_curve[3].offset) - 0x199A, 16) << 8 : UNITY;
1727 logf("Makeup gain:\t%.6f", (float)comp_makeup_gain / UNITY);
1729 /* calculate per-sample gain change a rate of 10db over release time */
1730 comp_rel_slope = 0xAF0BB2 / c_menu.release;
1731 logf("Release slope:\t%.6f", (float)comp_rel_slope / UNITY);
1733 release_gain = UNITY;
1736 /* enable/disable the compressor */
1737 AUDIO_DSP.compressor_process = active ? compressor_process : NULL;
1740 /** GET COMPRESSION GAIN
1741 * Returns the required gain factor in S7.24 format in order to compress the
1742 * sample in accordance with the compression curve. Always 1 or less.
1744 static inline int32_t get_compression_gain(int32_t sample)
1746 const int frac_bits_offset = AUDIO_DSP.frac_bits - 15;
1748 /* sample must be positive */
1749 if (sample < 0)
1750 sample = -(sample + 1);
1752 /* shift sample into 15 frac bit range */
1753 if (frac_bits_offset > 0)
1754 sample >>= frac_bits_offset;
1755 if (frac_bits_offset < 0)
1756 sample <<= -frac_bits_offset;
1758 /* normal case: sample isn't clipped */
1759 if (sample < (1 << 15))
1761 /* index is 6 MSB, rem is 9 LSB */
1762 int index = sample >> 9;
1763 int32_t rem = (sample & 0x1FF) << 22;
1765 /* interpolate from the compression curve:
1766 higher gain - ((rem / (1 << 31)) * (higher gain - lower gain)) */
1767 return comp_curve[index] - (FRACMUL(rem,
1768 (comp_curve[index] - comp_curve[index + 1])));
1770 /* sample is somewhat clipped, up to 2 bits of overhead */
1771 if (sample < (1 << 17))
1773 /* straight interpolation:
1774 higher gain - ((clipped portion of sample * 4/3
1775 / (1 << 31)) * (higher gain - lower gain)) */
1776 return comp_curve[64] - (FRACMUL(((sample - (1 << 15)) / 3) << 16,
1777 (comp_curve[64] - comp_curve[65])));
1780 /* sample is too clipped, return invalid value */
1781 return -1;
1784 /** COMPRESSOR PROCESS
1785 * Changes the gain of the samples according to the compressor curve
1787 static void compressor_process(int count, int32_t *buf[])
1789 const int num_chan = AUDIO_DSP.data.num_channels;
1790 int32_t *in_buf[2] = {buf[0], buf[1]};
1792 while (count-- > 0)
1794 int ch;
1795 /* use lowest (most compressed) gain factor of the output buffer
1796 sample pair for both samples (mono is also handled correctly here) */
1797 int32_t sample_gain = UNITY;
1798 for (ch = 0; ch < num_chan; ch++)
1800 int32_t this_gain = get_compression_gain(*in_buf[ch]);
1801 if (this_gain < sample_gain)
1802 sample_gain = this_gain;
1805 /* perform release slope; skip if no compression and no release slope */
1806 if ((sample_gain != UNITY) || (release_gain != UNITY))
1808 /* if larger offset than previous slope, start new release slope */
1809 if ((sample_gain <= release_gain) && (sample_gain > 0))
1811 release_gain = sample_gain;
1813 else
1814 /* keep sloping towards unity gain (and ignore invalid value) */
1816 release_gain += comp_rel_slope;
1817 if (release_gain > UNITY)
1819 release_gain = UNITY;
1824 /* total gain factor is the product of release gain and makeup gain,
1825 but avoid computation if possible */
1826 int32_t total_gain = ((release_gain == UNITY) ? comp_makeup_gain :
1827 (comp_makeup_gain == UNITY) ? release_gain :
1828 FRACMUL_SHL(release_gain, comp_makeup_gain, 7));
1830 /* Implement the compressor: apply total gain factor (if any) to the
1831 output buffer sample pair/mono sample */
1832 if (total_gain != UNITY)
1834 for (ch = 0; ch < num_chan; ch++)
1836 *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7);
1839 in_buf[0]++;
1840 in_buf[1]++;