| /*************************************************************************** |
| * __________ __ ___. |
| * Open \______ \ ____ ____ | | _\_ |__ _______ ___ |
| * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / |
| * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < |
| * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ |
| * \/ \/ \/ \/ \/ |
| * $Id$ |
| * |
| * Copyright (C) 2006 Thom Johansen |
| * Copyright (C) 2010 Bertrik Sikken |
| * Copyright (C) 2012 Michael Sevakis |
| * |
| * 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 "rbcodecconfig.h" |
| #include "fixedpoint.h" |
| #include "fracmul.h" |
| #include "replaygain.h" |
| #include "dsp_misc.h" |
| #include "dsp_proc_entry.h" |
| #include "dsp_filter.h" |
| #include "crossfeed.h" |
| #include <string.h> |
| |
| /* Implemented here or in target assembly code */ |
| void crossfeed_process(struct dsp_proc_entry *this, |
| struct dsp_buffer **buf_p); |
| void crossfeed_meier_process(struct dsp_proc_entry *this, |
| struct dsp_buffer **buf_p); |
| |
| |
| /** |
| * Applies crossfeed to the stereo signal. |
| * |
| * 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. |
| */ |
| |
| #define DELAY_LEN(fs) ((300*(fs) / 1000000)*2) /* ~300 uS */ |
| |
| /* Crossfeed */ |
| static struct crossfeed_state |
| { |
| union |
| { |
| struct /* Data for meier crossfeed */ |
| { |
| int32_t reserved; /* 00h: Reserved: overlaps gain */ |
| int32_t vcl; /* 04h: Left filter output */ |
| int32_t vcr; /* 08h: Right filter output */ |
| int32_t vdiff; /* 0ch: L-R difference signal */ |
| int32_t coef1; /* 10h: Left/right filter coef */ |
| int32_t coef2; /* 14h: Crossfeed filter coef */ |
| }; |
| struct /* Data for custom crossfeed */ |
| { |
| int32_t gain; /* 00h: Direct path gain */ |
| int32_t coefs[3]; /* 04h: Filter coefficients: b0, b1, a1 */ |
| int32_t history[4]; /* 10h: Format is x[n - 1], y[n - 1] (L + R) */ |
| int32_t *index; /* 20h: Current pointer into the delay line */ |
| int32_t *index_max; /* 24h: Current max pointer of delay line */ |
| /* 28h: Delay line buffer (L + R interleaved) */ |
| int32_t delay[DELAY_LEN(DSP_OUT_MAX_HZ)]; /* Target-dependent size */ |
| }; |
| }; |
| } crossfeed_state IBSS_ATTR; |
| |
| static int crossfeed_type = CROSSFEED_TYPE_NONE; |
| /* Cached custom settings */ |
| static long crossfeed_lf_gain; |
| static long crossfeed_hf_gain; |
| static long crossfeed_cutoff; |
| |
| /* Discard the sample histories */ |
| static void crossfeed_flush(struct dsp_proc_entry *this) |
| { |
| struct crossfeed_state *state = (void *)this->data; |
| |
| if (crossfeed_type != CROSSFEED_TYPE_CUSTOM) |
| { |
| state->vcl = state->vcr = state->vdiff = 0; |
| } |
| else |
| { |
| memset(state->history, 0, sizeof (state->history)); |
| memset(state->delay, 0, sizeof (state->delay)); |
| state->index = state->delay; |
| } |
| } |
| |
| static void crossfeed_meier_update_filter(struct crossfeed_state *state, |
| unsigned int fout) |
| { |
| /* 1 / (F.Rforward.C) */ |
| state->coef1 = fp_div(2128, fout, 31); |
| /* 1 / (F.Rcross.C) */ |
| state->coef2 = fp_div(1000, fout, 31); |
| } |
| |
| static void crossfeed_custom_update_filter(struct crossfeed_state *state, |
| unsigned int fout) |
| { |
| long lf_gain = crossfeed_lf_gain; |
| long hf_gain = crossfeed_hf_gain; |
| long cutoff = crossfeed_cutoff; |
| int32_t *c = state->coefs; |
| |
| long scaler = get_replaygain_int(lf_gain * 10) << 7; |
| |
| cutoff = fp_div(cutoff, fout, 32); |
| 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; |
| } |
| |
| |
| /** DSP interface **/ |
| |
| /* Set the type of crossfeed to use */ |
| void dsp_set_crossfeed_type(int type) |
| { |
| if (type == crossfeed_type) |
| return; /* No change */ |
| |
| crossfeed_type = type; |
| |
| struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); |
| dsp_proc_enable(dsp, DSP_PROC_CROSSFEED, type != CROSSFEED_TYPE_NONE); |
| } |
| |
| /* Set the gain of the dry mix */ |
| void dsp_set_crossfeed_direct_gain(int gain) |
| { |
| uint32_t gain32 = get_replaygain_int(gain * 10); |
| crossfeed_state.gain = |
| gain32 >= (0x80000000ul >> 7) ? 0x7ffffffful: (gain32 << 7); |
| } |
| |
| /* Both gains should be below 0 dB */ |
| void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) |
| { |
| crossfeed_lf_gain = lf_gain; |
| crossfeed_hf_gain = hf_gain; |
| crossfeed_cutoff = cutoff; |
| |
| struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); |
| crossfeed_custom_update_filter(&crossfeed_state, |
| dsp_get_output_frequency(dsp)); |
| } |
| |
| #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) |
| /* Apply the crossfade to the buffer in place */ |
| void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) |
| { |
| struct crossfeed_state *state = (void *)this->data; |
| struct dsp_buffer *buf = *buf_p; |
| |
| int32_t *hist_l = &state->history[0]; |
| int32_t *hist_r = &state->history[2]; |
| int32_t *delay = state->delay; |
| int32_t *coefs = &state->coefs[0]; |
| int32_t gain = state->gain; |
| int32_t *di = state->index; |
| int32_t *di_max = state->index_max; |
| |
| int count = buf->remcount; |
| |
| for (int i = 0; i < count; i++) |
| { |
| int32_t left = buf->p32[0][i]; |
| int32_t right = buf->p32[1][i]; |
| |
| /* Filter delayed sample from left speaker */ |
| int32_t 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->p32[0][i] = FRACMUL(left, gain) + hist_r[1]; |
| buf->p32[1][i] = FRACMUL(right, gain) + hist_l[1]; |
| |
| /* Wrap delay line index if bigger than delay line size */ |
| if (di >= di_max) |
| di = delay; |
| } |
| |
| /* Write back local copies of data we've modified */ |
| state->index = di; |
| } |
| #endif /* CPU */ |
| |
| #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) |
| /** |
| * Implementation of the "simple" passive crossfeed circuit by Jan Meier. |
| * See also: http://www.meier-audio.homepage.t-online.de/passivefilter.htm |
| */ |
| |
| void crossfeed_meier_process(struct dsp_proc_entry *this, |
| struct dsp_buffer **buf_p) |
| { |
| struct dsp_buffer *buf = *buf_p; |
| |
| /* Get filter state */ |
| struct crossfeed_state *state = (struct crossfeed_state *)this->data; |
| int32_t vcl = state->vcl; |
| int32_t vcr = state->vcr; |
| int32_t vdiff = state->vdiff; |
| int32_t coef1 = state->coef1; |
| int32_t coef2 = state->coef2; |
| |
| int count = buf->remcount; |
| |
| for (int i = 0; i < count; i++) |
| { |
| /* Calculate new output */ |
| int32_t lout = buf->p32[0][i] + vcl; |
| int32_t rout = buf->p32[1][i] + vcr; |
| buf->p32[0][i] = lout; |
| buf->p32[1][i] = rout; |
| |
| /* Update filter state */ |
| int32_t common = FRACMUL(vdiff, coef2); |
| vcl -= FRACMUL(vcl, coef1) + common; |
| vcr -= FRACMUL(vcr, coef1) - common; |
| |
| vdiff = lout - rout; |
| } |
| |
| /* Store filter state */ |
| state->vcl = vcl; |
| state->vcr = vcr; |
| state->vdiff = vdiff; |
| } |
| #endif /* CPU */ |
| |
| /* Update the processing function according to crossfeed type */ |
| static void update_process_fn(struct dsp_proc_entry *this, |
| struct dsp_config *dsp) |
| { |
| struct crossfeed_state *state = (struct crossfeed_state *)this->data; |
| dsp_proc_fn_type fn; |
| |
| unsigned int fout = dsp_get_output_frequency(dsp); |
| |
| if (crossfeed_type != CROSSFEED_TYPE_CUSTOM) |
| { |
| crossfeed_meier_update_filter(state, fout); |
| fn = crossfeed_meier_process; |
| } |
| else |
| { |
| state->index_max = state->delay + DELAY_LEN(fout); |
| crossfeed_custom_update_filter(state, fout); |
| fn = crossfeed_process; |
| } |
| |
| if (this->process != fn) |
| { |
| this->process = fn; /* Set proper function */ |
| if (dsp_proc_active(dsp, DSP_PROC_CROSSFEED)) |
| crossfeed_flush(this); |
| } |
| } |
| |
| /* Crossfeed boot/format change function */ |
| static intptr_t crossfeed_new_format(struct dsp_proc_entry *this, |
| struct dsp_config *dsp, |
| struct sample_format *format) |
| { |
| DSP_PRINT_FORMAT(DSP_PROC_CROSSFEED, format); |
| |
| bool was_active = dsp_proc_active(dsp, DSP_PROC_CROSSFEED); |
| bool active = format->num_channels >= 2; |
| dsp_proc_activate(dsp, DSP_PROC_CROSSFEED, active); |
| |
| if (active) |
| { |
| if (!was_active) |
| crossfeed_flush(this); /* Going online */ |
| |
| return PROC_NEW_FORMAT_OK; |
| } |
| |
| /* Can't do this. Sleep until next change */ |
| DEBUGF(" DSP_PROC_CROSSFEED- deactivated\n"); |
| return PROC_NEW_FORMAT_DEACTIVATED; |
| } |
| |
| /* DSP message hook */ |
| static intptr_t crossfeed_configure(struct dsp_proc_entry *this, |
| struct dsp_config *dsp, |
| unsigned int setting, |
| intptr_t value) |
| { |
| intptr_t retval = 0; |
| |
| switch (setting) |
| { |
| case DSP_PROC_INIT: |
| if (value == 0) |
| this->data = (intptr_t)&crossfeed_state; |
| |
| case DSP_SET_OUT_FREQUENCY: |
| update_process_fn(this, dsp); |
| break; |
| |
| case DSP_FLUSH: |
| crossfeed_flush(this); |
| break; |
| |
| case DSP_PROC_NEW_FORMAT: |
| retval = crossfeed_new_format(this, dsp, |
| (struct sample_format *)value); |
| break; |
| } |
| |
| return retval; |
| } |
| |
| /* Database entry */ |
| DSP_PROC_DB_ENTRY( |
| CROSSFEED, |
| crossfeed_configure); |