Oops.
[Rockbox.git] / apps / dsp.c
blobf05c6f14ce5739dbefa594c7dc7df7ab802d25fa
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2005 Miika Pekkarinen
12 * All files in this archive are subject to the GNU General Public License.
13 * See the file COPYING in the source tree root for full license agreement.
15 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
16 * KIND, either express or implied.
18 ****************************************************************************/
19 #include "config.h"
20 #include <stdbool.h>
21 #include <inttypes.h>
22 #include <string.h>
23 #include <sound.h>
24 #include "dsp.h"
25 #include "eq.h"
26 #include "kernel.h"
27 #include "playback.h"
28 #include "system.h"
29 #include "settings.h"
30 #include "replaygain.h"
31 #include "misc.h"
32 #include "debug.h"
34 /* 16-bit samples are scaled based on these constants. The shift should be
35 * no more than 15.
37 #define WORD_SHIFT 12
38 #define WORD_FRACBITS 27
40 #define NATIVE_DEPTH 16
41 /* If the buffer sizes change, check the assembly code! */
42 #define SAMPLE_BUF_COUNT 256
43 #define RESAMPLE_BUF_COUNT (256 * 4) /* Enough for 11,025 Hz -> 44,100 Hz*/
44 #define DEFAULT_GAIN 0x01000000
45 #define SAMPLE_BUF_LEFT_CHANNEL 0
46 #define SAMPLE_BUF_RIGHT_CHANNEL (SAMPLE_BUF_COUNT/2)
47 #define RESAMPLE_BUF_LEFT_CHANNEL 0
48 #define RESAMPLE_BUF_RIGHT_CHANNEL (RESAMPLE_BUF_COUNT/2)
50 /* enums to index conversion properly with stereo mode and other settings */
51 enum
53 SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED,
54 SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED,
55 SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO,
56 SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES,
57 SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES,
58 SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES,
59 SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES
62 enum
64 SAMPLE_OUTPUT_MONO = 0,
65 SAMPLE_OUTPUT_STEREO,
66 SAMPLE_OUTPUT_DITHERED_MONO,
67 SAMPLE_OUTPUT_DITHERED_STEREO
70 /****************************************************************************
71 * NOTE: Any assembly routines that use these structures must be updated
72 * if current data members are moved or changed.
74 struct resample_data
76 uint32_t delta; /* 00h */
77 uint32_t phase; /* 04h */
78 int32_t last_sample[2]; /* 08h */
79 /* 10h */
82 /* This is for passing needed data to assembly dsp routines. If another
83 * dsp parameter needs to be passed, add to the end of the structure
84 * and remove from dsp_config.
85 * If another function type becomes assembly optimized and requires dsp
86 * config info, add a pointer paramter of type "struct dsp_data *".
87 * If removing something from other than the end, reserve the spot or
88 * else update every implementation for every target.
89 * Be sure to add the offset of the new member for easy viewing as well. :)
90 * It is the first member of dsp_config and all members can be accessesed
91 * through the main aggregate but this is intended to make a safe haven
92 * for these items whereas the c part can be rearranged at will. dsp_data
93 * could even moved within dsp_config without disurbing the order.
95 struct dsp_data
97 int output_scale; /* 00h */
98 int num_channels; /* 04h */
99 struct resample_data resample_data; /* 08h */
100 int32_t clip_min; /* 18h */
101 int32_t clip_max; /* 1ch */
102 int32_t gain; /* 20h - Note that this is in S8.23 format. */
103 /* 24h */
106 /* No asm...yet */
107 struct dither_data
109 long error[3]; /* 00h */
110 long random; /* 0ch */
111 /* 10h */
114 struct crossfeed_data
116 int32_t gain; /* 00h - Direct path gain */
117 int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */
118 int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */
119 int32_t delay[13][2]; /* 20h */
120 int32_t *index; /* 88h - Current pointer into the delay line */
121 /* 8ch */
124 /* Current setup is one lowshelf filters three peaking filters and one
125 * highshelf filter. Varying the number of shelving filters make no sense,
126 * but adding peaking filters is possible.
128 struct eq_state
130 char enabled[5]; /* 00h - Flags for active filters */
131 struct eqfilter filters[5]; /* 08h - packing is 4? */
132 /* 10ch */
135 /* Include header with defines which functions are implemented in assembly
136 code for the target */
137 #include <dsp_asm.h>
139 /* Typedefs keep things much neater in this case */
140 typedef void (*sample_input_fn_type)(int count, const char *src[],
141 int32_t *dst[]);
142 typedef int (*resample_fn_type)(int count, struct dsp_data *data,
143 int32_t *src[], int32_t *dst[]);
144 typedef void (*sample_output_fn_type)(int count, struct dsp_data *data,
145 int32_t *src[], int16_t *dst);
146 /* Single-DSP channel processing in place */
147 typedef void (*channels_process_fn_type)(int count, int32_t *buf[]);
148 /* DSP local channel processing in place */
149 typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data,
150 int32_t *buf[]);
154 ***************************************************************************/
156 struct dsp_config
158 struct dsp_data data; /* Config members for use in asm routines */
159 long codec_frequency; /* Sample rate of data coming from the codec */
160 long frequency; /* Effective sample rate after pitch shift (if any) */
161 int sample_depth;
162 int sample_bytes;
163 int stereo_mode;
164 int frac_bits;
165 /* Functions that change depending upon settings - NULL if stage is
166 disabled */
167 sample_input_fn_type input_samples;
168 resample_fn_type resample;
169 sample_output_fn_type output_samples;
170 /* These will be NULL for the voice codec and is more economical that
171 way */
172 channels_process_dsp_fn_type apply_gain;
173 channels_process_fn_type apply_crossfeed;
174 channels_process_fn_type channels_process;
177 /* General DSP config */
178 static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */
179 /* Dithering */
180 static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */
181 static long dither_mask IBSS_ATTR;
182 static long dither_bias IBSS_ATTR;
183 /* Crossfeed */
184 struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */
186 .index = (int32_t *)crossfeed_data.delay
189 /* Equalizer */
190 static struct eq_state eq_data; /* A/V */
191 #ifdef HAVE_SW_TONE_CONTROLS
192 static int prescale;
193 static int bass;
194 static int treble;
195 /* Filter struct for software bass/treble controls */
196 static struct eqfilter tone_filter;
197 #endif
199 /* Settings applicable to audio codec only */
200 static int pitch_ratio = 1000;
201 static int channels_mode;
202 long dsp_sw_gain;
203 long dsp_sw_cross;
204 static bool dither_enabled;
205 static bool eq_enabled IBSS_ATTR;
206 static long eq_precut;
207 static long track_gain;
208 static bool new_gain;
209 static long album_gain;
210 static long track_peak;
211 static long album_peak;
212 static long replaygain;
213 static bool crossfeed_enabled;
215 #define audio_dsp (&dsp_conf[CODEC_IDX_AUDIO])
216 #define voice_dsp (&dsp_conf[CODEC_IDX_VOICE])
217 static struct dsp_config *dsp IDATA_ATTR = audio_dsp;
219 /* The internal format is 32-bit samples, non-interleaved, stereo. This
220 * format is similar to the raw output from several codecs, so the amount
221 * of copying needed is minimized for that case.
224 int32_t sample_buf[SAMPLE_BUF_COUNT] IBSS_ATTR;
225 static int32_t resample_buf[RESAMPLE_BUF_COUNT] IBSS_ATTR;
227 /* set a new dsp and return old one */
228 static inline struct dsp_config * switch_dsp(struct dsp_config *_dsp)
230 struct dsp_config * old_dsp = dsp;
231 dsp = _dsp;
232 return old_dsp;
235 #if 0
236 /* Clip sample to arbitrary limits where range > 0 and min + range = max */
237 static inline long clip_sample(int32_t sample, int32_t min, int32_t range)
239 int32_t c = sample - min;
240 if ((uint32_t)c > (uint32_t)range)
242 sample -= c;
243 if (c > 0)
244 sample += range;
247 return sample;
249 #endif
251 /* Clip sample to signed 16 bit range */
252 static inline int32_t clip_sample_16(int32_t sample)
254 if ((int16_t)sample != sample)
255 sample = 0x7fff ^ (sample >> 31);
256 return sample;
259 int sound_get_pitch(void)
261 return pitch_ratio;
264 void sound_set_pitch(int permille)
266 pitch_ratio = permille;
268 dsp_configure(DSP_SWITCH_FREQUENCY, dsp->codec_frequency);
271 /* Convert count samples to the internal format, if needed. Updates src
272 * to point past the samples "consumed" and dst is set to point to the
273 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
274 * is no point in processing the same data twice.
277 /* convert count 16-bit mono to 32-bit mono */
278 static void sample_input_lte_native_mono(
279 int count, const char *src[], int32_t *dst[])
281 const int16_t *s = (int16_t *) src[0];
282 const int16_t * const send = s + count;
283 int32_t *d = dst[0] = dst[1] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
284 int scale = WORD_SHIFT;
288 *d++ = *s++ << scale;
290 while (s < send);
292 src[0] = (char *)s;
295 /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
296 static void sample_input_lte_native_i_stereo(
297 int count, const char *src[], int32_t *dst[])
299 const int32_t *s = (int32_t *) src[0];
300 const int32_t * const send = s + count;
301 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
302 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
303 int scale = WORD_SHIFT;
307 int32_t slr = *s++;
308 #ifdef ROCKBOX_LITTLE_ENDIAN
309 *dl++ = (slr >> 16) << scale;
310 *dr++ = (int32_t)(int16_t)slr << scale;
311 #else /* ROCKBOX_BIG_ENDIAN */
312 *dl++ = (int32_t)(int16_t)slr << scale;
313 *dr++ = (slr >> 16) << scale;
314 #endif
316 while (s < send);
318 src[0] = (char *)s;
321 /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
322 static void sample_input_lte_native_ni_stereo(
323 int count, const char *src[], int32_t *dst[])
325 const int16_t *sl = (int16_t *) src[0];
326 const int16_t *sr = (int16_t *) src[1];
327 const int16_t * const slend = sl + count;
328 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
329 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
330 int scale = WORD_SHIFT;
334 *dl++ = *sl++ << scale;
335 *dr++ = *sr++ << scale;
337 while (sl < slend);
339 src[0] = (char *)sl;
340 src[1] = (char *)sr;
343 /* convert count 32-bit mono to 32-bit mono */
344 static void sample_input_gt_native_mono(
345 int count, const char *src[], int32_t *dst[])
347 dst[0] = dst[1] = (int32_t *)src[0];
348 src[0] = (char *)(dst[0] + count);
351 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
352 static void sample_input_gt_native_i_stereo(
353 int count, const char *src[], int32_t *dst[])
355 const int32_t *s = (int32_t *)src[0];
356 const int32_t * const send = s + 2*count;
357 int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL];
358 int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL];
362 *dl++ = *s++;
363 *dr++ = *s++;
365 while (s < send);
367 src[0] = (char *)send;
370 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
371 static void sample_input_gt_native_ni_stereo(
372 int count, const char *src[], int32_t *dst[])
374 dst[0] = (int32_t *)src[0];
375 dst[1] = (int32_t *)src[1];
376 src[0] = (char *)(dst[0] + count);
377 src[1] = (char *)(dst[1] + count);
381 * sample_input_new_format()
383 * set the to-native sample conversion function based on dsp sample parameters
385 * !DSPPARAMSYNC
386 * needs syncing with changes to the following dsp parameters:
387 * * dsp->stereo_mode (A/V)
388 * * dsp->sample_depth (A/V)
390 static void sample_input_new_format(void)
392 static const sample_input_fn_type sample_input_functions[] =
394 [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono,
395 [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo,
396 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo,
397 [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono,
398 [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo,
399 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo,
402 int convert = dsp->stereo_mode;
404 if (dsp->sample_depth > NATIVE_DEPTH)
405 convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX;
407 dsp->input_samples = sample_input_functions[convert];
410 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
411 /* write mono internal format to output format */
412 static void sample_output_mono(int count, struct dsp_data *data,
413 int32_t *src[], int16_t *dst)
415 const int32_t *s0 = src[0];
416 const int scale = data->output_scale;
420 int32_t lr = clip_sample_16(*s0++ >> scale);
421 *dst++ = lr;
422 *dst++ = lr;
424 while (--count > 0);
426 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
428 /* write stereo internal format to output format */
429 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
430 static void sample_output_stereo(int count, struct dsp_data *data,
431 int32_t *src[], int16_t *dst)
433 const int32_t *s0 = src[0];
434 const int32_t *s1 = src[1];
435 const int scale = data->output_scale;
439 *dst++ = clip_sample_16(*s0++ >> scale);
440 *dst++ = clip_sample_16(*s1++ >> scale);
442 while (--count > 0);
444 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
447 * The "dither" code to convert the 24-bit samples produced by libmad was
448 * taken from the coolplayer project - coolplayer.sourceforge.net
450 * This function handles mono and stereo outputs.
452 static void sample_output_dithered(int count, struct dsp_data *data,
453 int32_t *src[], int16_t *dst)
455 const int32_t mask = dither_mask;
456 const int32_t bias = dither_bias;
457 const int scale = data->output_scale;
458 const int32_t min = data->clip_min;
459 const int32_t max = data->clip_max;
460 const int32_t range = max - min;
461 int ch;
462 int16_t *d;
464 for (ch = 0; ch < dsp->data.num_channels; ch++)
466 struct dither_data * const dither = &dither_data[ch];
467 int32_t *s = src[ch];
468 int i;
470 for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2)
472 int32_t output, sample;
473 int32_t random;
475 /* Noise shape and bias */
476 sample = *s;
477 sample += dither->error[0] - dither->error[1] + dither->error[2];
478 dither->error[2] = dither->error[1];
479 dither->error[1] = dither->error[0]/2;
481 output = sample + bias;
483 /* Dither */
484 random = dither->random*0x0019660dL + 0x3c6ef35fL;
485 output += (random & mask) - (dither->random & mask);
486 dither->random = random;
488 /* Clip */
489 int32_t c = output - min;
490 if ((uint32_t)c > (uint32_t)range)
492 output -= c;
493 if (c > 0)
495 output += range;
496 if (sample > max)
497 sample = max;
499 else if (sample < min)
501 sample = min;
505 output &= ~mask;
507 /* Error feedback */
508 dither->error[0] = sample - output;
510 /* Quantize */
511 *d = output >> scale;
515 if (dsp->data.num_channels == 2)
516 return;
518 /* Have to duplicate left samples into the right channel since
519 pcm buffer and hardware is interleaved stereo */
520 d = &dst[0];
524 int16_t s = *d++;
525 *d++ = s;
527 while (--count > 0);
531 * sample_output_new_format()
533 * set the from-native to ouput sample conversion routine
535 * !DSPPARAMSYNC
536 * needs syncing with changes to the following dsp parameters:
537 * * dsp->stereo_mode (A/V)
538 * * dither_enabled (A)
540 static void sample_output_new_format(void)
542 static const sample_output_fn_type sample_output_functions[] =
544 sample_output_mono,
545 sample_output_stereo,
546 sample_output_dithered,
547 sample_output_dithered
550 int out = dsp->data.num_channels - 1;
552 if (dsp == audio_dsp && dither_enabled)
553 out += 2;
555 dsp->output_samples = sample_output_functions[out];
559 * Linear interpolation resampling that introduces a one sample delay because
560 * of our inability to look into the future at the end of a frame.
562 #ifndef DSP_HAVE_ASM_RESAMPLING
563 static int dsp_downsample(int count, struct dsp_data *data,
564 int32_t *src[], int32_t *dst[])
566 int ch = data->num_channels - 1;
567 uint32_t delta = data->resample_data.delta;
568 uint32_t phase, pos;
569 int32_t *d;
571 /* Rolled channel loop actually showed slightly faster. */
574 /* Just initialize things and not worry too much about the relatively
575 * uncommon case of not being able to spit out a sample for the frame.
577 int32_t *s = src[ch];
578 int32_t last = data->resample_data.last_sample[ch];
580 data->resample_data.last_sample[ch] = s[count - 1];
581 d = dst[ch];
582 phase = data->resample_data.phase;
583 pos = phase >> 16;
585 /* Do we need last sample of previous frame for interpolation? */
586 if (pos > 0)
587 last = s[pos - 1];
589 while (pos < (uint32_t)count)
591 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
592 phase += delta;
593 pos = phase >> 16;
594 last = s[pos - 1];
597 while (--ch >= 0);
599 /* Wrap phase accumulator back to start of next frame. */
600 data->resample_data.phase = phase - (count << 16);
601 return d - dst[0];
604 static int dsp_upsample(int count, struct dsp_data *data,
605 int32_t *src[], int32_t *dst[])
607 int ch = data->num_channels - 1;
608 uint32_t delta = data->resample_data.delta;
609 uint32_t phase, pos;
610 int32_t *d;
612 /* Rolled channel loop actually showed slightly faster. */
615 /* Should always be able to output a sample for a ratio up to
616 RESAMPLE_BUF_COUNT / SAMPLE_BUF_COUNT. */
617 int32_t *s = src[ch];
618 int32_t last = data->resample_data.last_sample[ch];
620 data->resample_data.last_sample[ch] = s[count - 1];
621 d = dst[ch];
622 phase = data->resample_data.phase;
623 pos = phase >> 16;
625 while (pos == 0)
627 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last);
628 phase += delta;
629 pos = phase >> 16;
632 while (pos < (uint32_t)count)
634 last = s[pos - 1];
635 *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last);
636 phase += delta;
637 pos = phase >> 16;
640 while (--ch >= 0);
642 /* Wrap phase accumulator back to start of next frame. */
643 data->resample_data.phase = phase & 0xffff;
644 return d - dst[0];
646 #endif /* DSP_HAVE_ASM_RESAMPLING */
648 static void resampler_new_delta(void)
650 dsp->data.resample_data.delta = (unsigned long)
651 dsp->frequency * 65536LL / NATIVE_FREQUENCY;
653 if (dsp->frequency == NATIVE_FREQUENCY)
655 /* NOTE: If fully glitch-free transistions from no resampling to
656 resampling are desired, last_sample history should be maintained
657 even when not resampling. */
658 dsp->resample = NULL;
659 dsp->data.resample_data.phase = 0;
660 dsp->data.resample_data.last_sample[0] = 0;
661 dsp->data.resample_data.last_sample[1] = 0;
663 else if (dsp->frequency < NATIVE_FREQUENCY)
664 dsp->resample = dsp_upsample;
665 else
666 dsp->resample = dsp_downsample;
669 /* Resample count stereo samples. Updates the src array, if resampling is
670 * done, to refer to the resampled data. Returns number of stereo samples
671 * for further processing.
673 static inline int resample(int count, int32_t *src[])
675 int32_t *dst[2] =
677 &resample_buf[RESAMPLE_BUF_LEFT_CHANNEL],
678 &resample_buf[RESAMPLE_BUF_RIGHT_CHANNEL],
681 count = dsp->resample(count, &dsp->data, src, dst);
683 src[0] = dst[0];
684 src[1] = dst[dsp->data.num_channels - 1];
686 return count;
689 static void dither_init(void)
691 /* Voice codec should not reset the audio codec's dither data */
692 if (dsp != audio_dsp)
693 return;
695 memset(dither_data, 0, sizeof (dither_data));
696 dither_bias = (1L << (dsp->frac_bits - NATIVE_DEPTH));
697 dither_mask = (1L << (dsp->frac_bits + 1 - NATIVE_DEPTH)) - 1;
700 void dsp_dither_enable(bool enable)
702 /* Be sure audio dsp is current to set correct function */
703 struct dsp_config *old_dsp = switch_dsp(audio_dsp);
704 dither_enabled = enable;
705 sample_output_new_format();
706 switch_dsp(old_dsp);
709 /* Applies crossfeed to the stereo signal in src.
710 * Crossfeed is a process where listening over speakers is simulated. This
711 * is good for old hard panned stereo records, which might be quite fatiguing
712 * to listen to on headphones with no crossfeed.
714 #ifndef DSP_HAVE_ASM_CROSSFEED
715 static void apply_crossfeed(int count, int32_t *buf[])
717 int32_t *hist_l = &crossfeed_data.history[0];
718 int32_t *hist_r = &crossfeed_data.history[2];
719 int32_t *delay = &crossfeed_data.delay[0][0];
720 int32_t *coefs = &crossfeed_data.coefs[0];
721 int32_t gain = crossfeed_data.gain;
722 int32_t *di = crossfeed_data.index;
724 int32_t acc;
725 int32_t left, right;
726 int i;
728 for (i = 0; i < count; i++)
730 left = buf[0][i];
731 right = buf[1][i];
733 /* Filter delayed sample from left speaker */
734 ACC_INIT(acc, *di, coefs[0]);
735 ACC(acc, hist_l[0], coefs[1]);
736 ACC(acc, hist_l[1], coefs[2]);
737 /* Save filter history for left speaker */
738 hist_l[1] = GET_ACC(acc);
739 hist_l[0] = *di;
740 *di++ = left;
741 /* Filter delayed sample from right speaker */
742 ACC_INIT(acc, *di, coefs[0]);
743 ACC(acc, hist_r[0], coefs[1]);
744 ACC(acc, hist_r[1], coefs[2]);
745 /* Save filter history for right speaker */
746 hist_r[1] = GET_ACC(acc);
747 hist_r[0] = *di;
748 *di++ = right;
749 /* Now add the attenuated direct sound and write to outputs */
750 buf[0][i] = FRACMUL(left, gain) + hist_r[1];
751 buf[1][i] = FRACMUL(right, gain) + hist_l[1];
753 /* Wrap delay line index if bigger than delay line size */
754 if (di >= delay + 13*2)
755 di = delay;
757 /* Write back local copies of data we've modified */
758 crossfeed_data.index = di;
760 #endif /* DSP_HAVE_ASM_CROSSFEED */
763 * dsp_set_crossfeed(bool enable)
765 * !DSPPARAMSYNC
766 * needs syncing with changes to the following dsp parameters:
767 * * dsp->stereo_mode (A)
769 void dsp_set_crossfeed(bool enable)
771 crossfeed_enabled = enable;
772 audio_dsp->apply_crossfeed =
773 (enable && audio_dsp->data.num_channels > 1)
774 ? apply_crossfeed : NULL;
777 void dsp_set_crossfeed_direct_gain(int gain)
779 crossfeed_data.gain = get_replaygain_int(gain * -10) << 7;
780 /* If gain is negative, the calculation overflowed and we need to clamp */
781 if (crossfeed_data.gain < 0)
782 crossfeed_data.gain = 0x7fffffff;
785 /* Both gains should be below 0 dB (when inverted) */
786 void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
788 int32_t *c = crossfeed_data.coefs;
789 long scaler = get_replaygain_int(lf_gain * -10) << 7;
791 cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff;
792 hf_gain -= lf_gain;
793 /* Divide cutoff by sqrt(10^(-hf_gain/20)) to place cutoff at the -3 dB
794 * point instead of shelf midpoint. This is for compatibility with the old
795 * crossfeed shelf filter and should be removed if crossfeed settings are
796 * ever made incompatible for any other good reason.
798 cutoff = DIV64(cutoff, get_replaygain_int(-hf_gain*5), 24);
799 filter_shelf_coefs(cutoff, -hf_gain, false, c);
800 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
801 * over 1 and can do this safely
803 c[0] = FRACMUL_SHL(c[0], scaler, 4);
804 c[1] = FRACMUL_SHL(c[1], scaler, 4);
805 c[2] <<= 4;
808 /* Apply a constant gain to the samples (e.g., for ReplayGain).
809 * Note that this must be called before the resampler.
811 #ifndef DSP_HAVE_ASM_APPLY_GAIN
812 static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
814 const int32_t gain = data->gain;
815 int ch = data->num_channels - 1;
819 int32_t *s = buf[ch];
820 int32_t *d = buf[ch];
821 int32_t samp = *s++;
822 int i = 0;
826 FRACMUL_8_LOOP(samp, gain, s, d);
828 while (++i < count);
830 while (--ch >= 0);
832 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
834 /* Combine all gains to a global gain. */
835 static void set_gain(struct dsp_config *dsp)
837 dsp->data.gain = DEFAULT_GAIN;
839 /* Replay gain not relevant to voice */
840 if (dsp == audio_dsp && replaygain)
842 dsp->data.gain = replaygain;
845 if (eq_enabled && eq_precut)
847 dsp->data.gain =
848 (long) (((int64_t) dsp->data.gain * eq_precut) >> 24);
851 if (dsp->data.gain == DEFAULT_GAIN)
853 dsp->data.gain = 0;
855 else
857 dsp->data.gain >>= 1;
860 dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL;
864 * Use to enable the equalizer.
866 * @param enable true to enable the equalizer
868 void dsp_set_eq(bool enable)
870 eq_enabled = enable;
874 * Update the amount to cut the audio before applying the equalizer.
876 * @param precut to apply in decibels (multiplied by 10)
878 void dsp_set_eq_precut(int precut)
880 eq_precut = get_replaygain_int(precut * -10);
881 set_gain(audio_dsp);
882 set_gain(voice_dsp); /* For EQ precut */
886 * Synchronize the equalizer filter coefficients with the global settings.
888 * @param band the equalizer band to synchronize
890 void dsp_set_eq_coefs(int band)
892 const int *setting;
893 long gain;
894 unsigned long cutoff, q;
896 /* Adjust setting pointer to the band we actually want to change */
897 setting = &global_settings.eq_band0_cutoff + (band * 3);
899 /* Convert user settings to format required by coef generator functions */
900 cutoff = 0xffffffff / NATIVE_FREQUENCY * (*setting++);
901 q = *setting++;
902 gain = *setting++;
904 if (q == 0)
905 q = 1;
907 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
908 which it should be, since we're executed from the main thread. */
910 /* Assume a band is disabled if the gain is zero */
911 if (gain == 0)
913 eq_data.enabled[band] = 0;
915 else
917 if (band == 0)
918 eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
919 else if (band == 4)
920 eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
921 else
922 eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
924 eq_data.enabled[band] = 1;
928 /* Apply EQ filters to those bands that have got it switched on. */
929 static void eq_process(int count, int32_t *buf[])
931 static const int shifts[] =
933 EQ_SHELF_SHIFT, /* low shelf */
934 EQ_PEAK_SHIFT, /* peaking */
935 EQ_PEAK_SHIFT, /* peaking */
936 EQ_PEAK_SHIFT, /* peaking */
937 EQ_SHELF_SHIFT, /* high shelf */
939 unsigned int channels = dsp->data.num_channels;
940 int i;
942 /* filter configuration currently is 1 low shelf filter, 3 band peaking
943 filters and 1 high shelf filter, in that order. we need to know this
944 so we can choose the correct shift factor.
946 for (i = 0; i < 5; i++)
948 if (!eq_data.enabled[i])
949 continue;
950 eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]);
954 void dsp_set_stereo_width(int value)
956 long width, straight, cross;
958 width = value * 0x7fffff / 100;
960 if (value <= 100)
962 straight = (0x7fffff + width) / 2;
963 cross = straight - width;
965 else
967 /* straight = (1 + width) / (2 * width) */
968 straight = ((int64_t)(0x7fffff + width) << 22) / width;
969 cross = straight - 0x7fffff;
972 dsp_sw_gain = straight << 8;
973 dsp_sw_cross = cross << 8;
976 #if CONFIG_CODEC == SWCODEC
978 #ifdef HAVE_SW_TONE_CONTROLS
979 static void set_tone_controls(void)
981 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
982 0xffffffff/NATIVE_FREQUENCY*3500,
983 bass, treble, -prescale, tone_filter.coefs);
985 #endif
987 int dsp_callback(int msg, intptr_t param)
989 switch (msg) {
990 #ifdef HAVE_SW_TONE_CONTROLS
991 case DSP_CALLBACK_SET_PRESCALE:
992 prescale = param;
993 set_tone_controls();
994 break;
995 /* prescaler is always set after calling any of these, so we wait with
996 * calculating coefs until the above case is hit.
998 case DSP_CALLBACK_SET_BASS:
999 bass = param;
1000 break;
1001 case DSP_CALLBACK_SET_TREBLE:
1002 treble = param;
1003 #endif
1004 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
1005 dsp_set_channel_config(param);
1006 break;
1007 case DSP_CALLBACK_SET_STEREO_WIDTH:
1008 dsp_set_stereo_width(param);
1009 break;
1010 default:
1011 break;
1013 return 0;
1015 #endif
1018 * Implements the different channel configurations and stereo width.
1021 /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1022 * completeness. */
1023 #if 0
1024 static void channels_process_sound_chan_stereo(int count, int32_t *buf[])
1026 /* The channels are each just themselves */
1027 (void)count; (void)buf;
1029 #endif
1031 #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1032 static void channels_process_sound_chan_mono(int count, int32_t *buf[])
1034 int32_t *sl = buf[0], *sr = buf[1];
1038 int32_t lr = *sl/2 + *sr/2;
1039 *sl++ = lr;
1040 *sr++ = lr;
1042 while (--count > 0);
1044 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1046 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1047 static void channels_process_sound_chan_custom(int count, int32_t *buf[])
1049 const int32_t gain = dsp_sw_gain;
1050 const int32_t cross = dsp_sw_cross;
1051 int32_t *sl = buf[0], *sr = buf[1];
1055 int32_t l = *sl;
1056 int32_t r = *sr;
1057 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
1058 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
1060 while (--count > 0);
1062 #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1064 static void channels_process_sound_chan_mono_left(int count, int32_t *buf[])
1066 /* Just copy over the other channel */
1067 memcpy(buf[1], buf[0], count * sizeof (*buf));
1070 static void channels_process_sound_chan_mono_right(int count, int32_t *buf[])
1072 /* Just copy over the other channel */
1073 memcpy(buf[0], buf[1], count * sizeof (*buf));
1076 #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1077 static void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
1079 int32_t *sl = buf[0], *sr = buf[1];
1083 int32_t ch = *sl/2 - *sr/2;
1084 *sl++ = ch;
1085 *sr++ = -ch;
1087 while (--count > 0);
1089 #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1091 void dsp_set_channel_config(int value)
1093 static const channels_process_fn_type channels_process_functions[] =
1095 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1096 [SOUND_CHAN_STEREO] = NULL,
1097 [SOUND_CHAN_MONO] = channels_process_sound_chan_mono,
1098 [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom,
1099 [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left,
1100 [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right,
1101 [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke,
1104 if ((unsigned)value >= ARRAYLEN(channels_process_functions) ||
1105 audio_dsp->stereo_mode == STEREO_MONO)
1106 value = SOUND_CHAN_STEREO;
1108 /* This doesn't apply to voice */
1109 channels_mode = value;
1110 audio_dsp->channels_process = channels_process_functions[value];
1113 /* Process and convert src audio to dst based on the DSP configuration,
1114 * reading count number of audio samples. dst is assumed to be large
1115 * enough; use dsp_output_count() to get the required number. src is an
1116 * array of pointers; for mono and interleaved stereo, it contains one
1117 * pointer to the start of the audio data and the other is ignored; for
1118 * non-interleaved stereo, it contains two pointers, one for each audio
1119 * channel. Returns number of bytes written to dst.
1121 int dsp_process(char *dst, const char *src[], int count)
1123 int32_t *tmp[2];
1124 int written = 0;
1125 int samples;
1127 #if defined(CPU_COLDFIRE)
1128 /* set emac unit for dsp processing, and save old macsr, we're running in
1129 codec thread context at this point, so can't clobber it */
1130 unsigned long old_macsr = coldfire_get_macsr();
1131 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
1132 #endif
1134 if (new_gain)
1135 dsp_set_replaygain(); /* Gain has changed */
1137 /* Testing function pointers for NULL is preferred since the pointer
1138 will be preloaded to be used for the call if not. */
1139 while (count > 0)
1141 samples = MIN(SAMPLE_BUF_COUNT/2, count);
1142 count -= samples;
1144 dsp->input_samples(samples, src, tmp);
1146 if (dsp->apply_gain)
1147 dsp->apply_gain(samples, &dsp->data, tmp);
1149 if (dsp->resample && (samples = resample(samples, tmp)) <= 0)
1150 break; /* I'm pretty sure we're downsampling here */
1152 if (dsp->apply_crossfeed)
1153 dsp->apply_crossfeed(samples, tmp);
1155 /* TODO: EQ and tone controls need separate structs for audio and voice
1156 * DSP processing thanks to filter history. isn't really audible now, but
1157 * might be the day we start handling voice more delicately. Planned
1158 * changes may well run all relevent channels through the same EQ so
1159 * perhaps not.
1161 if (eq_enabled)
1162 eq_process(samples, tmp);
1164 #ifdef HAVE_SW_TONE_CONTROLS
1165 if ((bass | treble) != 0)
1166 eq_filter(tmp, &tone_filter, samples, dsp->data.num_channels,
1167 FILTER_BISHELF_SHIFT);
1168 #endif
1170 if (dsp->channels_process)
1171 dsp->channels_process(samples, tmp);
1173 dsp->output_samples(samples, &dsp->data, tmp, (int16_t *)dst);
1175 written += samples;
1176 dst += samples * sizeof (int16_t) * 2;
1177 yield();
1180 #if defined(CPU_COLDFIRE)
1181 /* set old macsr again */
1182 coldfire_set_macsr(old_macsr);
1183 #endif
1184 return written;
1187 /* Given count number of input samples, calculate the maximum number of
1188 * samples of output data that would be generated (the calculation is not
1189 * entirely exact and rounds upwards to be on the safe side; during
1190 * resampling, the number of samples generated depends on the current state
1191 * of the resampler).
1193 /* dsp_input_size MUST be called afterwards */
1194 int dsp_output_count(int count)
1196 if (dsp->resample)
1198 count = (int)(((unsigned long)count * NATIVE_FREQUENCY
1199 + (dsp->frequency - 1)) / dsp->frequency);
1202 /* Now we have the resampled sample count which must not exceed
1203 * RESAMPLE_BUF_COUNT/2 to avoid resample buffer overflow. One
1204 * must call dsp_input_count() to get the correct input sample
1205 * count.
1207 if (count > RESAMPLE_BUF_COUNT/2)
1208 count = RESAMPLE_BUF_COUNT/2;
1210 return count;
1213 /* Given count output samples, calculate number of input samples
1214 * that would be consumed in order to fill the output buffer.
1216 int dsp_input_count(int count)
1218 /* count is now the number of resampled input samples. Convert to
1219 original input samples. */
1220 if (dsp->resample)
1222 /* Use the real resampling delta =
1223 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1224 * round towards zero to avoid buffer overflows. */
1225 count = (int)(((unsigned long)count *
1226 dsp->data.resample_data.delta) >> 16);
1229 return count;
1232 int dsp_stereo_mode(void)
1234 return dsp->stereo_mode;
1237 static void dsp_set_gain_var(long *var, long value)
1239 /* Voice shouldn't mess with these */
1240 if (dsp == audio_dsp)
1242 *var = value;
1243 new_gain = true;
1247 static void dsp_update_functions(void)
1249 sample_input_new_format();
1250 sample_output_new_format();
1251 if (dsp == audio_dsp)
1252 dsp_set_crossfeed(crossfeed_enabled);
1255 bool dsp_configure(int setting, intptr_t value)
1257 switch (setting)
1259 case DSP_SWITCH_CODEC:
1260 if ((uintptr_t)value <= 1)
1261 switch_dsp(&dsp_conf[value]);
1262 break;
1264 case DSP_SET_FREQUENCY:
1265 memset(&dsp->data.resample_data, 0,
1266 sizeof (dsp->data.resample_data));
1267 /* Fall through!!! */
1268 case DSP_SWITCH_FREQUENCY:
1269 dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value;
1270 /* Account for playback speed adjustment when setting dsp->frequency
1271 if we're called from the main audio thread. Voice UI thread should
1272 not need this feature.
1274 if (dsp == audio_dsp)
1275 dsp->frequency = pitch_ratio * dsp->codec_frequency / 1000;
1276 else
1277 dsp->frequency = dsp->codec_frequency;
1279 resampler_new_delta();
1280 break;
1282 case DSP_SET_SAMPLE_DEPTH:
1283 dsp->sample_depth = value;
1285 if (dsp->sample_depth <= NATIVE_DEPTH)
1287 dsp->frac_bits = WORD_FRACBITS;
1288 dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */
1289 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1290 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1292 else
1294 dsp->frac_bits = value;
1295 dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */
1296 dsp->data.clip_max = (1 << value) - 1;
1297 dsp->data.clip_min = -(1 << value);
1300 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1301 sample_input_new_format();
1302 dither_init();
1303 break;
1305 case DSP_SET_STEREO_MODE:
1306 dsp->stereo_mode = value;
1307 dsp->data.num_channels = value == STEREO_MONO ? 1 : 2;
1308 dsp_update_functions();
1309 break;
1311 case DSP_RESET:
1312 dsp->stereo_mode = STEREO_NONINTERLEAVED;
1313 dsp->data.num_channels = 2;
1314 dsp->sample_depth = NATIVE_DEPTH;
1315 dsp->frac_bits = WORD_FRACBITS;
1316 dsp->sample_bytes = sizeof (int16_t);
1317 dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH;
1318 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1319 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1320 dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY;
1322 if (dsp == audio_dsp)
1324 track_gain = 0;
1325 album_gain = 0;
1326 track_peak = 0;
1327 album_peak = 0;
1328 new_gain = true;
1331 dsp_update_functions();
1332 resampler_new_delta();
1333 break;
1335 case DSP_FLUSH:
1336 memset(&dsp->data.resample_data, 0,
1337 sizeof (dsp->data.resample_data));
1338 resampler_new_delta();
1339 dither_init();
1340 break;
1342 case DSP_SET_TRACK_GAIN:
1343 dsp_set_gain_var(&track_gain, value);
1344 break;
1346 case DSP_SET_ALBUM_GAIN:
1347 dsp_set_gain_var(&album_gain, value);
1348 break;
1350 case DSP_SET_TRACK_PEAK:
1351 dsp_set_gain_var(&track_peak, value);
1352 break;
1354 case DSP_SET_ALBUM_PEAK:
1355 dsp_set_gain_var(&album_peak, value);
1356 break;
1358 default:
1359 return 0;
1362 return 1;
1365 void dsp_set_replaygain(void)
1367 long gain = 0;
1369 new_gain = false;
1371 if (global_settings.replaygain || global_settings.replaygain_noclip)
1373 bool track_mode = get_replaygain_mode(track_gain != 0,
1374 album_gain != 0) == REPLAYGAIN_TRACK;
1375 long peak = (track_mode || !album_peak) ? track_peak : album_peak;
1377 if (global_settings.replaygain)
1379 gain = (track_mode || !album_gain) ? track_gain : album_gain;
1381 if (global_settings.replaygain_preamp)
1383 long preamp = get_replaygain_int(
1384 global_settings.replaygain_preamp * 10);
1386 gain = (long) (((int64_t) gain * preamp) >> 24);
1390 if (gain == 0)
1392 /* So that noclip can work even with no gain information. */
1393 gain = DEFAULT_GAIN;
1396 if (global_settings.replaygain_noclip && (peak != 0)
1397 && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN))
1399 gain = (((int64_t) DEFAULT_GAIN << 24) / peak);
1402 if (gain == DEFAULT_GAIN)
1404 /* Nothing to do, disable processing. */
1405 gain = 0;
1409 /* Store in S8.23 format to simplify calculations. */
1410 replaygain = gain;
1411 set_gain(audio_dsp);