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"
39 /* 16-bit samples are scaled based on these constants. The shift should be
43 #define WORD_FRACBITS 27
45 #define NATIVE_DEPTH 16
46 /* If the small buffer size changes, check the assembly code! */
47 #define SMALL_SAMPLE_BUF_COUNT 256
48 #define DEFAULT_GAIN 0x01000000
50 /* enums to index conversion properly with stereo mode and other settings */
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
64 SAMPLE_OUTPUT_MONO
= 0,
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.
76 uint32_t delta
; /* 00h */
77 uint32_t phase
; /* 04h */
78 int32_t last_sample
[2]; /* 08h */
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.
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. */
109 long error
[3]; /* 00h */
110 long random
; /* 0ch */
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 */
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.
130 char enabled
[5]; /* 00h - Flags for active filters */
131 struct eqfilter filters
[5]; /* 08h - packing is 4? */
135 /* Include header with defines which functions are implemented in assembly
136 code for the target */
139 /* Typedefs keep things much neater in this case */
140 typedef void (*sample_input_fn_type
)(int count
, const char *src
[],
142 typedef int (*resample_fn_type
)(int count
, struct dsp_data
*data
,
143 const int32_t *src
[], int32_t *dst
[]);
144 typedef void (*sample_output_fn_type
)(int count
, struct dsp_data
*data
,
145 const int32_t *src
[], int16_t *dst
);
147 /* Single-DSP channel processing in place */
148 typedef void (*channels_process_fn_type
)(int count
, int32_t *buf
[]);
149 /* DSP local channel processing in place */
150 typedef void (*channels_process_dsp_fn_type
)(int count
, struct dsp_data
*data
,
155 ***************************************************************************/
159 struct dsp_data data
; /* Config members for use in asm routines */
160 long codec_frequency
; /* Sample rate of data coming from the codec */
161 long frequency
; /* Effective sample rate after pitch shift (if any) */
165 int32_t tdspeed_percent
; /* Speed% * PITCH_SPEED_PRECISION */
166 bool tdspeed_active
; /* Timestretch is in use */
168 #ifdef HAVE_SW_TONE_CONTROLS
169 /* Filter struct for software bass/treble controls */
170 struct eqfilter tone_filter
;
172 /* Functions that change depending upon settings - NULL if stage is
174 sample_input_fn_type input_samples
;
175 resample_fn_type resample
;
176 sample_output_fn_type output_samples
;
177 /* These will be NULL for the voice codec and is more economical that
179 channels_process_dsp_fn_type apply_gain
;
180 channels_process_fn_type apply_crossfeed
;
181 channels_process_fn_type eq_process
;
182 channels_process_fn_type channels_process
;
185 /* General DSP config */
186 static struct dsp_config dsp_conf
[2] IBSS_ATTR
; /* 0=A, 1=V */
188 static struct dither_data dither_data
[2] IBSS_ATTR
; /* 0=left, 1=right */
189 static long dither_mask IBSS_ATTR
;
190 static long dither_bias IBSS_ATTR
;
192 struct crossfeed_data crossfeed_data IDATA_ATTR
= /* A */
194 .index
= (int32_t *)crossfeed_data
.delay
198 static struct eq_state eq_data
; /* A */
200 /* Software tone controls */
201 #ifdef HAVE_SW_TONE_CONTROLS
202 static int prescale
; /* A/V */
203 static int bass
; /* A/V */
204 static int treble
; /* A/V */
207 /* Settings applicable to audio codec only */
208 static int32_t pitch_ratio
= PITCH_SPEED_100
;
209 static int channels_mode
;
212 static bool dither_enabled
;
213 static long eq_precut
;
214 static long track_gain
;
215 static bool new_gain
;
216 static long album_gain
;
217 static long track_peak
;
218 static long album_peak
;
219 static long replaygain
;
220 static bool crossfeed_enabled
;
222 #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
223 #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
225 /* The internal format is 32-bit samples, non-interleaved, stereo. This
226 * format is similar to the raw output from several codecs, so the amount
227 * of copying needed is minimized for that case.
230 #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
232 static int32_t small_sample_buf
[SMALL_SAMPLE_BUF_COUNT
] IBSS_ATTR
;
233 static int32_t small_resample_buf
[SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
] IBSS_ATTR
;
235 static int32_t *big_sample_buf
= NULL
;
236 static int32_t *big_resample_buf
= NULL
;
237 static int big_sample_buf_count
= -1; /* -1=unknown, 0=not available */
239 static int sample_buf_count
;
240 static int32_t *sample_buf
;
241 static int32_t *resample_buf
;
243 #define SAMPLE_BUF_LEFT_CHANNEL 0
244 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2)
245 #define RESAMPLE_BUF_LEFT_CHANNEL 0
246 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO)
249 /* Clip sample to signed 16 bit range */
250 static inline int32_t clip_sample_16(int32_t sample
)
252 if ((int16_t)sample
!= sample
)
253 sample
= 0x7fff ^ (sample
>> 31);
257 int32_t sound_get_pitch(void)
262 void sound_set_pitch(int32_t percent
)
264 pitch_ratio
= percent
;
265 dsp_configure(&AUDIO_DSP
, DSP_SWITCH_FREQUENCY
,
266 AUDIO_DSP
.codec_frequency
);
269 static void tdspeed_setup(struct dsp_config
*dspc
)
271 /* Assume timestretch will not be used */
272 dspc
->tdspeed_active
= false;
273 sample_buf
= small_sample_buf
;
274 resample_buf
= small_resample_buf
;
275 sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
;
277 if(!dsp_timestretch_available())
278 return; /* Timestretch not enabled or buffer not allocated */
279 if (dspc
->tdspeed_percent
== 0)
280 dspc
->tdspeed_percent
= PITCH_SPEED_100
;
282 dspc
->codec_frequency
== 0 ? NATIVE_FREQUENCY
: dspc
->codec_frequency
,
283 dspc
->stereo_mode
!= STEREO_MONO
,
284 dspc
->tdspeed_percent
))
285 return; /* Timestretch not possible or needed with these parameters */
287 /* Timestretch is to be used */
288 dspc
->tdspeed_active
= true;
289 sample_buf
= big_sample_buf
;
290 sample_buf_count
= big_sample_buf_count
;
291 resample_buf
= big_resample_buf
;
294 void dsp_timestretch_enable(bool enabled
)
296 /* Hook to set up timestretch buffer on first call to settings_apply() */
297 if (big_sample_buf_count
< 0) /* Only do something on first call */
301 /* Set up timestretch buffers */
302 big_sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
;
303 big_sample_buf
= small_resample_buf
;
304 big_resample_buf
= (int32_t *) buffer_alloc(big_sample_buf_count
* RESAMPLE_RATIO
* sizeof(int32_t));
308 /* Not enabled at startup, "big" buffers will never be available */
309 big_sample_buf_count
= 0;
311 tdspeed_setup(&AUDIO_DSP
);
315 void dsp_set_timestretch(int32_t percent
)
317 AUDIO_DSP
.tdspeed_percent
= percent
;
318 tdspeed_setup(&AUDIO_DSP
);
321 int32_t dsp_get_timestretch()
323 return AUDIO_DSP
.tdspeed_percent
;
326 bool dsp_timestretch_available()
328 return (global_settings
.timestretch_enabled
&& big_sample_buf_count
> 0);
331 /* Convert count samples to the internal format, if needed. Updates src
332 * to point past the samples "consumed" and dst is set to point to the
333 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
334 * is no point in processing the same data twice.
337 /* convert count 16-bit mono to 32-bit mono */
338 static void sample_input_lte_native_mono(
339 int count
, const char *src
[], int32_t *dst
[])
341 const int16_t *s
= (int16_t *) src
[0];
342 const int16_t * const send
= s
+ count
;
343 int32_t *d
= dst
[0] = dst
[1] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
344 int scale
= WORD_SHIFT
;
348 *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
;
379 /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */
380 static void sample_input_lte_native_ni_stereo(
381 int count
, const char *src
[], int32_t *dst
[])
383 const int16_t *sl
= (int16_t *) src
[0];
384 const int16_t *sr
= (int16_t *) src
[1];
385 const int16_t * const slend
= sl
+ count
;
386 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
387 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
388 int scale
= WORD_SHIFT
;
392 *dl
++ = *sl
++ << scale
;
393 *dr
++ = *sr
++ << scale
;
400 /* convert count 32-bit mono to 32-bit mono */
401 static void sample_input_gt_native_mono(
402 int count
, const char *src
[], int32_t *dst
[])
404 dst
[0] = dst
[1] = (int32_t *)src
[0];
405 src
[0] = (char *)(dst
[0] + count
);
408 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
409 static void sample_input_gt_native_i_stereo(
410 int count
, const char *src
[], int32_t *dst
[])
412 const int32_t *s
= (int32_t *)src
[0];
413 const int32_t * const send
= s
+ 2*count
;
414 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
415 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
423 src
[0] = (char *)send
;
426 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
427 static void sample_input_gt_native_ni_stereo(
428 int count
, const char *src
[], int32_t *dst
[])
430 dst
[0] = (int32_t *)src
[0];
431 dst
[1] = (int32_t *)src
[1];
432 src
[0] = (char *)(dst
[0] + count
);
433 src
[1] = (char *)(dst
[1] + count
);
437 * sample_input_new_format()
439 * set the to-native sample conversion function based on dsp sample parameters
442 * needs syncing with changes to the following dsp parameters:
443 * * dsp->stereo_mode (A/V)
444 * * dsp->sample_depth (A/V)
446 static void sample_input_new_format(struct dsp_config
*dsp
)
448 static const sample_input_fn_type sample_input_functions
[] =
450 [SAMPLE_INPUT_LE_NATIVE_I_STEREO
] = sample_input_lte_native_i_stereo
,
451 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO
] = sample_input_lte_native_ni_stereo
,
452 [SAMPLE_INPUT_LE_NATIVE_MONO
] = sample_input_lte_native_mono
,
453 [SAMPLE_INPUT_GT_NATIVE_I_STEREO
] = sample_input_gt_native_i_stereo
,
454 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO
] = sample_input_gt_native_ni_stereo
,
455 [SAMPLE_INPUT_GT_NATIVE_MONO
] = sample_input_gt_native_mono
,
458 int convert
= dsp
->stereo_mode
;
460 if (dsp
->sample_depth
> NATIVE_DEPTH
)
461 convert
+= SAMPLE_INPUT_GT_NATIVE_1ST_INDEX
;
463 dsp
->input_samples
= sample_input_functions
[convert
];
467 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
468 /* write mono internal format to output format */
469 static void sample_output_mono(int count
, struct dsp_data
*data
,
470 const int32_t *src
[], int16_t *dst
)
472 const int32_t *s0
= src
[0];
473 const int scale
= data
->output_scale
;
474 const int dc_bias
= 1 << (scale
- 1);
478 int32_t lr
= clip_sample_16((*s0
++ + dc_bias
) >> scale
);
483 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
485 /* write stereo internal format to output format */
486 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
487 static void sample_output_stereo(int count
, struct dsp_data
*data
,
488 const int32_t *src
[], int16_t *dst
)
490 const int32_t *s0
= src
[0];
491 const int32_t *s1
= src
[1];
492 const int scale
= data
->output_scale
;
493 const int dc_bias
= 1 << (scale
- 1);
497 *dst
++ = clip_sample_16((*s0
++ + dc_bias
) >> scale
);
498 *dst
++ = clip_sample_16((*s1
++ + dc_bias
) >> scale
);
501 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
504 * The "dither" code to convert the 24-bit samples produced by libmad was
505 * taken from the coolplayer project - coolplayer.sourceforge.net
507 * This function handles mono and stereo outputs.
509 static void sample_output_dithered(int count
, struct dsp_data
*data
,
510 const int32_t *src
[], int16_t *dst
)
512 const int32_t mask
= dither_mask
;
513 const int32_t bias
= dither_bias
;
514 const int scale
= data
->output_scale
;
515 const int32_t min
= data
->clip_min
;
516 const int32_t max
= data
->clip_max
;
517 const int32_t range
= max
- min
;
521 for (ch
= 0; ch
< data
->num_channels
; ch
++)
523 struct dither_data
* const dither
= &dither_data
[ch
];
524 const int32_t *s
= src
[ch
];
527 for (i
= 0, d
= &dst
[ch
]; i
< count
; i
++, s
++, d
+= 2)
529 int32_t output
, sample
;
532 /* Noise shape and bias (for correct rounding later) */
534 sample
+= dither
->error
[0] - dither
->error
[1] + dither
->error
[2];
535 dither
->error
[2] = dither
->error
[1];
536 dither
->error
[1] = dither
->error
[0]/2;
538 output
= sample
+ bias
;
540 /* Dither, highpass triangle PDF */
541 random
= dither
->random
*0x0019660dL
+ 0x3c6ef35fL
;
542 output
+= (random
& mask
) - (dither
->random
& mask
);
543 dither
->random
= random
;
545 /* Round sample to output range */
549 dither
->error
[0] = sample
- output
;
552 if ((uint32_t)(output
- min
) > (uint32_t)range
)
560 /* Quantize and store */
561 *d
= output
>> scale
;
565 if (data
->num_channels
== 2)
568 /* Have to duplicate left samples into the right channel since
569 pcm buffer and hardware is interleaved stereo */
580 * sample_output_new_format()
582 * set the from-native to ouput sample conversion routine
585 * needs syncing with changes to the following dsp parameters:
586 * * dsp->stereo_mode (A/V)
587 * * dither_enabled (A)
589 static void sample_output_new_format(struct dsp_config
*dsp
)
591 static const sample_output_fn_type sample_output_functions
[] =
594 sample_output_stereo
,
595 sample_output_dithered
,
596 sample_output_dithered
599 int out
= dsp
->data
.num_channels
- 1;
601 if (dsp
== &AUDIO_DSP
&& dither_enabled
)
604 dsp
->output_samples
= sample_output_functions
[out
];
608 * Linear interpolation resampling that introduces a one sample delay because
609 * of our inability to look into the future at the end of a frame.
611 #ifndef DSP_HAVE_ASM_RESAMPLING
612 static int dsp_downsample(int count
, struct dsp_data
*data
,
613 const int32_t *src
[], int32_t *dst
[])
615 int ch
= data
->num_channels
- 1;
616 uint32_t delta
= data
->resample_data
.delta
;
620 /* Rolled channel loop actually showed slightly faster. */
623 /* Just initialize things and not worry too much about the relatively
624 * uncommon case of not being able to spit out a sample for the frame.
626 const int32_t *s
= src
[ch
];
627 int32_t last
= data
->resample_data
.last_sample
[ch
];
629 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
631 phase
= data
->resample_data
.phase
;
634 /* Do we need last sample of previous frame for interpolation? */
638 while (pos
< (uint32_t)count
)
640 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
648 /* Wrap phase accumulator back to start of next frame. */
649 data
->resample_data
.phase
= phase
- (count
<< 16);
653 static int dsp_upsample(int count
, struct dsp_data
*data
,
654 const int32_t *src
[], int32_t *dst
[])
656 int ch
= data
->num_channels
- 1;
657 uint32_t delta
= data
->resample_data
.delta
;
661 /* Rolled channel loop actually showed slightly faster. */
664 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
665 const int32_t *s
= src
[ch
];
666 int32_t last
= data
->resample_data
.last_sample
[ch
];
668 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
670 phase
= data
->resample_data
.phase
;
675 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[0] - last
);
680 while (pos
< (uint32_t)count
)
683 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
690 /* Wrap phase accumulator back to start of next frame. */
691 data
->resample_data
.phase
= phase
& 0xffff;
694 #endif /* DSP_HAVE_ASM_RESAMPLING */
696 static void resampler_new_delta(struct dsp_config
*dsp
)
698 dsp
->data
.resample_data
.delta
= (unsigned long)
699 dsp
->frequency
* 65536LL / NATIVE_FREQUENCY
;
701 if (dsp
->frequency
== NATIVE_FREQUENCY
)
703 /* NOTE: If fully glitch-free transistions from no resampling to
704 resampling are desired, last_sample history should be maintained
705 even when not resampling. */
706 dsp
->resample
= NULL
;
707 dsp
->data
.resample_data
.phase
= 0;
708 dsp
->data
.resample_data
.last_sample
[0] = 0;
709 dsp
->data
.resample_data
.last_sample
[1] = 0;
711 else if (dsp
->frequency
< NATIVE_FREQUENCY
)
712 dsp
->resample
= dsp_upsample
;
714 dsp
->resample
= dsp_downsample
;
717 /* Resample count stereo samples. Updates the src array, if resampling is
718 * done, to refer to the resampled data. Returns number of stereo samples
719 * for further processing.
721 static inline int resample(struct dsp_config
*dsp
, int count
, int32_t *src
[])
725 &resample_buf
[RESAMPLE_BUF_LEFT_CHANNEL
],
726 &resample_buf
[RESAMPLE_BUF_RIGHT_CHANNEL
],
729 count
= dsp
->resample(count
, &dsp
->data
, (const int32_t **)src
, dst
);
732 src
[1] = dst
[dsp
->data
.num_channels
- 1];
737 static void dither_init(struct dsp_config
*dsp
)
739 memset(dither_data
, 0, sizeof (dither_data
));
740 dither_bias
= (1L << (dsp
->frac_bits
- NATIVE_DEPTH
));
741 dither_mask
= (1L << (dsp
->frac_bits
+ 1 - NATIVE_DEPTH
)) - 1;
744 void dsp_dither_enable(bool enable
)
746 struct dsp_config
*dsp
= &AUDIO_DSP
;
747 dither_enabled
= enable
;
748 sample_output_new_format(dsp
);
751 /* Applies crossfeed to the stereo signal in src.
752 * Crossfeed is a process where listening over speakers is simulated. This
753 * is good for old hard panned stereo records, which might be quite fatiguing
754 * to listen to on headphones with no crossfeed.
756 #ifndef DSP_HAVE_ASM_CROSSFEED
757 static void apply_crossfeed(int count
, int32_t *buf
[])
759 int32_t *hist_l
= &crossfeed_data
.history
[0];
760 int32_t *hist_r
= &crossfeed_data
.history
[2];
761 int32_t *delay
= &crossfeed_data
.delay
[0][0];
762 int32_t *coefs
= &crossfeed_data
.coefs
[0];
763 int32_t gain
= crossfeed_data
.gain
;
764 int32_t *di
= crossfeed_data
.index
;
770 for (i
= 0; i
< count
; i
++)
775 /* Filter delayed sample from left speaker */
776 acc
= FRACMUL(*di
, coefs
[0]);
777 acc
+= FRACMUL(hist_l
[0], coefs
[1]);
778 acc
+= FRACMUL(hist_l
[1], coefs
[2]);
779 /* Save filter history for left speaker */
783 /* Filter delayed sample from right speaker */
784 acc
= FRACMUL(*di
, coefs
[0]);
785 acc
+= FRACMUL(hist_r
[0], coefs
[1]);
786 acc
+= FRACMUL(hist_r
[1], coefs
[2]);
787 /* Save filter history for right speaker */
791 /* Now add the attenuated direct sound and write to outputs */
792 buf
[0][i
] = FRACMUL(left
, gain
) + hist_r
[1];
793 buf
[1][i
] = FRACMUL(right
, gain
) + hist_l
[1];
795 /* Wrap delay line index if bigger than delay line size */
796 if (di
>= delay
+ 13*2)
799 /* Write back local copies of data we've modified */
800 crossfeed_data
.index
= di
;
802 #endif /* DSP_HAVE_ASM_CROSSFEED */
805 * dsp_set_crossfeed(bool enable)
808 * needs syncing with changes to the following dsp parameters:
809 * * dsp->stereo_mode (A)
811 void dsp_set_crossfeed(bool enable
)
813 crossfeed_enabled
= enable
;
814 AUDIO_DSP
.apply_crossfeed
= (enable
&& AUDIO_DSP
.data
.num_channels
> 1)
815 ? apply_crossfeed
: NULL
;
818 void dsp_set_crossfeed_direct_gain(int gain
)
820 crossfeed_data
.gain
= get_replaygain_int(gain
* 10) << 7;
821 /* If gain is negative, the calculation overflowed and we need to clamp */
822 if (crossfeed_data
.gain
< 0)
823 crossfeed_data
.gain
= 0x7fffffff;
826 /* Both gains should be below 0 dB */
827 void dsp_set_crossfeed_cross_params(long lf_gain
, long hf_gain
, long cutoff
)
829 int32_t *c
= crossfeed_data
.coefs
;
830 long scaler
= get_replaygain_int(lf_gain
* 10) << 7;
832 cutoff
= 0xffffffff/NATIVE_FREQUENCY
*cutoff
;
834 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
835 * point instead of shelf midpoint. This is for compatibility with the old
836 * crossfeed shelf filter and should be removed if crossfeed settings are
837 * ever made incompatible for any other good reason.
839 cutoff
= fp_div(cutoff
, get_replaygain_int(hf_gain
*5), 24);
840 filter_shelf_coefs(cutoff
, hf_gain
, false, c
);
841 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
842 * over 1 and can do this safely
844 c
[0] = FRACMUL_SHL(c
[0], scaler
, 4);
845 c
[1] = FRACMUL_SHL(c
[1], scaler
, 4);
849 /* Apply a constant gain to the samples (e.g., for ReplayGain).
850 * Note that this must be called before the resampler.
852 #ifndef DSP_HAVE_ASM_APPLY_GAIN
853 static void dsp_apply_gain(int count
, struct dsp_data
*data
, int32_t *buf
[])
855 const int32_t gain
= data
->gain
;
858 for (ch
= 0; ch
< data
->num_channels
; ch
++)
860 int32_t *d
= buf
[ch
];
863 for (i
= 0; i
< count
; i
++)
864 d
[i
] = FRACMUL_SHL(d
[i
], gain
, 8);
867 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
869 /* Combine all gains to a global gain. */
870 static void set_gain(struct dsp_config
*dsp
)
872 dsp
->data
.gain
= DEFAULT_GAIN
;
874 /* Replay gain not relevant to voice */
875 if (dsp
== &AUDIO_DSP
&& replaygain
)
877 dsp
->data
.gain
= replaygain
;
880 if (dsp
->eq_process
&& eq_precut
)
883 (long) (((int64_t) dsp
->data
.gain
* eq_precut
) >> 24);
886 #ifdef HAVE_SW_VOLUME_CONTROL
887 if (global_settings
.volume
< SW_VOLUME_MAX
||
888 global_settings
.volume
> SW_VOLUME_MIN
)
890 int vol_gain
= get_replaygain_int(global_settings
.volume
* 100);
891 dsp
->data
.gain
= (long) (((int64_t) dsp
->data
.gain
* vol_gain
) >> 24);
895 if (dsp
->data
.gain
== DEFAULT_GAIN
)
901 dsp
->data
.gain
>>= 1;
904 dsp
->apply_gain
= dsp
->data
.gain
!= 0 ? dsp_apply_gain
: NULL
;
908 * Update the amount to cut the audio before applying the equalizer.
910 * @param precut to apply in decibels (multiplied by 10)
912 void dsp_set_eq_precut(int precut
)
914 eq_precut
= get_replaygain_int(precut
* -10);
915 set_gain(&AUDIO_DSP
);
919 * Synchronize the equalizer filter coefficients with the global settings.
921 * @param band the equalizer band to synchronize
923 void dsp_set_eq_coefs(int band
)
927 unsigned long cutoff
, q
;
929 /* Adjust setting pointer to the band we actually want to change */
930 setting
= &global_settings
.eq_band0_cutoff
+ (band
* 3);
932 /* Convert user settings to format required by coef generator functions */
933 cutoff
= 0xffffffff / NATIVE_FREQUENCY
* (*setting
++);
940 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
941 which it should be, since we're executed from the main thread. */
943 /* Assume a band is disabled if the gain is zero */
946 eq_data
.enabled
[band
] = 0;
951 eq_ls_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
953 eq_hs_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
955 eq_pk_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
957 eq_data
.enabled
[band
] = 1;
961 /* Apply EQ filters to those bands that have got it switched on. */
962 static void eq_process(int count
, int32_t *buf
[])
964 static const int shifts
[] =
966 EQ_SHELF_SHIFT
, /* low shelf */
967 EQ_PEAK_SHIFT
, /* peaking */
968 EQ_PEAK_SHIFT
, /* peaking */
969 EQ_PEAK_SHIFT
, /* peaking */
970 EQ_SHELF_SHIFT
, /* high shelf */
972 unsigned int channels
= AUDIO_DSP
.data
.num_channels
;
975 /* filter configuration currently is 1 low shelf filter, 3 band peaking
976 filters and 1 high shelf filter, in that order. we need to know this
977 so we can choose the correct shift factor.
979 for (i
= 0; i
< 5; i
++)
981 if (!eq_data
.enabled
[i
])
983 eq_filter(buf
, &eq_data
.filters
[i
], count
, channels
, shifts
[i
]);
988 * Use to enable the equalizer.
990 * @param enable true to enable the equalizer
992 void dsp_set_eq(bool enable
)
994 AUDIO_DSP
.eq_process
= enable
? eq_process
: NULL
;
995 set_gain(&AUDIO_DSP
);
998 static void dsp_set_stereo_width(int value
)
1000 long width
, straight
, cross
;
1002 width
= value
* 0x7fffff / 100;
1006 straight
= (0x7fffff + width
) / 2;
1007 cross
= straight
- width
;
1011 /* straight = (1 + width) / (2 * width) */
1012 straight
= ((int64_t)(0x7fffff + width
) << 22) / width
;
1013 cross
= straight
- 0x7fffff;
1016 dsp_sw_gain
= straight
<< 8;
1017 dsp_sw_cross
= cross
<< 8;
1021 * Implements the different channel configurations and stereo width.
1024 /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1027 static void channels_process_sound_chan_stereo(int count
, int32_t *buf
[])
1029 /* The channels are each just themselves */
1030 (void)count
; (void)buf
;
1034 #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1035 static void channels_process_sound_chan_mono(int count
, int32_t *buf
[])
1037 int32_t *sl
= buf
[0], *sr
= buf
[1];
1041 int32_t lr
= *sl
/2 + *sr
/2;
1046 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1048 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1049 static void channels_process_sound_chan_custom(int count
, int32_t *buf
[])
1051 const int32_t gain
= dsp_sw_gain
;
1052 const int32_t cross
= dsp_sw_cross
;
1053 int32_t *sl
= buf
[0], *sr
= buf
[1];
1059 *sl
++ = FRACMUL(l
, gain
) + FRACMUL(r
, cross
);
1060 *sr
++ = FRACMUL(r
, gain
) + FRACMUL(l
, cross
);
1063 #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1065 static void channels_process_sound_chan_mono_left(int count
, int32_t *buf
[])
1067 /* Just copy over the other channel */
1068 memcpy(buf
[1], buf
[0], count
* sizeof (*buf
));
1071 static void channels_process_sound_chan_mono_right(int count
, int32_t *buf
[])
1073 /* Just copy over the other channel */
1074 memcpy(buf
[0], buf
[1], count
* sizeof (*buf
));
1077 #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1078 static void channels_process_sound_chan_karaoke(int count
, int32_t *buf
[])
1080 int32_t *sl
= buf
[0], *sr
= buf
[1];
1084 int32_t ch
= *sl
/2 - *sr
/2;
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
:
1151 #ifdef HAVE_SW_VOLUME_CONTROL
1152 case DSP_CALLBACK_SET_SW_VOLUME
:
1153 set_gain(&AUDIO_DSP
);
1157 case DSP_CALLBACK_SET_CHANNEL_CONFIG
:
1158 dsp_set_channel_config(param
);
1160 case DSP_CALLBACK_SET_STEREO_WIDTH
:
1161 dsp_set_stereo_width(param
);
1170 /* Process and convert src audio to dst based on the DSP configuration,
1171 * reading count number of audio samples. dst is assumed to be large
1172 * enough; use dsp_output_count() to get the required number. src is an
1173 * array of pointers; for mono and interleaved stereo, it contains one
1174 * pointer to the start of the audio data and the other is ignored; for
1175 * non-interleaved stereo, it contains two pointers, one for each audio
1176 * channel. Returns number of bytes written to dst.
1178 int dsp_process(struct dsp_config
*dsp
, char *dst
, const char *src
[], int count
)
1181 static long last_yield
;
1185 #if defined(CPU_COLDFIRE)
1186 /* set emac unit for dsp processing, and save old macsr, we're running in
1187 codec thread context at this point, so can't clobber it */
1188 unsigned long old_macsr
= coldfire_get_macsr();
1189 coldfire_set_macsr(EMAC_FRACTIONAL
| EMAC_SATURATE
);
1193 dsp_set_replaygain(); /* Gain has changed */
1195 /* Perform at least one yield before starting */
1196 last_yield
= current_tick
;
1199 /* Testing function pointers for NULL is preferred since the pointer
1200 will be preloaded to be used for the call if not. */
1203 int samples
= MIN(sample_buf_count
/2, count
);
1206 dsp
->input_samples(samples
, src
, tmp
);
1208 if (dsp
->tdspeed_active
)
1209 samples
= tdspeed_doit(tmp
, samples
);
1211 int chunk_offset
= 0;
1215 t2
[0] = tmp
[0]+chunk_offset
;
1216 t2
[1] = tmp
[1]+chunk_offset
;
1218 int chunk
= MIN(sample_buf_count
/2, samples
);
1219 chunk_offset
+= chunk
;
1222 if (dsp
->apply_gain
)
1223 dsp
->apply_gain(chunk
, &dsp
->data
, t2
);
1225 if (dsp
->resample
&& (chunk
= resample(dsp
, chunk
, t2
)) <= 0)
1226 break; /* I'm pretty sure we're downsampling here */
1228 if (dsp
->apply_crossfeed
)
1229 dsp
->apply_crossfeed(chunk
, t2
);
1231 if (dsp
->eq_process
)
1232 dsp
->eq_process(chunk
, t2
);
1234 #ifdef HAVE_SW_TONE_CONTROLS
1235 if ((bass
| treble
) != 0)
1236 eq_filter(t2
, &dsp
->tone_filter
, chunk
,
1237 dsp
->data
.num_channels
, FILTER_BISHELF_SHIFT
);
1240 if (dsp
->channels_process
)
1241 dsp
->channels_process(chunk
, t2
);
1243 dsp
->output_samples(chunk
, &dsp
->data
, (const int32_t **)t2
, (int16_t *)dst
);
1246 dst
+= chunk
* sizeof (int16_t) * 2;
1248 /* yield at least once each tick */
1249 tick
= current_tick
;
1250 if (TIME_AFTER(tick
, last_yield
))
1258 #if defined(CPU_COLDFIRE)
1259 /* set old macsr again */
1260 coldfire_set_macsr(old_macsr
);
1265 /* Given count number of input samples, calculate the maximum number of
1266 * samples of output data that would be generated (the calculation is not
1267 * entirely exact and rounds upwards to be on the safe side; during
1268 * resampling, the number of samples generated depends on the current state
1269 * of the resampler).
1271 /* dsp_input_size MUST be called afterwards */
1272 int dsp_output_count(struct dsp_config
*dsp
, int count
)
1274 if (dsp
->tdspeed_active
)
1275 count
= tdspeed_est_output_size();
1278 count
= (int)(((unsigned long)count
* NATIVE_FREQUENCY
1279 + (dsp
->frequency
- 1)) / dsp
->frequency
);
1282 /* Now we have the resampled sample count which must not exceed
1283 * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One
1284 * must call dsp_input_count() to get the correct input sample
1287 if (count
> RESAMPLE_BUF_RIGHT_CHANNEL
)
1288 count
= RESAMPLE_BUF_RIGHT_CHANNEL
;
1293 /* Given count output samples, calculate number of input samples
1294 * that would be consumed in order to fill the output buffer.
1296 int dsp_input_count(struct dsp_config
*dsp
, int count
)
1298 /* count is now the number of resampled input samples. Convert to
1299 original input samples. */
1302 /* Use the real resampling delta =
1303 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1304 * round towards zero to avoid buffer overflows. */
1305 count
= (int)(((unsigned long)count
*
1306 dsp
->data
.resample_data
.delta
) >> 16);
1309 if (dsp
->tdspeed_active
)
1310 count
= tdspeed_est_input_size(count
);
1315 static void dsp_set_gain_var(long *var
, long value
)
1321 static void dsp_update_functions(struct dsp_config
*dsp
)
1323 sample_input_new_format(dsp
);
1324 sample_output_new_format(dsp
);
1325 if (dsp
== &AUDIO_DSP
)
1326 dsp_set_crossfeed(crossfeed_enabled
);
1329 intptr_t dsp_configure(struct dsp_config
*dsp
, int setting
, intptr_t value
)
1336 case CODEC_IDX_AUDIO
:
1337 return (intptr_t)&AUDIO_DSP
;
1338 case CODEC_IDX_VOICE
:
1339 return (intptr_t)&VOICE_DSP
;
1341 return (intptr_t)NULL
;
1344 case DSP_SET_FREQUENCY
:
1345 memset(&dsp
->data
.resample_data
, 0, sizeof (dsp
->data
.resample_data
));
1346 /* Fall through!!! */
1347 case DSP_SWITCH_FREQUENCY
:
1348 dsp
->codec_frequency
= (value
== 0) ? NATIVE_FREQUENCY
: value
;
1349 /* Account for playback speed adjustment when setting dsp->frequency
1350 if we're called from the main audio thread. Voice UI thread should
1351 not need this feature.
1353 if (dsp
== &AUDIO_DSP
)
1354 dsp
->frequency
= pitch_ratio
* dsp
->codec_frequency
/ PITCH_SPEED_100
;
1356 dsp
->frequency
= dsp
->codec_frequency
;
1358 resampler_new_delta(dsp
);
1362 case DSP_SET_SAMPLE_DEPTH
:
1363 dsp
->sample_depth
= value
;
1365 if (dsp
->sample_depth
<= NATIVE_DEPTH
)
1367 dsp
->frac_bits
= WORD_FRACBITS
;
1368 dsp
->sample_bytes
= sizeof (int16_t); /* samples are 16 bits */
1369 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1370 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1374 dsp
->frac_bits
= value
;
1375 dsp
->sample_bytes
= sizeof (int32_t); /* samples are 32 bits */
1376 dsp
->data
.clip_max
= (1 << value
) - 1;
1377 dsp
->data
.clip_min
= -(1 << value
);
1380 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1381 sample_input_new_format(dsp
);
1385 case DSP_SET_STEREO_MODE
:
1386 dsp
->stereo_mode
= value
;
1387 dsp
->data
.num_channels
= value
== STEREO_MONO
? 1 : 2;
1388 dsp_update_functions(dsp
);
1393 dsp
->stereo_mode
= STEREO_NONINTERLEAVED
;
1394 dsp
->data
.num_channels
= 2;
1395 dsp
->sample_depth
= NATIVE_DEPTH
;
1396 dsp
->frac_bits
= WORD_FRACBITS
;
1397 dsp
->sample_bytes
= sizeof (int16_t);
1398 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1399 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1400 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1401 dsp
->codec_frequency
= dsp
->frequency
= NATIVE_FREQUENCY
;
1403 if (dsp
== &AUDIO_DSP
)
1412 dsp_update_functions(dsp
);
1413 resampler_new_delta(dsp
);
1418 memset(&dsp
->data
.resample_data
, 0,
1419 sizeof (dsp
->data
.resample_data
));
1420 resampler_new_delta(dsp
);
1425 case DSP_SET_TRACK_GAIN
:
1426 if (dsp
== &AUDIO_DSP
)
1427 dsp_set_gain_var(&track_gain
, value
);
1430 case DSP_SET_ALBUM_GAIN
:
1431 if (dsp
== &AUDIO_DSP
)
1432 dsp_set_gain_var(&album_gain
, value
);
1435 case DSP_SET_TRACK_PEAK
:
1436 if (dsp
== &AUDIO_DSP
)
1437 dsp_set_gain_var(&track_peak
, value
);
1440 case DSP_SET_ALBUM_PEAK
:
1441 if (dsp
== &AUDIO_DSP
)
1442 dsp_set_gain_var(&album_peak
, value
);
1452 void dsp_set_replaygain(void)
1458 if ((global_settings
.replaygain_type
!= REPLAYGAIN_OFF
) ||
1459 global_settings
.replaygain_noclip
)
1461 bool track_mode
= get_replaygain_mode(track_gain
!= 0,
1462 album_gain
!= 0) == REPLAYGAIN_TRACK
;
1463 long peak
= (track_mode
|| !album_peak
) ? track_peak
: album_peak
;
1465 if (global_settings
.replaygain_type
!= REPLAYGAIN_OFF
)
1467 gain
= (track_mode
|| !album_gain
) ? track_gain
: album_gain
;
1469 if (global_settings
.replaygain_preamp
)
1471 long preamp
= get_replaygain_int(
1472 global_settings
.replaygain_preamp
* 10);
1474 gain
= (long) (((int64_t) gain
* preamp
) >> 24);
1480 /* So that noclip can work even with no gain information. */
1481 gain
= DEFAULT_GAIN
;
1484 if (global_settings
.replaygain_noclip
&& (peak
!= 0)
1485 && ((((int64_t) gain
* peak
) >> 24) >= DEFAULT_GAIN
))
1487 gain
= (((int64_t) DEFAULT_GAIN
<< 24) / peak
);
1490 if (gain
== DEFAULT_GAIN
)
1492 /* Nothing to do, disable processing. */
1497 /* Store in S7.24 format to simplify calculations. */
1499 set_gain(&AUDIO_DSP
);