1 /***************************************************************************
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
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 ****************************************************************************/
32 #include "replaygain.h"
36 #include "fixedpoint.h"
38 /* 16-bit samples are scaled based on these constants. The shift should be
42 #define WORD_FRACBITS 27
44 #define NATIVE_DEPTH 16
45 /* If the small buffer size changes, check the assembly code! */
46 #define SMALL_SAMPLE_BUF_COUNT 256
47 #define DEFAULT_GAIN 0x01000000
49 /* enums to index conversion properly with stereo mode and other settings */
52 SAMPLE_INPUT_LE_NATIVE_I_STEREO
= STEREO_INTERLEAVED
,
53 SAMPLE_INPUT_LE_NATIVE_NI_STEREO
= STEREO_NONINTERLEAVED
,
54 SAMPLE_INPUT_LE_NATIVE_MONO
= STEREO_MONO
,
55 SAMPLE_INPUT_GT_NATIVE_I_STEREO
= STEREO_INTERLEAVED
+ STEREO_NUM_MODES
,
56 SAMPLE_INPUT_GT_NATIVE_NI_STEREO
= STEREO_NONINTERLEAVED
+ STEREO_NUM_MODES
,
57 SAMPLE_INPUT_GT_NATIVE_MONO
= STEREO_MONO
+ STEREO_NUM_MODES
,
58 SAMPLE_INPUT_GT_NATIVE_1ST_INDEX
= STEREO_NUM_MODES
63 SAMPLE_OUTPUT_MONO
= 0,
65 SAMPLE_OUTPUT_DITHERED_MONO
,
66 SAMPLE_OUTPUT_DITHERED_STEREO
69 /****************************************************************************
70 * NOTE: Any assembly routines that use these structures must be updated
71 * if current data members are moved or changed.
75 uint32_t delta
; /* 00h */
76 uint32_t phase
; /* 04h */
77 int32_t last_sample
[2]; /* 08h */
81 /* This is for passing needed data to assembly dsp routines. If another
82 * dsp parameter needs to be passed, add to the end of the structure
83 * and remove from dsp_config.
84 * If another function type becomes assembly optimized and requires dsp
85 * config info, add a pointer paramter of type "struct dsp_data *".
86 * If removing something from other than the end, reserve the spot or
87 * else update every implementation for every target.
88 * Be sure to add the offset of the new member for easy viewing as well. :)
89 * It is the first member of dsp_config and all members can be accessesed
90 * through the main aggregate but this is intended to make a safe haven
91 * for these items whereas the c part can be rearranged at will. dsp_data
92 * could even moved within dsp_config without disurbing the order.
96 int output_scale
; /* 00h */
97 int num_channels
; /* 04h */
98 struct resample_data resample_data
; /* 08h */
99 int32_t clip_min
; /* 18h */
100 int32_t clip_max
; /* 1ch */
101 int32_t gain
; /* 20h - Note that this is in S8.23 format. */
108 long error
[3]; /* 00h */
109 long random
; /* 0ch */
113 struct crossfeed_data
115 int32_t gain
; /* 00h - Direct path gain */
116 int32_t coefs
[3]; /* 04h - Coefficients for the shelving filter */
117 int32_t history
[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */
118 int32_t delay
[13][2]; /* 20h */
119 int32_t *index
; /* 88h - Current pointer into the delay line */
123 /* Current setup is one lowshelf filters three peaking filters and one
124 * highshelf filter. Varying the number of shelving filters make no sense,
125 * but adding peaking filters is possible.
129 char enabled
[5]; /* 00h - Flags for active filters */
130 struct eqfilter filters
[5]; /* 08h - packing is 4? */
134 /* Include header with defines which functions are implemented in assembly
135 code for the target */
138 /* Typedefs keep things much neater in this case */
139 typedef void (*sample_input_fn_type
)(int count
, const char *src
[],
141 typedef int (*resample_fn_type
)(int count
, struct dsp_data
*data
,
142 const int32_t *src
[], int32_t *dst
[]);
143 typedef void (*sample_output_fn_type
)(int count
, struct dsp_data
*data
,
144 const 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
,
154 ***************************************************************************/
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) */
164 int tdspeed_percent
; /* Speed % */
165 bool tdspeed_active
; /* Timestretch is in use */
167 #ifdef HAVE_SW_TONE_CONTROLS
168 /* Filter struct for software bass/treble controls */
169 struct eqfilter tone_filter
;
171 /* Functions that change depending upon settings - NULL if stage is
173 sample_input_fn_type input_samples
;
174 resample_fn_type resample
;
175 sample_output_fn_type output_samples
;
176 /* These will be NULL for the voice codec and is more economical that
178 channels_process_dsp_fn_type apply_gain
;
179 channels_process_fn_type apply_crossfeed
;
180 channels_process_fn_type eq_process
;
181 channels_process_fn_type channels_process
;
184 /* General DSP config */
185 static struct dsp_config dsp_conf
[2] IBSS_ATTR
; /* 0=A, 1=V */
187 static struct dither_data dither_data
[2] IBSS_ATTR
; /* 0=left, 1=right */
188 static long dither_mask IBSS_ATTR
;
189 static long dither_bias IBSS_ATTR
;
191 struct crossfeed_data crossfeed_data IDATA_ATTR
= /* A */
193 .index
= (int32_t *)crossfeed_data
.delay
197 static struct eq_state eq_data
; /* A */
199 /* Software tone controls */
200 #ifdef HAVE_SW_TONE_CONTROLS
201 static int prescale
; /* A/V */
202 static int bass
; /* A/V */
203 static int treble
; /* A/V */
206 /* Settings applicable to audio codec only */
207 static int pitch_ratio
= 1000;
208 static int channels_mode
;
211 static bool dither_enabled
;
212 static long eq_precut
;
213 static long track_gain
;
214 static bool new_gain
;
215 static long album_gain
;
216 static long track_peak
;
217 static long album_peak
;
218 static long replaygain
;
219 static bool crossfeed_enabled
;
221 #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
222 #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
224 /* The internal format is 32-bit samples, non-interleaved, stereo. This
225 * format is similar to the raw output from several codecs, so the amount
226 * of copying needed is minimized for that case.
229 #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
231 static int32_t small_sample_buf
[SMALL_SAMPLE_BUF_COUNT
] IBSS_ATTR
;
232 static int32_t small_resample_buf
[SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
] IBSS_ATTR
;
234 static int32_t *big_sample_buf
= NULL
;
235 static int32_t *big_resample_buf
= NULL
;
236 static int big_sample_buf_count
= -1; /* -1=unknown, 0=not available */
238 static int sample_buf_count
;
239 static int32_t *sample_buf
;
240 static int32_t *resample_buf
;
242 #define SAMPLE_BUF_LEFT_CHANNEL 0
243 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2)
244 #define RESAMPLE_BUF_LEFT_CHANNEL 0
245 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO)
248 /* Clip sample to signed 16 bit range */
249 static inline int32_t clip_sample_16(int32_t sample
)
251 if ((int16_t)sample
!= sample
)
252 sample
= 0x7fff ^ (sample
>> 31);
256 int sound_get_pitch(void)
261 void sound_set_pitch(int permille
)
263 pitch_ratio
= permille
;
264 dsp_configure(&AUDIO_DSP
, DSP_SWITCH_FREQUENCY
,
265 AUDIO_DSP
.codec_frequency
);
268 static void tdspeed_setup(struct dsp_config
*dspc
)
270 /* Assume timestretch will not be used */
271 dspc
->tdspeed_active
= false;
272 sample_buf
= small_sample_buf
;
273 resample_buf
= small_resample_buf
;
274 sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
;
276 if(!dsp_timestretch_available())
277 return; /* Timestretch not enabled or buffer not allocated */
278 if (dspc
->tdspeed_percent
== 0)
279 dspc
->tdspeed_percent
= 100;
281 dspc
->codec_frequency
== 0 ? NATIVE_FREQUENCY
: dspc
->codec_frequency
,
282 dspc
->stereo_mode
!= STEREO_MONO
,
283 dspc
->tdspeed_percent
))
284 return; /* Timestretch not possible or needed with these parameters */
286 /* Timestretch is to be used */
287 dspc
->tdspeed_active
= true;
288 sample_buf
= big_sample_buf
;
289 sample_buf_count
= big_sample_buf_count
;
290 resample_buf
= big_resample_buf
;
293 void dsp_timestretch_enable(bool enabled
)
295 /* Hook to set up timestretch buffer on first call to settings_apply() */
296 if (big_sample_buf_count
< 0) /* Only do something on first call */
300 /* Set up timestretch buffers */
301 big_sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
;
302 big_sample_buf
= small_resample_buf
;
303 big_resample_buf
= (int32_t *) buffer_alloc(big_sample_buf_count
* RESAMPLE_RATIO
* sizeof(int32_t));
307 /* Not enabled at startup, "big" buffers will never be available */
308 big_sample_buf_count
= 0;
310 tdspeed_setup(&AUDIO_DSP
);
314 void dsp_set_timestretch(int percent
)
316 AUDIO_DSP
.tdspeed_percent
= percent
;
317 tdspeed_setup(&AUDIO_DSP
);
320 int dsp_get_timestretch()
322 return AUDIO_DSP
.tdspeed_percent
;
325 bool dsp_timestretch_available()
327 return (global_settings
.timestretch_enabled
&& big_sample_buf_count
> 0);
330 /* Convert count samples to the internal format, if needed. Updates src
331 * to point past the samples "consumed" and dst is set to point to the
332 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
333 * is no point in processing the same data twice.
336 /* convert count 16-bit mono to 32-bit mono */
337 static void sample_input_lte_native_mono(
338 int count
, const char *src
[], int32_t *dst
[])
340 const int16_t *s
= (int16_t *) src
[0];
341 const int16_t * const send
= s
+ count
;
342 int32_t *d
= dst
[0] = dst
[1] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
343 int scale
= WORD_SHIFT
;
347 *d
++ = *s
++ << scale
;
354 /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
355 static void sample_input_lte_native_i_stereo(
356 int count
, const char *src
[], int32_t *dst
[])
358 const int32_t *s
= (int32_t *) src
[0];
359 const int32_t * const send
= s
+ count
;
360 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
361 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
362 int scale
= WORD_SHIFT
;
367 #ifdef ROCKBOX_LITTLE_ENDIAN
368 *dl
++ = (slr
>> 16) << scale
;
369 *dr
++ = (int32_t)(int16_t)slr
<< scale
;
370 #else /* ROCKBOX_BIG_ENDIAN */
371 *dl
++ = (int32_t)(int16_t)slr
<< scale
;
372 *dr
++ = (slr
>> 16) << scale
;
380 /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
381 static void sample_input_lte_native_ni_stereo(
382 int count
, const char *src
[], int32_t *dst
[])
384 const int16_t *sl
= (int16_t *) src
[0];
385 const int16_t *sr
= (int16_t *) src
[1];
386 const int16_t * const slend
= sl
+ count
;
387 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
388 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
389 int scale
= WORD_SHIFT
;
393 *dl
++ = *sl
++ << scale
;
394 *dr
++ = *sr
++ << scale
;
402 /* convert count 32-bit mono to 32-bit mono */
403 static void sample_input_gt_native_mono(
404 int count
, const char *src
[], int32_t *dst
[])
406 dst
[0] = dst
[1] = (int32_t *)src
[0];
407 src
[0] = (char *)(dst
[0] + count
);
410 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
411 static void sample_input_gt_native_i_stereo(
412 int count
, const char *src
[], int32_t *dst
[])
414 const int32_t *s
= (int32_t *)src
[0];
415 const int32_t * const send
= s
+ 2*count
;
416 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
417 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
426 src
[0] = (char *)send
;
429 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
430 static void sample_input_gt_native_ni_stereo(
431 int count
, const char *src
[], int32_t *dst
[])
433 dst
[0] = (int32_t *)src
[0];
434 dst
[1] = (int32_t *)src
[1];
435 src
[0] = (char *)(dst
[0] + count
);
436 src
[1] = (char *)(dst
[1] + count
);
440 * sample_input_new_format()
442 * set the to-native sample conversion function based on dsp sample parameters
445 * needs syncing with changes to the following dsp parameters:
446 * * dsp->stereo_mode (A/V)
447 * * dsp->sample_depth (A/V)
449 static void sample_input_new_format(struct dsp_config
*dsp
)
451 static const sample_input_fn_type sample_input_functions
[] =
453 [SAMPLE_INPUT_LE_NATIVE_MONO
] = sample_input_lte_native_mono
,
454 [SAMPLE_INPUT_LE_NATIVE_I_STEREO
] = sample_input_lte_native_i_stereo
,
455 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO
] = sample_input_lte_native_ni_stereo
,
456 [SAMPLE_INPUT_GT_NATIVE_MONO
] = sample_input_gt_native_mono
,
457 [SAMPLE_INPUT_GT_NATIVE_I_STEREO
] = sample_input_gt_native_i_stereo
,
458 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO
] = sample_input_gt_native_ni_stereo
,
461 int convert
= dsp
->stereo_mode
;
463 if (dsp
->sample_depth
> NATIVE_DEPTH
)
464 convert
+= SAMPLE_INPUT_GT_NATIVE_1ST_INDEX
;
466 dsp
->input_samples
= sample_input_functions
[convert
];
470 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
471 /* write mono internal format to output format */
472 static void sample_output_mono(int count
, struct dsp_data
*data
,
473 const int32_t *src
[], int16_t *dst
)
475 const int32_t *s0
= src
[0];
476 const int scale
= data
->output_scale
;
477 const int dc_bias
= 1 << (scale
- 1);
481 int32_t lr
= clip_sample_16((*s0
++ + dc_bias
) >> scale
);
487 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
489 /* write stereo internal format to output format */
490 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
491 static void sample_output_stereo(int count
, struct dsp_data
*data
,
492 const int32_t *src
[], int16_t *dst
)
494 const int32_t *s0
= src
[0];
495 const int32_t *s1
= src
[1];
496 const int scale
= data
->output_scale
;
497 const int dc_bias
= 1 << (scale
- 1);
501 *dst
++ = clip_sample_16((*s0
++ + dc_bias
) >> scale
);
502 *dst
++ = clip_sample_16((*s1
++ + dc_bias
) >> scale
);
506 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
509 * The "dither" code to convert the 24-bit samples produced by libmad was
510 * taken from the coolplayer project - coolplayer.sourceforge.net
512 * This function handles mono and stereo outputs.
514 static void sample_output_dithered(int count
, struct dsp_data
*data
,
515 const int32_t *src
[], int16_t *dst
)
517 const int32_t mask
= dither_mask
;
518 const int32_t bias
= dither_bias
;
519 const int scale
= data
->output_scale
;
520 const int32_t min
= data
->clip_min
;
521 const int32_t max
= data
->clip_max
;
522 const int32_t range
= max
- min
;
526 for (ch
= 0; ch
< data
->num_channels
; ch
++)
528 struct dither_data
* const dither
= &dither_data
[ch
];
529 const int32_t *s
= src
[ch
];
532 for (i
= 0, d
= &dst
[ch
]; i
< count
; i
++, s
++, d
+= 2)
534 int32_t output
, sample
;
537 /* Noise shape and bias (for correct rounding later) */
539 sample
+= dither
->error
[0] - dither
->error
[1] + dither
->error
[2];
540 dither
->error
[2] = dither
->error
[1];
541 dither
->error
[1] = dither
->error
[0]/2;
543 output
= sample
+ bias
;
545 /* Dither, highpass triangle PDF */
546 random
= dither
->random
*0x0019660dL
+ 0x3c6ef35fL
;
547 output
+= (random
& mask
) - (dither
->random
& mask
);
548 dither
->random
= random
;
550 /* Round sample to output range */
554 dither
->error
[0] = sample
- output
;
557 if ((uint32_t)(output
- min
) > (uint32_t)range
)
565 /* Quantize and store */
566 *d
= output
>> scale
;
570 if (data
->num_channels
== 2)
573 /* Have to duplicate left samples into the right channel since
574 pcm buffer and hardware is interleaved stereo */
586 * sample_output_new_format()
588 * set the from-native to ouput sample conversion routine
591 * needs syncing with changes to the following dsp parameters:
592 * * dsp->stereo_mode (A/V)
593 * * dither_enabled (A)
595 static void sample_output_new_format(struct dsp_config
*dsp
)
597 static const sample_output_fn_type sample_output_functions
[] =
600 sample_output_stereo
,
601 sample_output_dithered
,
602 sample_output_dithered
605 int out
= dsp
->data
.num_channels
- 1;
607 if (dsp
== &AUDIO_DSP
&& dither_enabled
)
610 dsp
->output_samples
= sample_output_functions
[out
];
614 * Linear interpolation resampling that introduces a one sample delay because
615 * of our inability to look into the future at the end of a frame.
617 #ifndef DSP_HAVE_ASM_RESAMPLING
618 static int dsp_downsample(int count
, struct dsp_data
*data
,
619 const int32_t *src
[], int32_t *dst
[])
621 int ch
= data
->num_channels
- 1;
622 uint32_t delta
= data
->resample_data
.delta
;
626 /* Rolled channel loop actually showed slightly faster. */
629 /* Just initialize things and not worry too much about the relatively
630 * uncommon case of not being able to spit out a sample for the frame.
632 const int32_t *s
= src
[ch
];
633 int32_t last
= data
->resample_data
.last_sample
[ch
];
635 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
637 phase
= data
->resample_data
.phase
;
640 /* Do we need last sample of previous frame for interpolation? */
644 while (pos
< (uint32_t)count
)
646 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
654 /* Wrap phase accumulator back to start of next frame. */
655 data
->resample_data
.phase
= phase
- (count
<< 16);
659 static int dsp_upsample(int count
, struct dsp_data
*data
,
660 const int32_t *src
[], int32_t *dst
[])
662 int ch
= data
->num_channels
- 1;
663 uint32_t delta
= data
->resample_data
.delta
;
667 /* Rolled channel loop actually showed slightly faster. */
670 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
671 const int32_t *s
= src
[ch
];
672 int32_t last
= data
->resample_data
.last_sample
[ch
];
674 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
676 phase
= data
->resample_data
.phase
;
681 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[0] - last
);
686 while (pos
< (uint32_t)count
)
689 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
696 /* Wrap phase accumulator back to start of next frame. */
697 data
->resample_data
.phase
= phase
& 0xffff;
700 #endif /* DSP_HAVE_ASM_RESAMPLING */
702 static void resampler_new_delta(struct dsp_config
*dsp
)
704 dsp
->data
.resample_data
.delta
= (unsigned long)
705 dsp
->frequency
* 65536LL / NATIVE_FREQUENCY
;
707 if (dsp
->frequency
== NATIVE_FREQUENCY
)
709 /* NOTE: If fully glitch-free transistions from no resampling to
710 resampling are desired, last_sample history should be maintained
711 even when not resampling. */
712 dsp
->resample
= NULL
;
713 dsp
->data
.resample_data
.phase
= 0;
714 dsp
->data
.resample_data
.last_sample
[0] = 0;
715 dsp
->data
.resample_data
.last_sample
[1] = 0;
717 else if (dsp
->frequency
< NATIVE_FREQUENCY
)
718 dsp
->resample
= dsp_upsample
;
720 dsp
->resample
= dsp_downsample
;
723 /* Resample count stereo samples. Updates the src array, if resampling is
724 * done, to refer to the resampled data. Returns number of stereo samples
725 * for further processing.
727 static inline int resample(struct dsp_config
*dsp
, int count
, int32_t *src
[])
731 &resample_buf
[RESAMPLE_BUF_LEFT_CHANNEL
],
732 &resample_buf
[RESAMPLE_BUF_RIGHT_CHANNEL
],
735 count
= dsp
->resample(count
, &dsp
->data
, (const int32_t **)src
, dst
);
738 src
[1] = dst
[dsp
->data
.num_channels
- 1];
743 static void dither_init(struct dsp_config
*dsp
)
745 memset(dither_data
, 0, sizeof (dither_data
));
746 dither_bias
= (1L << (dsp
->frac_bits
- NATIVE_DEPTH
));
747 dither_mask
= (1L << (dsp
->frac_bits
+ 1 - NATIVE_DEPTH
)) - 1;
750 void dsp_dither_enable(bool enable
)
752 struct dsp_config
*dsp
= &AUDIO_DSP
;
753 dither_enabled
= enable
;
754 sample_output_new_format(dsp
);
757 /* Applies crossfeed to the stereo signal in src.
758 * Crossfeed is a process where listening over speakers is simulated. This
759 * is good for old hard panned stereo records, which might be quite fatiguing
760 * to listen to on headphones with no crossfeed.
762 #ifndef DSP_HAVE_ASM_CROSSFEED
763 static void apply_crossfeed(int count
, int32_t *buf
[])
765 int32_t *hist_l
= &crossfeed_data
.history
[0];
766 int32_t *hist_r
= &crossfeed_data
.history
[2];
767 int32_t *delay
= &crossfeed_data
.delay
[0][0];
768 int32_t *coefs
= &crossfeed_data
.coefs
[0];
769 int32_t gain
= crossfeed_data
.gain
;
770 int32_t *di
= crossfeed_data
.index
;
776 for (i
= 0; i
< count
; i
++)
781 /* Filter delayed sample from left speaker */
782 acc
= FRACMUL(*di
, coefs
[0]);
783 acc
+= FRACMUL(hist_l
[0], coefs
[1]);
784 acc
+= FRACMUL(hist_l
[1], coefs
[2]);
785 /* Save filter history for left speaker */
789 /* Filter delayed sample from right speaker */
790 acc
= FRACMUL(*di
, coefs
[0]);
791 acc
+= FRACMUL(hist_r
[0], coefs
[1]);
792 acc
+= FRACMUL(hist_r
[1], coefs
[2]);
793 /* Save filter history for right speaker */
797 /* Now add the attenuated direct sound and write to outputs */
798 buf
[0][i
] = FRACMUL(left
, gain
) + hist_r
[1];
799 buf
[1][i
] = FRACMUL(right
, gain
) + hist_l
[1];
801 /* Wrap delay line index if bigger than delay line size */
802 if (di
>= delay
+ 13*2)
805 /* Write back local copies of data we've modified */
806 crossfeed_data
.index
= di
;
808 #endif /* DSP_HAVE_ASM_CROSSFEED */
811 * dsp_set_crossfeed(bool enable)
814 * needs syncing with changes to the following dsp parameters:
815 * * dsp->stereo_mode (A)
817 void dsp_set_crossfeed(bool enable
)
819 crossfeed_enabled
= enable
;
820 AUDIO_DSP
.apply_crossfeed
= (enable
&& AUDIO_DSP
.data
.num_channels
> 1)
821 ? apply_crossfeed
: NULL
;
824 void dsp_set_crossfeed_direct_gain(int gain
)
826 crossfeed_data
.gain
= get_replaygain_int(gain
* 10) << 7;
827 /* If gain is negative, the calculation overflowed and we need to clamp */
828 if (crossfeed_data
.gain
< 0)
829 crossfeed_data
.gain
= 0x7fffffff;
832 /* Both gains should be below 0 dB */
833 void dsp_set_crossfeed_cross_params(long lf_gain
, long hf_gain
, long cutoff
)
835 int32_t *c
= crossfeed_data
.coefs
;
836 long scaler
= get_replaygain_int(lf_gain
* 10) << 7;
838 cutoff
= 0xffffffff/NATIVE_FREQUENCY
*cutoff
;
840 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
841 * point instead of shelf midpoint. This is for compatibility with the old
842 * crossfeed shelf filter and should be removed if crossfeed settings are
843 * ever made incompatible for any other good reason.
845 cutoff
= DIV64(cutoff
, get_replaygain_int(hf_gain
*5), 24);
846 filter_shelf_coefs(cutoff
, hf_gain
, false, c
);
847 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
848 * over 1 and can do this safely
850 c
[0] = FRACMUL_SHL(c
[0], scaler
, 4);
851 c
[1] = FRACMUL_SHL(c
[1], scaler
, 4);
855 /* Apply a constant gain to the samples (e.g., for ReplayGain).
856 * Note that this must be called before the resampler.
858 #ifndef DSP_HAVE_ASM_APPLY_GAIN
859 static void dsp_apply_gain(int count
, struct dsp_data
*data
, int32_t *buf
[])
861 const int32_t gain
= data
->gain
;
864 for (ch
= 0; ch
< data
->num_channels
; ch
++)
866 int32_t *d
= buf
[ch
];
869 for (i
= 0; i
< count
; i
++)
870 d
[i
] = FRACMUL_SHL(d
[i
], gain
, 8);
873 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
875 /* Combine all gains to a global gain. */
876 static void set_gain(struct dsp_config
*dsp
)
878 dsp
->data
.gain
= DEFAULT_GAIN
;
880 /* Replay gain not relevant to voice */
881 if (dsp
== &AUDIO_DSP
&& replaygain
)
883 dsp
->data
.gain
= replaygain
;
886 if (dsp
->eq_process
&& eq_precut
)
889 (long) (((int64_t) dsp
->data
.gain
* eq_precut
) >> 24);
892 if (dsp
->data
.gain
== DEFAULT_GAIN
)
898 dsp
->data
.gain
>>= 1;
901 dsp
->apply_gain
= dsp
->data
.gain
!= 0 ? dsp_apply_gain
: NULL
;
905 * Update the amount to cut the audio before applying the equalizer.
907 * @param precut to apply in decibels (multiplied by 10)
909 void dsp_set_eq_precut(int precut
)
911 eq_precut
= get_replaygain_int(precut
* -10);
912 set_gain(&AUDIO_DSP
);
916 * Synchronize the equalizer filter coefficients with the global settings.
918 * @param band the equalizer band to synchronize
920 void dsp_set_eq_coefs(int band
)
924 unsigned long cutoff
, q
;
926 /* Adjust setting pointer to the band we actually want to change */
927 setting
= &global_settings
.eq_band0_cutoff
+ (band
* 3);
929 /* Convert user settings to format required by coef generator functions */
930 cutoff
= 0xffffffff / NATIVE_FREQUENCY
* (*setting
++);
937 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
938 which it should be, since we're executed from the main thread. */
940 /* Assume a band is disabled if the gain is zero */
943 eq_data
.enabled
[band
] = 0;
948 eq_ls_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
950 eq_hs_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
952 eq_pk_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
954 eq_data
.enabled
[band
] = 1;
958 /* Apply EQ filters to those bands that have got it switched on. */
959 static void eq_process(int count
, int32_t *buf
[])
961 static const int shifts
[] =
963 EQ_SHELF_SHIFT
, /* low shelf */
964 EQ_PEAK_SHIFT
, /* peaking */
965 EQ_PEAK_SHIFT
, /* peaking */
966 EQ_PEAK_SHIFT
, /* peaking */
967 EQ_SHELF_SHIFT
, /* high shelf */
969 unsigned int channels
= AUDIO_DSP
.data
.num_channels
;
972 /* filter configuration currently is 1 low shelf filter, 3 band peaking
973 filters and 1 high shelf filter, in that order. we need to know this
974 so we can choose the correct shift factor.
976 for (i
= 0; i
< 5; i
++)
978 if (!eq_data
.enabled
[i
])
980 eq_filter(buf
, &eq_data
.filters
[i
], count
, channels
, shifts
[i
]);
985 * Use to enable the equalizer.
987 * @param enable true to enable the equalizer
989 void dsp_set_eq(bool enable
)
991 AUDIO_DSP
.eq_process
= enable
? eq_process
: NULL
;
992 set_gain(&AUDIO_DSP
);
995 static void dsp_set_stereo_width(int value
)
997 long width
, straight
, cross
;
999 width
= value
* 0x7fffff / 100;
1003 straight
= (0x7fffff + width
) / 2;
1004 cross
= straight
- width
;
1008 /* straight = (1 + width) / (2 * width) */
1009 straight
= ((int64_t)(0x7fffff + width
) << 22) / width
;
1010 cross
= straight
- 0x7fffff;
1013 dsp_sw_gain
= straight
<< 8;
1014 dsp_sw_cross
= cross
<< 8;
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
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
;
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;
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];
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;
1087 while (--count
> 0);
1089 #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1091 static 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
)
1107 value
= SOUND_CHAN_STEREO
;
1110 /* This doesn't apply to voice */
1111 channels_mode
= value
;
1112 AUDIO_DSP
.channels_process
= channels_process_functions
[value
];
1115 #if CONFIG_CODEC == SWCODEC
1117 #ifdef HAVE_SW_TONE_CONTROLS
1118 static void set_tone_controls(void)
1120 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY
*200,
1121 0xffffffff/NATIVE_FREQUENCY
*3500,
1122 bass
, treble
, -prescale
,
1123 AUDIO_DSP
.tone_filter
.coefs
);
1124 /* Sync the voice dsp coefficients */
1125 memcpy(&VOICE_DSP
.tone_filter
.coefs
, AUDIO_DSP
.tone_filter
.coefs
,
1126 sizeof (VOICE_DSP
.tone_filter
.coefs
));
1130 /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1133 int dsp_callback(int msg
, intptr_t param
)
1137 #ifdef HAVE_SW_TONE_CONTROLS
1138 case DSP_CALLBACK_SET_PRESCALE
:
1140 set_tone_controls();
1142 /* prescaler is always set after calling any of these, so we wait with
1143 * calculating coefs until the above case is hit.
1145 case DSP_CALLBACK_SET_BASS
:
1148 case DSP_CALLBACK_SET_TREBLE
:
1152 case DSP_CALLBACK_SET_CHANNEL_CONFIG
:
1153 dsp_set_channel_config(param
);
1155 case DSP_CALLBACK_SET_STEREO_WIDTH
:
1156 dsp_set_stereo_width(param
);
1165 /* Process and convert src audio to dst based on the DSP configuration,
1166 * reading count number of audio samples. dst is assumed to be large
1167 * enough; use dsp_output_count() to get the required number. src is an
1168 * array of pointers; for mono and interleaved stereo, it contains one
1169 * pointer to the start of the audio data and the other is ignored; for
1170 * non-interleaved stereo, it contains two pointers, one for each audio
1171 * channel. Returns number of bytes written to dst.
1173 int dsp_process(struct dsp_config
*dsp
, char *dst
, const char *src
[], int count
)
1176 static long last_yield
;
1180 #if defined(CPU_COLDFIRE)
1181 /* set emac unit for dsp processing, and save old macsr, we're running in
1182 codec thread context at this point, so can't clobber it */
1183 unsigned long old_macsr
= coldfire_get_macsr();
1184 coldfire_set_macsr(EMAC_FRACTIONAL
| EMAC_SATURATE
);
1188 dsp_set_replaygain(); /* Gain has changed */
1190 /* Perform at least one yield before starting */
1191 last_yield
= current_tick
;
1194 /* Testing function pointers for NULL is preferred since the pointer
1195 will be preloaded to be used for the call if not. */
1198 int samples
= MIN(sample_buf_count
/2, count
);
1201 dsp
->input_samples(samples
, src
, tmp
);
1203 if (dsp
->tdspeed_active
)
1204 samples
= tdspeed_doit(tmp
, samples
);
1206 int chunk_offset
= 0;
1210 t2
[0] = tmp
[0]+chunk_offset
;
1211 t2
[1] = tmp
[1]+chunk_offset
;
1213 int chunk
= MIN(sample_buf_count
/2, samples
);
1214 chunk_offset
+= chunk
;
1217 if (dsp
->apply_gain
)
1218 dsp
->apply_gain(chunk
, &dsp
->data
, t2
);
1220 if (dsp
->resample
&& (chunk
= resample(dsp
, chunk
, t2
)) <= 0)
1221 break; /* I'm pretty sure we're downsampling here */
1223 if (dsp
->apply_crossfeed
)
1224 dsp
->apply_crossfeed(chunk
, t2
);
1226 if (dsp
->eq_process
)
1227 dsp
->eq_process(chunk
, t2
);
1229 #ifdef HAVE_SW_TONE_CONTROLS
1230 if ((bass
| treble
) != 0)
1231 eq_filter(t2
, &dsp
->tone_filter
, chunk
,
1232 dsp
->data
.num_channels
, FILTER_BISHELF_SHIFT
);
1235 if (dsp
->channels_process
)
1236 dsp
->channels_process(chunk
, t2
);
1238 dsp
->output_samples(chunk
, &dsp
->data
, (const int32_t **)t2
, (int16_t *)dst
);
1241 dst
+= chunk
* sizeof (int16_t) * 2;
1243 /* yield at least once each tick */
1244 tick
= current_tick
;
1245 if (TIME_AFTER(tick
, last_yield
))
1253 #if defined(CPU_COLDFIRE)
1254 /* set old macsr again */
1255 coldfire_set_macsr(old_macsr
);
1260 /* Given count number of input samples, calculate the maximum number of
1261 * samples of output data that would be generated (the calculation is not
1262 * entirely exact and rounds upwards to be on the safe side; during
1263 * resampling, the number of samples generated depends on the current state
1264 * of the resampler).
1266 /* dsp_input_size MUST be called afterwards */
1267 int dsp_output_count(struct dsp_config
*dsp
, int count
)
1269 if (dsp
->tdspeed_active
)
1270 count
= tdspeed_est_output_size();
1273 count
= (int)(((unsigned long)count
* NATIVE_FREQUENCY
1274 + (dsp
->frequency
- 1)) / dsp
->frequency
);
1277 /* Now we have the resampled sample count which must not exceed
1278 * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One
1279 * must call dsp_input_count() to get the correct input sample
1282 if (count
> RESAMPLE_BUF_RIGHT_CHANNEL
)
1283 count
= RESAMPLE_BUF_RIGHT_CHANNEL
;
1288 /* Given count output samples, calculate number of input samples
1289 * that would be consumed in order to fill the output buffer.
1291 int dsp_input_count(struct dsp_config
*dsp
, int count
)
1293 /* count is now the number of resampled input samples. Convert to
1294 original input samples. */
1297 /* Use the real resampling delta =
1298 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1299 * round towards zero to avoid buffer overflows. */
1300 count
= (int)(((unsigned long)count
*
1301 dsp
->data
.resample_data
.delta
) >> 16);
1304 if (dsp
->tdspeed_active
)
1305 count
= tdspeed_est_input_size(count
);
1310 static void dsp_set_gain_var(long *var
, long value
)
1316 static void dsp_update_functions(struct dsp_config
*dsp
)
1318 sample_input_new_format(dsp
);
1319 sample_output_new_format(dsp
);
1320 if (dsp
== &AUDIO_DSP
)
1321 dsp_set_crossfeed(crossfeed_enabled
);
1324 intptr_t dsp_configure(struct dsp_config
*dsp
, int setting
, intptr_t value
)
1331 case CODEC_IDX_AUDIO
:
1332 return (intptr_t)&AUDIO_DSP
;
1333 case CODEC_IDX_VOICE
:
1334 return (intptr_t)&VOICE_DSP
;
1336 return (intptr_t)NULL
;
1339 case DSP_SET_FREQUENCY
:
1340 memset(&dsp
->data
.resample_data
, 0, sizeof (dsp
->data
.resample_data
));
1341 /* Fall through!!! */
1342 case DSP_SWITCH_FREQUENCY
:
1343 dsp
->codec_frequency
= (value
== 0) ? NATIVE_FREQUENCY
: value
;
1344 /* Account for playback speed adjustment when setting dsp->frequency
1345 if we're called from the main audio thread. Voice UI thread should
1346 not need this feature.
1348 if (dsp
== &AUDIO_DSP
)
1349 dsp
->frequency
= pitch_ratio
* dsp
->codec_frequency
/ 1000;
1351 dsp
->frequency
= dsp
->codec_frequency
;
1353 resampler_new_delta(dsp
);
1357 case DSP_SET_SAMPLE_DEPTH
:
1358 dsp
->sample_depth
= value
;
1360 if (dsp
->sample_depth
<= NATIVE_DEPTH
)
1362 dsp
->frac_bits
= WORD_FRACBITS
;
1363 dsp
->sample_bytes
= sizeof (int16_t); /* samples are 16 bits */
1364 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1365 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1369 dsp
->frac_bits
= value
;
1370 dsp
->sample_bytes
= sizeof (int32_t); /* samples are 32 bits */
1371 dsp
->data
.clip_max
= (1 << value
) - 1;
1372 dsp
->data
.clip_min
= -(1 << value
);
1375 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1376 sample_input_new_format(dsp
);
1380 case DSP_SET_STEREO_MODE
:
1381 dsp
->stereo_mode
= value
;
1382 dsp
->data
.num_channels
= value
== STEREO_MONO
? 1 : 2;
1383 dsp_update_functions(dsp
);
1388 dsp
->stereo_mode
= STEREO_NONINTERLEAVED
;
1389 dsp
->data
.num_channels
= 2;
1390 dsp
->sample_depth
= NATIVE_DEPTH
;
1391 dsp
->frac_bits
= WORD_FRACBITS
;
1392 dsp
->sample_bytes
= sizeof (int16_t);
1393 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1394 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1395 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1396 dsp
->codec_frequency
= dsp
->frequency
= NATIVE_FREQUENCY
;
1398 if (dsp
== &AUDIO_DSP
)
1407 dsp_update_functions(dsp
);
1408 resampler_new_delta(dsp
);
1413 memset(&dsp
->data
.resample_data
, 0,
1414 sizeof (dsp
->data
.resample_data
));
1415 resampler_new_delta(dsp
);
1420 case DSP_SET_TRACK_GAIN
:
1421 if (dsp
== &AUDIO_DSP
)
1422 dsp_set_gain_var(&track_gain
, value
);
1425 case DSP_SET_ALBUM_GAIN
:
1426 if (dsp
== &AUDIO_DSP
)
1427 dsp_set_gain_var(&album_gain
, value
);
1430 case DSP_SET_TRACK_PEAK
:
1431 if (dsp
== &AUDIO_DSP
)
1432 dsp_set_gain_var(&track_peak
, value
);
1435 case DSP_SET_ALBUM_PEAK
:
1436 if (dsp
== &AUDIO_DSP
)
1437 dsp_set_gain_var(&album_peak
, value
);
1447 void dsp_set_replaygain(void)
1453 if ((global_settings
.replaygain_type
!= REPLAYGAIN_OFF
) ||
1454 global_settings
.replaygain_noclip
)
1456 bool track_mode
= get_replaygain_mode(track_gain
!= 0,
1457 album_gain
!= 0) == REPLAYGAIN_TRACK
;
1458 long peak
= (track_mode
|| !album_peak
) ? track_peak
: album_peak
;
1460 if (global_settings
.replaygain_type
!= REPLAYGAIN_OFF
)
1462 gain
= (track_mode
|| !album_gain
) ? track_gain
: album_gain
;
1464 if (global_settings
.replaygain_preamp
)
1466 long preamp
= get_replaygain_int(
1467 global_settings
.replaygain_preamp
* 10);
1469 gain
= (long) (((int64_t) gain
* preamp
) >> 24);
1475 /* So that noclip can work even with no gain information. */
1476 gain
= DEFAULT_GAIN
;
1479 if (global_settings
.replaygain_noclip
&& (peak
!= 0)
1480 && ((((int64_t) gain
* peak
) >> 24) >= DEFAULT_GAIN
))
1482 gain
= (((int64_t) DEFAULT_GAIN
<< 24) / peak
);
1485 if (gain
== DEFAULT_GAIN
)
1487 /* Nothing to do, disable processing. */
1492 /* Store in S8.23 format to simplify calculations. */
1494 set_gain(&AUDIO_DSP
);