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"
37 /* 16-bit samples are scaled based on these constants. The shift should be
41 #define WORD_FRACBITS 27
43 #define NATIVE_DEPTH 16
44 /* If the small buffer size changes, check the assembly code! */
45 #define SMALL_SAMPLE_BUF_COUNT 256
46 #define DEFAULT_GAIN 0x01000000
48 /* enums to index conversion properly with stereo mode and other settings */
51 SAMPLE_INPUT_LE_NATIVE_I_STEREO
= STEREO_INTERLEAVED
,
52 SAMPLE_INPUT_LE_NATIVE_NI_STEREO
= STEREO_NONINTERLEAVED
,
53 SAMPLE_INPUT_LE_NATIVE_MONO
= STEREO_MONO
,
54 SAMPLE_INPUT_GT_NATIVE_I_STEREO
= STEREO_INTERLEAVED
+ STEREO_NUM_MODES
,
55 SAMPLE_INPUT_GT_NATIVE_NI_STEREO
= STEREO_NONINTERLEAVED
+ STEREO_NUM_MODES
,
56 SAMPLE_INPUT_GT_NATIVE_MONO
= STEREO_MONO
+ STEREO_NUM_MODES
,
57 SAMPLE_INPUT_GT_NATIVE_1ST_INDEX
= STEREO_NUM_MODES
62 SAMPLE_OUTPUT_MONO
= 0,
64 SAMPLE_OUTPUT_DITHERED_MONO
,
65 SAMPLE_OUTPUT_DITHERED_STEREO
68 /****************************************************************************
69 * NOTE: Any assembly routines that use these structures must be updated
70 * if current data members are moved or changed.
74 uint32_t delta
; /* 00h */
75 uint32_t phase
; /* 04h */
76 int32_t last_sample
[2]; /* 08h */
80 /* This is for passing needed data to assembly dsp routines. If another
81 * dsp parameter needs to be passed, add to the end of the structure
82 * and remove from dsp_config.
83 * If another function type becomes assembly optimized and requires dsp
84 * config info, add a pointer paramter of type "struct dsp_data *".
85 * If removing something from other than the end, reserve the spot or
86 * else update every implementation for every target.
87 * Be sure to add the offset of the new member for easy viewing as well. :)
88 * It is the first member of dsp_config and all members can be accessesed
89 * through the main aggregate but this is intended to make a safe haven
90 * for these items whereas the c part can be rearranged at will. dsp_data
91 * could even moved within dsp_config without disurbing the order.
95 int output_scale
; /* 00h */
96 int num_channels
; /* 04h */
97 struct resample_data resample_data
; /* 08h */
98 int32_t clip_min
; /* 18h */
99 int32_t clip_max
; /* 1ch */
100 int32_t gain
; /* 20h - Note that this is in S8.23 format. */
107 long error
[3]; /* 00h */
108 long random
; /* 0ch */
112 struct crossfeed_data
114 int32_t gain
; /* 00h - Direct path gain */
115 int32_t coefs
[3]; /* 04h - Coefficients for the shelving filter */
116 int32_t history
[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */
117 int32_t delay
[13][2]; /* 20h */
118 int32_t *index
; /* 88h - Current pointer into the delay line */
122 /* Current setup is one lowshelf filters three peaking filters and one
123 * highshelf filter. Varying the number of shelving filters make no sense,
124 * but adding peaking filters is possible.
128 char enabled
[5]; /* 00h - Flags for active filters */
129 struct eqfilter filters
[5]; /* 08h - packing is 4? */
133 /* Include header with defines which functions are implemented in assembly
134 code for the target */
137 /* Typedefs keep things much neater in this case */
138 typedef void (*sample_input_fn_type
)(int count
, const char *src
[],
140 typedef int (*resample_fn_type
)(int count
, struct dsp_data
*data
,
141 const int32_t *src
[], int32_t *dst
[]);
142 typedef void (*sample_output_fn_type
)(int count
, struct dsp_data
*data
,
143 const int32_t *src
[], int16_t *dst
);
145 /* Single-DSP channel processing in place */
146 typedef void (*channels_process_fn_type
)(int count
, int32_t *buf
[]);
147 /* DSP local channel processing in place */
148 typedef void (*channels_process_dsp_fn_type
)(int count
, struct dsp_data
*data
,
153 ***************************************************************************/
157 struct dsp_data data
; /* Config members for use in asm routines */
158 long codec_frequency
; /* Sample rate of data coming from the codec */
159 long frequency
; /* Effective sample rate after pitch shift (if any) */
163 int tdspeed_percent
; /* Speed % */
164 bool tdspeed_active
; /* Timestretch is in use */
166 #ifdef HAVE_SW_TONE_CONTROLS
167 /* Filter struct for software bass/treble controls */
168 struct eqfilter tone_filter
;
170 /* Functions that change depending upon settings - NULL if stage is
172 sample_input_fn_type input_samples
;
173 resample_fn_type resample
;
174 sample_output_fn_type output_samples
;
175 /* These will be NULL for the voice codec and is more economical that
177 channels_process_dsp_fn_type apply_gain
;
178 channels_process_fn_type apply_crossfeed
;
179 channels_process_fn_type eq_process
;
180 channels_process_fn_type channels_process
;
183 /* General DSP config */
184 static struct dsp_config dsp_conf
[2] IBSS_ATTR
; /* 0=A, 1=V */
186 static struct dither_data dither_data
[2] IBSS_ATTR
; /* 0=left, 1=right */
187 static long dither_mask IBSS_ATTR
;
188 static long dither_bias IBSS_ATTR
;
190 struct crossfeed_data crossfeed_data IDATA_ATTR
= /* A */
192 .index
= (int32_t *)crossfeed_data
.delay
196 static struct eq_state eq_data
; /* A */
198 /* Software tone controls */
199 #ifdef HAVE_SW_TONE_CONTROLS
200 static int prescale
; /* A/V */
201 static int bass
; /* A/V */
202 static int treble
; /* A/V */
205 /* Settings applicable to audio codec only */
206 static int pitch_ratio
= 1000;
207 static int channels_mode
;
210 static bool dither_enabled
;
211 static long eq_precut
;
212 static long track_gain
;
213 static bool new_gain
;
214 static long album_gain
;
215 static long track_peak
;
216 static long album_peak
;
217 static long replaygain
;
218 static bool crossfeed_enabled
;
220 #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO])
221 #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE])
223 /* The internal format is 32-bit samples, non-interleaved, stereo. This
224 * format is similar to the raw output from several codecs, so the amount
225 * of copying needed is minimized for that case.
228 #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */
230 static int32_t small_sample_buf
[SMALL_SAMPLE_BUF_COUNT
] IBSS_ATTR
;
231 static int32_t small_resample_buf
[SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
] IBSS_ATTR
;
233 static int32_t *big_sample_buf
= NULL
;
234 static int32_t *big_resample_buf
= NULL
;
235 static int big_sample_buf_count
= -1; /* -1=unknown, 0=not available */
237 static int sample_buf_count
;
238 static int32_t *sample_buf
;
239 static int32_t *resample_buf
;
241 #define SAMPLE_BUF_LEFT_CHANNEL 0
242 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2)
243 #define RESAMPLE_BUF_LEFT_CHANNEL 0
244 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO)
247 /* Clip sample to signed 16 bit range */
248 static inline int32_t clip_sample_16(int32_t sample
)
250 if ((int16_t)sample
!= sample
)
251 sample
= 0x7fff ^ (sample
>> 31);
255 int sound_get_pitch(void)
260 void sound_set_pitch(int permille
)
262 pitch_ratio
= permille
;
263 dsp_configure(&AUDIO_DSP
, DSP_SWITCH_FREQUENCY
,
264 AUDIO_DSP
.codec_frequency
);
267 static void tdspeed_setup(struct dsp_config
*dspc
)
269 /* Assume timestretch will not be used */
270 dspc
->tdspeed_active
= false;
271 sample_buf
= small_sample_buf
;
272 resample_buf
= small_resample_buf
;
273 sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
;
275 if(!dsp_timestretch_available())
276 return; /* Timestretch not enabled or buffer not allocated */
277 if (dspc
->tdspeed_percent
== 0)
278 dspc
->tdspeed_percent
= 100;
280 dspc
->codec_frequency
== 0 ? NATIVE_FREQUENCY
: dspc
->codec_frequency
,
281 dspc
->stereo_mode
!= STEREO_MONO
,
282 dspc
->tdspeed_percent
))
283 return; /* Timestretch not possible or needed with these parameters */
285 /* Timestretch is to be used */
286 dspc
->tdspeed_active
= true;
287 sample_buf
= big_sample_buf
;
288 sample_buf_count
= big_sample_buf_count
;
289 resample_buf
= big_resample_buf
;
292 void dsp_timestretch_enable(bool enabled
)
294 /* Hook to set up timestretch buffer on first call to settings_apply() */
295 if (big_sample_buf_count
< 0) /* Only do something on first call */
299 /* Set up timestretch buffers */
300 big_sample_buf_count
= SMALL_SAMPLE_BUF_COUNT
* RESAMPLE_RATIO
;
301 big_sample_buf
= small_resample_buf
;
302 big_resample_buf
= (int32_t *) buffer_alloc(big_sample_buf_count
* RESAMPLE_RATIO
* sizeof(int32_t));
306 /* Not enabled at startup, "big" buffers will never be available */
307 big_sample_buf_count
= 0;
309 tdspeed_setup(&AUDIO_DSP
);
313 void dsp_set_timestretch(int percent
)
315 AUDIO_DSP
.tdspeed_percent
= percent
;
316 tdspeed_setup(&AUDIO_DSP
);
319 int dsp_get_timestretch()
321 return AUDIO_DSP
.tdspeed_percent
;
324 bool dsp_timestretch_available()
326 return (global_settings
.timestretch_enabled
&& big_sample_buf_count
> 0);
329 /* Convert count samples to the internal format, if needed. Updates src
330 * to point past the samples "consumed" and dst is set to point to the
331 * samples to consume. Note that for mono, dst[0] equals dst[1], as there
332 * is no point in processing the same data twice.
335 /* convert count 16-bit mono to 32-bit mono */
336 static void sample_input_lte_native_mono(
337 int count
, const char *src
[], int32_t *dst
[])
339 const int16_t *s
= (int16_t *) src
[0];
340 const int16_t * const send
= s
+ count
;
341 int32_t *d
= dst
[0] = dst
[1] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
342 int scale
= WORD_SHIFT
;
346 *d
++ = *s
++ << scale
;
353 /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */
354 static void sample_input_lte_native_i_stereo(
355 int count
, const char *src
[], int32_t *dst
[])
357 const int32_t *s
= (int32_t *) src
[0];
358 const int32_t * const send
= s
+ count
;
359 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
360 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
361 int scale
= WORD_SHIFT
;
366 #ifdef ROCKBOX_LITTLE_ENDIAN
367 *dl
++ = (slr
>> 16) << scale
;
368 *dr
++ = (int32_t)(int16_t)slr
<< scale
;
369 #else /* ROCKBOX_BIG_ENDIAN */
370 *dl
++ = (int32_t)(int16_t)slr
<< scale
;
371 *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
;
401 /* convert count 32-bit mono to 32-bit mono */
402 static void sample_input_gt_native_mono(
403 int count
, const char *src
[], int32_t *dst
[])
405 dst
[0] = dst
[1] = (int32_t *)src
[0];
406 src
[0] = (char *)(dst
[0] + count
);
409 /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */
410 static void sample_input_gt_native_i_stereo(
411 int count
, const char *src
[], int32_t *dst
[])
413 const int32_t *s
= (int32_t *)src
[0];
414 const int32_t * const send
= s
+ 2*count
;
415 int32_t *dl
= dst
[0] = &sample_buf
[SAMPLE_BUF_LEFT_CHANNEL
];
416 int32_t *dr
= dst
[1] = &sample_buf
[SAMPLE_BUF_RIGHT_CHANNEL
];
425 src
[0] = (char *)send
;
428 /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */
429 static void sample_input_gt_native_ni_stereo(
430 int count
, const char *src
[], int32_t *dst
[])
432 dst
[0] = (int32_t *)src
[0];
433 dst
[1] = (int32_t *)src
[1];
434 src
[0] = (char *)(dst
[0] + count
);
435 src
[1] = (char *)(dst
[1] + count
);
439 * sample_input_new_format()
441 * set the to-native sample conversion function based on dsp sample parameters
444 * needs syncing with changes to the following dsp parameters:
445 * * dsp->stereo_mode (A/V)
446 * * dsp->sample_depth (A/V)
448 static void sample_input_new_format(struct dsp_config
*dsp
)
450 static const sample_input_fn_type sample_input_functions
[] =
452 [SAMPLE_INPUT_LE_NATIVE_MONO
] = sample_input_lte_native_mono
,
453 [SAMPLE_INPUT_LE_NATIVE_I_STEREO
] = sample_input_lte_native_i_stereo
,
454 [SAMPLE_INPUT_LE_NATIVE_NI_STEREO
] = sample_input_lte_native_ni_stereo
,
455 [SAMPLE_INPUT_GT_NATIVE_MONO
] = sample_input_gt_native_mono
,
456 [SAMPLE_INPUT_GT_NATIVE_I_STEREO
] = sample_input_gt_native_i_stereo
,
457 [SAMPLE_INPUT_GT_NATIVE_NI_STEREO
] = sample_input_gt_native_ni_stereo
,
460 int convert
= dsp
->stereo_mode
;
462 if (dsp
->sample_depth
> NATIVE_DEPTH
)
463 convert
+= SAMPLE_INPUT_GT_NATIVE_1ST_INDEX
;
465 dsp
->input_samples
= sample_input_functions
[convert
];
469 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO
470 /* write mono internal format to output format */
471 static void sample_output_mono(int count
, struct dsp_data
*data
,
472 const int32_t *src
[], int16_t *dst
)
474 const int32_t *s0
= src
[0];
475 const int scale
= data
->output_scale
;
476 const int dc_bias
= 1 << (scale
- 1);
480 int32_t lr
= clip_sample_16((*s0
++ + dc_bias
) >> scale
);
486 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */
488 /* write stereo internal format to output format */
489 #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
490 static void sample_output_stereo(int count
, struct dsp_data
*data
,
491 const int32_t *src
[], int16_t *dst
)
493 const int32_t *s0
= src
[0];
494 const int32_t *s1
= src
[1];
495 const int scale
= data
->output_scale
;
496 const int dc_bias
= 1 << (scale
- 1);
500 *dst
++ = clip_sample_16((*s0
++ + dc_bias
) >> scale
);
501 *dst
++ = clip_sample_16((*s1
++ + dc_bias
) >> scale
);
505 #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */
508 * The "dither" code to convert the 24-bit samples produced by libmad was
509 * taken from the coolplayer project - coolplayer.sourceforge.net
511 * This function handles mono and stereo outputs.
513 static void sample_output_dithered(int count
, struct dsp_data
*data
,
514 const int32_t *src
[], int16_t *dst
)
516 const int32_t mask
= dither_mask
;
517 const int32_t bias
= dither_bias
;
518 const int scale
= data
->output_scale
;
519 const int32_t min
= data
->clip_min
;
520 const int32_t max
= data
->clip_max
;
521 const int32_t range
= max
- min
;
525 for (ch
= 0; ch
< data
->num_channels
; ch
++)
527 struct dither_data
* const dither
= &dither_data
[ch
];
528 const int32_t *s
= src
[ch
];
531 for (i
= 0, d
= &dst
[ch
]; i
< count
; i
++, s
++, d
+= 2)
533 int32_t output
, sample
;
536 /* Noise shape and bias (for correct rounding later) */
538 sample
+= dither
->error
[0] - dither
->error
[1] + dither
->error
[2];
539 dither
->error
[2] = dither
->error
[1];
540 dither
->error
[1] = dither
->error
[0]/2;
542 output
= sample
+ bias
;
544 /* Dither, highpass triangle PDF */
545 random
= dither
->random
*0x0019660dL
+ 0x3c6ef35fL
;
546 output
+= (random
& mask
) - (dither
->random
& mask
);
547 dither
->random
= random
;
549 /* Round sample to output range */
553 dither
->error
[0] = sample
- output
;
556 if ((uint32_t)(output
- min
) > (uint32_t)range
)
564 /* Quantize and store */
565 *d
= output
>> scale
;
569 if (data
->num_channels
== 2)
572 /* Have to duplicate left samples into the right channel since
573 pcm buffer and hardware is interleaved stereo */
585 * sample_output_new_format()
587 * set the from-native to ouput sample conversion routine
590 * needs syncing with changes to the following dsp parameters:
591 * * dsp->stereo_mode (A/V)
592 * * dither_enabled (A)
594 static void sample_output_new_format(struct dsp_config
*dsp
)
596 static const sample_output_fn_type sample_output_functions
[] =
599 sample_output_stereo
,
600 sample_output_dithered
,
601 sample_output_dithered
604 int out
= dsp
->data
.num_channels
- 1;
606 if (dsp
== &AUDIO_DSP
&& dither_enabled
)
609 dsp
->output_samples
= sample_output_functions
[out
];
613 * Linear interpolation resampling that introduces a one sample delay because
614 * of our inability to look into the future at the end of a frame.
616 #ifndef DSP_HAVE_ASM_RESAMPLING
617 static int dsp_downsample(int count
, struct dsp_data
*data
,
618 const int32_t *src
[], int32_t *dst
[])
620 int ch
= data
->num_channels
- 1;
621 uint32_t delta
= data
->resample_data
.delta
;
625 /* Rolled channel loop actually showed slightly faster. */
628 /* Just initialize things and not worry too much about the relatively
629 * uncommon case of not being able to spit out a sample for the frame.
631 const int32_t *s
= src
[ch
];
632 int32_t last
= data
->resample_data
.last_sample
[ch
];
634 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
636 phase
= data
->resample_data
.phase
;
639 /* Do we need last sample of previous frame for interpolation? */
643 while (pos
< (uint32_t)count
)
645 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
653 /* Wrap phase accumulator back to start of next frame. */
654 data
->resample_data
.phase
= phase
- (count
<< 16);
658 static int dsp_upsample(int count
, struct dsp_data
*data
,
659 const int32_t *src
[], int32_t *dst
[])
661 int ch
= data
->num_channels
- 1;
662 uint32_t delta
= data
->resample_data
.delta
;
666 /* Rolled channel loop actually showed slightly faster. */
669 /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */
670 const int32_t *s
= src
[ch
];
671 int32_t last
= data
->resample_data
.last_sample
[ch
];
673 data
->resample_data
.last_sample
[ch
] = s
[count
- 1];
675 phase
= data
->resample_data
.phase
;
680 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[0] - last
);
685 while (pos
< (uint32_t)count
)
688 *d
++ = last
+ FRACMUL((phase
& 0xffff) << 15, s
[pos
] - last
);
695 /* Wrap phase accumulator back to start of next frame. */
696 data
->resample_data
.phase
= phase
& 0xffff;
699 #endif /* DSP_HAVE_ASM_RESAMPLING */
701 static void resampler_new_delta(struct dsp_config
*dsp
)
703 dsp
->data
.resample_data
.delta
= (unsigned long)
704 dsp
->frequency
* 65536LL / NATIVE_FREQUENCY
;
706 if (dsp
->frequency
== NATIVE_FREQUENCY
)
708 /* NOTE: If fully glitch-free transistions from no resampling to
709 resampling are desired, last_sample history should be maintained
710 even when not resampling. */
711 dsp
->resample
= NULL
;
712 dsp
->data
.resample_data
.phase
= 0;
713 dsp
->data
.resample_data
.last_sample
[0] = 0;
714 dsp
->data
.resample_data
.last_sample
[1] = 0;
716 else if (dsp
->frequency
< NATIVE_FREQUENCY
)
717 dsp
->resample
= dsp_upsample
;
719 dsp
->resample
= dsp_downsample
;
722 /* Resample count stereo samples. Updates the src array, if resampling is
723 * done, to refer to the resampled data. Returns number of stereo samples
724 * for further processing.
726 static inline int resample(struct dsp_config
*dsp
, int count
, int32_t *src
[])
730 &resample_buf
[RESAMPLE_BUF_LEFT_CHANNEL
],
731 &resample_buf
[RESAMPLE_BUF_RIGHT_CHANNEL
],
734 count
= dsp
->resample(count
, &dsp
->data
, (const int32_t **)src
, dst
);
737 src
[1] = dst
[dsp
->data
.num_channels
- 1];
742 static void dither_init(struct dsp_config
*dsp
)
744 memset(dither_data
, 0, sizeof (dither_data
));
745 dither_bias
= (1L << (dsp
->frac_bits
- NATIVE_DEPTH
));
746 dither_mask
= (1L << (dsp
->frac_bits
+ 1 - NATIVE_DEPTH
)) - 1;
749 void dsp_dither_enable(bool enable
)
751 struct dsp_config
*dsp
= &AUDIO_DSP
;
752 dither_enabled
= enable
;
753 sample_output_new_format(dsp
);
756 /* Applies crossfeed to the stereo signal in src.
757 * Crossfeed is a process where listening over speakers is simulated. This
758 * is good for old hard panned stereo records, which might be quite fatiguing
759 * to listen to on headphones with no crossfeed.
761 #ifndef DSP_HAVE_ASM_CROSSFEED
762 static void apply_crossfeed(int count
, int32_t *buf
[])
764 int32_t *hist_l
= &crossfeed_data
.history
[0];
765 int32_t *hist_r
= &crossfeed_data
.history
[2];
766 int32_t *delay
= &crossfeed_data
.delay
[0][0];
767 int32_t *coefs
= &crossfeed_data
.coefs
[0];
768 int32_t gain
= crossfeed_data
.gain
;
769 int32_t *di
= crossfeed_data
.index
;
775 for (i
= 0; i
< count
; i
++)
780 /* Filter delayed sample from left speaker */
781 acc
= FRACMUL(*di
, coefs
[0]);
782 acc
+= FRACMUL(hist_l
[0], coefs
[1]);
783 acc
+= FRACMUL(hist_l
[1], coefs
[2]);
784 /* Save filter history for left speaker */
788 /* Filter delayed sample from right speaker */
789 acc
= FRACMUL(*di
, coefs
[0]);
790 acc
+= FRACMUL(hist_r
[0], coefs
[1]);
791 acc
+= FRACMUL(hist_r
[1], coefs
[2]);
792 /* Save filter history for right speaker */
796 /* Now add the attenuated direct sound and write to outputs */
797 buf
[0][i
] = FRACMUL(left
, gain
) + hist_r
[1];
798 buf
[1][i
] = FRACMUL(right
, gain
) + hist_l
[1];
800 /* Wrap delay line index if bigger than delay line size */
801 if (di
>= delay
+ 13*2)
804 /* Write back local copies of data we've modified */
805 crossfeed_data
.index
= di
;
807 #endif /* DSP_HAVE_ASM_CROSSFEED */
810 * dsp_set_crossfeed(bool enable)
813 * needs syncing with changes to the following dsp parameters:
814 * * dsp->stereo_mode (A)
816 void dsp_set_crossfeed(bool enable
)
818 crossfeed_enabled
= enable
;
819 AUDIO_DSP
.apply_crossfeed
= (enable
&& AUDIO_DSP
.data
.num_channels
> 1)
820 ? apply_crossfeed
: NULL
;
823 void dsp_set_crossfeed_direct_gain(int gain
)
825 crossfeed_data
.gain
= get_replaygain_int(gain
* 10) << 7;
826 /* If gain is negative, the calculation overflowed and we need to clamp */
827 if (crossfeed_data
.gain
< 0)
828 crossfeed_data
.gain
= 0x7fffffff;
831 /* Both gains should be below 0 dB */
832 void dsp_set_crossfeed_cross_params(long lf_gain
, long hf_gain
, long cutoff
)
834 int32_t *c
= crossfeed_data
.coefs
;
835 long scaler
= get_replaygain_int(lf_gain
* 10) << 7;
837 cutoff
= 0xffffffff/NATIVE_FREQUENCY
*cutoff
;
839 /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
840 * point instead of shelf midpoint. This is for compatibility with the old
841 * crossfeed shelf filter and should be removed if crossfeed settings are
842 * ever made incompatible for any other good reason.
844 cutoff
= DIV64(cutoff
, get_replaygain_int(hf_gain
*5), 24);
845 filter_shelf_coefs(cutoff
, hf_gain
, false, c
);
846 /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
847 * over 1 and can do this safely
849 c
[0] = FRACMUL_SHL(c
[0], scaler
, 4);
850 c
[1] = FRACMUL_SHL(c
[1], scaler
, 4);
854 /* Apply a constant gain to the samples (e.g., for ReplayGain).
855 * Note that this must be called before the resampler.
857 #ifndef DSP_HAVE_ASM_APPLY_GAIN
858 static void dsp_apply_gain(int count
, struct dsp_data
*data
, int32_t *buf
[])
860 const int32_t gain
= data
->gain
;
863 for (ch
= 0; ch
< data
->num_channels
; ch
++)
865 int32_t *d
= buf
[ch
];
868 for (i
= 0; i
< count
; i
++)
869 d
[i
] = FRACMUL_SHL(d
[i
], gain
, 8);
872 #endif /* DSP_HAVE_ASM_APPLY_GAIN */
874 /* Combine all gains to a global gain. */
875 static void set_gain(struct dsp_config
*dsp
)
877 dsp
->data
.gain
= DEFAULT_GAIN
;
879 /* Replay gain not relevant to voice */
880 if (dsp
== &AUDIO_DSP
&& replaygain
)
882 dsp
->data
.gain
= replaygain
;
885 if (dsp
->eq_process
&& eq_precut
)
888 (long) (((int64_t) dsp
->data
.gain
* eq_precut
) >> 24);
891 if (dsp
->data
.gain
== DEFAULT_GAIN
)
897 dsp
->data
.gain
>>= 1;
900 dsp
->apply_gain
= dsp
->data
.gain
!= 0 ? dsp_apply_gain
: NULL
;
904 * Update the amount to cut the audio before applying the equalizer.
906 * @param precut to apply in decibels (multiplied by 10)
908 void dsp_set_eq_precut(int precut
)
910 eq_precut
= get_replaygain_int(precut
* -10);
911 set_gain(&AUDIO_DSP
);
915 * Synchronize the equalizer filter coefficients with the global settings.
917 * @param band the equalizer band to synchronize
919 void dsp_set_eq_coefs(int band
)
923 unsigned long cutoff
, q
;
925 /* Adjust setting pointer to the band we actually want to change */
926 setting
= &global_settings
.eq_band0_cutoff
+ (band
* 3);
928 /* Convert user settings to format required by coef generator functions */
929 cutoff
= 0xffffffff / NATIVE_FREQUENCY
* (*setting
++);
936 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
937 which it should be, since we're executed from the main thread. */
939 /* Assume a band is disabled if the gain is zero */
942 eq_data
.enabled
[band
] = 0;
947 eq_ls_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
949 eq_hs_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
951 eq_pk_coefs(cutoff
, q
, gain
, eq_data
.filters
[band
].coefs
);
953 eq_data
.enabled
[band
] = 1;
957 /* Apply EQ filters to those bands that have got it switched on. */
958 static void eq_process(int count
, int32_t *buf
[])
960 static const int shifts
[] =
962 EQ_SHELF_SHIFT
, /* low shelf */
963 EQ_PEAK_SHIFT
, /* peaking */
964 EQ_PEAK_SHIFT
, /* peaking */
965 EQ_PEAK_SHIFT
, /* peaking */
966 EQ_SHELF_SHIFT
, /* high shelf */
968 unsigned int channels
= AUDIO_DSP
.data
.num_channels
;
971 /* filter configuration currently is 1 low shelf filter, 3 band peaking
972 filters and 1 high shelf filter, in that order. we need to know this
973 so we can choose the correct shift factor.
975 for (i
= 0; i
< 5; i
++)
977 if (!eq_data
.enabled
[i
])
979 eq_filter(buf
, &eq_data
.filters
[i
], count
, channels
, shifts
[i
]);
984 * Use to enable the equalizer.
986 * @param enable true to enable the equalizer
988 void dsp_set_eq(bool enable
)
990 AUDIO_DSP
.eq_process
= enable
? eq_process
: NULL
;
991 set_gain(&AUDIO_DSP
);
994 static void dsp_set_stereo_width(int value
)
996 long width
, straight
, cross
;
998 width
= value
* 0x7fffff / 100;
1002 straight
= (0x7fffff + width
) / 2;
1003 cross
= straight
- width
;
1007 /* straight = (1 + width) / (2 * width) */
1008 straight
= ((int64_t)(0x7fffff + width
) << 22) / width
;
1009 cross
= straight
- 0x7fffff;
1012 dsp_sw_gain
= straight
<< 8;
1013 dsp_sw_cross
= cross
<< 8;
1017 * Implements the different channel configurations and stereo width.
1020 /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1023 static void channels_process_sound_chan_stereo(int count
, int32_t *buf
[])
1025 /* The channels are each just themselves */
1026 (void)count
; (void)buf
;
1030 #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1031 static void channels_process_sound_chan_mono(int count
, int32_t *buf
[])
1033 int32_t *sl
= buf
[0], *sr
= buf
[1];
1037 int32_t lr
= *sl
/2 + *sr
/2;
1041 while (--count
> 0);
1043 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1045 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1046 static void channels_process_sound_chan_custom(int count
, int32_t *buf
[])
1048 const int32_t gain
= dsp_sw_gain
;
1049 const int32_t cross
= dsp_sw_cross
;
1050 int32_t *sl
= buf
[0], *sr
= buf
[1];
1056 *sl
++ = FRACMUL(l
, gain
) + FRACMUL(r
, cross
);
1057 *sr
++ = FRACMUL(r
, gain
) + FRACMUL(l
, cross
);
1059 while (--count
> 0);
1061 #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1063 static void channels_process_sound_chan_mono_left(int count
, int32_t *buf
[])
1065 /* Just copy over the other channel */
1066 memcpy(buf
[1], buf
[0], count
* sizeof (*buf
));
1069 static void channels_process_sound_chan_mono_right(int count
, int32_t *buf
[])
1071 /* Just copy over the other channel */
1072 memcpy(buf
[0], buf
[1], count
* sizeof (*buf
));
1075 #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1076 static void channels_process_sound_chan_karaoke(int count
, int32_t *buf
[])
1078 int32_t *sl
= buf
[0], *sr
= buf
[1];
1082 int32_t ch
= *sl
/2 - *sr
/2;
1086 while (--count
> 0);
1088 #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1090 static void dsp_set_channel_config(int value
)
1092 static const channels_process_fn_type channels_process_functions
[] =
1094 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1095 [SOUND_CHAN_STEREO
] = NULL
,
1096 [SOUND_CHAN_MONO
] = channels_process_sound_chan_mono
,
1097 [SOUND_CHAN_CUSTOM
] = channels_process_sound_chan_custom
,
1098 [SOUND_CHAN_MONO_LEFT
] = channels_process_sound_chan_mono_left
,
1099 [SOUND_CHAN_MONO_RIGHT
] = channels_process_sound_chan_mono_right
,
1100 [SOUND_CHAN_KARAOKE
] = channels_process_sound_chan_karaoke
,
1103 if ((unsigned)value
>= ARRAYLEN(channels_process_functions
) ||
1104 AUDIO_DSP
.stereo_mode
== STEREO_MONO
)
1106 value
= SOUND_CHAN_STEREO
;
1109 /* This doesn't apply to voice */
1110 channels_mode
= value
;
1111 AUDIO_DSP
.channels_process
= channels_process_functions
[value
];
1114 #if CONFIG_CODEC == SWCODEC
1116 #ifdef HAVE_SW_TONE_CONTROLS
1117 static void set_tone_controls(void)
1119 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY
*200,
1120 0xffffffff/NATIVE_FREQUENCY
*3500,
1121 bass
, treble
, -prescale
,
1122 AUDIO_DSP
.tone_filter
.coefs
);
1123 /* Sync the voice dsp coefficients */
1124 memcpy(&VOICE_DSP
.tone_filter
.coefs
, AUDIO_DSP
.tone_filter
.coefs
,
1125 sizeof (VOICE_DSP
.tone_filter
.coefs
));
1129 /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1132 int dsp_callback(int msg
, intptr_t param
)
1136 #ifdef HAVE_SW_TONE_CONTROLS
1137 case DSP_CALLBACK_SET_PRESCALE
:
1139 set_tone_controls();
1141 /* prescaler is always set after calling any of these, so we wait with
1142 * calculating coefs until the above case is hit.
1144 case DSP_CALLBACK_SET_BASS
:
1147 case DSP_CALLBACK_SET_TREBLE
:
1151 case DSP_CALLBACK_SET_CHANNEL_CONFIG
:
1152 dsp_set_channel_config(param
);
1154 case DSP_CALLBACK_SET_STEREO_WIDTH
:
1155 dsp_set_stereo_width(param
);
1164 /* Process and convert src audio to dst based on the DSP configuration,
1165 * reading count number of audio samples. dst is assumed to be large
1166 * enough; use dsp_output_count() to get the required number. src is an
1167 * array of pointers; for mono and interleaved stereo, it contains one
1168 * pointer to the start of the audio data and the other is ignored; for
1169 * non-interleaved stereo, it contains two pointers, one for each audio
1170 * channel. Returns number of bytes written to dst.
1172 int dsp_process(struct dsp_config
*dsp
, char *dst
, const char *src
[], int count
)
1175 static long last_yield
;
1179 #if defined(CPU_COLDFIRE)
1180 /* set emac unit for dsp processing, and save old macsr, we're running in
1181 codec thread context at this point, so can't clobber it */
1182 unsigned long old_macsr
= coldfire_get_macsr();
1183 coldfire_set_macsr(EMAC_FRACTIONAL
| EMAC_SATURATE
);
1187 dsp_set_replaygain(); /* Gain has changed */
1189 /* Perform at least one yield before starting */
1190 last_yield
= current_tick
;
1193 /* Testing function pointers for NULL is preferred since the pointer
1194 will be preloaded to be used for the call if not. */
1197 int samples
= MIN(sample_buf_count
/2, count
);
1200 dsp
->input_samples(samples
, src
, tmp
);
1202 if (dsp
->tdspeed_active
)
1203 samples
= tdspeed_doit(tmp
, samples
);
1205 int chunk_offset
= 0;
1209 t2
[0] = tmp
[0]+chunk_offset
;
1210 t2
[1] = tmp
[1]+chunk_offset
;
1212 int chunk
= MIN(sample_buf_count
/2, samples
);
1213 chunk_offset
+= chunk
;
1216 if (dsp
->apply_gain
)
1217 dsp
->apply_gain(chunk
, &dsp
->data
, t2
);
1219 if (dsp
->resample
&& (chunk
= resample(dsp
, chunk
, t2
)) <= 0)
1220 break; /* I'm pretty sure we're downsampling here */
1222 if (dsp
->apply_crossfeed
)
1223 dsp
->apply_crossfeed(chunk
, t2
);
1225 if (dsp
->eq_process
)
1226 dsp
->eq_process(chunk
, t2
);
1228 #ifdef HAVE_SW_TONE_CONTROLS
1229 if ((bass
| treble
) != 0)
1230 eq_filter(t2
, &dsp
->tone_filter
, chunk
,
1231 dsp
->data
.num_channels
, FILTER_BISHELF_SHIFT
);
1234 if (dsp
->channels_process
)
1235 dsp
->channels_process(chunk
, t2
);
1237 dsp
->output_samples(chunk
, &dsp
->data
, (const int32_t **)t2
, (int16_t *)dst
);
1240 dst
+= chunk
* sizeof (int16_t) * 2;
1242 /* yield at least once each tick */
1243 tick
= current_tick
;
1244 if (TIME_AFTER(tick
, last_yield
))
1252 #if defined(CPU_COLDFIRE)
1253 /* set old macsr again */
1254 coldfire_set_macsr(old_macsr
);
1259 /* Given count number of input samples, calculate the maximum number of
1260 * samples of output data that would be generated (the calculation is not
1261 * entirely exact and rounds upwards to be on the safe side; during
1262 * resampling, the number of samples generated depends on the current state
1263 * of the resampler).
1265 /* dsp_input_size MUST be called afterwards */
1266 int dsp_output_count(struct dsp_config
*dsp
, int count
)
1268 if (dsp
->tdspeed_active
)
1269 count
= tdspeed_est_output_size();
1272 count
= (int)(((unsigned long)count
* NATIVE_FREQUENCY
1273 + (dsp
->frequency
- 1)) / dsp
->frequency
);
1276 /* Now we have the resampled sample count which must not exceed
1277 * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One
1278 * must call dsp_input_count() to get the correct input sample
1281 if (count
> RESAMPLE_BUF_RIGHT_CHANNEL
)
1282 count
= RESAMPLE_BUF_RIGHT_CHANNEL
;
1287 /* Given count output samples, calculate number of input samples
1288 * that would be consumed in order to fill the output buffer.
1290 int dsp_input_count(struct dsp_config
*dsp
, int count
)
1292 /* count is now the number of resampled input samples. Convert to
1293 original input samples. */
1296 /* Use the real resampling delta =
1297 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1298 * round towards zero to avoid buffer overflows. */
1299 count
= (int)(((unsigned long)count
*
1300 dsp
->data
.resample_data
.delta
) >> 16);
1303 if (dsp
->tdspeed_active
)
1304 count
= tdspeed_est_input_size(count
);
1309 static void dsp_set_gain_var(long *var
, long value
)
1315 static void dsp_update_functions(struct dsp_config
*dsp
)
1317 sample_input_new_format(dsp
);
1318 sample_output_new_format(dsp
);
1319 if (dsp
== &AUDIO_DSP
)
1320 dsp_set_crossfeed(crossfeed_enabled
);
1323 intptr_t dsp_configure(struct dsp_config
*dsp
, int setting
, intptr_t value
)
1330 case CODEC_IDX_AUDIO
:
1331 return (intptr_t)&AUDIO_DSP
;
1332 case CODEC_IDX_VOICE
:
1333 return (intptr_t)&VOICE_DSP
;
1335 return (intptr_t)NULL
;
1338 case DSP_SET_FREQUENCY
:
1339 memset(&dsp
->data
.resample_data
, 0, sizeof (dsp
->data
.resample_data
));
1340 /* Fall through!!! */
1341 case DSP_SWITCH_FREQUENCY
:
1342 dsp
->codec_frequency
= (value
== 0) ? NATIVE_FREQUENCY
: value
;
1343 /* Account for playback speed adjustment when setting dsp->frequency
1344 if we're called from the main audio thread. Voice UI thread should
1345 not need this feature.
1347 if (dsp
== &AUDIO_DSP
)
1348 dsp
->frequency
= pitch_ratio
* dsp
->codec_frequency
/ 1000;
1350 dsp
->frequency
= dsp
->codec_frequency
;
1352 resampler_new_delta(dsp
);
1356 case DSP_SET_SAMPLE_DEPTH
:
1357 dsp
->sample_depth
= value
;
1359 if (dsp
->sample_depth
<= NATIVE_DEPTH
)
1361 dsp
->frac_bits
= WORD_FRACBITS
;
1362 dsp
->sample_bytes
= sizeof (int16_t); /* samples are 16 bits */
1363 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1364 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1368 dsp
->frac_bits
= value
;
1369 dsp
->sample_bytes
= sizeof (int32_t); /* samples are 32 bits */
1370 dsp
->data
.clip_max
= (1 << value
) - 1;
1371 dsp
->data
.clip_min
= -(1 << value
);
1374 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1375 sample_input_new_format(dsp
);
1379 case DSP_SET_STEREO_MODE
:
1380 dsp
->stereo_mode
= value
;
1381 dsp
->data
.num_channels
= value
== STEREO_MONO
? 1 : 2;
1382 dsp_update_functions(dsp
);
1387 dsp
->stereo_mode
= STEREO_NONINTERLEAVED
;
1388 dsp
->data
.num_channels
= 2;
1389 dsp
->sample_depth
= NATIVE_DEPTH
;
1390 dsp
->frac_bits
= WORD_FRACBITS
;
1391 dsp
->sample_bytes
= sizeof (int16_t);
1392 dsp
->data
.output_scale
= dsp
->frac_bits
+ 1 - NATIVE_DEPTH
;
1393 dsp
->data
.clip_max
= ((1 << WORD_FRACBITS
) - 1);
1394 dsp
->data
.clip_min
= -((1 << WORD_FRACBITS
));
1395 dsp
->codec_frequency
= dsp
->frequency
= NATIVE_FREQUENCY
;
1397 if (dsp
== &AUDIO_DSP
)
1406 dsp_update_functions(dsp
);
1407 resampler_new_delta(dsp
);
1412 memset(&dsp
->data
.resample_data
, 0,
1413 sizeof (dsp
->data
.resample_data
));
1414 resampler_new_delta(dsp
);
1419 case DSP_SET_TRACK_GAIN
:
1420 if (dsp
== &AUDIO_DSP
)
1421 dsp_set_gain_var(&track_gain
, value
);
1424 case DSP_SET_ALBUM_GAIN
:
1425 if (dsp
== &AUDIO_DSP
)
1426 dsp_set_gain_var(&album_gain
, value
);
1429 case DSP_SET_TRACK_PEAK
:
1430 if (dsp
== &AUDIO_DSP
)
1431 dsp_set_gain_var(&track_peak
, value
);
1434 case DSP_SET_ALBUM_PEAK
:
1435 if (dsp
== &AUDIO_DSP
)
1436 dsp_set_gain_var(&album_peak
, value
);
1446 void dsp_set_replaygain(void)
1452 if ((global_settings
.replaygain_type
!= REPLAYGAIN_OFF
) ||
1453 global_settings
.replaygain_noclip
)
1455 bool track_mode
= get_replaygain_mode(track_gain
!= 0,
1456 album_gain
!= 0) == REPLAYGAIN_TRACK
;
1457 long peak
= (track_mode
|| !album_peak
) ? track_peak
: album_peak
;
1459 if (global_settings
.replaygain_type
!= REPLAYGAIN_OFF
)
1461 gain
= (track_mode
|| !album_gain
) ? track_gain
: album_gain
;
1463 if (global_settings
.replaygain_preamp
)
1465 long preamp
= get_replaygain_int(
1466 global_settings
.replaygain_preamp
* 10);
1468 gain
= (long) (((int64_t) gain
* preamp
) >> 24);
1474 /* So that noclip can work even with no gain information. */
1475 gain
= DEFAULT_GAIN
;
1478 if (global_settings
.replaygain_noclip
&& (peak
!= 0)
1479 && ((((int64_t) gain
* peak
) >> 24) >= DEFAULT_GAIN
))
1481 gain
= (((int64_t) DEFAULT_GAIN
<< 24) / peak
);
1484 if (gain
== DEFAULT_GAIN
)
1486 /* Nothing to do, disable processing. */
1491 /* Store in S8.23 format to simplify calculations. */
1493 set_gain(&AUDIO_DSP
);