diff options
Diffstat (limited to 'lib/rbcodec/dsp/crossfeed.c')
-rw-r--r-- | lib/rbcodec/dsp/crossfeed.c | 214 |
1 files changed, 214 insertions, 0 deletions
diff --git a/lib/rbcodec/dsp/crossfeed.c b/lib/rbcodec/dsp/crossfeed.c new file mode 100644 index 0000000000..ecb55644ee --- /dev/null +++ b/lib/rbcodec/dsp/crossfeed.c | |||
@@ -0,0 +1,214 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 Thom Johansen | ||
11 | * Copyright (C) 2012 Michael Sevakis | ||
12 | * | ||
13 | * This program is free software; you can redistribute it and/or | ||
14 | * modify it under the terms of the GNU General Public License | ||
15 | * as published by the Free Software Foundation; either version 2 | ||
16 | * of the License, or (at your option) any later version. | ||
17 | * | ||
18 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
19 | * KIND, either express or implied. | ||
20 | * | ||
21 | ****************************************************************************/ | ||
22 | #include "config.h" | ||
23 | #include "system.h" | ||
24 | #include "dsp.h" | ||
25 | #include "dsp_filter.h" | ||
26 | #include "fixedpoint.h" | ||
27 | #include "fracmul.h" | ||
28 | #include "replaygain.h" | ||
29 | #include <string.h> | ||
30 | #include "dsp_proc_entry.h" | ||
31 | |||
32 | /* Implemented here or in target assembly code */ | ||
33 | void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p); | ||
34 | |||
35 | /** | ||
36 | * Applies crossfeed to the stereo signal. | ||
37 | * | ||
38 | * Crossfeed is a process where listening over speakers is simulated. This | ||
39 | * is good for old hard panned stereo records, which might be quite fatiguing | ||
40 | * to listen to on headphones with no crossfeed. | ||
41 | */ | ||
42 | |||
43 | /* Crossfeed */ | ||
44 | static struct crossfeed_state | ||
45 | { | ||
46 | int32_t gain; /* 00h: Direct path gain */ | ||
47 | int32_t coefs[3]; /* 04h: Coefficients for the shelving filter */ | ||
48 | int32_t history[4]; /* 10h: Format is x[n - 1], y[n - 1] (L + R) */ | ||
49 | int32_t delay[13*2]; /* 20h: Delay line buffer (L + R interleaved) */ | ||
50 | int32_t *index; /* 88h: Current pointer into the delay line */ | ||
51 | struct dsp_config *dsp; /* 8ch: Current DSP */ | ||
52 | /* 90h */ | ||
53 | } crossfeed_state IBSS_ATTR; | ||
54 | |||
55 | /* Discard the sample histories */ | ||
56 | static void crossfeed_flush(struct dsp_proc_entry *this) | ||
57 | { | ||
58 | struct crossfeed_state *state = (void *)this->data; | ||
59 | memset(state->history, 0, sizeof (state->history)); | ||
60 | memset(state->delay, 0, sizeof (state->delay)); | ||
61 | state->index = state->delay; | ||
62 | } | ||
63 | |||
64 | |||
65 | /** DSP interface **/ | ||
66 | |||
67 | /* Crossfeed boot/format change function */ | ||
68 | static void crossfeed_process_new_format(struct dsp_proc_entry *this, | ||
69 | struct dsp_buffer **buf_p) | ||
70 | { | ||
71 | struct crossfeed_state *state = (void *)this->data; | ||
72 | struct dsp_buffer *buf = *buf_p; | ||
73 | |||
74 | DSP_PRINT_FORMAT(DSP_PROC_CROSSFEED, DSP_PROC_CROSSFEED, buf->format); | ||
75 | |||
76 | bool active = buf->format.num_channels >= 2; | ||
77 | dsp_proc_activate(state->dsp, DSP_PROC_CROSSFEED, active); | ||
78 | |||
79 | if (!active) | ||
80 | { | ||
81 | /* Can't do this. Sleep until next change */ | ||
82 | crossfeed_flush(this); | ||
83 | DEBUGF(" DSP_PROC_CROSSFEED- deactivated\n"); | ||
84 | return; | ||
85 | } | ||
86 | |||
87 | /* Switch to the real function and call it once */ | ||
88 | this->process[0] = crossfeed_process; | ||
89 | dsp_proc_call(this, buf_p, (unsigned)buf->format.changed - 1); | ||
90 | } | ||
91 | |||
92 | /* Enable or disable the crossfeed */ | ||
93 | void dsp_crossfeed_enable(bool enable) | ||
94 | { | ||
95 | if (enable != !crossfeed_state.dsp) | ||
96 | return; | ||
97 | |||
98 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
99 | dsp_proc_enable(dsp, DSP_PROC_CROSSFEED, enable); | ||
100 | } | ||
101 | |||
102 | /* Set the gain of the dry mix */ | ||
103 | void dsp_set_crossfeed_direct_gain(int gain) | ||
104 | { | ||
105 | uint32_t gain32 = get_replaygain_int(gain * 10); | ||
106 | crossfeed_state.gain = | ||
107 | gain32 >= (0x80000000ul >> 7) ? 0x7ffffffful: (gain32 << 7); | ||
108 | } | ||
109 | |||
110 | /* Both gains should be below 0 dB */ | ||
111 | void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) | ||
112 | { | ||
113 | int32_t *c = crossfeed_state.coefs; | ||
114 | long scaler = get_replaygain_int(lf_gain * 10) << 7; | ||
115 | |||
116 | cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff; | ||
117 | hf_gain -= lf_gain; | ||
118 | /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB | ||
119 | * point instead of shelf midpoint. This is for compatibility with the old | ||
120 | * crossfeed shelf filter and should be removed if crossfeed settings are | ||
121 | * ever made incompatible for any other good reason. | ||
122 | */ | ||
123 | cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24); | ||
124 | filter_shelf_coefs(cutoff, hf_gain, false, c); | ||
125 | /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains | ||
126 | * over 1 and can do this safely | ||
127 | */ | ||
128 | c[0] = FRACMUL_SHL(c[0], scaler, 4); | ||
129 | c[1] = FRACMUL_SHL(c[1], scaler, 4); | ||
130 | c[2] <<= 4; | ||
131 | } | ||
132 | |||
133 | #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) | ||
134 | /* Apply the crossfade to the buffer in place */ | ||
135 | void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) | ||
136 | { | ||
137 | struct crossfeed_state *state = (void *)this->data; | ||
138 | struct dsp_buffer *buf = *buf_p; | ||
139 | |||
140 | int32_t *hist_l = &state->history[0]; | ||
141 | int32_t *hist_r = &state->history[2]; | ||
142 | int32_t *delay = state->delay; | ||
143 | int32_t *coefs = &state->coefs[0]; | ||
144 | int32_t gain = state->gain; | ||
145 | int32_t *di = state->index; | ||
146 | |||
147 | int count = buf->remcount; | ||
148 | |||
149 | for (int i = 0; i < count; i++) | ||
150 | { | ||
151 | int32_t left = buf->p32[0][i]; | ||
152 | int32_t right = buf->p32[1][i]; | ||
153 | |||
154 | /* Filter delayed sample from left speaker */ | ||
155 | int32_t acc = FRACMUL(*di, coefs[0]); | ||
156 | acc += FRACMUL(hist_l[0], coefs[1]); | ||
157 | acc += FRACMUL(hist_l[1], coefs[2]); | ||
158 | /* Save filter history for left speaker */ | ||
159 | hist_l[1] = acc; | ||
160 | hist_l[0] = *di; | ||
161 | *di++ = left; | ||
162 | /* Filter delayed sample from right speaker */ | ||
163 | acc = FRACMUL(*di, coefs[0]); | ||
164 | acc += FRACMUL(hist_r[0], coefs[1]); | ||
165 | acc += FRACMUL(hist_r[1], coefs[2]); | ||
166 | /* Save filter history for right speaker */ | ||
167 | hist_r[1] = acc; | ||
168 | hist_r[0] = *di; | ||
169 | *di++ = right; | ||
170 | /* Now add the attenuated direct sound and write to outputs */ | ||
171 | buf->p32[0][i] = FRACMUL(left, gain) + hist_r[1]; | ||
172 | buf->p32[1][i] = FRACMUL(right, gain) + hist_l[1]; | ||
173 | |||
174 | /* Wrap delay line index if bigger than delay line size */ | ||
175 | if (di >= delay + 13*2) | ||
176 | di = delay; | ||
177 | } | ||
178 | |||
179 | /* Write back local copies of data we've modified */ | ||
180 | state->index = di; | ||
181 | } | ||
182 | #endif /* CPU */ | ||
183 | |||
184 | /* DSP message hook */ | ||
185 | static intptr_t crossfeed_configure(struct dsp_proc_entry *this, | ||
186 | struct dsp_config *dsp, | ||
187 | unsigned int setting, | ||
188 | intptr_t value) | ||
189 | { | ||
190 | switch (setting) | ||
191 | { | ||
192 | case DSP_PROC_INIT: | ||
193 | this->data = (intptr_t)&crossfeed_state; | ||
194 | this->process[0] = crossfeed_process_new_format; | ||
195 | this->process[1] = crossfeed_process_new_format; | ||
196 | ((struct crossfeed_state *)this->data)->dsp = dsp; | ||
197 | dsp_proc_activate(dsp, DSP_PROC_CROSSFEED, true); | ||
198 | case DSP_FLUSH: | ||
199 | crossfeed_flush(this); | ||
200 | break; | ||
201 | |||
202 | case DSP_PROC_CLOSE: | ||
203 | ((struct crossfeed_state *)this->data)->dsp = NULL; | ||
204 | break; | ||
205 | } | ||
206 | |||
207 | return 1; | ||
208 | (void)value; | ||
209 | } | ||
210 | |||
211 | /* Database entry */ | ||
212 | DSP_PROC_DB_ENTRY( | ||
213 | CROSSFEED, | ||
214 | crossfeed_configure); | ||