/*************************************************************************** * __________ __ ___. * Open \______ \ ____ ____ | | _\_ |__ _______ ___ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ * $Id$ * * Copyright (C) 2005 Miika Pekkarinen * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY * KIND, either express or implied. * ****************************************************************************/ #include "config.h" #include #include #include #include #include "dsp.h" #include "eq.h" #include "kernel.h" #include "playback.h" #include "system.h" #include "settings.h" #include "replaygain.h" #include "misc.h" #include "tdspeed.h" #include "buffer.h" #include "fixedpoint.h" #include "fracmul.h" #include "pcmbuf.h" /* Define LOGF_ENABLE to enable logf output in this file */ /*#define LOGF_ENABLE*/ #include "logf.h" /* 16-bit samples are scaled based on these constants. The shift should be * no more than 15. */ #define WORD_SHIFT 12 #define WORD_FRACBITS 27 #define NATIVE_DEPTH 16 /* If the small buffer size changes, check the assembly code! */ #define SMALL_SAMPLE_BUF_COUNT 256 #define DEFAULT_GAIN 0x01000000 /* enums to index conversion properly with stereo mode and other settings */ enum { SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED, SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED, SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO, SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES, SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES, SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES, SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES }; enum { SAMPLE_OUTPUT_MONO = 0, SAMPLE_OUTPUT_STEREO, SAMPLE_OUTPUT_DITHERED_MONO, SAMPLE_OUTPUT_DITHERED_STEREO }; /**************************************************************************** * NOTE: Any assembly routines that use these structures must be updated * if current data members are moved or changed. */ struct resample_data { uint32_t delta; /* 00h */ uint32_t phase; /* 04h */ int32_t last_sample[2]; /* 08h */ /* 10h */ }; /* This is for passing needed data to assembly dsp routines. If another * dsp parameter needs to be passed, add to the end of the structure * and remove from dsp_config. * If another function type becomes assembly optimized and requires dsp * config info, add a pointer paramter of type "struct dsp_data *". * If removing something from other than the end, reserve the spot or * else update every implementation for every target. * Be sure to add the offset of the new member for easy viewing as well. :) * It is the first member of dsp_config and all members can be accessesed * through the main aggregate but this is intended to make a safe haven * for these items whereas the c part can be rearranged at will. dsp_data * could even moved within dsp_config without disurbing the order. */ struct dsp_data { int output_scale; /* 00h */ int num_channels; /* 04h */ struct resample_data resample_data; /* 08h */ int32_t clip_min; /* 18h */ int32_t clip_max; /* 1ch */ int32_t gain; /* 20h - Note that this is in S8.23 format. */ /* 24h */ }; /* No asm...yet */ struct dither_data { long error[3]; /* 00h */ long random; /* 0ch */ /* 10h */ }; struct crossfeed_data { int32_t gain; /* 00h - Direct path gain */ int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */ int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */ int32_t delay[13][2]; /* 20h */ int32_t *index; /* 88h - Current pointer into the delay line */ /* 8ch */ }; /* Current setup is one lowshelf filters three peaking filters and one * highshelf filter. Varying the number of shelving filters make no sense, * but adding peaking filters is possible. */ struct eq_state { char enabled[5]; /* 00h - Flags for active filters */ struct eqfilter filters[5]; /* 08h - packing is 4? */ /* 10ch */ }; /* Include header with defines which functions are implemented in assembly code for the target */ #include /* Typedefs keep things much neater in this case */ typedef void (*sample_input_fn_type)(int count, const char *src[], int32_t *dst[]); typedef int (*resample_fn_type)(int count, struct dsp_data *data, const int32_t *src[], int32_t *dst[]); typedef void (*sample_output_fn_type)(int count, struct dsp_data *data, const int32_t *src[], int16_t *dst); /* Single-DSP channel processing in place */ typedef void (*channels_process_fn_type)(int count, int32_t *buf[]); /* DSP local channel processing in place */ typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data, int32_t *buf[]); /* DSP processes that return a value */ typedef int (*return_fn_type)(int count, int32_t *buf[]); /* ***************************************************************************/ struct dsp_config { struct dsp_data data; /* Config members for use in asm routines */ long codec_frequency; /* Sample rate of data coming from the codec */ long frequency; /* Effective sample rate after pitch shift (if any) */ int sample_depth; int sample_bytes; int stereo_mode; int32_t tdspeed_percent; /* Speed% * PITCH_SPEED_PRECISION */ bool tdspeed_active; /* Timestretch is in use */ int frac_bits; long limiter_preamp; /* limiter amp gain in S7.24 format */ #ifdef HAVE_SW_TONE_CONTROLS /* Filter struct for software bass/treble controls */ struct eqfilter tone_filter; #endif /* Functions that change depending upon settings - NULL if stage is disabled */ sample_input_fn_type input_samples; resample_fn_type resample; sample_output_fn_type output_samples; /* These will be NULL for the voice codec and is more economical that way */ channels_process_dsp_fn_type apply_gain; channels_process_fn_type apply_crossfeed; channels_process_fn_type eq_process; channels_process_fn_type channels_process; return_fn_type limiter_process; }; /* General DSP config */ static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */ /* Dithering */ static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */ static long dither_mask IBSS_ATTR; static long dither_bias IBSS_ATTR; /* Crossfeed */ struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */ { .index = (int32_t *)crossfeed_data.delay }; /* Equalizer */ static struct eq_state eq_data; /* A */ /* Software tone controls */ #ifdef HAVE_SW_TONE_CONTROLS static int prescale; /* A/V */ static int bass; /* A/V */ static int treble; /* A/V */ #endif /* Settings applicable to audio codec only */ static int32_t pitch_ratio = PITCH_SPEED_100; static int channels_mode; long dsp_sw_gain; long dsp_sw_cross; static bool dither_enabled; static long eq_precut; static long track_gain; static bool new_gain; static long album_gain; static long track_peak; static long album_peak; static long replaygain; static bool crossfeed_enabled; #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO]) #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE]) /* The internal format is 32-bit samples, non-interleaved, stereo. This * format is similar to the raw output from several codecs, so the amount * of copying needed is minimized for that case. */ #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */ static int32_t small_sample_buf[SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR; static int32_t small_resample_buf[SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO] IBSS_ATTR; static int32_t *big_sample_buf = NULL; static int32_t *big_resample_buf = NULL; static int big_sample_buf_count = -1; /* -1=unknown, 0=not available */ static int sample_buf_count; static int32_t *sample_buf; static int32_t *resample_buf; #define SAMPLE_BUF_LEFT_CHANNEL 0 #define SAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2) #define RESAMPLE_BUF_LEFT_CHANNEL 0 #define RESAMPLE_BUF_RIGHT_CHANNEL (sample_buf_count/2 * RESAMPLE_RATIO) /* limiter */ /* MAX_COUNT is largest possible sample count in limiter_process. This is needed in case time stretch makes the count in dsp_process larger than the limiter buffer. */ #define MAX_COUNT MAX(SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO / 2, LIMITER_BUFFER_SIZE) static int count_adjust; static bool limiter_buffer_active; static bool limiter_buffer_full; static bool limiter_buffer_emptying; static int32_t limiter_buffer[2][LIMITER_BUFFER_SIZE] IBSS_ATTR; static int32_t *start_lim_buf[2] IBSS_ATTR, *end_lim_buf[2] IBSS_ATTR; static uint16_t lim_buf_peak[LIMITER_BUFFER_SIZE] IBSS_ATTR; static uint16_t *start_peak IBSS_ATTR, *end_peak IBSS_ATTR; static uint16_t out_buf_peak[MAX_COUNT] IBSS_ATTR; static uint16_t *out_buf_peak_index IBSS_ATTR; static uint16_t release_peak IBSS_ATTR; static int32_t in_samp IBSS_ATTR, samp0 IBSS_ATTR; static void reset_limiter_buffer(struct dsp_config *dsp); static int limiter_buffer_count(bool buf_count); static int limiter_process(int count, int32_t *buf[]); static uint16_t get_peak_value(int32_t sample); /* The clip_steps array essentially stores the results of fp_factor from * 0 to 12 dB, in 48 equal steps, in S3.28 format. */ const long clip_steps[49] ICONST_ATTR = { 0x10000000, 0x10779AFA, 0x10F2B409, 0x1171654C, 0x11F3C9A0, 0x1279FCAD, 0x13041AE9, 0x139241A2, 0x14248EF9, 0x14BB21F9, 0x15561A92, 0x15F599A0, 0x1699C0F9, 0x1742B36B, 0x17F094CE, 0x18A38A01, 0x195BB8F9, 0x1A1948C5, 0x1ADC619B, 0x1BA52CDC, 0x1C73D51D, 0x1D488632, 0x1E236D3A, 0x1F04B8A1, 0x1FEC982C, 0x20DB3D0E, 0x21D0D9E2, 0x22CDA2BE, 0x23D1CD41, 0x24DD9099, 0x25F12590, 0x270CC693, 0x2830AFD3, 0x295D1F37, 0x2A925471, 0x2BD0911F, 0x2D1818B3, 0x2E6930AD, 0x2FC42095, 0x312931EC, 0x3298B072, 0x3412EA24, 0x35982F3A, 0x3728D22E, 0x38C52808, 0x3A6D8847, 0x3C224CD9, 0x3DE3D264, 0x3FB2783F}; /* The gain_steps array essentially stores the results of fp_factor from * 0 to -12 dB, in 48 equal steps, in S3.28 format. */ const long gain_steps[49] ICONST_ATTR = { 0x10000000, 0xF8BC9C0, 0xF1ADF94, 0xEAD2988, 0xE429058, 0xDDAFD68, 0xD765AC1, 0xD149309, 0xCB59186, 0xC594210, 0xBFF9112, 0xBA86B88, 0xB53BEF5, 0xB017965, 0xAB18964, 0xA63DDFE, 0xA1866BA, 0x9CF1397, 0x987D507, 0x9429BEE, 0x8FF599E, 0x8BDFFD3, 0x87E80B0, 0x840CEBE, 0x804DCE8, 0x7CA9E76, 0x792070E, 0x75B0AB0, 0x7259DB2, 0x6F1B4BF, 0x6BF44D5, 0x68E4342, 0x65EA5A0, 0x63061D6, 0x6036E15, 0x5D7C0D3, 0x5AD50CE, 0x5841505, 0x55C04B8, 0x535176A, 0x50F44D9, 0x4EA84FE, 0x4C6D00E, 0x4A41E78, 0x48268DF, 0x461A81C, 0x441D53E, 0x422E985, 0x404DE62}; /* Clip sample to signed 16 bit range */ static inline int32_t clip_sample_16(int32_t sample) { if ((int16_t)sample != sample) sample = 0x7fff ^ (sample >> 31); return sample; } int32_t sound_get_pitch(void) { return pitch_ratio; } void sound_set_pitch(int32_t percent) { pitch_ratio = percent; dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY, AUDIO_DSP.codec_frequency); } static void tdspeed_setup(struct dsp_config *dspc) { /* Assume timestretch will not be used */ dspc->tdspeed_active = false; sample_buf = small_sample_buf; resample_buf = small_resample_buf; sample_buf_count = SMALL_SAMPLE_BUF_COUNT; if(!dsp_timestretch_available()) return; /* Timestretch not enabled or buffer not allocated */ if (dspc->tdspeed_percent == 0) dspc->tdspeed_percent = PITCH_SPEED_100; if (!tdspeed_config( dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency, dspc->stereo_mode != STEREO_MONO, dspc->tdspeed_percent)) return; /* Timestretch not possible or needed with these parameters */ /* Timestretch is to be used */ dspc->tdspeed_active = true; sample_buf = big_sample_buf; sample_buf_count = big_sample_buf_count; resample_buf = big_resample_buf; } void dsp_timestretch_enable(bool enabled) { /* Hook to set up timestretch buffer on first call to settings_apply() */ if (big_sample_buf_count < 0) /* Only do something on first call */ { if (enabled) { /* Set up timestretch buffers */ big_sample_buf_count = SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO; big_sample_buf = small_resample_buf; big_resample_buf = (int32_t *) buffer_alloc(big_sample_buf_count * RESAMPLE_RATIO * sizeof(int32_t)); } else { /* Not enabled at startup, "big" buffers will never be available */ big_sample_buf_count = 0; } tdspeed_setup(&AUDIO_DSP); } } void dsp_set_timestretch(int32_t percent) { AUDIO_DSP.tdspeed_percent = percent; tdspeed_setup(&AUDIO_DSP); } int32_t dsp_get_timestretch() { return AUDIO_DSP.tdspeed_percent; } bool dsp_timestretch_available() { return (global_settings.timestretch_enabled && big_sample_buf_count > 0); } /* Convert count samples to the internal format, if needed. Updates src * to point past the samples "consumed" and dst is set to point to the * samples to consume. Note that for mono, dst[0] equals dst[1], as there * is no point in processing the same data twice. */ /* convert count 16-bit mono to 32-bit mono */ static void sample_input_lte_native_mono( int count, const char *src[], int32_t *dst[]) { const int16_t *s = (int16_t *) src[0]; const int16_t * const send = s + count; int32_t *d = dst[0] = dst[1] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL]; int scale = WORD_SHIFT; while (s < send) { *d++ = *s++ << scale; } src[0] = (char *)s; } /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */ static void sample_input_lte_native_i_stereo( int count, const char *src[], int32_t *dst[]) { const int32_t *s = (int32_t *) src[0]; const int32_t * const send = s + count; int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL]; int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL]; int scale = WORD_SHIFT; while (s < send) { int32_t slr = *s++; #ifdef ROCKBOX_LITTLE_ENDIAN *dl++ = (slr >> 16) << scale; *dr++ = (int32_t)(int16_t)slr << scale; #else /* ROCKBOX_BIG_ENDIAN */ *dl++ = (int32_t)(int16_t)slr << scale; *dr++ = (slr >> 16) << scale; #endif } src[0] = (char *)s; } /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */ static void sample_input_lte_native_ni_stereo( int count, const char *src[], int32_t *dst[]) { const int16_t *sl = (int16_t *) src[0]; const int16_t *sr = (int16_t *) src[1]; const int16_t * const slend = sl + count; int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL]; int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL]; int scale = WORD_SHIFT; while (sl < slend) { *dl++ = *sl++ << scale; *dr++ = *sr++ << scale; } src[0] = (char *)sl; src[1] = (char *)sr; } /* convert count 32-bit mono to 32-bit mono */ static void sample_input_gt_native_mono( int count, const char *src[], int32_t *dst[]) { dst[0] = dst[1] = (int32_t *)src[0]; src[0] = (char *)(dst[0] + count); } /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */ static void sample_input_gt_native_i_stereo( int count, const char *src[], int32_t *dst[]) { const int32_t *s = (int32_t *)src[0]; const int32_t * const send = s + 2*count; int32_t *dl = dst[0] = &sample_buf[SAMPLE_BUF_LEFT_CHANNEL]; int32_t *dr = dst[1] = &sample_buf[SAMPLE_BUF_RIGHT_CHANNEL]; while (s < send) { *dl++ = *s++; *dr++ = *s++; } src[0] = (char *)send; } /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */ static void sample_input_gt_native_ni_stereo( int count, const char *src[], int32_t *dst[]) { dst[0] = (int32_t *)src[0]; dst[1] = (int32_t *)src[1]; src[0] = (char *)(dst[0] + count); src[1] = (char *)(dst[1] + count); } /** * sample_input_new_format() * * set the to-native sample conversion function based on dsp sample parameters * * !DSPPARAMSYNC * needs syncing with changes to the following dsp parameters: * * dsp->stereo_mode (A/V) * * dsp->sample_depth (A/V) */ static void sample_input_new_format(struct dsp_config *dsp) { static const sample_input_fn_type sample_input_functions[] = { [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo, [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo, [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono, [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo, [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo, [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono, }; int convert = dsp->stereo_mode; if (dsp->sample_depth > NATIVE_DEPTH) convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX; dsp->input_samples = sample_input_functions[convert]; } #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO /* write mono internal format to output format */ static void sample_output_mono(int count, struct dsp_data *data, const int32_t *src[], int16_t *dst) { const int32_t *s0 = src[0]; const int scale = data->output_scale; const int dc_bias = 1 << (scale - 1); while (count-- > 0) { int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale); *dst++ = lr; *dst++ = lr; } } #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */ /* write stereo internal format to output format */ #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO static void sample_output_stereo(int count, struct dsp_data *data, const int32_t *src[], int16_t *dst) { const int32_t *s0 = src[0]; const int32_t *s1 = src[1]; const int scale = data->output_scale; const int dc_bias = 1 << (scale - 1); while (count-- > 0) { *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale); *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale); } } #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */ /** * The "dither" code to convert the 24-bit samples produced by libmad was * taken from the coolplayer project - coolplayer.sourceforge.net * * This function handles mono and stereo outputs. */ static void sample_output_dithered(int count, struct dsp_data *data, const int32_t *src[], int16_t *dst) { const int32_t mask = dither_mask; const int32_t bias = dither_bias; const int scale = data->output_scale; const int32_t min = data->clip_min; const int32_t max = data->clip_max; const int32_t range = max - min; int ch; int16_t *d; for (ch = 0; ch < data->num_channels; ch++) { struct dither_data * const dither = &dither_data[ch]; const int32_t *s = src[ch]; int i; for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2) { int32_t output, sample; int32_t random; /* Noise shape and bias (for correct rounding later) */ sample = *s; sample += dither->error[0] - dither->error[1] + dither->error[2]; dither->error[2] = dither->error[1]; dither->error[1] = dither->error[0]/2; output = sample + bias; /* Dither, highpass triangle PDF */ random = dither->random*0x0019660dL + 0x3c6ef35fL; output += (random & mask) - (dither->random & mask); dither->random = random; /* Round sample to output range */ output &= ~mask; /* Error feedback */ dither->error[0] = sample - output; /* Clip */ if ((uint32_t)(output - min) > (uint32_t)range) { int32_t c = min; if (output > min) c += range; output = c; } /* Quantize and store */ *d = output >> scale; } } if (data->num_channels == 2) return; /* Have to duplicate left samples into the right channel since pcm buffer and hardware is interleaved stereo */ d = &dst[0]; while (count-- > 0) { int16_t s = *d++; *d++ = s; } } /** * sample_output_new_format() * * set the from-native to ouput sample conversion routine * * !DSPPARAMSYNC * needs syncing with changes to the following dsp parameters: * * dsp->stereo_mode (A/V) * * dither_enabled (A) */ static void sample_output_new_format(struct dsp_config *dsp) { static const sample_output_fn_type sample_output_functions[] = { sample_output_mono, sample_output_stereo, sample_output_dithered, sample_output_dithered }; int out = dsp->data.num_channels - 1; if (dsp == &AUDIO_DSP && dither_enabled) out += 2; dsp->output_samples = sample_output_functions[out]; } /** * Linear interpolation resampling that introduces a one sample delay because * of our inability to look into the future at the end of a frame. */ #ifndef DSP_HAVE_ASM_RESAMPLING static int dsp_downsample(int count, struct dsp_data *data, const int32_t *src[], int32_t *dst[]) { int ch = data->num_channels - 1; uint32_t delta = data->resample_data.delta; uint32_t phase, pos; int32_t *d; /* Rolled channel loop actually showed slightly faster. */ do { /* Just initialize things and not worry too much about the relatively * uncommon case of not being able to spit out a sample for the frame. */ const int32_t *s = src[ch]; int32_t last = data->resample_data.last_sample[ch]; data->resample_data.last_sample[ch] = s[count - 1]; d = dst[ch]; phase = data->resample_data.phase; pos = phase >> 16; /* Do we need last sample of previous frame for interpolation? */ if (pos > 0) last = s[pos - 1]; while (pos < (uint32_t)count) { *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); phase += delta; pos = phase >> 16; last = s[pos - 1]; } } while (--ch >= 0); /* Wrap phase accumulator back to start of next frame. */ data->resample_data.phase = phase - (count << 16); return d - dst[0]; } static int dsp_upsample(int count, struct dsp_data *data, const int32_t *src[], int32_t *dst[]) { int ch = data->num_channels - 1; uint32_t delta = data->resample_data.delta; uint32_t phase, pos; int32_t *d; /* Rolled channel loop actually showed slightly faster. */ do { /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */ const int32_t *s = src[ch]; int32_t last = data->resample_data.last_sample[ch]; data->resample_data.last_sample[ch] = s[count - 1]; d = dst[ch]; phase = data->resample_data.phase; pos = phase >> 16; while (pos == 0) { *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last); phase += delta; pos = phase >> 16; } while (pos < (uint32_t)count) { last = s[pos - 1]; *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); phase += delta; pos = phase >> 16; } } while (--ch >= 0); /* Wrap phase accumulator back to start of next frame. */ data->resample_data.phase = phase & 0xffff; return d - dst[0]; } #endif /* DSP_HAVE_ASM_RESAMPLING */ static void resampler_new_delta(struct dsp_config *dsp) { dsp->data.resample_data.delta = (unsigned long) dsp->frequency * 65536LL / NATIVE_FREQUENCY; if (dsp->frequency == NATIVE_FREQUENCY) { /* NOTE: If fully glitch-free transistions from no resampling to resampling are desired, last_sample history should be maintained even when not resampling. */ dsp->resample = NULL; dsp->data.resample_data.phase = 0; dsp->data.resample_data.last_sample[0] = 0; dsp->data.resample_data.last_sample[1] = 0; } else if (dsp->frequency < NATIVE_FREQUENCY) dsp->resample = dsp_upsample; else dsp->resample = dsp_downsample; } /* Resample count stereo samples. Updates the src array, if resampling is * done, to refer to the resampled data. Returns number of stereo samples * for further processing. */ static inline int resample(struct dsp_config *dsp, int count, int32_t *src[]) { int32_t *dst[2] = { &resample_buf[RESAMPLE_BUF_LEFT_CHANNEL], &resample_buf[RESAMPLE_BUF_RIGHT_CHANNEL], }; count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst); src[0] = dst[0]; src[1] = dst[dsp->data.num_channels - 1]; return count; } static void dither_init(struct dsp_config *dsp) { memset(dither_data, 0, sizeof (dither_data)); dither_bias = (1L << (dsp->frac_bits - NATIVE_DEPTH)); dither_mask = (1L << (dsp->frac_bits + 1 - NATIVE_DEPTH)) - 1; } void dsp_dither_enable(bool enable) { struct dsp_config *dsp = &AUDIO_DSP; dither_enabled = enable; sample_output_new_format(dsp); } /* Applies crossfeed to the stereo signal in src. * Crossfeed is a process where listening over speakers is simulated. This * is good for old hard panned stereo records, which might be quite fatiguing * to listen to on headphones with no crossfeed. */ #ifndef DSP_HAVE_ASM_CROSSFEED static void apply_crossfeed(int count, int32_t *buf[]) { int32_t *hist_l = &crossfeed_data.history[0]; int32_t *hist_r = &crossfeed_data.history[2]; int32_t *delay = &crossfeed_data.delay[0][0]; int32_t *coefs = &crossfeed_data.coefs[0]; int32_t gain = crossfeed_data.gain; int32_t *di = crossfeed_data.index; int32_t acc; int32_t left, right; int i; for (i = 0; i < count; i++) { left = buf[0][i]; right = buf[1][i]; /* Filter delayed sample from left speaker */ acc = FRACMUL(*di, coefs[0]); acc += FRACMUL(hist_l[0], coefs[1]); acc += FRACMUL(hist_l[1], coefs[2]); /* Save filter history for left speaker */ hist_l[1] = acc; hist_l[0] = *di; *di++ = left; /* Filter delayed sample from right speaker */ acc = FRACMUL(*di, coefs[0]); acc += FRACMUL(hist_r[0], coefs[1]); acc += FRACMUL(hist_r[1], coefs[2]); /* Save filter history for right speaker */ hist_r[1] = acc; hist_r[0] = *di; *di++ = right; /* Now add the attenuated direct sound and write to outputs */ buf[0][i] = FRACMUL(left, gain) + hist_r[1]; buf[1][i] = FRACMUL(right, gain) + hist_l[1]; /* Wrap delay line index if bigger than delay line size */ if (di >= delay + 13*2) di = delay; } /* Write back local copies of data we've modified */ crossfeed_data.index = di; } #endif /* DSP_HAVE_ASM_CROSSFEED */ /** * dsp_set_crossfeed(bool enable) * * !DSPPARAMSYNC * needs syncing with changes to the following dsp parameters: * * dsp->stereo_mode (A) */ void dsp_set_crossfeed(bool enable) { crossfeed_enabled = enable; AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1) ? apply_crossfeed : NULL; } void dsp_set_crossfeed_direct_gain(int gain) { crossfeed_data.gain = get_replaygain_int(gain * 10) << 7; /* If gain is negative, the calculation overflowed and we need to clamp */ if (crossfeed_data.gain < 0) crossfeed_data.gain = 0x7fffffff; } /* Both gains should be below 0 dB */ void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) { int32_t *c = crossfeed_data.coefs; long scaler = get_replaygain_int(lf_gain * 10) << 7; cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff; hf_gain -= lf_gain; /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB * point instead of shelf midpoint. This is for compatibility with the old * crossfeed shelf filter and should be removed if crossfeed settings are * ever made incompatible for any other good reason. */ cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24); filter_shelf_coefs(cutoff, hf_gain, false, c); /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains * over 1 and can do this safely */ c[0] = FRACMUL_SHL(c[0], scaler, 4); c[1] = FRACMUL_SHL(c[1], scaler, 4); c[2] <<= 4; } /* Apply a constant gain to the samples (e.g., for ReplayGain). * Note that this must be called before the resampler. */ #ifndef DSP_HAVE_ASM_APPLY_GAIN static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) { const int32_t gain = data->gain; int ch; for (ch = 0; ch < data->num_channels; ch++) { int32_t *d = buf[ch]; int i; for (i = 0; i < count; i++) d[i] = FRACMUL_SHL(d[i], gain, 8); } } #endif /* DSP_HAVE_ASM_APPLY_GAIN */ /* Combine all gains to a global gain. */ static void set_gain(struct dsp_config *dsp) { /* gains are in S7.24 format */ dsp->data.gain = DEFAULT_GAIN; /* Replay gain not relevant to voice */ if (dsp == &AUDIO_DSP && replaygain) { dsp->data.gain = replaygain; } if (dsp->eq_process && eq_precut) { dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24); } /* only preamp for the limiter if limiter is active and sample depth * allows safe pre-amping (12 dB is OK with 29 or less frac bits) */ if ((dsp->limiter_preamp) && (dsp->frac_bits <= 29)) { dsp->data.gain = fp_mul(dsp->data.gain, dsp->limiter_preamp, 24); } #ifdef HAVE_SW_VOLUME_CONTROL if (global_settings.volume < SW_VOLUME_MAX || global_settings.volume > SW_VOLUME_MIN) { int vol_gain = get_replaygain_int(global_settings.volume * 100); dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24); } #endif if (dsp->data.gain == DEFAULT_GAIN) { dsp->data.gain = 0; } else { dsp->data.gain >>= 1; /* convert gain to S8.23 format */ } dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL; } /** * Update the amount to cut the audio before applying the equalizer. * * @param precut to apply in decibels (multiplied by 10) */ void dsp_set_eq_precut(int precut) { eq_precut = get_replaygain_int(precut * -10); set_gain(&AUDIO_DSP); } /** * Synchronize the equalizer filter coefficients with the global settings. * * @param band the equalizer band to synchronize */ void dsp_set_eq_coefs(int band) { const int *setting; long gain; unsigned long cutoff, q; /* Adjust setting pointer to the band we actually want to change */ setting = &global_settings.eq_band0_cutoff + (band * 3); /* Convert user settings to format required by coef generator functions */ cutoff = 0xffffffff / NATIVE_FREQUENCY * (*setting++); q = *setting++; gain = *setting++; if (q == 0) q = 1; /* NOTE: The coef functions assume the EMAC unit is in fractional mode, which it should be, since we're executed from the main thread. */ /* Assume a band is disabled if the gain is zero */ if (gain == 0) { eq_data.enabled[band] = 0; } else { if (band == 0) eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs); else if (band == 4) eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs); else eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs); eq_data.enabled[band] = 1; } } /* Apply EQ filters to those bands that have got it switched on. */ static void eq_process(int count, int32_t *buf[]) { static const int shifts[] = { EQ_SHELF_SHIFT, /* low shelf */ EQ_PEAK_SHIFT, /* peaking */ EQ_PEAK_SHIFT, /* peaking */ EQ_PEAK_SHIFT, /* peaking */ EQ_SHELF_SHIFT, /* high shelf */ }; unsigned int channels = AUDIO_DSP.data.num_channels; int i; /* filter configuration currently is 1 low shelf filter, 3 band peaking filters and 1 high shelf filter, in that order. we need to know this so we can choose the correct shift factor. */ for (i = 0; i < 5; i++) { if (!eq_data.enabled[i]) continue; eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]); } } /** * Use to enable the equalizer. * * @param enable true to enable the equalizer */ void dsp_set_eq(bool enable) { AUDIO_DSP.eq_process = enable ? eq_process : NULL; set_gain(&AUDIO_DSP); } static void dsp_set_stereo_width(int value) { long width, straight, cross; width = value * 0x7fffff / 100; if (value <= 100) { straight = (0x7fffff + width) / 2; cross = straight - width; } else { /* straight = (1 + width) / (2 * width) */ straight = ((int64_t)(0x7fffff + width) << 22) / width; cross = straight - 0x7fffff; } dsp_sw_gain = straight << 8; dsp_sw_cross = cross << 8; } /** * Implements the different channel configurations and stereo width. */ /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for * completeness. */ #if 0 static void channels_process_sound_chan_stereo(int count, int32_t *buf[]) { /* The channels are each just themselves */ (void)count; (void)buf; } #endif #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO static void channels_process_sound_chan_mono(int count, int32_t *buf[]) { int32_t *sl = buf[0], *sr = buf[1]; while (count-- > 0) { int32_t lr = *sl/2 + *sr/2; *sl++ = lr; *sr++ = lr; } } #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */ #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM static void channels_process_sound_chan_custom(int count, int32_t *buf[]) { const int32_t gain = dsp_sw_gain; const int32_t cross = dsp_sw_cross; int32_t *sl = buf[0], *sr = buf[1]; while (count-- > 0) { int32_t l = *sl; int32_t r = *sr; *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross); *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross); } } #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */ static void channels_process_sound_chan_mono_left(int count, int32_t *buf[]) { /* Just copy over the other channel */ memcpy(buf[1], buf[0], count * sizeof (*buf)); } static void channels_process_sound_chan_mono_right(int count, int32_t *buf[]) { /* Just copy over the other channel */ memcpy(buf[0], buf[1], count * sizeof (*buf)); } #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE static void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) { int32_t *sl = buf[0], *sr = buf[1]; while (count-- > 0) { int32_t ch = *sl/2 - *sr/2; *sl++ = ch; *sr++ = -ch; } } #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */ static void dsp_set_channel_config(int value) { static const channels_process_fn_type channels_process_functions[] = { /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */ [SOUND_CHAN_STEREO] = NULL, [SOUND_CHAN_MONO] = channels_process_sound_chan_mono, [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom, [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left, [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right, [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke, }; if ((unsigned)value >= ARRAYLEN(channels_process_functions) || AUDIO_DSP.stereo_mode == STEREO_MONO) { value = SOUND_CHAN_STEREO; } /* This doesn't apply to voice */ channels_mode = value; AUDIO_DSP.channels_process = channels_process_functions[value]; } #if CONFIG_CODEC == SWCODEC #ifdef HAVE_SW_TONE_CONTROLS static void set_tone_controls(void) { filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200, 0xffffffff/NATIVE_FREQUENCY*3500, bass, treble, -prescale, AUDIO_DSP.tone_filter.coefs); /* Sync the voice dsp coefficients */ memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs, sizeof (VOICE_DSP.tone_filter.coefs)); } #endif /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/ * code directly. */ int dsp_callback(int msg, intptr_t param) { switch (msg) { #ifdef HAVE_SW_TONE_CONTROLS case DSP_CALLBACK_SET_PRESCALE: prescale = param; set_tone_controls(); break; /* prescaler is always set after calling any of these, so we wait with * calculating coefs until the above case is hit. */ case DSP_CALLBACK_SET_BASS: bass = param; break; case DSP_CALLBACK_SET_TREBLE: treble = param; break; #ifdef HAVE_SW_VOLUME_CONTROL case DSP_CALLBACK_SET_SW_VOLUME: set_gain(&AUDIO_DSP); break; #endif #endif case DSP_CALLBACK_SET_CHANNEL_CONFIG: dsp_set_channel_config(param); break; case DSP_CALLBACK_SET_STEREO_WIDTH: dsp_set_stereo_width(param); break; default: break; } return 0; } #endif /* Process and convert src audio to dst based on the DSP configuration, * reading count number of audio samples. dst is assumed to be large * enough; use dsp_output_count() to get the required number. src is an * array of pointers; for mono and interleaved stereo, it contains one * pointer to the start of the audio data and the other is ignored; for * non-interleaved stereo, it contains two pointers, one for each audio * channel. Returns number of bytes written to dst. */ int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count) { int32_t *tmp[2]; static long last_yield; long tick; int written = 0; #if defined(CPU_COLDFIRE) /* set emac unit for dsp processing, and save old macsr, we're running in codec thread context at this point, so can't clobber it */ unsigned long old_macsr = coldfire_get_macsr(); coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE); #endif if (new_gain) dsp_set_replaygain(); /* Gain has changed */ /* Perform at least one yield before starting */ last_yield = current_tick; yield(); /* Testing function pointers for NULL is preferred since the pointer will be preloaded to be used for the call if not. */ while (count > 0) { int samples = MIN(sample_buf_count/2, count); count -= samples; dsp->input_samples(samples, src, tmp); if (dsp->tdspeed_active) samples = tdspeed_doit(tmp, samples); int chunk_offset = 0; while (samples > 0) { int32_t *t2[2]; t2[0] = tmp[0]+chunk_offset; t2[1] = tmp[1]+chunk_offset; int chunk = MIN(sample_buf_count/2, samples); chunk_offset += chunk; samples -= chunk; if (dsp->apply_gain) dsp->apply_gain(chunk, &dsp->data, t2); if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0) break; /* I'm pretty sure we're downsampling here */ if (dsp->apply_crossfeed) dsp->apply_crossfeed(chunk, t2); if (dsp->eq_process) dsp->eq_process(chunk, t2); #ifdef HAVE_SW_TONE_CONTROLS if ((bass | treble) != 0) eq_filter(t2, &dsp->tone_filter, chunk, dsp->data.num_channels, FILTER_BISHELF_SHIFT); #endif if (dsp->channels_process) dsp->channels_process(chunk, t2); if (dsp->limiter_process) chunk = dsp->limiter_process(chunk, t2); dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst); written += chunk; dst += chunk * sizeof (int16_t) * 2; /* yield at least once each tick */ tick = current_tick; if (TIME_AFTER(tick, last_yield)) { last_yield = tick; yield(); } } } #if defined(CPU_COLDFIRE) /* set old macsr again */ coldfire_set_macsr(old_macsr); #endif return written; } /* Given count number of input samples, calculate the maximum number of * samples of output data that would be generated (the calculation is not * entirely exact and rounds upwards to be on the safe side; during * resampling, the number of samples generated depends on the current state * of the resampler). */ /* dsp_input_size MUST be called afterwards */ int dsp_output_count(struct dsp_config *dsp, int count) { if (dsp->tdspeed_active) count = tdspeed_est_output_size(); if (dsp->resample) { count = (int)(((unsigned long)count * NATIVE_FREQUENCY + (dsp->frequency - 1)) / dsp->frequency); } /* Now we have the resampled sample count which must not exceed * RESAMPLE_BUF_RIGHT_CHANNEL to avoid resample buffer overflow. One * must call dsp_input_count() to get the correct input sample * count. */ if (count > RESAMPLE_BUF_RIGHT_CHANNEL) count = RESAMPLE_BUF_RIGHT_CHANNEL; /* If the limiter buffer is filling, some or all samples will * be captured by it, so expect fewer samples coming out. */ if (limiter_buffer_active && !limiter_buffer_full) { int empty_space = limiter_buffer_count(false); count_adjust = MIN(empty_space, count); count -= count_adjust; } return count; } /* Given count output samples, calculate number of input samples * that would be consumed in order to fill the output buffer. */ int dsp_input_count(struct dsp_config *dsp, int count) { /* If the limiter buffer is filling, the output count was * adjusted downward. This adjusts it back so that input * count is not affected. */ if (limiter_buffer_active && !limiter_buffer_full) count += count_adjust; /* count is now the number of resampled input samples. Convert to original input samples. */ if (dsp->resample) { /* Use the real resampling delta = * dsp->frequency * 65536 / NATIVE_FREQUENCY, and * round towards zero to avoid buffer overflows. */ count = (int)(((unsigned long)count * dsp->data.resample_data.delta) >> 16); } if (dsp->tdspeed_active) count = tdspeed_est_input_size(count); return count; } static void dsp_set_gain_var(long *var, long value) { *var = value; new_gain = true; } static void dsp_update_functions(struct dsp_config *dsp) { sample_input_new_format(dsp); sample_output_new_format(dsp); if (dsp == &AUDIO_DSP) dsp_set_crossfeed(crossfeed_enabled); } intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value) { switch (setting) { case DSP_MYDSP: switch (value) { case CODEC_IDX_AUDIO: return (intptr_t)&AUDIO_DSP; case CODEC_IDX_VOICE: return (intptr_t)&VOICE_DSP; default: return (intptr_t)NULL; } case DSP_SET_FREQUENCY: memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data)); /* Fall through!!! */ case DSP_SWITCH_FREQUENCY: dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value; /* Account for playback speed adjustment when setting dsp->frequency if we're called from the main audio thread. Voice UI thread should not need this feature. */ if (dsp == &AUDIO_DSP) dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100; else dsp->frequency = dsp->codec_frequency; resampler_new_delta(dsp); tdspeed_setup(dsp); break; case DSP_SET_SAMPLE_DEPTH: dsp->sample_depth = value; if (dsp->sample_depth <= NATIVE_DEPTH) { dsp->frac_bits = WORD_FRACBITS; dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */ dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); dsp->data.clip_min = -((1 << WORD_FRACBITS)); } else { dsp->frac_bits = value; dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */ dsp->data.clip_max = (1 << value) - 1; dsp->data.clip_min = -(1 << value); } dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH; sample_input_new_format(dsp); dither_init(dsp); break; case DSP_SET_STEREO_MODE: dsp->stereo_mode = value; dsp->data.num_channels = value == STEREO_MONO ? 1 : 2; dsp_update_functions(dsp); tdspeed_setup(dsp); break; case DSP_RESET: dsp->stereo_mode = STEREO_NONINTERLEAVED; dsp->data.num_channels = 2; dsp->sample_depth = NATIVE_DEPTH; dsp->frac_bits = WORD_FRACBITS; dsp->sample_bytes = sizeof (int16_t); dsp->data.output_scale = dsp->frac_bits + 1 - NATIVE_DEPTH; dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); dsp->data.clip_min = -((1 << WORD_FRACBITS)); dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY; if (dsp == &AUDIO_DSP) { track_gain = 0; album_gain = 0; track_peak = 0; album_peak = 0; new_gain = true; } dsp_update_functions(dsp); resampler_new_delta(dsp); tdspeed_setup(dsp); reset_limiter_buffer(dsp); break; case DSP_FLUSH: memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data)); resampler_new_delta(dsp); dither_init(dsp); tdspeed_setup(dsp); reset_limiter_buffer(dsp); break; case DSP_SET_TRACK_GAIN: if (dsp == &AUDIO_DSP) dsp_set_gain_var(&track_gain, value); break; case DSP_SET_ALBUM_GAIN: if (dsp == &AUDIO_DSP) dsp_set_gain_var(&album_gain, value); break; case DSP_SET_TRACK_PEAK: if (dsp == &AUDIO_DSP) dsp_set_gain_var(&track_peak, value); break; case DSP_SET_ALBUM_PEAK: if (dsp == &AUDIO_DSP) dsp_set_gain_var(&album_peak, value); break; default: return 0; } return 1; } void dsp_set_replaygain(void) { long gain = 0; new_gain = false; if ((global_settings.replaygain_type != REPLAYGAIN_OFF) || global_settings.replaygain_noclip) { bool track_mode = get_replaygain_mode(track_gain != 0, album_gain != 0) == REPLAYGAIN_TRACK; long peak = (track_mode || !album_peak) ? track_peak : album_peak; if (global_settings.replaygain_type != REPLAYGAIN_OFF) { gain = (track_mode || !album_gain) ? track_gain : album_gain; if (global_settings.replaygain_preamp) { long preamp = get_replaygain_int( global_settings.replaygain_preamp * 10); gain = (long) (((int64_t) gain * preamp) >> 24); } } if (gain == 0) { /* So that noclip can work even with no gain information. */ gain = DEFAULT_GAIN; } if (global_settings.replaygain_noclip && (peak != 0) && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN)) { gain = (((int64_t) DEFAULT_GAIN << 24) / peak); } if (gain == DEFAULT_GAIN) { /* Nothing to do, disable processing. */ gain = 0; } } /* Store in S7.24 format to simplify calculations. */ replaygain = gain; set_gain(&AUDIO_DSP); } /** RESET THE LIMITER BUFFER * Force the limiter buffer to its initial state and discard * any samples held there. */ static void reset_limiter_buffer(struct dsp_config *dsp) { if (dsp == &AUDIO_DSP) { int i; logf(" reset_limiter_buffer"); for (i = 0; i < 2; i++) start_lim_buf[i] = end_lim_buf[i] = limiter_buffer[i]; start_peak = end_peak = lim_buf_peak; limiter_buffer_full = false; limiter_buffer_emptying = false; release_peak = 0; } } /** OPERATE THE LIMITER BUFFER * Handle all samples entering or exiting the limiter buffer. */ static inline int set_limiter_buffer(int count, int32_t *buf[]) { int32_t *in_buf[] = {buf[0], buf[1]}, *out_buf[] = {buf[0], buf[1]}; int empty_space, i, out_count; const long clip_max = AUDIO_DSP.data.clip_max; const int ch = AUDIO_DSP.data.num_channels - 1; out_buf_peak_index = out_buf_peak; if (limiter_buffer_emptying) /** EMPTY THE BUFFER * since the empty flag has been set, assume no inbound samples and return all samples in the limiter buffer to the outbound buffer */ { count = limiter_buffer_count(true); out_count = count; logf(" Emptying limiter buffer: %d", count); while (count-- > 0) { for (i = 0; i <= ch; i++) { /* move samples in limiter buffer to output buffer */ *out_buf[i]++ = *start_lim_buf[i]++; if (start_lim_buf[i] == &limiter_buffer[i][LIMITER_BUFFER_SIZE]) start_lim_buf[i] = limiter_buffer[i]; /* move limiter buffer peak values to output peak values */ if (i == 0) { *out_buf_peak_index++ = *start_peak++; if (start_peak == &lim_buf_peak[LIMITER_BUFFER_SIZE]) start_peak = lim_buf_peak; } } } limiter_buffer_full = false; limiter_buffer_emptying = false; } else /* limiter buffer NOT emptying */ { if (count <= 0) return 0; empty_space = limiter_buffer_count(false); if (empty_space > 0) /** FILL BUFFER * use as many inbound samples as necessary to fill the buffer */ { /* don't try to fill with more samples than available */ if (empty_space > count) empty_space = count; logf(" Filling limiter buffer: %d", empty_space); while (empty_space-- > 0) { for (i = 0; i <= ch; i++) { /* put inbound samples in the limiter buffer */ in_samp = *in_buf[i]++; *end_lim_buf[i]++ = in_samp; if (end_lim_buf[i] == &limiter_buffer[i][LIMITER_BUFFER_SIZE]) end_lim_buf[i] = limiter_buffer[i]; if (in_samp < 0) /* make positive for comparison */ in_samp = -in_samp - 1; if (in_samp <= clip_max) in_samp = 0; /* disregard if not clipped */ if (i == 0) samp0 = in_samp; if (i == ch) { /* assign peak value for each inbound sample pair */ *end_peak++ = ((samp0 > 0) || (in_samp > 0)) ? get_peak_value(MAX(samp0, in_samp)) : 0; if (end_peak == &lim_buf_peak[LIMITER_BUFFER_SIZE]) end_peak = lim_buf_peak; } } count--; } /* after buffer fills, the remaining inbound samples are cycled */ } limiter_buffer_full = (end_lim_buf[0] == start_lim_buf[0]); out_count = count; /** CYCLE BUFFER * return buffered samples and backfill limiter buffer with new ones. * The buffer is always full when cycling. */ while (count-- > 0) { for (i = 0; i <= ch; i++) { /* copy incoming sample */ in_samp = *in_buf[i]++; /* put limiter buffer sample into outbound buffer */ *out_buf[i]++ = *start_lim_buf[i]++; /* put incoming sample on the end of the limiter buffer */ *end_lim_buf[i]++ = in_samp; /* ring buffer pointer wrap */ if (start_lim_buf[i] == &limiter_buffer[i][LIMITER_BUFFER_SIZE]) start_lim_buf[i] = limiter_buffer[i]; if (end_lim_buf[i] == &limiter_buffer[i][LIMITER_BUFFER_SIZE]) end_lim_buf[i] = limiter_buffer[i]; if (in_samp < 0) /* make positive for comparison */ in_samp = -in_samp - 1; if (in_samp <= clip_max) in_samp = 0; /* disregard if not clipped */ if (i == 0) { samp0 = in_samp; /* assign outgoing sample its associated peak value */ *out_buf_peak_index++ = *start_peak++; if (start_peak == &lim_buf_peak[LIMITER_BUFFER_SIZE]) start_peak = lim_buf_peak; } if (i == ch) { /* assign peak value for each inbound sample pair */ *end_peak++ = ((samp0 > 0) || (in_samp > 0)) ? get_peak_value(MAX(samp0, in_samp)) : 0; if (end_peak == &lim_buf_peak[LIMITER_BUFFER_SIZE]) end_peak = lim_buf_peak; } } } } return out_count; } /** RETURN LIMITER BUFFER COUNT * If argument is true, returns number of samples in the buffer, * otherwise, returns empty space remaining */ static int limiter_buffer_count(bool buf_count) { int count; if (limiter_buffer_full) count = LIMITER_BUFFER_SIZE; else if (end_lim_buf[0] >= start_lim_buf[0]) count = (end_lim_buf[0] - start_lim_buf[0]); else count = (end_lim_buf[0] - start_lim_buf[0]) + LIMITER_BUFFER_SIZE; return buf_count ? count : (LIMITER_BUFFER_SIZE - count); } /** FLUSH THE LIMITER BUFFER * Empties the limiter buffer into the buffer pointed to by the argument * and returns the number of samples in that buffer */ int dsp_flush_limiter_buffer(char *dest) { if ((!limiter_buffer_active) || (limiter_buffer_count(true) <= 0)) return 0; logf(" dsp_flush_limiter_buffer"); int32_t flush_buf[2][LIMITER_BUFFER_SIZE]; int32_t *src[2] = {flush_buf[0], flush_buf[1]}; limiter_buffer_emptying = true; int count = limiter_process(0, src); AUDIO_DSP.output_samples(count, &AUDIO_DSP.data, (const int32_t **)src, (int16_t *)dest); return count; } /** GET PEAK VALUE * Return a small value representing how much the sample is clipped. This * should only be called if a sample is actually clipped. Sample is a * positive value. */ static uint16_t get_peak_value(int32_t sample) { const int frac_bits = AUDIO_DSP.frac_bits; int mid, hi = 48, lo = 0; /* shift sample into 28 frac bit range for comparison */ if (frac_bits > 28) sample >>= (frac_bits - 28); if (frac_bits < 28) sample <<= (28 - frac_bits); /* if clipped out of range, return maximum value */ if (sample >= clip_steps[48]) return 48 * 90; /* find amount of sample clipping on the table */ do { mid = (hi + lo) / 2; if (sample < clip_steps[mid]) hi = mid; else if (sample > clip_steps[mid]) lo = mid; else return mid * 90; } while (hi > (lo + 1)); /* interpolate linearly between steps (less accurate but faster) */ return ((hi-1) * 90) + (((sample - clip_steps[hi-1]) * 90) / (clip_steps[hi] - clip_steps[hi-1])); } /** SET LIMITER * Called by the menu system to configure the limiter process */ void dsp_set_limiter(int limiter_level) { if (limiter_level > 0) { if (!limiter_buffer_active) { /* enable limiter process */ AUDIO_DSP.limiter_process = limiter_process; limiter_buffer_active = true; } /* limiter preamp is a gain factor in S7.24 format */ long old_preamp = AUDIO_DSP.limiter_preamp; long new_preamp = fp_factor((((long)limiter_level << 24) / 10), 24); if (old_preamp != new_preamp) { AUDIO_DSP.limiter_preamp = new_preamp; set_gain(&AUDIO_DSP); logf(" Limiter enable: Yes\tLimiter amp: %.8f", (float)AUDIO_DSP.limiter_preamp / (1 << 24)); } } else { /* disable limiter process*/ if (limiter_buffer_active) { AUDIO_DSP.limiter_preamp = (1 << 24); set_gain(&AUDIO_DSP); /* pcmbuf_flush_limiter_buffer(); */ limiter_buffer_active = false; AUDIO_DSP.limiter_process = NULL; reset_limiter_buffer(&AUDIO_DSP); logf(" Limiter enable: No\tLimiter amp: %.8f", (float)AUDIO_DSP.limiter_preamp / (1 << 24)); } } } /** LIMITER PROCESS * Checks pre-amplified signal for clipped samples and smoothly reduces gain * around the clipped samples using a preset attack/release schedule. */ static int limiter_process(int count, int32_t *buf[]) { /* Limiter process passes through if limiter buffer isn't active, or the * sample depth is too large for safe pre-amping */ if ((!limiter_buffer_active) || (AUDIO_DSP.frac_bits > 29)) return count; count = set_limiter_buffer(count, buf); if (count <= 0) return 0; const int attack_slope = 15; /* 15:1 ratio between attack and release */ const int buffer_count = limiter_buffer_count(true); int i, ch; uint16_t max_peak = 0, gain_peak, gain_rem; long gain; /* step through limiter buffer in reverse order, in order to find the * appropriate max_peak for modifying the output buffer */ for (i = buffer_count - 1; i >= 0; i--) { const uint16_t peak_i = lim_buf_peak[(start_peak - lim_buf_peak + i) % LIMITER_BUFFER_SIZE]; /* if no attack slope, nothing to do */ if ((peak_i == 0) && (max_peak == 0)) continue; /* if new peak, start attack slope */ if (peak_i >= max_peak) { max_peak = peak_i; } /* keep sloping */ else { if (max_peak > attack_slope) max_peak -= attack_slope; else max_peak = 0; } } /* step through output buffer the same way, but this time modifying peak * values to create a smooth attack slope. */ for (i = count - 1; i >= 0; i--) { /* if no attack slope, nothing to do */ if ((out_buf_peak[i] == 0) && (max_peak == 0)) continue; /* if new peak, start attack slope */ if (out_buf_peak[i] >= max_peak) { max_peak = out_buf_peak[i]; } /* keep sloping */ else { if (max_peak > attack_slope) max_peak -= attack_slope; else max_peak = 0; out_buf_peak[i] = max_peak; } } /* Now step forward through the output buffer, and modify the peak values * to establish a smooth, slow release slope.*/ for (i = 0; i < count; i++) { /* if no release slope, nothing to do */ if ((out_buf_peak[i] == 0) && (release_peak == 0)) continue; /* if new peak, start release slope */ if (out_buf_peak[i] >= release_peak) { release_peak = out_buf_peak[i]; } /* keep sloping */ else { release_peak--; out_buf_peak[i] = release_peak; } } /* Implement the limiter: adjust gain of the outbound samples by the gain * amounts in the gain steps array corresponding to the peak values. */ for (i = 0; i < count; i++) { if (out_buf_peak[i] > 0) { gain_peak = (out_buf_peak[i] + 1) / 90; gain_rem = (out_buf_peak[i] + 1) % 90; gain = gain_steps[gain_peak]; if ((gain_peak < 48) && (gain_rem > 0)) gain -= gain_rem * ((gain_steps[gain_peak] - gain_steps[gain_peak + 1]) / 90); for (ch = 0; ch < AUDIO_DSP.data.num_channels; ch++) buf[ch][i] = FRACMUL_SHL(buf[ch][i], gain, 3); } } return count; }