diff options
Diffstat (limited to 'lib')
37 files changed, 4597 insertions, 2804 deletions
diff --git a/lib/rbcodec/SOURCES b/lib/rbcodec/SOURCES index 3ac2660a38..c293f3c028 100644 --- a/lib/rbcodec/SOURCES +++ b/lib/rbcodec/SOURCES | |||
@@ -3,22 +3,31 @@ metadata/id3tags.c | |||
3 | metadata/mp3.c | 3 | metadata/mp3.c |
4 | metadata/mp3data.c | 4 | metadata/mp3data.c |
5 | #if CONFIG_CODEC == SWCODEC | 5 | #if CONFIG_CODEC == SWCODEC |
6 | dsp/channel_mode.c | ||
6 | dsp/compressor.c | 7 | dsp/compressor.c |
7 | dsp/dsp.c | 8 | dsp/crossfeed.c |
9 | dsp/dsp_core.c | ||
10 | dsp/dsp_filter.c | ||
11 | dsp/dsp_misc.c | ||
12 | dsp/dsp_sample_input.c | ||
13 | dsp/dsp_sample_output.c | ||
8 | dsp/eq.c | 14 | dsp/eq.c |
15 | dsp/lin_resample.c | ||
16 | dsp/pga.c | ||
17 | # ifdef HAVE_PITCHSCREEN | ||
18 | dsp/tdspeed.c | ||
19 | # endif | ||
20 | # ifdef HAVE_SW_TONE_CONTROLS | ||
21 | dsp/tone_controls.c | ||
22 | # endif | ||
9 | # if defined(CPU_COLDFIRE) | 23 | # if defined(CPU_COLDFIRE) |
10 | dsp/dsp_cf.S | 24 | dsp/dsp_cf.S |
11 | dsp/eq_cf.S | ||
12 | # elif defined(CPU_ARM) | 25 | # elif defined(CPU_ARM) |
13 | dsp/dsp_arm.S | 26 | dsp/dsp_arm.S |
14 | dsp/eq_arm.S | ||
15 | # if ARM_ARCH >= 6 | 27 | # if ARM_ARCH >= 6 |
16 | dsp/dsp_arm_v6.S | 28 | dsp/dsp_arm_v6.S |
17 | # endif | 29 | # endif |
18 | # endif | 30 | # endif |
19 | # ifdef HAVE_PITCHSCREEN | ||
20 | dsp/tdspeed.c | ||
21 | # endif | ||
22 | metadata/replaygain.c | 31 | metadata/replaygain.c |
23 | metadata/metadata_common.c | 32 | metadata/metadata_common.c |
24 | metadata/a52.c | 33 | metadata/a52.c |
diff --git a/lib/rbcodec/codecs/codecs.h b/lib/rbcodec/codecs/codecs.h index bad8cdd469..d4a51a4aba 100644 --- a/lib/rbcodec/codecs/codecs.h +++ b/lib/rbcodec/codecs/codecs.h | |||
@@ -75,12 +75,12 @@ | |||
75 | #define CODEC_ENC_MAGIC 0x52454E43 /* RENC */ | 75 | #define CODEC_ENC_MAGIC 0x52454E43 /* RENC */ |
76 | 76 | ||
77 | /* increase this every time the api struct changes */ | 77 | /* increase this every time the api struct changes */ |
78 | #define CODEC_API_VERSION 44 | 78 | #define CODEC_API_VERSION 45 |
79 | 79 | ||
80 | /* update this to latest version if a change to the api struct breaks | 80 | /* update this to latest version if a change to the api struct breaks |
81 | backwards compatibility (and please take the opportunity to sort in any | 81 | backwards compatibility (and please take the opportunity to sort in any |
82 | new function which are "waiting" at the end of the function table) */ | 82 | new function which are "waiting" at the end of the function table) */ |
83 | #define CODEC_MIN_API_VERSION 43 | 83 | #define CODEC_MIN_API_VERSION 45 |
84 | 84 | ||
85 | /* reasons for calling codec main entrypoint */ | 85 | /* reasons for calling codec main entrypoint */ |
86 | enum codec_entry_call_reason { | 86 | enum codec_entry_call_reason { |
@@ -171,6 +171,7 @@ struct codec_api { | |||
171 | 171 | ||
172 | void (*commit_dcache)(void); | 172 | void (*commit_dcache)(void); |
173 | void (*commit_discard_dcache)(void); | 173 | void (*commit_discard_dcache)(void); |
174 | void (*commit_discard_idcache)(void); | ||
174 | 175 | ||
175 | /* strings and memory */ | 176 | /* strings and memory */ |
176 | char* (*strcpy)(char *dst, const char *src); | 177 | char* (*strcpy)(char *dst, const char *src); |
@@ -223,7 +224,6 @@ struct codec_api { | |||
223 | 224 | ||
224 | /* new stuff at the end, sort into place next time | 225 | /* new stuff at the end, sort into place next time |
225 | the API gets incompatible */ | 226 | the API gets incompatible */ |
226 | void (*commit_discard_idcache)(void); | ||
227 | }; | 227 | }; |
228 | 228 | ||
229 | /* codec header */ | 229 | /* codec header */ |
diff --git a/lib/rbcodec/codecs/lib/codeclib.c b/lib/rbcodec/codecs/lib/codeclib.c index 36f4279941..4ca6c8c993 100644 --- a/lib/rbcodec/codecs/lib/codeclib.c +++ b/lib/rbcodec/codecs/lib/codeclib.c | |||
@@ -26,6 +26,7 @@ | |||
26 | #include "dsp.h" | 26 | #include "dsp.h" |
27 | #include "codeclib.h" | 27 | #include "codeclib.h" |
28 | #include "metadata.h" | 28 | #include "metadata.h" |
29 | #include "dsp_proc_entry.h" | ||
29 | 30 | ||
30 | /* The following variables are used by codec_malloc() to make use of free RAM | 31 | /* The following variables are used by codec_malloc() to make use of free RAM |
31 | * within the statically allocated codec buffer. */ | 32 | * within the statically allocated codec buffer. */ |
@@ -44,10 +45,15 @@ int codec_init(void) | |||
44 | 45 | ||
45 | void codec_set_replaygain(const struct mp3entry *id3) | 46 | void codec_set_replaygain(const struct mp3entry *id3) |
46 | { | 47 | { |
47 | ci->configure(DSP_SET_TRACK_GAIN, id3->track_gain); | 48 | struct dsp_replay_gains gains = |
48 | ci->configure(DSP_SET_ALBUM_GAIN, id3->album_gain); | 49 | { |
49 | ci->configure(DSP_SET_TRACK_PEAK, id3->track_peak); | 50 | .track_gain = id3->track_gain, |
50 | ci->configure(DSP_SET_ALBUM_PEAK, id3->album_peak); | 51 | .album_gain = id3->album_gain, |
52 | .track_peak = id3->track_peak, | ||
53 | .album_peak = id3->album_peak, | ||
54 | }; | ||
55 | |||
56 | ci->configure(REPLAYGAIN_SET_GAINS, (intptr_t)&gains); | ||
51 | } | 57 | } |
52 | 58 | ||
53 | /* Various "helper functions" common to all the xxx2wav decoder plugins */ | 59 | /* Various "helper functions" common to all the xxx2wav decoder plugins */ |
diff --git a/lib/rbcodec/dsp/channel_mode.c b/lib/rbcodec/dsp/channel_mode.c new file mode 100644 index 0000000000..5b678887c2 --- /dev/null +++ b/lib/rbcodec/dsp/channel_mode.c | |||
@@ -0,0 +1,264 @@ | |||
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 "settings.h" | ||
26 | #include "sound.h" | ||
27 | #include "fixedpoint.h" | ||
28 | #include "fracmul.h" | ||
29 | #include "dsp_proc_entry.h" | ||
30 | |||
31 | #if 0 | ||
32 | /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for | ||
33 | * completeness. */ | ||
34 | void channel_mode_proc_stereo(struct dsp_proc_entry *this, | ||
35 | struct dsp_buffer **buf_p); | ||
36 | #endif | ||
37 | void channel_mode_proc_mono(struct dsp_proc_entry *this, | ||
38 | struct dsp_buffer **buf_p); | ||
39 | void channel_mode_proc_mono_left(struct dsp_proc_entry *this, | ||
40 | struct dsp_buffer **buf_p); | ||
41 | void channel_mode_proc_mono_right(struct dsp_proc_entry *this, | ||
42 | struct dsp_buffer **buf_p); | ||
43 | void channel_mode_proc_custom(struct dsp_proc_entry *this, | ||
44 | struct dsp_buffer **buf_p); | ||
45 | void channel_mode_proc_karaoke(struct dsp_proc_entry *this, | ||
46 | struct dsp_buffer **buf_p); | ||
47 | |||
48 | static struct channel_mode_data | ||
49 | { | ||
50 | long sw_gain; /* 00h: for mode: custom */ | ||
51 | long sw_cross; /* 04h: for mode: custom */ | ||
52 | struct dsp_config *dsp; | ||
53 | int mode; | ||
54 | const dsp_proc_fn_type fns[SOUND_CHAN_NUM_MODES]; | ||
55 | } channel_mode_data = | ||
56 | { | ||
57 | .sw_gain = 0, | ||
58 | .sw_cross = 0, | ||
59 | .mode = SOUND_CHAN_STEREO, | ||
60 | .fns = | ||
61 | { | ||
62 | [SOUND_CHAN_STEREO] = NULL, | ||
63 | [SOUND_CHAN_MONO] = channel_mode_proc_mono, | ||
64 | [SOUND_CHAN_CUSTOM] = channel_mode_proc_custom, | ||
65 | [SOUND_CHAN_MONO_LEFT] = channel_mode_proc_mono_left, | ||
66 | [SOUND_CHAN_MONO_RIGHT] = channel_mode_proc_mono_right, | ||
67 | [SOUND_CHAN_KARAOKE] = channel_mode_proc_karaoke, | ||
68 | }, | ||
69 | }; | ||
70 | |||
71 | static dsp_proc_fn_type get_process_fn(void) | ||
72 | { | ||
73 | return channel_mode_data.fns[channel_mode_data.mode]; | ||
74 | } | ||
75 | |||
76 | #if 0 | ||
77 | /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for | ||
78 | * completeness. */ | ||
79 | void channel_mode_proc_stereo(struct dsp_proc_entry *this, | ||
80 | struct dsp_buffer **buf_p) | ||
81 | { | ||
82 | /* The channels are each just themselves */ | ||
83 | (void)this; (void)buf_p; | ||
84 | } | ||
85 | #endif | ||
86 | |||
87 | #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) | ||
88 | /* Unoptimized routines */ | ||
89 | void channel_mode_proc_mono(struct dsp_proc_entry *this, | ||
90 | struct dsp_buffer **buf_p) | ||
91 | { | ||
92 | struct dsp_buffer *buf = *buf_p; | ||
93 | int32_t *sl = buf->p32[0]; | ||
94 | int32_t *sr = buf->p32[1]; | ||
95 | int count = buf->remcount; | ||
96 | |||
97 | do | ||
98 | { | ||
99 | int32_t lr = *sl / 2 + *sr / 2; | ||
100 | *sl++ = lr; | ||
101 | *sr++ = lr; | ||
102 | } | ||
103 | while (--count > 0); | ||
104 | |||
105 | (void)this; | ||
106 | } | ||
107 | |||
108 | void channel_mode_proc_custom(struct dsp_proc_entry *this, | ||
109 | struct dsp_buffer **buf_p) | ||
110 | { | ||
111 | struct channel_mode_data *data = (void *)this->data; | ||
112 | struct dsp_buffer *buf = *buf_p; | ||
113 | |||
114 | int32_t *sl = buf->p32[0]; | ||
115 | int32_t *sr = buf->p32[1]; | ||
116 | int count = buf->remcount; | ||
117 | |||
118 | const int32_t gain = data->sw_gain; | ||
119 | const int32_t cross = data->sw_cross; | ||
120 | |||
121 | do | ||
122 | { | ||
123 | int32_t l = *sl; | ||
124 | int32_t r = *sr; | ||
125 | *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross); | ||
126 | *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross); | ||
127 | } | ||
128 | while (--count > 0); | ||
129 | } | ||
130 | |||
131 | void channel_mode_proc_karaoke(struct dsp_proc_entry *this, | ||
132 | struct dsp_buffer **buf_p) | ||
133 | { | ||
134 | struct dsp_buffer *buf = *buf_p; | ||
135 | int32_t *sl = buf->p32[0]; | ||
136 | int32_t *sr = buf->p32[1]; | ||
137 | int count = buf->remcount; | ||
138 | |||
139 | do | ||
140 | { | ||
141 | int32_t ch = *sl / 2 - *sr / 2; | ||
142 | *sl++ = ch; | ||
143 | *sr++ = -ch; | ||
144 | } | ||
145 | while (--count > 0); | ||
146 | |||
147 | (void)this; | ||
148 | } | ||
149 | #endif /* CPU */ | ||
150 | |||
151 | void channel_mode_proc_mono_left(struct dsp_proc_entry *this, | ||
152 | struct dsp_buffer **buf_p) | ||
153 | { | ||
154 | /* Just copy over the other channel */ | ||
155 | struct dsp_buffer *buf = *buf_p; | ||
156 | memcpy(buf->p32[1], buf->p32[0], buf->remcount * sizeof (int32_t)); | ||
157 | (void)this; | ||
158 | } | ||
159 | |||
160 | void channel_mode_proc_mono_right(struct dsp_proc_entry *this, | ||
161 | struct dsp_buffer **buf_p) | ||
162 | { | ||
163 | /* Just copy over the other channel */ | ||
164 | struct dsp_buffer *buf = *buf_p; | ||
165 | memcpy(buf->p32[0], buf->p32[1], buf->remcount * sizeof (int32_t)); | ||
166 | (void)this; | ||
167 | } | ||
168 | |||
169 | /* This is the initial function pointer when first enabled/changed in order | ||
170 | * to facilitate verification of the format compatibility at the proper time | ||
171 | * This gets called for changes even if stage is inactive. */ | ||
172 | static void channel_mode_process_new_format(struct dsp_proc_entry *this, | ||
173 | struct dsp_buffer **buf_p) | ||
174 | { | ||
175 | struct channel_mode_data *data = (void *)this->data; | ||
176 | struct dsp_buffer *buf = *buf_p; | ||
177 | |||
178 | DSP_PRINT_FORMAT(DSP_PROC_CHANNEL_MODE, DSP_PROC_CHANNEL_MODE, | ||
179 | buf->format); | ||
180 | |||
181 | bool active = buf->format.num_channels >= 2; | ||
182 | dsp_proc_activate(data->dsp, DSP_PROC_CHANNEL_MODE, active); | ||
183 | |||
184 | if (!active) | ||
185 | { | ||
186 | /* Can't do this. Sleep until next change. */ | ||
187 | DEBUGF(" DSP_PROC_CHANNEL_MODE- deactivated\n"); | ||
188 | return; | ||
189 | } | ||
190 | |||
191 | /* Switch to the real function and call it once */ | ||
192 | this->process[0] = get_process_fn(); | ||
193 | dsp_proc_call(this, buf_p, (unsigned)buf->format.changed - 1); | ||
194 | } | ||
195 | |||
196 | void channel_mode_set_config(int value) | ||
197 | { | ||
198 | if (value < 0 || value >= SOUND_CHAN_NUM_MODES) | ||
199 | value = SOUND_CHAN_STEREO; /* Out of range */ | ||
200 | |||
201 | if (value == channel_mode_data.mode) | ||
202 | return; | ||
203 | |||
204 | channel_mode_data.mode = value; | ||
205 | dsp_proc_enable(dsp_get_config(CODEC_IDX_AUDIO), DSP_PROC_CHANNEL_MODE, | ||
206 | value != SOUND_CHAN_STEREO); | ||
207 | } | ||
208 | |||
209 | void channel_mode_custom_set_width(int value) | ||
210 | { | ||
211 | long width, straight, cross; | ||
212 | |||
213 | width = value * 0x7fffff / 100; | ||
214 | |||
215 | if (value <= 100) | ||
216 | { | ||
217 | straight = (0x7fffff + width) / 2; | ||
218 | cross = straight - width; | ||
219 | } | ||
220 | else | ||
221 | { | ||
222 | /* straight = (1 + width) / (2 * width) */ | ||
223 | straight = fp_div(0x7fffff + width, width, 22); | ||
224 | cross = straight - 0x7fffff; | ||
225 | } | ||
226 | |||
227 | channel_mode_data.sw_gain = straight << 8; | ||
228 | channel_mode_data.sw_cross = cross << 8; | ||
229 | } | ||
230 | |||
231 | /* DSP message hook */ | ||
232 | static intptr_t channel_mode_configure(struct dsp_proc_entry *this, | ||
233 | struct dsp_config *dsp, | ||
234 | unsigned int setting, | ||
235 | intptr_t value) | ||
236 | { | ||
237 | switch (setting) | ||
238 | { | ||
239 | case DSP_PROC_INIT: | ||
240 | if (value == 0) | ||
241 | { | ||
242 | /* New object */ | ||
243 | this->data = (intptr_t)&channel_mode_data; | ||
244 | this->process[1] = channel_mode_process_new_format; | ||
245 | ((struct channel_mode_data *)this->data)->dsp = dsp; | ||
246 | } | ||
247 | |||
248 | /* Force format change call each time */ | ||
249 | this->process[0] = channel_mode_process_new_format; | ||
250 | dsp_proc_activate(dsp, DSP_PROC_CHANNEL_MODE, true); | ||
251 | break; | ||
252 | |||
253 | case DSP_PROC_CLOSE: | ||
254 | ((struct channel_mode_data *)this->data)->dsp = NULL; | ||
255 | break; | ||
256 | } | ||
257 | |||
258 | return 1; | ||
259 | } | ||
260 | |||
261 | /* Database entry */ | ||
262 | DSP_PROC_DB_ENTRY( | ||
263 | CHANNEL_MODE, | ||
264 | channel_mode_configure); | ||
diff --git a/lib/rbcodec/dsp/channel_mode.h b/lib/rbcodec/dsp/channel_mode.h new file mode 100644 index 0000000000..7ca0f74204 --- /dev/null +++ b/lib/rbcodec/dsp/channel_mode.h | |||
@@ -0,0 +1,27 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef CHANNEL_MODE_H | ||
22 | #define CHANNEL_MODE_H | ||
23 | |||
24 | void channel_mode_set_config(int value); | ||
25 | void channel_mode_custom_set_width(int value); | ||
26 | |||
27 | #endif /* CHANNEL_MODE_H */ | ||
diff --git a/lib/rbcodec/dsp/compressor.c b/lib/rbcodec/dsp/compressor.c index a6c1ac1018..1816bfef9c 100644 --- a/lib/rbcodec/dsp/compressor.c +++ b/lib/rbcodec/dsp/compressor.c | |||
@@ -19,16 +19,18 @@ | |||
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | #include "config.h" | 21 | #include "config.h" |
22 | #include "system.h" | ||
22 | #include "fixedpoint.h" | 23 | #include "fixedpoint.h" |
23 | #include "fracmul.h" | 24 | #include "fracmul.h" |
24 | #include "settings.h" | ||
25 | #include "dsp.h" | 25 | #include "dsp.h" |
26 | #include "compressor.h" | 26 | #include <string.h> |
27 | 27 | ||
28 | /* Define LOGF_ENABLE to enable logf output in this file */ | 28 | /* Define LOGF_ENABLE to enable logf output in this file */ |
29 | /*#define LOGF_ENABLE*/ | 29 | /*#define LOGF_ENABLE*/ |
30 | #include "logf.h" | 30 | #include "logf.h" |
31 | 31 | ||
32 | #include "dsp_proc_entry.h" | ||
33 | |||
32 | static struct compressor_settings curr_set; /* Cached settings */ | 34 | static struct compressor_settings curr_set; /* Cached settings */ |
33 | 35 | ||
34 | static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */ | 36 | static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */ |
@@ -251,10 +253,10 @@ bool compressor_update(const struct compressor_settings *settings) | |||
251 | * Returns the required gain factor in S7.24 format in order to compress the | 253 | * Returns the required gain factor in S7.24 format in order to compress the |
252 | * sample in accordance with the compression curve. Always 1 or less. | 254 | * sample in accordance with the compression curve. Always 1 or less. |
253 | */ | 255 | */ |
254 | static inline int32_t get_compression_gain(struct dsp_data *data, | 256 | static inline int32_t get_compression_gain(struct sample_format *format, |
255 | int32_t sample) | 257 | int32_t sample) |
256 | { | 258 | { |
257 | const int frac_bits_offset = data->frac_bits - 15; | 259 | const int frac_bits_offset = format->frac_bits - 15; |
258 | 260 | ||
259 | /* sample must be positive */ | 261 | /* sample must be positive */ |
260 | if (sample < 0) | 262 | if (sample < 0) |
@@ -292,24 +294,40 @@ static inline int32_t get_compression_gain(struct dsp_data *data, | |||
292 | return -1; | 294 | return -1; |
293 | } | 295 | } |
294 | 296 | ||
297 | /** DSP interface **/ | ||
298 | |||
299 | /** SET COMPRESSOR | ||
300 | * Enable or disable the compressor based upon the settings | ||
301 | */ | ||
302 | void dsp_set_compressor(const struct compressor_settings *settings) | ||
303 | { | ||
304 | /* enable/disable the compressor depending upon settings */ | ||
305 | bool enable = compressor_update(settings); | ||
306 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
307 | dsp_proc_enable(dsp, DSP_PROC_COMPRESSOR, enable); | ||
308 | dsp_proc_activate(dsp, DSP_PROC_COMPRESSOR, true); | ||
309 | } | ||
310 | |||
295 | /** COMPRESSOR PROCESS | 311 | /** COMPRESSOR PROCESS |
296 | * Changes the gain of the samples according to the compressor curve | 312 | * Changes the gain of the samples according to the compressor curve |
297 | */ | 313 | */ |
298 | void compressor_process(int count, struct dsp_data *data, int32_t *buf[]) | 314 | static void compressor_process(struct dsp_proc_entry *this, |
315 | struct dsp_buffer **buf_p) | ||
299 | { | 316 | { |
300 | const int num_chan = data->num_channels; | 317 | struct dsp_buffer *buf = *buf_p; |
301 | int32_t *in_buf[2] = {buf[0], buf[1]}; | 318 | int count = buf->remcount; |
302 | 319 | int32_t *in_buf[2] = { buf->p32[0], buf->p32[1] }; | |
320 | const int num_chan = buf->format.num_channels; | ||
321 | |||
303 | while (count-- > 0) | 322 | while (count-- > 0) |
304 | { | 323 | { |
305 | int ch; | ||
306 | /* use lowest (most compressed) gain factor of the output buffer | 324 | /* use lowest (most compressed) gain factor of the output buffer |
307 | sample pair for both samples (mono is also handled correctly here) | 325 | sample pair for both samples (mono is also handled correctly here) |
308 | */ | 326 | */ |
309 | int32_t sample_gain = UNITY; | 327 | int32_t sample_gain = UNITY; |
310 | for (ch = 0; ch < num_chan; ch++) | 328 | for (int ch = 0; ch < num_chan; ch++) |
311 | { | 329 | { |
312 | int32_t this_gain = get_compression_gain(data, *in_buf[ch]); | 330 | int32_t this_gain = get_compression_gain(&buf->format, *in_buf[ch]); |
313 | if (this_gain < sample_gain) | 331 | if (this_gain < sample_gain) |
314 | sample_gain = this_gain; | 332 | sample_gain = this_gain; |
315 | } | 333 | } |
@@ -345,7 +363,7 @@ void compressor_process(int count, struct dsp_data *data, int32_t *buf[]) | |||
345 | output buffer sample pair/mono sample */ | 363 | output buffer sample pair/mono sample */ |
346 | if (total_gain != UNITY) | 364 | if (total_gain != UNITY) |
347 | { | 365 | { |
348 | for (ch = 0; ch < num_chan; ch++) | 366 | for (int ch = 0; ch < num_chan; ch++) |
349 | { | 367 | { |
350 | *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7); | 368 | *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7); |
351 | } | 369 | } |
@@ -353,9 +371,33 @@ void compressor_process(int count, struct dsp_data *data, int32_t *buf[]) | |||
353 | in_buf[0]++; | 371 | in_buf[0]++; |
354 | in_buf[1]++; | 372 | in_buf[1]++; |
355 | } | 373 | } |
374 | |||
375 | (void)this; | ||
356 | } | 376 | } |
357 | 377 | ||
358 | void compressor_reset(void) | 378 | /* DSP message hook */ |
379 | static intptr_t compressor_configure(struct dsp_proc_entry *this, | ||
380 | struct dsp_config *dsp, | ||
381 | unsigned int setting, | ||
382 | intptr_t value) | ||
359 | { | 383 | { |
360 | release_gain = UNITY; | 384 | switch (setting) |
385 | { | ||
386 | case DSP_PROC_INIT: | ||
387 | if (value != 0) | ||
388 | break; /* Already enabled */ | ||
389 | this->process[0] = compressor_process; | ||
390 | case DSP_RESET: | ||
391 | case DSP_FLUSH: | ||
392 | release_gain = UNITY; | ||
393 | break; | ||
394 | } | ||
395 | |||
396 | return 1; | ||
397 | (void)dsp; | ||
361 | } | 398 | } |
399 | |||
400 | /* Database entry */ | ||
401 | DSP_PROC_DB_ENTRY( | ||
402 | COMPRESSOR, | ||
403 | compressor_configure); | ||
diff --git a/lib/rbcodec/dsp/compressor.h b/lib/rbcodec/dsp/compressor.h index d0e33f6e2c..e41950926e 100644 --- a/lib/rbcodec/dsp/compressor.h +++ b/lib/rbcodec/dsp/compressor.h | |||
@@ -18,12 +18,18 @@ | |||
18 | * KIND, either express or implied. | 18 | * KIND, either express or implied. |
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | |||
22 | #ifndef COMPRESSOR_H | 21 | #ifndef COMPRESSOR_H |
23 | #define COMPRESSOR_H | 22 | #define COMPRESSOR_H |
24 | 23 | ||
25 | void compressor_process(int count, struct dsp_data *data, int32_t *buf[]); | 24 | struct compressor_settings |
26 | bool compressor_update(const struct compressor_settings *settings); | 25 | { |
27 | void compressor_reset(void); | 26 | int threshold; |
27 | int makeup_gain; | ||
28 | int ratio; | ||
29 | int knee; | ||
30 | int release_time; | ||
31 | }; | ||
32 | |||
33 | void dsp_set_compressor(const struct compressor_settings *settings); | ||
28 | 34 | ||
29 | #endif /* COMPRESSOR_H */ | 35 | #endif /* COMPRESSOR_H */ |
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); | ||
diff --git a/lib/rbcodec/dsp/crossfeed.h b/lib/rbcodec/dsp/crossfeed.h new file mode 100644 index 0000000000..63261bde9f --- /dev/null +++ b/lib/rbcodec/dsp/crossfeed.h | |||
@@ -0,0 +1,28 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef CROSSFEED_H | ||
22 | #define CROSSFEED_H | ||
23 | |||
24 | void dsp_crossfeed_enable(bool enable); | ||
25 | void dsp_set_crossfeed_direct_gain(int gain); | ||
26 | void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff); | ||
27 | |||
28 | #endif /* CROSSFEED_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp.c b/lib/rbcodec/dsp/dsp.c deleted file mode 100644 index de647dc0dd..0000000000 --- a/lib/rbcodec/dsp/dsp.c +++ /dev/null | |||
@@ -1,1568 +0,0 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include "config.h" | ||
22 | #include "system.h" | ||
23 | #include <sound.h> | ||
24 | #include "dsp.h" | ||
25 | #include "dsp-util.h" | ||
26 | #include "eq.h" | ||
27 | #include "compressor.h" | ||
28 | #include "kernel.h" | ||
29 | #include "settings.h" | ||
30 | #include "replaygain.h" | ||
31 | #include "tdspeed.h" | ||
32 | #include "core_alloc.h" | ||
33 | #include "fixedpoint.h" | ||
34 | #include "fracmul.h" | ||
35 | |||
36 | /* Define LOGF_ENABLE to enable logf output in this file */ | ||
37 | /*#define LOGF_ENABLE*/ | ||
38 | #include "logf.h" | ||
39 | |||
40 | /* 16-bit samples are scaled based on these constants. The shift should be | ||
41 | * no more than 15. | ||
42 | */ | ||
43 | #define WORD_SHIFT 12 | ||
44 | #define WORD_FRACBITS 27 | ||
45 | |||
46 | #define NATIVE_DEPTH 16 | ||
47 | #define SMALL_SAMPLE_BUF_COUNT 128 /* Per channel */ | ||
48 | #define DEFAULT_GAIN 0x01000000 | ||
49 | |||
50 | /* enums to index conversion properly with stereo mode and other settings */ | ||
51 | enum | ||
52 | { | ||
53 | SAMPLE_INPUT_LE_NATIVE_I_STEREO = STEREO_INTERLEAVED, | ||
54 | SAMPLE_INPUT_LE_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED, | ||
55 | SAMPLE_INPUT_LE_NATIVE_MONO = STEREO_MONO, | ||
56 | SAMPLE_INPUT_GT_NATIVE_I_STEREO = STEREO_INTERLEAVED + STEREO_NUM_MODES, | ||
57 | SAMPLE_INPUT_GT_NATIVE_NI_STEREO = STEREO_NONINTERLEAVED + STEREO_NUM_MODES, | ||
58 | SAMPLE_INPUT_GT_NATIVE_MONO = STEREO_MONO + STEREO_NUM_MODES, | ||
59 | SAMPLE_INPUT_GT_NATIVE_1ST_INDEX = STEREO_NUM_MODES | ||
60 | }; | ||
61 | |||
62 | enum | ||
63 | { | ||
64 | SAMPLE_OUTPUT_MONO = 0, | ||
65 | SAMPLE_OUTPUT_STEREO, | ||
66 | SAMPLE_OUTPUT_DITHERED_MONO, | ||
67 | SAMPLE_OUTPUT_DITHERED_STEREO | ||
68 | }; | ||
69 | |||
70 | /* No asm...yet */ | ||
71 | struct dither_data | ||
72 | { | ||
73 | long error[3]; /* 00h */ | ||
74 | long random; /* 0ch */ | ||
75 | /* 10h */ | ||
76 | }; | ||
77 | |||
78 | struct crossfeed_data | ||
79 | { | ||
80 | int32_t gain; /* 00h - Direct path gain */ | ||
81 | int32_t coefs[3]; /* 04h - Coefficients for the shelving filter */ | ||
82 | int32_t history[4]; /* 10h - Format is x[n - 1], y[n - 1] for both channels */ | ||
83 | int32_t delay[13][2]; /* 20h */ | ||
84 | int32_t *index; /* 88h - Current pointer into the delay line */ | ||
85 | /* 8ch */ | ||
86 | }; | ||
87 | |||
88 | /* Current setup is one lowshelf filters three peaking filters and one | ||
89 | * highshelf filter. Varying the number of shelving filters make no sense, | ||
90 | * but adding peaking filters is possible. | ||
91 | */ | ||
92 | struct eq_state | ||
93 | { | ||
94 | char enabled[5]; /* 00h - Flags for active filters */ | ||
95 | struct eqfilter filters[5]; /* 08h - packing is 4? */ | ||
96 | /* 10ch */ | ||
97 | }; | ||
98 | |||
99 | /* Include header with defines which functions are implemented in assembly | ||
100 | code for the target */ | ||
101 | #include <dsp_asm.h> | ||
102 | |||
103 | /* Typedefs keep things much neater in this case */ | ||
104 | typedef void (*sample_input_fn_type)(int count, const char *src[], | ||
105 | int32_t *dst[]); | ||
106 | typedef int (*resample_fn_type)(int count, struct dsp_data *data, | ||
107 | const int32_t *src[], int32_t *dst[]); | ||
108 | typedef void (*sample_output_fn_type)(int count, struct dsp_data *data, | ||
109 | const int32_t *src[], int16_t *dst); | ||
110 | |||
111 | /* Single-DSP channel processing in place */ | ||
112 | typedef void (*channels_process_fn_type)(int count, int32_t *buf[]); | ||
113 | /* DSP local channel processing in place */ | ||
114 | typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data, | ||
115 | int32_t *buf[]); | ||
116 | |||
117 | /* | ||
118 | ***************************************************************************/ | ||
119 | |||
120 | struct dsp_config | ||
121 | { | ||
122 | struct dsp_data data; /* Config members for use in external routines */ | ||
123 | long codec_frequency; /* Sample rate of data coming from the codec */ | ||
124 | long frequency; /* Effective sample rate after pitch shift (if any) */ | ||
125 | int sample_depth; | ||
126 | int sample_bytes; | ||
127 | int stereo_mode; | ||
128 | int32_t tdspeed_percent; /* Speed% * PITCH_SPEED_PRECISION */ | ||
129 | #ifdef HAVE_PITCHSCREEN | ||
130 | bool tdspeed_active; /* Timestretch is in use */ | ||
131 | #endif | ||
132 | #ifdef HAVE_SW_TONE_CONTROLS | ||
133 | /* Filter struct for software bass/treble controls */ | ||
134 | struct eqfilter tone_filter; | ||
135 | #endif | ||
136 | /* Functions that change depending upon settings - NULL if stage is | ||
137 | disabled */ | ||
138 | sample_input_fn_type input_samples; | ||
139 | resample_fn_type resample; | ||
140 | sample_output_fn_type output_samples; | ||
141 | /* These will be NULL for the voice codec and is more economical that | ||
142 | way */ | ||
143 | channels_process_dsp_fn_type apply_gain; | ||
144 | channels_process_fn_type apply_crossfeed; | ||
145 | channels_process_fn_type eq_process; | ||
146 | channels_process_fn_type channels_process; | ||
147 | channels_process_dsp_fn_type compressor_process; | ||
148 | }; | ||
149 | |||
150 | /* General DSP config */ | ||
151 | static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */ | ||
152 | /* Dithering */ | ||
153 | static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */ | ||
154 | static long dither_mask IBSS_ATTR; | ||
155 | static long dither_bias IBSS_ATTR; | ||
156 | /* Crossfeed */ | ||
157 | struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */ | ||
158 | { | ||
159 | .index = (int32_t *)crossfeed_data.delay | ||
160 | }; | ||
161 | |||
162 | /* Equalizer */ | ||
163 | static struct eq_state eq_data; /* A */ | ||
164 | |||
165 | /* Software tone controls */ | ||
166 | #ifdef HAVE_SW_TONE_CONTROLS | ||
167 | static int prescale; /* A/V */ | ||
168 | static int bass; /* A/V */ | ||
169 | static int treble; /* A/V */ | ||
170 | #endif | ||
171 | |||
172 | /* Settings applicable to audio codec only */ | ||
173 | #ifdef HAVE_PITCHSCREEN | ||
174 | static int32_t pitch_ratio = PITCH_SPEED_100; | ||
175 | static int big_sample_locks; | ||
176 | #endif | ||
177 | static int channels_mode; | ||
178 | long dsp_sw_gain; | ||
179 | long dsp_sw_cross; | ||
180 | static bool dither_enabled; | ||
181 | static long eq_precut; | ||
182 | static long track_gain; | ||
183 | static bool new_gain; | ||
184 | static long album_gain; | ||
185 | static long track_peak; | ||
186 | static long album_peak; | ||
187 | static long replaygain; | ||
188 | static bool crossfeed_enabled; | ||
189 | |||
190 | #define AUDIO_DSP (dsp_conf[CODEC_IDX_AUDIO]) | ||
191 | #define VOICE_DSP (dsp_conf[CODEC_IDX_VOICE]) | ||
192 | |||
193 | /* The internal format is 32-bit samples, non-interleaved, stereo. This | ||
194 | * format is similar to the raw output from several codecs, so the amount | ||
195 | * of copying needed is minimized for that case. | ||
196 | */ | ||
197 | |||
198 | #define RESAMPLE_RATIO 4 /* Enough for 11,025 Hz -> 44,100 Hz */ | ||
199 | #define SMALL_RESAMPLE_BUF_COUNT (SMALL_SAMPLE_BUF_COUNT * RESAMPLE_RATIO) | ||
200 | #define BIG_SAMPLE_BUF_COUNT SMALL_RESAMPLE_BUF_COUNT | ||
201 | #define BIG_RESAMPLE_BUF_COUNT (BIG_SAMPLE_BUF_COUNT * RESAMPLE_RATIO) | ||
202 | |||
203 | static int32_t small_sample_buf[2][SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR; | ||
204 | static int32_t small_resample_buf[2][SMALL_RESAMPLE_BUF_COUNT] IBSS_ATTR; | ||
205 | |||
206 | #ifdef HAVE_PITCHSCREEN | ||
207 | static int32_t (* big_sample_buf)[BIG_SAMPLE_BUF_COUNT] = NULL; | ||
208 | static int32_t (* big_resample_buf)[BIG_RESAMPLE_BUF_COUNT] = NULL; | ||
209 | #endif | ||
210 | |||
211 | static int sample_buf_count = SMALL_SAMPLE_BUF_COUNT; | ||
212 | static int32_t *sample_buf[2] = { small_sample_buf[0], small_sample_buf[1] }; | ||
213 | static int resample_buf_count = SMALL_RESAMPLE_BUF_COUNT; | ||
214 | static int32_t *resample_buf[2] = { small_resample_buf[0], small_resample_buf[1] }; | ||
215 | |||
216 | #ifdef HAVE_PITCHSCREEN | ||
217 | int32_t sound_get_pitch(void) | ||
218 | { | ||
219 | return pitch_ratio; | ||
220 | } | ||
221 | |||
222 | void sound_set_pitch(int32_t percent) | ||
223 | { | ||
224 | pitch_ratio = percent; | ||
225 | dsp_configure(&AUDIO_DSP, DSP_SWITCH_FREQUENCY, | ||
226 | AUDIO_DSP.codec_frequency); | ||
227 | } | ||
228 | |||
229 | static void tdspeed_set_pointers( bool time_stretch_active ) | ||
230 | { | ||
231 | if( time_stretch_active ) | ||
232 | { | ||
233 | sample_buf_count = BIG_SAMPLE_BUF_COUNT; | ||
234 | resample_buf_count = BIG_RESAMPLE_BUF_COUNT; | ||
235 | sample_buf[0] = big_sample_buf[0]; | ||
236 | sample_buf[1] = big_sample_buf[1]; | ||
237 | resample_buf[0] = big_resample_buf[0]; | ||
238 | resample_buf[1] = big_resample_buf[1]; | ||
239 | } | ||
240 | else | ||
241 | { | ||
242 | sample_buf_count = SMALL_SAMPLE_BUF_COUNT; | ||
243 | resample_buf_count = SMALL_RESAMPLE_BUF_COUNT; | ||
244 | sample_buf[0] = small_sample_buf[0]; | ||
245 | sample_buf[1] = small_sample_buf[1]; | ||
246 | resample_buf[0] = small_resample_buf[0]; | ||
247 | resample_buf[1] = small_resample_buf[1]; | ||
248 | } | ||
249 | } | ||
250 | |||
251 | static void tdspeed_setup(struct dsp_config *dspc) | ||
252 | { | ||
253 | /* Assume timestretch will not be used */ | ||
254 | dspc->tdspeed_active = false; | ||
255 | |||
256 | tdspeed_set_pointers( false ); | ||
257 | |||
258 | if (!dsp_timestretch_available()) | ||
259 | return; /* Timestretch not enabled or buffer not allocated */ | ||
260 | |||
261 | if (dspc->tdspeed_percent == 0) | ||
262 | dspc->tdspeed_percent = PITCH_SPEED_100; | ||
263 | |||
264 | if (!tdspeed_config( | ||
265 | dspc->codec_frequency == 0 ? NATIVE_FREQUENCY : dspc->codec_frequency, | ||
266 | dspc->stereo_mode != STEREO_MONO, | ||
267 | dspc->tdspeed_percent)) | ||
268 | return; /* Timestretch not possible or needed with these parameters */ | ||
269 | |||
270 | /* Timestretch is to be used */ | ||
271 | dspc->tdspeed_active = true; | ||
272 | |||
273 | tdspeed_set_pointers( true ); | ||
274 | } | ||
275 | |||
276 | |||
277 | static int move_callback(int handle, void* current, void* new) | ||
278 | { | ||
279 | (void)handle;(void)current; | ||
280 | |||
281 | if ( big_sample_locks > 0 ) | ||
282 | return BUFLIB_CB_CANNOT_MOVE; | ||
283 | |||
284 | big_sample_buf = new; | ||
285 | |||
286 | /* no allocation without timestretch enabled */ | ||
287 | tdspeed_set_pointers( true ); | ||
288 | return BUFLIB_CB_OK; | ||
289 | } | ||
290 | |||
291 | static void lock_sample_buf( bool lock ) | ||
292 | { | ||
293 | if ( lock ) | ||
294 | big_sample_locks++; | ||
295 | else | ||
296 | big_sample_locks--; | ||
297 | } | ||
298 | |||
299 | static struct buflib_callbacks ops = { | ||
300 | .move_callback = move_callback, | ||
301 | .shrink_callback = NULL, | ||
302 | }; | ||
303 | |||
304 | |||
305 | void dsp_timestretch_enable(bool enabled) | ||
306 | { | ||
307 | /* Hook to set up timestretch buffer on first call to settings_apply() */ | ||
308 | static int handle = -1; | ||
309 | if (enabled) | ||
310 | { | ||
311 | if (big_sample_buf) | ||
312 | return; /* already allocated and enabled */ | ||
313 | |||
314 | /* Set up timestretch buffers */ | ||
315 | big_sample_buf = &small_resample_buf[0]; | ||
316 | handle = core_alloc_ex("resample buf", | ||
317 | 2 * BIG_RESAMPLE_BUF_COUNT * sizeof(int32_t), | ||
318 | &ops); | ||
319 | big_sample_locks = 0; | ||
320 | enabled = handle >= 0; | ||
321 | |||
322 | if (enabled) | ||
323 | { | ||
324 | /* success, now setup tdspeed */ | ||
325 | big_resample_buf = core_get_data(handle); | ||
326 | |||
327 | tdspeed_init(); | ||
328 | tdspeed_setup(&AUDIO_DSP); | ||
329 | } | ||
330 | } | ||
331 | |||
332 | if (!enabled) | ||
333 | { | ||
334 | dsp_set_timestretch(PITCH_SPEED_100); | ||
335 | tdspeed_finish(); | ||
336 | |||
337 | if (handle >= 0) | ||
338 | core_free(handle); | ||
339 | |||
340 | handle = -1; | ||
341 | big_sample_buf = NULL; | ||
342 | } | ||
343 | } | ||
344 | |||
345 | void dsp_set_timestretch(int32_t percent) | ||
346 | { | ||
347 | AUDIO_DSP.tdspeed_percent = percent; | ||
348 | tdspeed_setup(&AUDIO_DSP); | ||
349 | } | ||
350 | |||
351 | int32_t dsp_get_timestretch() | ||
352 | { | ||
353 | return AUDIO_DSP.tdspeed_percent; | ||
354 | } | ||
355 | |||
356 | bool dsp_timestretch_available() | ||
357 | { | ||
358 | return (global_settings.timestretch_enabled && big_sample_buf); | ||
359 | } | ||
360 | #endif /* HAVE_PITCHSCREEN */ | ||
361 | |||
362 | /* Convert count samples to the internal format, if needed. Updates src | ||
363 | * to point past the samples "consumed" and dst is set to point to the | ||
364 | * samples to consume. Note that for mono, dst[0] equals dst[1], as there | ||
365 | * is no point in processing the same data twice. | ||
366 | */ | ||
367 | |||
368 | /* convert count 16-bit mono to 32-bit mono */ | ||
369 | static void sample_input_lte_native_mono( | ||
370 | int count, const char *src[], int32_t *dst[]) | ||
371 | { | ||
372 | const int16_t *s = (int16_t *) src[0]; | ||
373 | const int16_t * const send = s + count; | ||
374 | int32_t *d = dst[0] = dst[1] = sample_buf[0]; | ||
375 | int scale = WORD_SHIFT; | ||
376 | |||
377 | while (s < send) | ||
378 | { | ||
379 | *d++ = *s++ << scale; | ||
380 | } | ||
381 | |||
382 | src[0] = (char *)s; | ||
383 | } | ||
384 | |||
385 | /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */ | ||
386 | static void sample_input_lte_native_i_stereo( | ||
387 | int count, const char *src[], int32_t *dst[]) | ||
388 | { | ||
389 | const int32_t *s = (int32_t *) src[0]; | ||
390 | const int32_t * const send = s + count; | ||
391 | int32_t *dl = dst[0] = sample_buf[0]; | ||
392 | int32_t *dr = dst[1] = sample_buf[1]; | ||
393 | int scale = WORD_SHIFT; | ||
394 | |||
395 | while (s < send) | ||
396 | { | ||
397 | int32_t slr = *s++; | ||
398 | #ifdef ROCKBOX_LITTLE_ENDIAN | ||
399 | *dl++ = (slr >> 16) << scale; | ||
400 | *dr++ = (int32_t)(int16_t)slr << scale; | ||
401 | #else /* ROCKBOX_BIG_ENDIAN */ | ||
402 | *dl++ = (int32_t)(int16_t)slr << scale; | ||
403 | *dr++ = (slr >> 16) << scale; | ||
404 | #endif | ||
405 | } | ||
406 | |||
407 | src[0] = (char *)s; | ||
408 | } | ||
409 | |||
410 | /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */ | ||
411 | static void sample_input_lte_native_ni_stereo( | ||
412 | int count, const char *src[], int32_t *dst[]) | ||
413 | { | ||
414 | const int16_t *sl = (int16_t *) src[0]; | ||
415 | const int16_t *sr = (int16_t *) src[1]; | ||
416 | const int16_t * const slend = sl + count; | ||
417 | int32_t *dl = dst[0] = sample_buf[0]; | ||
418 | int32_t *dr = dst[1] = sample_buf[1]; | ||
419 | int scale = WORD_SHIFT; | ||
420 | |||
421 | while (sl < slend) | ||
422 | { | ||
423 | *dl++ = *sl++ << scale; | ||
424 | *dr++ = *sr++ << scale; | ||
425 | } | ||
426 | |||
427 | src[0] = (char *)sl; | ||
428 | src[1] = (char *)sr; | ||
429 | } | ||
430 | |||
431 | /* convert count 32-bit mono to 32-bit mono */ | ||
432 | static void sample_input_gt_native_mono( | ||
433 | int count, const char *src[], int32_t *dst[]) | ||
434 | { | ||
435 | dst[0] = dst[1] = (int32_t *)src[0]; | ||
436 | src[0] = (char *)(dst[0] + count); | ||
437 | } | ||
438 | |||
439 | /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */ | ||
440 | static void sample_input_gt_native_i_stereo( | ||
441 | int count, const char *src[], int32_t *dst[]) | ||
442 | { | ||
443 | const int32_t *s = (int32_t *)src[0]; | ||
444 | const int32_t * const send = s + 2*count; | ||
445 | int32_t *dl = dst[0] = sample_buf[0]; | ||
446 | int32_t *dr = dst[1] = sample_buf[1]; | ||
447 | |||
448 | while (s < send) | ||
449 | { | ||
450 | *dl++ = *s++; | ||
451 | *dr++ = *s++; | ||
452 | } | ||
453 | |||
454 | src[0] = (char *)send; | ||
455 | } | ||
456 | |||
457 | /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */ | ||
458 | static void sample_input_gt_native_ni_stereo( | ||
459 | int count, const char *src[], int32_t *dst[]) | ||
460 | { | ||
461 | dst[0] = (int32_t *)src[0]; | ||
462 | dst[1] = (int32_t *)src[1]; | ||
463 | src[0] = (char *)(dst[0] + count); | ||
464 | src[1] = (char *)(dst[1] + count); | ||
465 | } | ||
466 | |||
467 | /** | ||
468 | * sample_input_new_format() | ||
469 | * | ||
470 | * set the to-native sample conversion function based on dsp sample parameters | ||
471 | * | ||
472 | * !DSPPARAMSYNC | ||
473 | * needs syncing with changes to the following dsp parameters: | ||
474 | * * dsp->stereo_mode (A/V) | ||
475 | * * dsp->sample_depth (A/V) | ||
476 | */ | ||
477 | static void sample_input_new_format(struct dsp_config *dsp) | ||
478 | { | ||
479 | static const sample_input_fn_type sample_input_functions[] = | ||
480 | { | ||
481 | [SAMPLE_INPUT_LE_NATIVE_I_STEREO] = sample_input_lte_native_i_stereo, | ||
482 | [SAMPLE_INPUT_LE_NATIVE_NI_STEREO] = sample_input_lte_native_ni_stereo, | ||
483 | [SAMPLE_INPUT_LE_NATIVE_MONO] = sample_input_lte_native_mono, | ||
484 | [SAMPLE_INPUT_GT_NATIVE_I_STEREO] = sample_input_gt_native_i_stereo, | ||
485 | [SAMPLE_INPUT_GT_NATIVE_NI_STEREO] = sample_input_gt_native_ni_stereo, | ||
486 | [SAMPLE_INPUT_GT_NATIVE_MONO] = sample_input_gt_native_mono, | ||
487 | }; | ||
488 | |||
489 | int convert = dsp->stereo_mode; | ||
490 | |||
491 | if (dsp->sample_depth > NATIVE_DEPTH) | ||
492 | convert += SAMPLE_INPUT_GT_NATIVE_1ST_INDEX; | ||
493 | |||
494 | dsp->input_samples = sample_input_functions[convert]; | ||
495 | } | ||
496 | |||
497 | |||
498 | #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO | ||
499 | /* write mono internal format to output format */ | ||
500 | static void sample_output_mono(int count, struct dsp_data *data, | ||
501 | const int32_t *src[], int16_t *dst) | ||
502 | { | ||
503 | const int32_t *s0 = src[0]; | ||
504 | const int scale = data->output_scale; | ||
505 | const int dc_bias = 1 << (scale - 1); | ||
506 | |||
507 | while (count-- > 0) | ||
508 | { | ||
509 | int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
510 | *dst++ = lr; | ||
511 | *dst++ = lr; | ||
512 | } | ||
513 | } | ||
514 | #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO */ | ||
515 | |||
516 | /* write stereo internal format to output format */ | ||
517 | #ifndef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO | ||
518 | static void sample_output_stereo(int count, struct dsp_data *data, | ||
519 | const int32_t *src[], int16_t *dst) | ||
520 | { | ||
521 | const int32_t *s0 = src[0]; | ||
522 | const int32_t *s1 = src[1]; | ||
523 | const int scale = data->output_scale; | ||
524 | const int dc_bias = 1 << (scale - 1); | ||
525 | |||
526 | while (count-- > 0) | ||
527 | { | ||
528 | *dst++ = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
529 | *dst++ = clip_sample_16((*s1++ + dc_bias) >> scale); | ||
530 | } | ||
531 | } | ||
532 | #endif /* DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO */ | ||
533 | |||
534 | /** | ||
535 | * The "dither" code to convert the 24-bit samples produced by libmad was | ||
536 | * taken from the coolplayer project - coolplayer.sourceforge.net | ||
537 | * | ||
538 | * This function handles mono and stereo outputs. | ||
539 | */ | ||
540 | static void sample_output_dithered(int count, struct dsp_data *data, | ||
541 | const int32_t *src[], int16_t *dst) | ||
542 | { | ||
543 | const int32_t mask = dither_mask; | ||
544 | const int32_t bias = dither_bias; | ||
545 | const int scale = data->output_scale; | ||
546 | const int32_t min = data->clip_min; | ||
547 | const int32_t max = data->clip_max; | ||
548 | const int32_t range = max - min; | ||
549 | int ch; | ||
550 | int16_t *d; | ||
551 | |||
552 | for (ch = 0; ch < data->num_channels; ch++) | ||
553 | { | ||
554 | struct dither_data * const dither = &dither_data[ch]; | ||
555 | const int32_t *s = src[ch]; | ||
556 | int i; | ||
557 | |||
558 | for (i = 0, d = &dst[ch]; i < count; i++, s++, d += 2) | ||
559 | { | ||
560 | int32_t output, sample; | ||
561 | int32_t random; | ||
562 | |||
563 | /* Noise shape and bias (for correct rounding later) */ | ||
564 | sample = *s; | ||
565 | sample += dither->error[0] - dither->error[1] + dither->error[2]; | ||
566 | dither->error[2] = dither->error[1]; | ||
567 | dither->error[1] = dither->error[0]/2; | ||
568 | |||
569 | output = sample + bias; | ||
570 | |||
571 | /* Dither, highpass triangle PDF */ | ||
572 | random = dither->random*0x0019660dL + 0x3c6ef35fL; | ||
573 | output += (random & mask) - (dither->random & mask); | ||
574 | dither->random = random; | ||
575 | |||
576 | /* Round sample to output range */ | ||
577 | output &= ~mask; | ||
578 | |||
579 | /* Error feedback */ | ||
580 | dither->error[0] = sample - output; | ||
581 | |||
582 | /* Clip */ | ||
583 | if ((uint32_t)(output - min) > (uint32_t)range) | ||
584 | { | ||
585 | int32_t c = min; | ||
586 | if (output > min) | ||
587 | c += range; | ||
588 | output = c; | ||
589 | } | ||
590 | |||
591 | /* Quantize and store */ | ||
592 | *d = output >> scale; | ||
593 | } | ||
594 | } | ||
595 | |||
596 | if (data->num_channels == 2) | ||
597 | return; | ||
598 | |||
599 | /* Have to duplicate left samples into the right channel since | ||
600 | pcm buffer and hardware is interleaved stereo */ | ||
601 | d = &dst[0]; | ||
602 | |||
603 | while (count-- > 0) | ||
604 | { | ||
605 | int16_t s = *d++; | ||
606 | *d++ = s; | ||
607 | } | ||
608 | } | ||
609 | |||
610 | /** | ||
611 | * sample_output_new_format() | ||
612 | * | ||
613 | * set the from-native to ouput sample conversion routine | ||
614 | * | ||
615 | * !DSPPARAMSYNC | ||
616 | * needs syncing with changes to the following dsp parameters: | ||
617 | * * dsp->stereo_mode (A/V) | ||
618 | * * dither_enabled (A) | ||
619 | */ | ||
620 | static void sample_output_new_format(struct dsp_config *dsp) | ||
621 | { | ||
622 | static const sample_output_fn_type sample_output_functions[] = | ||
623 | { | ||
624 | sample_output_mono, | ||
625 | sample_output_stereo, | ||
626 | sample_output_dithered, | ||
627 | sample_output_dithered | ||
628 | }; | ||
629 | |||
630 | int out = dsp->data.num_channels - 1; | ||
631 | |||
632 | if (dsp == &AUDIO_DSP && dither_enabled) | ||
633 | out += 2; | ||
634 | |||
635 | dsp->output_samples = sample_output_functions[out]; | ||
636 | } | ||
637 | |||
638 | /** | ||
639 | * Linear interpolation resampling that introduces a one sample delay because | ||
640 | * of our inability to look into the future at the end of a frame. | ||
641 | */ | ||
642 | #ifndef DSP_HAVE_ASM_RESAMPLING | ||
643 | static int dsp_downsample(int count, struct dsp_data *data, | ||
644 | const int32_t *src[], int32_t *dst[]) | ||
645 | { | ||
646 | int ch = data->num_channels - 1; | ||
647 | uint32_t delta = data->resample_data.delta; | ||
648 | uint32_t phase, pos; | ||
649 | int32_t *d; | ||
650 | |||
651 | /* Rolled channel loop actually showed slightly faster. */ | ||
652 | do | ||
653 | { | ||
654 | /* Just initialize things and not worry too much about the relatively | ||
655 | * uncommon case of not being able to spit out a sample for the frame. | ||
656 | */ | ||
657 | const int32_t *s = src[ch]; | ||
658 | int32_t last = data->resample_data.last_sample[ch]; | ||
659 | |||
660 | data->resample_data.last_sample[ch] = s[count - 1]; | ||
661 | d = dst[ch]; | ||
662 | phase = data->resample_data.phase; | ||
663 | pos = phase >> 16; | ||
664 | |||
665 | /* Do we need last sample of previous frame for interpolation? */ | ||
666 | if (pos > 0) | ||
667 | last = s[pos - 1]; | ||
668 | |||
669 | while (pos < (uint32_t)count) | ||
670 | { | ||
671 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); | ||
672 | phase += delta; | ||
673 | pos = phase >> 16; | ||
674 | last = s[pos - 1]; | ||
675 | } | ||
676 | } | ||
677 | while (--ch >= 0); | ||
678 | |||
679 | /* Wrap phase accumulator back to start of next frame. */ | ||
680 | data->resample_data.phase = phase - (count << 16); | ||
681 | return d - dst[0]; | ||
682 | } | ||
683 | |||
684 | static int dsp_upsample(int count, struct dsp_data *data, | ||
685 | const int32_t *src[], int32_t *dst[]) | ||
686 | { | ||
687 | int ch = data->num_channels - 1; | ||
688 | uint32_t delta = data->resample_data.delta; | ||
689 | uint32_t phase, pos; | ||
690 | int32_t *d; | ||
691 | |||
692 | /* Rolled channel loop actually showed slightly faster. */ | ||
693 | do | ||
694 | { | ||
695 | /* Should always be able to output a sample for a ratio up to RESAMPLE_RATIO */ | ||
696 | const int32_t *s = src[ch]; | ||
697 | int32_t last = data->resample_data.last_sample[ch]; | ||
698 | |||
699 | data->resample_data.last_sample[ch] = s[count - 1]; | ||
700 | d = dst[ch]; | ||
701 | phase = data->resample_data.phase; | ||
702 | pos = phase >> 16; | ||
703 | |||
704 | while (pos == 0) | ||
705 | { | ||
706 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[0] - last); | ||
707 | phase += delta; | ||
708 | pos = phase >> 16; | ||
709 | } | ||
710 | |||
711 | while (pos < (uint32_t)count) | ||
712 | { | ||
713 | last = s[pos - 1]; | ||
714 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); | ||
715 | phase += delta; | ||
716 | pos = phase >> 16; | ||
717 | } | ||
718 | } | ||
719 | while (--ch >= 0); | ||
720 | |||
721 | /* Wrap phase accumulator back to start of next frame. */ | ||
722 | data->resample_data.phase = phase & 0xffff; | ||
723 | return d - dst[0]; | ||
724 | } | ||
725 | #endif /* DSP_HAVE_ASM_RESAMPLING */ | ||
726 | |||
727 | static void resampler_new_delta(struct dsp_config *dsp) | ||
728 | { | ||
729 | dsp->data.resample_data.delta = (unsigned long) | ||
730 | dsp->frequency * 65536LL / NATIVE_FREQUENCY; | ||
731 | |||
732 | if (dsp->frequency == NATIVE_FREQUENCY) | ||
733 | { | ||
734 | /* NOTE: If fully glitch-free transistions from no resampling to | ||
735 | resampling are desired, last_sample history should be maintained | ||
736 | even when not resampling. */ | ||
737 | dsp->resample = NULL; | ||
738 | dsp->data.resample_data.phase = 0; | ||
739 | dsp->data.resample_data.last_sample[0] = 0; | ||
740 | dsp->data.resample_data.last_sample[1] = 0; | ||
741 | } | ||
742 | else if (dsp->frequency < NATIVE_FREQUENCY) | ||
743 | dsp->resample = dsp_upsample; | ||
744 | else | ||
745 | dsp->resample = dsp_downsample; | ||
746 | } | ||
747 | |||
748 | /* Resample count stereo samples. Updates the src array, if resampling is | ||
749 | * done, to refer to the resampled data. Returns number of stereo samples | ||
750 | * for further processing. | ||
751 | */ | ||
752 | static inline int resample(struct dsp_config *dsp, int count, int32_t *src[]) | ||
753 | { | ||
754 | int32_t *dst[2] = | ||
755 | { | ||
756 | resample_buf[0], | ||
757 | resample_buf[1] | ||
758 | }; | ||
759 | lock_sample_buf( true ); | ||
760 | count = dsp->resample(count, &dsp->data, (const int32_t **)src, dst); | ||
761 | |||
762 | src[0] = dst[0]; | ||
763 | src[1] = dst[dsp->data.num_channels - 1]; | ||
764 | lock_sample_buf( false ); | ||
765 | return count; | ||
766 | } | ||
767 | |||
768 | static void dither_init(struct dsp_config *dsp) | ||
769 | { | ||
770 | memset(dither_data, 0, sizeof (dither_data)); | ||
771 | dither_bias = (1L << (dsp->data.frac_bits - NATIVE_DEPTH)); | ||
772 | dither_mask = (1L << (dsp->data.frac_bits + 1 - NATIVE_DEPTH)) - 1; | ||
773 | } | ||
774 | |||
775 | void dsp_dither_enable(bool enable) | ||
776 | { | ||
777 | struct dsp_config *dsp = &AUDIO_DSP; | ||
778 | dither_enabled = enable; | ||
779 | sample_output_new_format(dsp); | ||
780 | } | ||
781 | |||
782 | /* Applies crossfeed to the stereo signal in src. | ||
783 | * Crossfeed is a process where listening over speakers is simulated. This | ||
784 | * is good for old hard panned stereo records, which might be quite fatiguing | ||
785 | * to listen to on headphones with no crossfeed. | ||
786 | */ | ||
787 | #ifndef DSP_HAVE_ASM_CROSSFEED | ||
788 | static void apply_crossfeed(int count, int32_t *buf[]) | ||
789 | { | ||
790 | int32_t *hist_l = &crossfeed_data.history[0]; | ||
791 | int32_t *hist_r = &crossfeed_data.history[2]; | ||
792 | int32_t *delay = &crossfeed_data.delay[0][0]; | ||
793 | int32_t *coefs = &crossfeed_data.coefs[0]; | ||
794 | int32_t gain = crossfeed_data.gain; | ||
795 | int32_t *di = crossfeed_data.index; | ||
796 | |||
797 | int32_t acc; | ||
798 | int32_t left, right; | ||
799 | int i; | ||
800 | |||
801 | for (i = 0; i < count; i++) | ||
802 | { | ||
803 | left = buf[0][i]; | ||
804 | right = buf[1][i]; | ||
805 | |||
806 | /* Filter delayed sample from left speaker */ | ||
807 | acc = FRACMUL(*di, coefs[0]); | ||
808 | acc += FRACMUL(hist_l[0], coefs[1]); | ||
809 | acc += FRACMUL(hist_l[1], coefs[2]); | ||
810 | /* Save filter history for left speaker */ | ||
811 | hist_l[1] = acc; | ||
812 | hist_l[0] = *di; | ||
813 | *di++ = left; | ||
814 | /* Filter delayed sample from right speaker */ | ||
815 | acc = FRACMUL(*di, coefs[0]); | ||
816 | acc += FRACMUL(hist_r[0], coefs[1]); | ||
817 | acc += FRACMUL(hist_r[1], coefs[2]); | ||
818 | /* Save filter history for right speaker */ | ||
819 | hist_r[1] = acc; | ||
820 | hist_r[0] = *di; | ||
821 | *di++ = right; | ||
822 | /* Now add the attenuated direct sound and write to outputs */ | ||
823 | buf[0][i] = FRACMUL(left, gain) + hist_r[1]; | ||
824 | buf[1][i] = FRACMUL(right, gain) + hist_l[1]; | ||
825 | |||
826 | /* Wrap delay line index if bigger than delay line size */ | ||
827 | if (di >= delay + 13*2) | ||
828 | di = delay; | ||
829 | } | ||
830 | /* Write back local copies of data we've modified */ | ||
831 | crossfeed_data.index = di; | ||
832 | } | ||
833 | #endif /* DSP_HAVE_ASM_CROSSFEED */ | ||
834 | |||
835 | /** | ||
836 | * dsp_set_crossfeed(bool enable) | ||
837 | * | ||
838 | * !DSPPARAMSYNC | ||
839 | * needs syncing with changes to the following dsp parameters: | ||
840 | * * dsp->stereo_mode (A) | ||
841 | */ | ||
842 | void dsp_set_crossfeed(bool enable) | ||
843 | { | ||
844 | crossfeed_enabled = enable; | ||
845 | AUDIO_DSP.apply_crossfeed = (enable && AUDIO_DSP.data.num_channels > 1) | ||
846 | ? apply_crossfeed : NULL; | ||
847 | } | ||
848 | |||
849 | void dsp_set_crossfeed_direct_gain(int gain) | ||
850 | { | ||
851 | crossfeed_data.gain = get_replaygain_int(gain * 10) << 7; | ||
852 | /* If gain is negative, the calculation overflowed and we need to clamp */ | ||
853 | if (crossfeed_data.gain < 0) | ||
854 | crossfeed_data.gain = 0x7fffffff; | ||
855 | } | ||
856 | |||
857 | /* Both gains should be below 0 dB */ | ||
858 | void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) | ||
859 | { | ||
860 | int32_t *c = crossfeed_data.coefs; | ||
861 | long scaler = get_replaygain_int(lf_gain * 10) << 7; | ||
862 | |||
863 | cutoff = 0xffffffff/NATIVE_FREQUENCY*cutoff; | ||
864 | hf_gain -= lf_gain; | ||
865 | /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB | ||
866 | * point instead of shelf midpoint. This is for compatibility with the old | ||
867 | * crossfeed shelf filter and should be removed if crossfeed settings are | ||
868 | * ever made incompatible for any other good reason. | ||
869 | */ | ||
870 | cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24); | ||
871 | filter_shelf_coefs(cutoff, hf_gain, false, c); | ||
872 | /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains | ||
873 | * over 1 and can do this safely | ||
874 | */ | ||
875 | c[0] = FRACMUL_SHL(c[0], scaler, 4); | ||
876 | c[1] = FRACMUL_SHL(c[1], scaler, 4); | ||
877 | c[2] <<= 4; | ||
878 | } | ||
879 | |||
880 | /* Apply a constant gain to the samples (e.g., for ReplayGain). | ||
881 | * Note that this must be called before the resampler. | ||
882 | */ | ||
883 | #ifndef DSP_HAVE_ASM_APPLY_GAIN | ||
884 | static void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) | ||
885 | { | ||
886 | const int32_t gain = data->gain; | ||
887 | int ch; | ||
888 | |||
889 | for (ch = 0; ch < data->num_channels; ch++) | ||
890 | { | ||
891 | int32_t *d = buf[ch]; | ||
892 | int i; | ||
893 | |||
894 | for (i = 0; i < count; i++) | ||
895 | d[i] = FRACMUL_SHL(d[i], gain, 8); | ||
896 | } | ||
897 | } | ||
898 | #endif /* DSP_HAVE_ASM_APPLY_GAIN */ | ||
899 | |||
900 | /* Combine all gains to a global gain. */ | ||
901 | static void set_gain(struct dsp_config *dsp) | ||
902 | { | ||
903 | /* gains are in S7.24 format */ | ||
904 | dsp->data.gain = DEFAULT_GAIN; | ||
905 | |||
906 | /* Replay gain not relevant to voice */ | ||
907 | if (dsp == &AUDIO_DSP && replaygain) | ||
908 | { | ||
909 | dsp->data.gain = replaygain; | ||
910 | } | ||
911 | |||
912 | if (dsp->eq_process && eq_precut) | ||
913 | { | ||
914 | dsp->data.gain = fp_mul(dsp->data.gain, eq_precut, 24); | ||
915 | } | ||
916 | |||
917 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
918 | if (global_settings.volume < SW_VOLUME_MAX || | ||
919 | global_settings.volume > SW_VOLUME_MIN) | ||
920 | { | ||
921 | int vol_gain = get_replaygain_int(global_settings.volume * 100); | ||
922 | dsp->data.gain = (long) (((int64_t) dsp->data.gain * vol_gain) >> 24); | ||
923 | } | ||
924 | #endif | ||
925 | |||
926 | if (dsp->data.gain == DEFAULT_GAIN) | ||
927 | { | ||
928 | dsp->data.gain = 0; | ||
929 | } | ||
930 | else | ||
931 | { | ||
932 | dsp->data.gain >>= 1; /* convert gain to S8.23 format */ | ||
933 | } | ||
934 | |||
935 | dsp->apply_gain = dsp->data.gain != 0 ? dsp_apply_gain : NULL; | ||
936 | } | ||
937 | |||
938 | /** | ||
939 | * Update the amount to cut the audio before applying the equalizer. | ||
940 | * | ||
941 | * @param precut to apply in decibels (multiplied by 10) | ||
942 | */ | ||
943 | void dsp_set_eq_precut(int precut) | ||
944 | { | ||
945 | eq_precut = get_replaygain_int(precut * -10); | ||
946 | set_gain(&AUDIO_DSP); | ||
947 | } | ||
948 | |||
949 | /** | ||
950 | * Synchronize the equalizer filter coefficients with the global settings. | ||
951 | * | ||
952 | * @param band the equalizer band to synchronize | ||
953 | */ | ||
954 | void dsp_set_eq_coefs(int band, int cutoff, int q, int gain) | ||
955 | { | ||
956 | /* Convert user settings to format required by coef generator functions */ | ||
957 | cutoff = 0xffffffff / NATIVE_FREQUENCY * cutoff; | ||
958 | |||
959 | if (q == 0) | ||
960 | q = 1; | ||
961 | |||
962 | /* NOTE: The coef functions assume the EMAC unit is in fractional mode, | ||
963 | which it should be, since we're executed from the main thread. */ | ||
964 | |||
965 | /* Assume a band is disabled if the gain is zero */ | ||
966 | if (gain == 0) | ||
967 | { | ||
968 | eq_data.enabled[band] = 0; | ||
969 | } | ||
970 | else | ||
971 | { | ||
972 | if (band == 0) | ||
973 | eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
974 | else if (band == 4) | ||
975 | eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
976 | else | ||
977 | eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs); | ||
978 | |||
979 | eq_data.enabled[band] = 1; | ||
980 | } | ||
981 | } | ||
982 | |||
983 | /* Apply EQ filters to those bands that have got it switched on. */ | ||
984 | static void eq_process(int count, int32_t *buf[]) | ||
985 | { | ||
986 | static const int shifts[] = | ||
987 | { | ||
988 | EQ_SHELF_SHIFT, /* low shelf */ | ||
989 | EQ_PEAK_SHIFT, /* peaking */ | ||
990 | EQ_PEAK_SHIFT, /* peaking */ | ||
991 | EQ_PEAK_SHIFT, /* peaking */ | ||
992 | EQ_SHELF_SHIFT, /* high shelf */ | ||
993 | }; | ||
994 | unsigned int channels = AUDIO_DSP.data.num_channels; | ||
995 | int i; | ||
996 | |||
997 | /* filter configuration currently is 1 low shelf filter, 3 band peaking | ||
998 | filters and 1 high shelf filter, in that order. we need to know this | ||
999 | so we can choose the correct shift factor. | ||
1000 | */ | ||
1001 | for (i = 0; i < 5; i++) | ||
1002 | { | ||
1003 | if (!eq_data.enabled[i]) | ||
1004 | continue; | ||
1005 | eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]); | ||
1006 | } | ||
1007 | } | ||
1008 | |||
1009 | /** | ||
1010 | * Use to enable the equalizer. | ||
1011 | * | ||
1012 | * @param enable true to enable the equalizer | ||
1013 | */ | ||
1014 | void dsp_set_eq(bool enable) | ||
1015 | { | ||
1016 | AUDIO_DSP.eq_process = enable ? eq_process : NULL; | ||
1017 | set_gain(&AUDIO_DSP); | ||
1018 | } | ||
1019 | |||
1020 | static void dsp_set_stereo_width(int value) | ||
1021 | { | ||
1022 | long width, straight, cross; | ||
1023 | |||
1024 | width = value * 0x7fffff / 100; | ||
1025 | |||
1026 | if (value <= 100) | ||
1027 | { | ||
1028 | straight = (0x7fffff + width) / 2; | ||
1029 | cross = straight - width; | ||
1030 | } | ||
1031 | else | ||
1032 | { | ||
1033 | /* straight = (1 + width) / (2 * width) */ | ||
1034 | straight = ((int64_t)(0x7fffff + width) << 22) / width; | ||
1035 | cross = straight - 0x7fffff; | ||
1036 | } | ||
1037 | |||
1038 | dsp_sw_gain = straight << 8; | ||
1039 | dsp_sw_cross = cross << 8; | ||
1040 | } | ||
1041 | |||
1042 | /** | ||
1043 | * Implements the different channel configurations and stereo width. | ||
1044 | */ | ||
1045 | |||
1046 | /* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for | ||
1047 | * completeness. */ | ||
1048 | #if 0 | ||
1049 | static void channels_process_sound_chan_stereo(int count, int32_t *buf[]) | ||
1050 | { | ||
1051 | /* The channels are each just themselves */ | ||
1052 | (void)count; (void)buf; | ||
1053 | } | ||
1054 | #endif | ||
1055 | |||
1056 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO | ||
1057 | static void channels_process_sound_chan_mono(int count, int32_t *buf[]) | ||
1058 | { | ||
1059 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1060 | |||
1061 | while (count-- > 0) | ||
1062 | { | ||
1063 | int32_t lr = *sl/2 + *sr/2; | ||
1064 | *sl++ = lr; | ||
1065 | *sr++ = lr; | ||
1066 | } | ||
1067 | } | ||
1068 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */ | ||
1069 | |||
1070 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM | ||
1071 | static void channels_process_sound_chan_custom(int count, int32_t *buf[]) | ||
1072 | { | ||
1073 | const int32_t gain = dsp_sw_gain; | ||
1074 | const int32_t cross = dsp_sw_cross; | ||
1075 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1076 | |||
1077 | while (count-- > 0) | ||
1078 | { | ||
1079 | int32_t l = *sl; | ||
1080 | int32_t r = *sr; | ||
1081 | *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross); | ||
1082 | *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross); | ||
1083 | } | ||
1084 | } | ||
1085 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */ | ||
1086 | |||
1087 | static void channels_process_sound_chan_mono_left(int count, int32_t *buf[]) | ||
1088 | { | ||
1089 | /* Just copy over the other channel */ | ||
1090 | memcpy(buf[1], buf[0], count * sizeof (*buf)); | ||
1091 | } | ||
1092 | |||
1093 | static void channels_process_sound_chan_mono_right(int count, int32_t *buf[]) | ||
1094 | { | ||
1095 | /* Just copy over the other channel */ | ||
1096 | memcpy(buf[0], buf[1], count * sizeof (*buf)); | ||
1097 | } | ||
1098 | |||
1099 | #ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE | ||
1100 | static void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) | ||
1101 | { | ||
1102 | int32_t *sl = buf[0], *sr = buf[1]; | ||
1103 | |||
1104 | while (count-- > 0) | ||
1105 | { | ||
1106 | int32_t ch = *sl/2 - *sr/2; | ||
1107 | *sl++ = ch; | ||
1108 | *sr++ = -ch; | ||
1109 | } | ||
1110 | } | ||
1111 | #endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */ | ||
1112 | |||
1113 | static void dsp_set_channel_config(int value) | ||
1114 | { | ||
1115 | static const channels_process_fn_type channels_process_functions[] = | ||
1116 | { | ||
1117 | /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */ | ||
1118 | [SOUND_CHAN_STEREO] = NULL, | ||
1119 | [SOUND_CHAN_MONO] = channels_process_sound_chan_mono, | ||
1120 | [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom, | ||
1121 | [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left, | ||
1122 | [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right, | ||
1123 | [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke, | ||
1124 | }; | ||
1125 | |||
1126 | if ((unsigned)value >= ARRAYLEN(channels_process_functions) || | ||
1127 | AUDIO_DSP.stereo_mode == STEREO_MONO) | ||
1128 | { | ||
1129 | value = SOUND_CHAN_STEREO; | ||
1130 | } | ||
1131 | |||
1132 | /* This doesn't apply to voice */ | ||
1133 | channels_mode = value; | ||
1134 | AUDIO_DSP.channels_process = channels_process_functions[value]; | ||
1135 | } | ||
1136 | |||
1137 | #if CONFIG_CODEC == SWCODEC | ||
1138 | |||
1139 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1140 | static void set_tone_controls(void) | ||
1141 | { | ||
1142 | filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200, | ||
1143 | 0xffffffff/NATIVE_FREQUENCY*3500, | ||
1144 | bass, treble, -prescale, | ||
1145 | AUDIO_DSP.tone_filter.coefs); | ||
1146 | /* Sync the voice dsp coefficients */ | ||
1147 | memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs, | ||
1148 | sizeof (VOICE_DSP.tone_filter.coefs)); | ||
1149 | } | ||
1150 | #endif | ||
1151 | |||
1152 | /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/ | ||
1153 | * code directly. | ||
1154 | */ | ||
1155 | int dsp_callback(int msg, intptr_t param) | ||
1156 | { | ||
1157 | switch (msg) | ||
1158 | { | ||
1159 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1160 | case DSP_CALLBACK_SET_PRESCALE: | ||
1161 | prescale = param; | ||
1162 | set_tone_controls(); | ||
1163 | break; | ||
1164 | /* prescaler is always set after calling any of these, so we wait with | ||
1165 | * calculating coefs until the above case is hit. | ||
1166 | */ | ||
1167 | case DSP_CALLBACK_SET_BASS: | ||
1168 | bass = param; | ||
1169 | break; | ||
1170 | case DSP_CALLBACK_SET_TREBLE: | ||
1171 | treble = param; | ||
1172 | break; | ||
1173 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
1174 | case DSP_CALLBACK_SET_SW_VOLUME: | ||
1175 | set_gain(&AUDIO_DSP); | ||
1176 | break; | ||
1177 | #endif | ||
1178 | #endif | ||
1179 | case DSP_CALLBACK_SET_CHANNEL_CONFIG: | ||
1180 | dsp_set_channel_config(param); | ||
1181 | break; | ||
1182 | case DSP_CALLBACK_SET_STEREO_WIDTH: | ||
1183 | dsp_set_stereo_width(param); | ||
1184 | break; | ||
1185 | default: | ||
1186 | break; | ||
1187 | } | ||
1188 | return 0; | ||
1189 | } | ||
1190 | #endif | ||
1191 | |||
1192 | /* Process and convert src audio to dst based on the DSP configuration, | ||
1193 | * reading count number of audio samples. dst is assumed to be large | ||
1194 | * enough; use dsp_output_count() to get the required number. src is an | ||
1195 | * array of pointers; for mono and interleaved stereo, it contains one | ||
1196 | * pointer to the start of the audio data and the other is ignored; for | ||
1197 | * non-interleaved stereo, it contains two pointers, one for each audio | ||
1198 | * channel. Returns number of bytes written to dst. | ||
1199 | */ | ||
1200 | int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count) | ||
1201 | { | ||
1202 | static int32_t *tmp[2]; /* tdspeed_doit() needs it static */ | ||
1203 | static long last_yield; | ||
1204 | long tick; | ||
1205 | int written = 0; | ||
1206 | |||
1207 | #if defined(CPU_COLDFIRE) | ||
1208 | /* set emac unit for dsp processing, and save old macsr, we're running in | ||
1209 | codec thread context at this point, so can't clobber it */ | ||
1210 | unsigned long old_macsr = coldfire_get_macsr(); | ||
1211 | coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE); | ||
1212 | #endif | ||
1213 | |||
1214 | if (new_gain) | ||
1215 | dsp_set_replaygain(); /* Gain has changed */ | ||
1216 | |||
1217 | /* Perform at least one yield before starting */ | ||
1218 | last_yield = current_tick; | ||
1219 | yield(); | ||
1220 | |||
1221 | /* Testing function pointers for NULL is preferred since the pointer | ||
1222 | will be preloaded to be used for the call if not. */ | ||
1223 | while (count > 0) | ||
1224 | { | ||
1225 | int samples = MIN(sample_buf_count, count); | ||
1226 | count -= samples; | ||
1227 | |||
1228 | dsp->input_samples(samples, src, tmp); | ||
1229 | |||
1230 | #ifdef HAVE_PITCHSCREEN | ||
1231 | if (dsp->tdspeed_active) | ||
1232 | samples = tdspeed_doit(tmp, samples); | ||
1233 | #endif | ||
1234 | |||
1235 | int chunk_offset = 0; | ||
1236 | while (samples > 0) | ||
1237 | { | ||
1238 | int32_t *t2[2]; | ||
1239 | t2[0] = tmp[0]+chunk_offset; | ||
1240 | t2[1] = tmp[1]+chunk_offset; | ||
1241 | |||
1242 | int chunk = MIN(sample_buf_count, samples); | ||
1243 | chunk_offset += chunk; | ||
1244 | samples -= chunk; | ||
1245 | |||
1246 | if (dsp->apply_gain) | ||
1247 | dsp->apply_gain(chunk, &dsp->data, t2); | ||
1248 | |||
1249 | if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0) | ||
1250 | break; /* I'm pretty sure we're downsampling here */ | ||
1251 | |||
1252 | if (dsp->apply_crossfeed) | ||
1253 | dsp->apply_crossfeed(chunk, t2); | ||
1254 | |||
1255 | if (dsp->eq_process) | ||
1256 | dsp->eq_process(chunk, t2); | ||
1257 | |||
1258 | #ifdef HAVE_SW_TONE_CONTROLS | ||
1259 | if ((bass | treble) != 0) | ||
1260 | eq_filter(t2, &dsp->tone_filter, chunk, | ||
1261 | dsp->data.num_channels, FILTER_BISHELF_SHIFT); | ||
1262 | #endif | ||
1263 | |||
1264 | if (dsp->channels_process) | ||
1265 | dsp->channels_process(chunk, t2); | ||
1266 | |||
1267 | if (dsp->compressor_process) | ||
1268 | dsp->compressor_process(chunk, &dsp->data, t2); | ||
1269 | |||
1270 | dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst); | ||
1271 | |||
1272 | written += chunk; | ||
1273 | dst += chunk * sizeof (int16_t) * 2; | ||
1274 | |||
1275 | /* yield at least once each tick */ | ||
1276 | tick = current_tick; | ||
1277 | if (TIME_AFTER(tick, last_yield)) | ||
1278 | { | ||
1279 | last_yield = tick; | ||
1280 | yield(); | ||
1281 | } | ||
1282 | } | ||
1283 | } | ||
1284 | |||
1285 | #if defined(CPU_COLDFIRE) | ||
1286 | /* set old macsr again */ | ||
1287 | coldfire_set_macsr(old_macsr); | ||
1288 | #endif | ||
1289 | return written; | ||
1290 | } | ||
1291 | |||
1292 | /* Given count number of input samples, calculate the maximum number of | ||
1293 | * samples of output data that would be generated (the calculation is not | ||
1294 | * entirely exact and rounds upwards to be on the safe side; during | ||
1295 | * resampling, the number of samples generated depends on the current state | ||
1296 | * of the resampler). | ||
1297 | */ | ||
1298 | /* dsp_input_size MUST be called afterwards */ | ||
1299 | int dsp_output_count(struct dsp_config *dsp, int count) | ||
1300 | { | ||
1301 | #ifdef HAVE_PITCHSCREEN | ||
1302 | if (dsp->tdspeed_active) | ||
1303 | count = tdspeed_est_output_size(); | ||
1304 | #endif | ||
1305 | if (dsp->resample) | ||
1306 | { | ||
1307 | count = (int)(((unsigned long)count * NATIVE_FREQUENCY | ||
1308 | + (dsp->frequency - 1)) / dsp->frequency); | ||
1309 | } | ||
1310 | |||
1311 | /* Now we have the resampled sample count which must not exceed | ||
1312 | * resample_buf_count to avoid resample buffer overflow. One | ||
1313 | * must call dsp_input_count() to get the correct input sample | ||
1314 | * count. | ||
1315 | */ | ||
1316 | if (count > resample_buf_count) | ||
1317 | count = resample_buf_count; | ||
1318 | |||
1319 | return count; | ||
1320 | } | ||
1321 | |||
1322 | /* Given count output samples, calculate number of input samples | ||
1323 | * that would be consumed in order to fill the output buffer. | ||
1324 | */ | ||
1325 | int dsp_input_count(struct dsp_config *dsp, int count) | ||
1326 | { | ||
1327 | /* count is now the number of resampled input samples. Convert to | ||
1328 | original input samples. */ | ||
1329 | if (dsp->resample) | ||
1330 | { | ||
1331 | /* Use the real resampling delta = | ||
1332 | * dsp->frequency * 65536 / NATIVE_FREQUENCY, and | ||
1333 | * round towards zero to avoid buffer overflows. */ | ||
1334 | count = (int)(((unsigned long)count * | ||
1335 | dsp->data.resample_data.delta) >> 16); | ||
1336 | } | ||
1337 | |||
1338 | #ifdef HAVE_PITCHSCREEN | ||
1339 | if (dsp->tdspeed_active) | ||
1340 | count = tdspeed_est_input_size(count); | ||
1341 | #endif | ||
1342 | |||
1343 | return count; | ||
1344 | } | ||
1345 | |||
1346 | static void dsp_set_gain_var(long *var, long value) | ||
1347 | { | ||
1348 | *var = value; | ||
1349 | new_gain = true; | ||
1350 | } | ||
1351 | |||
1352 | static void dsp_update_functions(struct dsp_config *dsp) | ||
1353 | { | ||
1354 | sample_input_new_format(dsp); | ||
1355 | sample_output_new_format(dsp); | ||
1356 | if (dsp == &AUDIO_DSP) | ||
1357 | dsp_set_crossfeed(crossfeed_enabled); | ||
1358 | } | ||
1359 | |||
1360 | intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value) | ||
1361 | { | ||
1362 | switch (setting) | ||
1363 | { | ||
1364 | case DSP_MYDSP: | ||
1365 | switch (value) | ||
1366 | { | ||
1367 | case CODEC_IDX_AUDIO: | ||
1368 | return (intptr_t)&AUDIO_DSP; | ||
1369 | case CODEC_IDX_VOICE: | ||
1370 | return (intptr_t)&VOICE_DSP; | ||
1371 | default: | ||
1372 | return (intptr_t)NULL; | ||
1373 | } | ||
1374 | |||
1375 | case DSP_SET_FREQUENCY: | ||
1376 | memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data)); | ||
1377 | /* Fall through!!! */ | ||
1378 | case DSP_SWITCH_FREQUENCY: | ||
1379 | dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value; | ||
1380 | /* Account for playback speed adjustment when setting dsp->frequency | ||
1381 | if we're called from the main audio thread. Voice UI thread should | ||
1382 | not need this feature. | ||
1383 | */ | ||
1384 | #ifdef HAVE_PITCHSCREEN | ||
1385 | if (dsp == &AUDIO_DSP) | ||
1386 | dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100; | ||
1387 | else | ||
1388 | #endif | ||
1389 | dsp->frequency = dsp->codec_frequency; | ||
1390 | |||
1391 | resampler_new_delta(dsp); | ||
1392 | #ifdef HAVE_PITCHSCREEN | ||
1393 | tdspeed_setup(dsp); | ||
1394 | #endif | ||
1395 | break; | ||
1396 | |||
1397 | case DSP_SET_SAMPLE_DEPTH: | ||
1398 | dsp->sample_depth = value; | ||
1399 | |||
1400 | if (dsp->sample_depth <= NATIVE_DEPTH) | ||
1401 | { | ||
1402 | dsp->data.frac_bits = WORD_FRACBITS; | ||
1403 | dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */ | ||
1404 | dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); | ||
1405 | dsp->data.clip_min = -((1 << WORD_FRACBITS)); | ||
1406 | } | ||
1407 | else | ||
1408 | { | ||
1409 | dsp->data.frac_bits = value; | ||
1410 | dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */ | ||
1411 | dsp->data.clip_max = (1 << value) - 1; | ||
1412 | dsp->data.clip_min = -(1 << value); | ||
1413 | } | ||
1414 | |||
1415 | dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH; | ||
1416 | sample_input_new_format(dsp); | ||
1417 | dither_init(dsp); | ||
1418 | break; | ||
1419 | |||
1420 | case DSP_SET_STEREO_MODE: | ||
1421 | dsp->stereo_mode = value; | ||
1422 | dsp->data.num_channels = value == STEREO_MONO ? 1 : 2; | ||
1423 | dsp_update_functions(dsp); | ||
1424 | #ifdef HAVE_PITCHSCREEN | ||
1425 | tdspeed_setup(dsp); | ||
1426 | #endif | ||
1427 | break; | ||
1428 | |||
1429 | case DSP_RESET: | ||
1430 | dsp->stereo_mode = STEREO_NONINTERLEAVED; | ||
1431 | dsp->data.num_channels = 2; | ||
1432 | dsp->sample_depth = NATIVE_DEPTH; | ||
1433 | dsp->data.frac_bits = WORD_FRACBITS; | ||
1434 | dsp->sample_bytes = sizeof (int16_t); | ||
1435 | dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH; | ||
1436 | dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1); | ||
1437 | dsp->data.clip_min = -((1 << WORD_FRACBITS)); | ||
1438 | dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY; | ||
1439 | |||
1440 | if (dsp == &AUDIO_DSP) | ||
1441 | { | ||
1442 | track_gain = 0; | ||
1443 | album_gain = 0; | ||
1444 | track_peak = 0; | ||
1445 | album_peak = 0; | ||
1446 | new_gain = true; | ||
1447 | } | ||
1448 | |||
1449 | dsp_update_functions(dsp); | ||
1450 | resampler_new_delta(dsp); | ||
1451 | #ifdef HAVE_PITCHSCREEN | ||
1452 | tdspeed_setup(dsp); | ||
1453 | #endif | ||
1454 | if (dsp == &AUDIO_DSP) | ||
1455 | compressor_reset(); | ||
1456 | break; | ||
1457 | |||
1458 | case DSP_FLUSH: | ||
1459 | memset(&dsp->data.resample_data, 0, | ||
1460 | sizeof (dsp->data.resample_data)); | ||
1461 | resampler_new_delta(dsp); | ||
1462 | dither_init(dsp); | ||
1463 | #ifdef HAVE_PITCHSCREEN | ||
1464 | tdspeed_setup(dsp); | ||
1465 | #endif | ||
1466 | if (dsp == &AUDIO_DSP) | ||
1467 | compressor_reset(); | ||
1468 | break; | ||
1469 | |||
1470 | case DSP_SET_TRACK_GAIN: | ||
1471 | if (dsp == &AUDIO_DSP) | ||
1472 | dsp_set_gain_var(&track_gain, value); | ||
1473 | break; | ||
1474 | |||
1475 | case DSP_SET_ALBUM_GAIN: | ||
1476 | if (dsp == &AUDIO_DSP) | ||
1477 | dsp_set_gain_var(&album_gain, value); | ||
1478 | break; | ||
1479 | |||
1480 | case DSP_SET_TRACK_PEAK: | ||
1481 | if (dsp == &AUDIO_DSP) | ||
1482 | dsp_set_gain_var(&track_peak, value); | ||
1483 | break; | ||
1484 | |||
1485 | case DSP_SET_ALBUM_PEAK: | ||
1486 | if (dsp == &AUDIO_DSP) | ||
1487 | dsp_set_gain_var(&album_peak, value); | ||
1488 | break; | ||
1489 | |||
1490 | default: | ||
1491 | return 0; | ||
1492 | } | ||
1493 | |||
1494 | return 1; | ||
1495 | } | ||
1496 | |||
1497 | int get_replaygain_mode(bool have_track_gain, bool have_album_gain) | ||
1498 | { | ||
1499 | int type; | ||
1500 | |||
1501 | bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK) | ||
1502 | || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE) | ||
1503 | && global_settings.playlist_shuffle)); | ||
1504 | |||
1505 | type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM | ||
1506 | : have_track_gain ? REPLAYGAIN_TRACK : -1; | ||
1507 | |||
1508 | return type; | ||
1509 | } | ||
1510 | |||
1511 | void dsp_set_replaygain(void) | ||
1512 | { | ||
1513 | long gain = 0; | ||
1514 | |||
1515 | new_gain = false; | ||
1516 | |||
1517 | if ((global_settings.replaygain_type != REPLAYGAIN_OFF) || | ||
1518 | global_settings.replaygain_noclip) | ||
1519 | { | ||
1520 | bool track_mode = get_replaygain_mode(track_gain != 0, | ||
1521 | album_gain != 0) == REPLAYGAIN_TRACK; | ||
1522 | long peak = (track_mode || !album_peak) ? track_peak : album_peak; | ||
1523 | |||
1524 | if (global_settings.replaygain_type != REPLAYGAIN_OFF) | ||
1525 | { | ||
1526 | gain = (track_mode || !album_gain) ? track_gain : album_gain; | ||
1527 | |||
1528 | if (global_settings.replaygain_preamp) | ||
1529 | { | ||
1530 | long preamp = get_replaygain_int( | ||
1531 | global_settings.replaygain_preamp * 10); | ||
1532 | |||
1533 | gain = (long) (((int64_t) gain * preamp) >> 24); | ||
1534 | } | ||
1535 | } | ||
1536 | |||
1537 | if (gain == 0) | ||
1538 | { | ||
1539 | /* So that noclip can work even with no gain information. */ | ||
1540 | gain = DEFAULT_GAIN; | ||
1541 | } | ||
1542 | |||
1543 | if (global_settings.replaygain_noclip && (peak != 0) | ||
1544 | && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN)) | ||
1545 | { | ||
1546 | gain = (((int64_t) DEFAULT_GAIN << 24) / peak); | ||
1547 | } | ||
1548 | |||
1549 | if (gain == DEFAULT_GAIN) | ||
1550 | { | ||
1551 | /* Nothing to do, disable processing. */ | ||
1552 | gain = 0; | ||
1553 | } | ||
1554 | } | ||
1555 | |||
1556 | /* Store in S7.24 format to simplify calculations. */ | ||
1557 | replaygain = gain; | ||
1558 | set_gain(&AUDIO_DSP); | ||
1559 | } | ||
1560 | |||
1561 | /** SET COMPRESSOR | ||
1562 | * Called by the menu system to configure the compressor process */ | ||
1563 | void dsp_set_compressor(const struct compressor_settings *settings) | ||
1564 | { | ||
1565 | /* enable/disable the compressor */ | ||
1566 | AUDIO_DSP.compressor_process = compressor_update(settings) ? | ||
1567 | compressor_process : NULL; | ||
1568 | } | ||
diff --git a/lib/rbcodec/dsp/dsp.h b/lib/rbcodec/dsp/dsp.h index a99df17468..feac4aa845 100644 --- a/lib/rbcodec/dsp/dsp.h +++ b/lib/rbcodec/dsp/dsp.h | |||
@@ -18,109 +18,159 @@ | |||
18 | * KIND, either express or implied. | 18 | * KIND, either express or implied. |
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | |||
22 | #ifndef _DSP_H | 21 | #ifndef _DSP_H |
23 | #define _DSP_H | 22 | #define _DSP_H |
24 | 23 | ||
25 | #include <stdlib.h> | 24 | struct dsp_config; |
26 | #include <stdbool.h> | ||
27 | 25 | ||
28 | #define NATIVE_FREQUENCY 44100 | 26 | /* Include all this junk here for now */ |
27 | #include "dsp_proc_settings.h" | ||
29 | 28 | ||
30 | enum | 29 | enum dsp_ids |
31 | { | 30 | { |
32 | STEREO_INTERLEAVED = 0, | 31 | CODEC_IDX_AUDIO, |
33 | STEREO_NONINTERLEAVED, | ||
34 | STEREO_MONO, | ||
35 | STEREO_NUM_MODES, | ||
36 | }; | ||
37 | |||
38 | enum | ||
39 | { | ||
40 | CODEC_IDX_AUDIO = 0, | ||
41 | CODEC_IDX_VOICE, | 32 | CODEC_IDX_VOICE, |
33 | DSP_COUNT, | ||
42 | }; | 34 | }; |
43 | 35 | ||
44 | enum | 36 | enum dsp_settings |
45 | { | 37 | { |
46 | DSP_MYDSP = 1, | 38 | DSP_INIT, /* For dsp_init */ |
39 | DSP_RESET, | ||
47 | DSP_SET_FREQUENCY, | 40 | DSP_SET_FREQUENCY, |
48 | DSP_SWITCH_FREQUENCY, | 41 | DSP_SWITCH_FREQUENCY = DSP_SET_FREQUENCY, /* deprecated */ |
49 | DSP_SET_SAMPLE_DEPTH, | 42 | DSP_SET_SAMPLE_DEPTH, |
50 | DSP_SET_STEREO_MODE, | 43 | DSP_SET_STEREO_MODE, |
51 | DSP_RESET, | ||
52 | DSP_FLUSH, | 44 | DSP_FLUSH, |
53 | DSP_SET_TRACK_GAIN, | 45 | DSP_PROC_INIT, |
54 | DSP_SET_ALBUM_GAIN, | 46 | DSP_PROC_CLOSE, |
55 | DSP_SET_TRACK_PEAK, | 47 | DSP_PROC_SETTING, /* stage-specific should be this + id */ |
56 | DSP_SET_ALBUM_PEAK, | ||
57 | DSP_CROSSFEED | ||
58 | }; | 48 | }; |
59 | 49 | ||
50 | #define NATIVE_FREQUENCY 44100 /* internal/output sample rate */ | ||
60 | 51 | ||
61 | /**************************************************************************** | 52 | enum dsp_stereo_modes |
62 | * NOTE: Any assembly routines that use these structures must be updated | ||
63 | * if current data members are moved or changed. | ||
64 | */ | ||
65 | struct resample_data | ||
66 | { | 53 | { |
67 | uint32_t delta; /* 00h */ | 54 | STEREO_INTERLEAVED, |
68 | uint32_t phase; /* 04h */ | 55 | STEREO_NONINTERLEAVED, |
69 | int32_t last_sample[2]; /* 08h */ | 56 | STEREO_MONO, |
70 | /* 10h */ | 57 | STEREO_NUM_MODES, |
71 | }; | 58 | }; |
72 | 59 | ||
73 | /* This is for passing needed data to external dsp routines. If another | 60 | /* Format into for the buffer (if .valid == true) */ |
74 | * dsp parameter needs to be passed, add to the end of the structure | 61 | struct sample_format |
75 | * and remove from dsp_config. | ||
76 | * If another function type becomes assembly/external and requires dsp | ||
77 | * config info, add a pointer paramter of type "struct dsp_data *". | ||
78 | * If removing something from other than the end, reserve the spot or | ||
79 | * else update every implementation for every target. | ||
80 | * Be sure to add the offset of the new member for easy viewing as well. :) | ||
81 | * It is the first member of dsp_config and all members can be accessesed | ||
82 | * through the main aggregate but this is intended to make a safe haven | ||
83 | * for these items whereas the c part can be rearranged at will. dsp_data | ||
84 | * could even moved within dsp_config without disurbing the order. | ||
85 | */ | ||
86 | struct dsp_data | ||
87 | { | 62 | { |
88 | int output_scale; /* 00h */ | 63 | uint8_t changed; /* 00h: 0=no change, 1=changed (is also index) */ |
89 | int num_channels; /* 04h */ | 64 | uint8_t num_channels; /* 01h: number of channels of data */ |
90 | struct resample_data resample_data; /* 08h */ | 65 | uint8_t frac_bits; /* 02h: number of fractional bits */ |
91 | int32_t clip_min; /* 18h */ | 66 | uint8_t output_scale; /* 03h: output scaling shift */ |
92 | int32_t clip_max; /* 1ch */ | 67 | int32_t frequency; /* 04h: pitch-adjusted sample rate */ |
93 | int32_t gain; /* 20h - Note that this is in S8.23 format. */ | 68 | int32_t codec_frequency; /* 08h: codec-specifed sample rate */ |
94 | int frac_bits; /* 24h */ | 69 | /* 0ch */ |
95 | /* 28h */ | ||
96 | }; | 70 | }; |
97 | 71 | ||
98 | struct dsp_config; | 72 | /* Compare format data only */ |
73 | #define EQU_SAMPLE_FORMAT(f1, f2) \ | ||
74 | (!memcmp(&(f1).num_channels, &(f2).num_channels, \ | ||
75 | sizeof (f1) - sizeof ((f1).changed))) | ||
76 | |||
77 | static inline void format_change_set(struct sample_format *f) | ||
78 | { f->changed = 1; } | ||
79 | static inline void format_change_ack(struct sample_format *f) | ||
80 | { f->changed = 0; } | ||
99 | 81 | ||
100 | int dsp_process(struct dsp_config *dsp, char *dest, | 82 | /* Used by ASM routines - keep field order or else fix the functions */ |
101 | const char *src[], int count); | 83 | struct dsp_buffer |
102 | int dsp_input_count(struct dsp_config *dsp, int count); | 84 | { |
103 | int dsp_output_count(struct dsp_config *dsp, int count); | 85 | int32_t remcount; /* 00h: Samples in buffer (In, Int, Out) */ |
104 | intptr_t dsp_configure(struct dsp_config *dsp, int setting, | 86 | union |
87 | { | ||
88 | const void *pin[2]; /* 04h: Channel pointers (In) */ | ||
89 | int32_t *p32[2]; /* 04h: Channel pointers (Int) */ | ||
90 | int16_t *p16out; /* 04h: DSP output buffer (Out) */ | ||
91 | }; | ||
92 | union | ||
93 | { | ||
94 | uint32_t proc_mask; /* 0Ch: In-place effects already appled to buffer | ||
95 | in order to avoid double-processing. Set | ||
96 | to zero on new buffer before passing to | ||
97 | DSP. */ | ||
98 | int bufcount; /* 0Ch: Buffer length/dest buffer remaining | ||
99 | Basically, pay no attention unless it's | ||
100 | *your* new buffer and is used internally | ||
101 | or is specifically the final output | ||
102 | buffer. */ | ||
103 | }; | ||
104 | struct sample_format format; /* 10h: Buffer format data */ | ||
105 | /* 1ch */ | ||
106 | }; | ||
107 | |||
108 | /* Remove samples from input buffer (In). Sample size is specified. | ||
109 | Provided to dsp_process(). */ | ||
110 | static inline void dsp_advance_buffer_input(struct dsp_buffer *buf, | ||
111 | int by_count, | ||
112 | size_t size_each) | ||
113 | { | ||
114 | buf->remcount -= by_count; | ||
115 | buf->pin[0] += by_count * size_each; | ||
116 | buf->pin[1] += by_count * size_each; | ||
117 | } | ||
118 | |||
119 | /* Add samples to output buffer and update remaining space (Out). | ||
120 | Provided to dsp_process() */ | ||
121 | static inline void dsp_advance_buffer_output(struct dsp_buffer *buf, | ||
122 | int by_count) | ||
123 | { | ||
124 | buf->bufcount -= by_count; | ||
125 | buf->remcount += by_count; | ||
126 | buf->p16out += 2 * by_count; /* Interleaved stereo */ | ||
127 | } | ||
128 | |||
129 | /* Remove samples from internal input buffer (In, Int). | ||
130 | Provided to dsp_process() or by another processing stage. */ | ||
131 | static inline void dsp_advance_buffer32(struct dsp_buffer *buf, | ||
132 | int by_count) | ||
133 | { | ||
134 | buf->remcount -= by_count; | ||
135 | buf->p32[0] += by_count; | ||
136 | buf->p32[1] += by_count; | ||
137 | } | ||
138 | |||
139 | /** For use by processing stages **/ | ||
140 | |||
141 | #define DSP_PRINT_FORMAT(name, id, format) \ | ||
142 | DEBUGF("DSP format- " #name "\n" \ | ||
143 | " id:%d chg:%c ch:%u fb:%u os:%u hz:%u chz:%u\n", \ | ||
144 | (int)id, \ | ||
145 | (format).changed ? 'y' : 'n', \ | ||
146 | (unsigned int)(format).num_channels, \ | ||
147 | (unsigned int)(format).frac_bits, \ | ||
148 | (unsigned int)(format).output_scale, \ | ||
149 | (unsigned int)(format).frequency, \ | ||
150 | (unsigned int)(format).codec_frequency); | ||
151 | |||
152 | /* Get DSP pointer */ | ||
153 | struct dsp_config * dsp_get_config(enum dsp_ids id); | ||
154 | |||
155 | /* Get DSP id */ | ||
156 | enum dsp_ids dsp_get_id(const struct dsp_config *dsp); | ||
157 | |||
158 | #if 0 /* Not needed now but enable if something must know this */ | ||
159 | /* Is the DSP processing a buffer? */ | ||
160 | bool dsp_is_busy(const struct dsp_config *dsp); | ||
161 | #endif /* 0 */ | ||
162 | |||
163 | /** General DSP processing **/ | ||
164 | |||
165 | /* Process the given buffer - see implementation in dsp.c for more */ | ||
166 | void dsp_process(struct dsp_config *dsp, struct dsp_buffer *src, | ||
167 | struct dsp_buffer *dst); | ||
168 | |||
169 | /* Change DSP settings */ | ||
170 | intptr_t dsp_configure(struct dsp_config *dsp, unsigned int setting, | ||
105 | intptr_t value); | 171 | intptr_t value); |
106 | int get_replaygain_mode(bool have_track_gain, bool have_album_gain); | 172 | |
107 | void dsp_set_replaygain(void); | 173 | /* One-time startup init that must come before settings reset/apply */ |
108 | void dsp_set_crossfeed(bool enable); | 174 | void dsp_init(void); |
109 | void dsp_set_crossfeed_direct_gain(int gain); | 175 | |
110 | void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, | 176 | #endif /* _DSP_H */ |
111 | long cutoff); | ||
112 | void dsp_set_eq(bool enable); | ||
113 | void dsp_set_eq_precut(int precut); | ||
114 | void dsp_set_eq_coefs(int band, int cutoff, int q, int gain); | ||
115 | void dsp_dither_enable(bool enable); | ||
116 | void dsp_timestretch_enable(bool enable); | ||
117 | bool dsp_timestretch_available(void); | ||
118 | void sound_set_pitch(int32_t r); | ||
119 | int32_t sound_get_pitch(void); | ||
120 | void dsp_set_timestretch(int32_t percent); | ||
121 | int32_t dsp_get_timestretch(void); | ||
122 | int dsp_callback(int msg, intptr_t param); | ||
123 | struct compressor_settings; | ||
124 | void dsp_set_compressor(const struct compressor_settings *settings); | ||
125 | |||
126 | #endif | ||
diff --git a/lib/rbcodec/dsp/dsp_arm.S b/lib/rbcodec/dsp/dsp_arm.S index 685aca411c..9fd19ae108 100644 --- a/lib/rbcodec/dsp/dsp_arm.S +++ b/lib/rbcodec/dsp/dsp_arm.S | |||
@@ -21,20 +21,19 @@ | |||
21 | #include "config.h" | 21 | #include "config.h" |
22 | 22 | ||
23 | /**************************************************************************** | 23 | /**************************************************************************** |
24 | * void channels_process_sound_chan_mono(int count, int32_t *buf[]) | 24 | * void channel_mode_proc_mono(struct dsp_proc_entry *this, |
25 | * struct dsp_buffer **buf_p) | ||
25 | */ | 26 | */ |
26 | 27 | .section .icode | |
27 | #include "config.h" | 28 | .global channel_mode_proc_mono |
28 | 29 | .type channel_mode_proc_mono, %function | |
29 | .section .icode, "ax", %progbits | 30 | channel_mode_proc_mono: |
30 | .align 2 | 31 | @ input: r0 = this, r1 = buf_p |
31 | .global channels_process_sound_chan_mono | 32 | ldr r1, [r1] @ r1 = buf = *buf_p; |
32 | .type channels_process_sound_chan_mono, %function | ||
33 | channels_process_sound_chan_mono: | ||
34 | @ input: r0 = count, r1 = buf | ||
35 | stmfd sp!, { r4, lr } @ | 33 | stmfd sp!, { r4, lr } @ |
36 | @ | 34 | @ |
37 | ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] | 35 | ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0], |
36 | @ r2 = buf->p32[1] | ||
38 | subs r0, r0, #1 @ odd: end at 0; even: end at -1 | 37 | subs r0, r0, #1 @ odd: end at 0; even: end at -1 |
39 | beq .mono_singlesample @ Zero? Only one sample! | 38 | beq .mono_singlesample @ Zero? Only one sample! |
40 | @ | 39 | @ |
@@ -61,25 +60,26 @@ channels_process_sound_chan_mono: | |||
61 | str r12, [r2] @ store Mo | 60 | str r12, [r2] @ store Mo |
62 | @ | 61 | @ |
63 | ldmpc regs=r4 @ | 62 | ldmpc regs=r4 @ |
64 | .size channels_process_sound_chan_mono, \ | 63 | .size channel_mode_proc_mono, .-channel_mode_proc_mono |
65 | .-channels_process_sound_chan_mono | ||
66 | 64 | ||
67 | /**************************************************************************** | 65 | /**************************************************************************** |
68 | * void channels_process_sound_chan_custom(int count, int32_t *buf[]) | 66 | * void channel_mode_proc_custom(struct dsp_proc_entry *this, |
67 | * struct dsp_buffer **buf_p) | ||
69 | */ | 68 | */ |
70 | .section .icode, "ax", %progbits | 69 | .section .icode |
71 | .align 2 | 70 | .global channel_mode_proc_custom |
72 | .global channels_process_sound_chan_custom | 71 | .type channel_mode_proc_custom, %function |
73 | .type channels_process_sound_chan_custom, %function | 72 | channel_mode_proc_custom: |
74 | channels_process_sound_chan_custom: | 73 | @ input: r0 = this, r1 = buf_p |
74 | ldr r2, [r0] @ r2 = &channel_mode_data = this->data | ||
75 | ldr r1, [r1] @ r1 = buf = *buf_p; | ||
76 | |||
75 | stmfd sp!, { r4-r10, lr } | 77 | stmfd sp!, { r4-r10, lr } |
76 | 78 | ||
77 | ldr r3, =dsp_sw_gain | 79 | ldmia r2, { r3, r4 } @ r3 = sw_gain, r4 = sw_cross |
78 | ldr r4, =dsp_sw_cross | ||
79 | 80 | ||
80 | ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] | 81 | ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0], |
81 | ldr r3, [r3] @ r3 = dsp_sw_gain | 82 | @ r2 = buf->p32[1] |
82 | ldr r4, [r4] @ r4 = dsp_sw_cross | ||
83 | 83 | ||
84 | subs r0, r0, #1 | 84 | subs r0, r0, #1 |
85 | beq .custom_single_sample @ Zero? Only one sample! | 85 | beq .custom_single_sample @ Zero? Only one sample! |
@@ -135,21 +135,22 @@ channels_process_sound_chan_custom: | |||
135 | str r7, [r2] @ Store Rc0 | 135 | str r7, [r2] @ Store Rc0 |
136 | 136 | ||
137 | ldmpc regs=r4-r10 | 137 | ldmpc regs=r4-r10 |
138 | .size channels_process_sound_chan_custom, \ | 138 | .size channel_mode_proc_custom, .-channel_mode_proc_custom |
139 | .-channels_process_sound_chan_custom | ||
140 | 139 | ||
141 | /**************************************************************************** | 140 | /**************************************************************************** |
142 | * void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) | 141 | * void channel_mode_proc_karaoke(struct dsp_proc_entry *this, |
142 | * struct dsp_buffer **buf_p) | ||
143 | */ | 143 | */ |
144 | .section .icode, "ax", %progbits | 144 | .section .icode |
145 | .align 2 | 145 | .global channel_mode_proc_karaoke |
146 | .global channels_process_sound_chan_karaoke | 146 | .type channel_mode_proc_karaoke, %function |
147 | .type channels_process_sound_chan_karaoke, %function | 147 | channel_mode_proc_karaoke: |
148 | channels_process_sound_chan_karaoke: | 148 | @ input: r0 = this, r1 = buf_p |
149 | @ input: r0 = count, r1 = buf | 149 | ldr r1, [r1] @ r1 = buf = *buf_p; |
150 | stmfd sp!, { r4, lr } @ | 150 | stmfd sp!, { r4, lr } @ |
151 | @ | 151 | @ |
152 | ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1] | 152 | ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0], |
153 | @ r2 = buf->p32[1] | ||
153 | subs r0, r0, #1 @ odd: end at 0; even: end at -1 | 154 | subs r0, r0, #1 @ odd: end at 0; even: end at -1 |
154 | beq .karaoke_singlesample @ Zero? Only one sample! | 155 | beq .karaoke_singlesample @ Zero? Only one sample! |
155 | @ | 156 | @ |
@@ -179,24 +180,313 @@ channels_process_sound_chan_karaoke: | |||
179 | str r12, [r2] @ store Ro | 180 | str r12, [r2] @ store Ro |
180 | @ | 181 | @ |
181 | ldmpc regs=r4 @ | 182 | ldmpc regs=r4 @ |
182 | .size channels_process_sound_chan_karaoke, \ | 183 | .size channel_mode_proc_karaoke, .-channel_mode_proc_karaoke |
183 | .-channels_process_sound_chan_karaoke | 184 | |
185 | /**************************************************************************** | ||
186 | * void crossfeed_process(struct dsp_proc_entry *this, | ||
187 | * struct dsp_buffer **buf_p) | ||
188 | */ | ||
189 | .section .text | ||
190 | .global crossfeed_process | ||
191 | crossfeed_process: | ||
192 | @ input: r0 = this, r1 = buf_p | ||
193 | @ unfortunately, we ended up in a bit of a register squeeze here, and need | ||
194 | @ to keep the count on the stack :/ | ||
195 | ldr r1, [r1] @ r1 = buf = *buf_p; | ||
196 | stmfd sp!, { r4-r11, lr } @ stack modified regs | ||
197 | ldr r12, [r1] @ r12 = buf->remcount | ||
198 | ldr r14, [r0] @ r14 = this->data = &crossfeed_state | ||
199 | ldmib r1, { r2-r3 } @ r2 = buf->p32[0], r3 = buf->p32[1] | ||
200 | ldmia r14!, { r4-r11 } @ load direct gain and filter data | ||
201 | add r0, r14, #13*2*4 @ calculate end of delay | ||
202 | stmfd sp!, { r0, r12 } @ stack end of delay adr, count and state | ||
203 | ldr r0, [r0] @ fetch current delay line address | ||
204 | |||
205 | /* Register usage in loop: | ||
206 | * r0 = &delay[index][0], r1 = accumulator high, r2 = buf->p32[0], | ||
207 | * r3 = buf->p32[1], r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs), | ||
208 | * r8-r11 = filter history, r12 = temp, r14 = accumulator low | ||
209 | */ | ||
210 | .cfloop: | ||
211 | smull r14, r1, r6, r8 @ acc = b1*dr[n - 1] | ||
212 | smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1] | ||
213 | ldr r8, [r0, #4] @ r8 = dr[n] | ||
214 | smlal r14, r1, r5, r8 @ acc += b0*dr[n] | ||
215 | mov r9, r1, lsl #1 @ fix format for filter history | ||
216 | ldr r12, [r2] @ load left input | ||
217 | smlal r14, r1, r4, r12 @ acc += gain*x_l[n] | ||
218 | mov r1, r1, lsl #1 @ fix format | ||
219 | str r1, [r2], #4 @ save result | ||
220 | |||
221 | smull r14, r1, r6, r10 @ acc = b1*dl[n - 1] | ||
222 | smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1] | ||
223 | ldr r10, [r0] @ r10 = dl[n] | ||
224 | str r12, [r0], #4 @ save left input to delay line | ||
225 | smlal r14, r1, r5, r10 @ acc += b0*dl[n] | ||
226 | mov r11, r1, lsl #1 @ fix format for filter history | ||
227 | ldr r12, [r3] @ load right input | ||
228 | smlal r14, r1, r4, r12 @ acc += gain*x_r[n] | ||
229 | str r12, [r0], #4 @ save right input to delay line | ||
230 | mov r1, r1, lsl #1 @ fix format | ||
231 | ldmia sp, { r12, r14 } @ fetch delay line end addr and count from stack | ||
232 | str r1, [r3], #4 @ save result | ||
233 | |||
234 | cmp r0, r12 @ need to wrap to start of delay? | ||
235 | subhs r0, r12, #13*2*4 @ wrap back delay line ptr to start | ||
236 | |||
237 | subs r14, r14, #1 @ are we finished? | ||
238 | strgt r14, [sp, #4] @ nope, save count back to stack | ||
239 | bgt .cfloop | ||
240 | |||
241 | @ save data back to struct | ||
242 | str r0, [r12] @ save delay line index | ||
243 | sub r12, r12, #13*2*4 + 4*4 @ r12 = data->history | ||
244 | stmia r12, { r8-r11 } @ save filter history | ||
245 | add sp, sp, #8 @ remove temp variables from stack | ||
246 | ldmpc regs=r4-r11 | ||
247 | .size crossfeed_process, .-crossfeed_process | ||
248 | |||
249 | /**************************************************************************** | ||
250 | * int lin_resample_resample(struct resample_data *data, | ||
251 | * struct dsp_buffer *src, | ||
252 | * struct dsp_buffer *dst) | ||
253 | */ | ||
254 | .section .text | ||
255 | .global lin_resample_resample | ||
256 | lin_resample_resample: | ||
257 | @input: r0 = data, r1 = src, r2 = dst | ||
258 | stmfd sp!, { r4-r11, lr } @ stack modified regs | ||
259 | ldr r4, [r0] @ r4 = data->delta | ||
260 | add r10, r0, #4 @ r10 = &data->phase | ||
261 | ldrb r3, [r1, #17] @ r3 = num_channels, | ||
262 | stmfd sp!, { r1, r10 } @ stack src, &data->phase | ||
263 | .lrs_channel_loop: | ||
264 | ldr r5, [r10] @ r5 = data->phase | ||
265 | ldr r6, [r1] @ r6 = srcrem = src->remcount | ||
266 | ldr r7, [r1, r3, lsl #2] @ r7 = src->p32[ch] | ||
267 | ldr r8, [r2, r3, lsl #2] @ r8 = dst->p32[ch] | ||
268 | ldr r9, [r2, #12] @ r9 = dstrem = dst->bufcount | ||
269 | |||
270 | cmp r6, #0x8000 @ srcrem = MIN(srcrem, 0x8000) | ||
271 | movgt r6, #0x8000 @ | ||
272 | mov r0, r5, lsr #16 @ pos = MIN(pos, srcrem) | ||
273 | cmp r0, r6 @ | ||
274 | movgt r0, r6 @ r0 = pos = phase >> 16 | ||
275 | cmp r0, #0 @ | ||
276 | ldrle r11, [r10, r3, lsl #2] @ pos <= 0? r11 = last = last_sample[ch] | ||
277 | addgt r12, r7, r0, lsl #2 @ pos > 0? r1 = last = s[pos - 1] | ||
278 | ldrgt r11, [r12, #-4] @ | ||
279 | cmp r0, r6 @ | ||
280 | bge .lrs_channel_done @ pos >= count? channel complete | ||
281 | |||
282 | cmp r4, #0x10000 @ delta >= 1.0? | ||
283 | ldrhs r12, [r7, r0, lsl #2] @ yes? r12 = s[pos] | ||
284 | bhs .lrs_dsstart @ yes? is downsampling | ||
285 | |||
286 | /** Upsampling **/ | ||
287 | mov r5, r5, lsl #16 @ Move phase into high halfword | ||
288 | add r7, r7, r0, lsl #2 @ r7 = &s[pos] | ||
289 | sub r0, r6, r0 @ r0 = dte = srcrem - pos | ||
290 | .lrs_usloop_1: | ||
291 | ldr r12, [r7], #4 @ r12 = s[pos] | ||
292 | sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1] | ||
293 | .lrs_usloop_0: | ||
294 | mov r1, r5, lsr #16 @ r1 = frac = phase >> 16 | ||
295 | @ keep frac in Rs to take advantage of multiplier early termination | ||
296 | smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi) | ||
297 | add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff | ||
298 | add r1, r1, r10, lsl #16 @ | ||
299 | str r1, [r8], #4 @ *d++ = out | ||
300 | subs r9, r9, #1 @ destination full? | ||
301 | bls .lrs_usfull @ yes? channel is done | ||
302 | adds r5, r5, r4, lsl #16 @ phase += delta << 16 | ||
303 | bcc .lrs_usloop_0 @ if carry is set, pos is incremented | ||
304 | subs r0, r0, #1 @ if srcrem > 0, do another sample | ||
305 | mov r11, r12 @ r11 = last = s[pos-1] (pos changed) | ||
306 | bgt .lrs_usloop_1 | ||
307 | b .lrs_usdone | ||
308 | |||
309 | .lrs_usfull: | ||
310 | adds r5, r5, r4, lsl #16 @ do missed phase increment | ||
311 | subcs r0, r0, #1 @ do missed srcrem decrement | ||
312 | movcs r11, r12 @ r11 = s[pos-1] (pos changed) | ||
313 | |||
314 | .lrs_usdone: | ||
315 | sub r0, r6, r0 @ r0 = pos = srcrem - dte | ||
316 | orr r5, r5, r0 @ reconstruct swapped phase | ||
317 | mov r5, r5, ror #16 @ swap pos and frac for phase | ||
318 | b .lrs_channel_done @ | ||
319 | |||
320 | /** Downsampling **/ | ||
321 | .lrs_dsloop: | ||
322 | add r10, r7, r0, lsl #2 @ r10 = &s[pos] | ||
323 | ldmda r10, { r11, r12 } @ r11 = last, r12 = s[pos] | ||
324 | .lrs_dsstart: | ||
325 | sub r14, r12, r11 @ r14 = diff = s[pos] - s[pos - 1] | ||
326 | @ keep frac in Rs to take advantage of multiplier early termination | ||
327 | bic r1, r5, r0, lsl #16 @ frac = phase & 0xffff | ||
328 | smull r1, r10, r14, r1 @ r1, r10 = diff * frac (lo, hi) | ||
329 | add r5, r5, r4 @ phase += delta | ||
330 | subs r9, r9, #1 @ destination full? ... | ||
331 | mov r0, r5, lsr #16 @ pos = phase >> 16 | ||
332 | add r1, r11, r1, lsr #16 @ r1 = out = last + frac*diff | ||
333 | add r1, r1, r10, lsl #16 @ | ||
334 | str r1, [r8], #4 @ *d++ = out | ||
335 | cmpgt r6, r0 @ ... || pos >= srcrem? ... | ||
336 | bgt .lrs_dsloop @ ... no, do more samples | ||
337 | |||
338 | cmp r0, r6 @ pos = MIN(pos, srcrem) | ||
339 | movgt r0, r6 @ | ||
340 | sub r1, r0, #1 @ pos must always be > 0 since step >= 1.0 | ||
341 | ldr r11, [r7, r1, lsl #2] @ r11 = s[pos - 1] | ||
342 | |||
343 | .lrs_channel_done: | ||
344 | ldmia sp, { r1, r10 } @ recover src, &data->phase | ||
345 | str r11, [r10, r3, lsl #2] @ last_sample[ch] = last | ||
346 | subs r3, r3, #1 @ | ||
347 | bgt .lrs_channel_loop @ | ||
348 | |||
349 | ldr r6, [r2, #12] @ r6 = dst->bufcount | ||
350 | sub r5, r5, r0, lsl #16 @ r5 = phase - (pos << 16) | ||
351 | str r5, [r10] @ data->phase = r5 | ||
352 | sub r6, r6, r9 @ r6 = dst->bufcount - dstrem = dstcount | ||
353 | str r6, [r2] @ dst->remcount = dstcount | ||
354 | add sp, sp, #8 @ adjust stack for temp variables | ||
355 | ldmpc regs=r4-r11 @ ... and we're out | ||
356 | .size lin_resample_resample, .-lin_resample_resample | ||
357 | |||
358 | /**************************************************************************** | ||
359 | * void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) | ||
360 | */ | ||
361 | .section .icode | ||
362 | .global pga_process | ||
363 | .type pga_process, %function | ||
364 | pga_process: | ||
365 | @ input: r0 = this, r1 = buf_p | ||
366 | ldr r0, [r0] @ r0 = data = this->data (&pga_data) | ||
367 | ldr r1, [r1] @ r1 = buf = *buf_p; | ||
368 | stmfd sp!, { r4-r8, lr } | ||
369 | |||
370 | ldr r4, [r0] @ r4 = data->gain | ||
371 | ldr r0, [r1], #4 @ r0 = buf->remcount, r1 = buf->p32 | ||
372 | ldrb r3, [r1, #13] @ r3 = buf->format.num_channels | ||
373 | |||
374 | .pga_channelloop: | ||
375 | ldr r2, [r1], #4 @ r2 = buf->p32[ch] and inc index of p32 | ||
376 | subs r12, r0, #1 @ r12 = count - 1 | ||
377 | beq .pga_singlesample @ Zero? Only one sample! | ||
378 | |||
379 | .pga_loop: | ||
380 | ldmia r2, { r5, r6 } @ load r5, r6 from r2 (*p32[ch]) | ||
381 | smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8) | ||
382 | smull r14, r5, r6, r4 @ r14 = FRACMUL_SHL(r6, r4, 8) | ||
383 | subs r12, r12, #2 | ||
384 | mov r7, r7, lsr #23 | ||
385 | mov r14, r14, lsr #23 | ||
386 | orr r7, r7, r8, asl #9 | ||
387 | orr r14, r14, r5, asl #9 | ||
388 | stmia r2!, { r7, r14 } @ save r7, r14 to *p32[ch] and increment | ||
389 | bgt .pga_loop @ end of pga loop | ||
390 | |||
391 | blt .pga_evencount @ < 0? even count | ||
392 | |||
393 | .pga_singlesample: | ||
394 | ldr r5, [r2] @ handle odd sample | ||
395 | smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8) | ||
396 | mov r7, r7, lsr #23 | ||
397 | orr r7, r7, r8, asl #9 | ||
398 | str r7, [r2] | ||
399 | |||
400 | .pga_evencount: | ||
401 | subs r3, r3, #1 | ||
402 | bgt .pga_channelloop @ end of channel loop | ||
403 | |||
404 | ldmpc regs=r4-r8 | ||
405 | .size pga_process, .-pga_process | ||
406 | |||
407 | /**************************************************************************** | ||
408 | * void filter_process(struct dsp_filter *f, int32_t *buf[], int count, | ||
409 | * unsigned int channels) | ||
410 | * | ||
411 | * define HIGH_PRECISION as '1' to make filtering calculate lower bits after | ||
412 | * shifting. without this, "shift" - 1 of the lower bits will be lost here. | ||
413 | */ | ||
414 | #define HIGH_PRECISION 0 | ||
415 | |||
416 | #if CONFIG_CPU == PP5002 | ||
417 | .section .icode,"ax",%progbits | ||
418 | #else | ||
419 | .text | ||
420 | #endif | ||
421 | .global filter_process | ||
422 | filter_process: | ||
423 | @input: r0 = f, r1 = buf, r2 = count, r3 = channels | ||
424 | stmfd sp!, { r4-r11, lr } @ save all clobbered regs | ||
425 | ldmia r0!, { r4-r8 } @ load coefs, r0 = f->history | ||
426 | sub r3, r3, #1 @ r3 = ch = channels - 1 | ||
427 | stmfd sp!, { r0-r3 } @ save adjusted params | ||
428 | ldrb r14, [r0, #32] @ r14 = shift | ||
429 | |||
430 | @ Channels are processed high to low while history is saved low to high | ||
431 | @ It's really noone's business how we do this | ||
432 | .fp_channelloop: | ||
433 | ldmia r0, { r9-r12 } @ load history, r0 = history[channels-ch-1] | ||
434 | ldr r3, [r1, r3, lsl #2] @ r3 = buf[ch] | ||
435 | |||
436 | @ r9-r12 = history, r4-r8 = coefs, r0..r1 = accumulator, | ||
437 | @ r2 = number of samples, r3 = buf[ch], r14 = shift amount | ||
438 | .fp_loop: | ||
439 | @ Direct form 1 filtering code. | ||
440 | @ y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | ||
441 | @ where y[] is output and x[] is input. This is performed out of order to | ||
442 | @ reuse registers, we're pretty short on regs. | ||
443 | smull r0, r1, r5, r9 @ acc = b1*x[i - 1] | ||
444 | smlal r0, r1, r6, r10 @ acc += b2*x[i - 2] | ||
445 | mov r10, r9 @ fix input history | ||
446 | ldr r9, [r3] @ load input and fix history | ||
447 | smlal r0, r1, r7, r11 @ acc += a1*y[i - 1] | ||
448 | smlal r0, r1, r8, r12 @ acc += a2*y[i - 2] | ||
449 | smlal r0, r1, r4, r9 @ acc += b0*x[i] /* avoid stall on arm9 */ | ||
450 | mov r12, r11 @ fix output history | ||
451 | mov r11, r1, asl r14 @ get upper part of result and shift left | ||
452 | #if HIGH_PRECISION | ||
453 | rsb r1, r14, #32 @ get shift amount for lower part | ||
454 | orr r11, r11, r0, lsr r1 @ then mix in correctly shifted lower part | ||
455 | #endif | ||
456 | str r11, [r3], #4 @ save result | ||
457 | subs r2, r2, #1 @ are we done with this channel? | ||
458 | bgt .fp_loop @ | ||
459 | |||
460 | ldr r3, [sp, #12] @ r3 = ch | ||
461 | ldr r0, [sp] @ r0 = history[channels-ch-1] | ||
462 | subs r3, r3, #1 @ all channels processed? | ||
463 | stmia r0!, { r9-r12 } @ save back history, history++ | ||
464 | ldmhsib sp, { r1-r2 } @ r1 = buf, r2 = count | ||
465 | strhs r3, [sp, #12] @ store ch | ||
466 | strhs r0, [sp] @ store history[channels-ch-1] | ||
467 | bhs .fp_channelloop | ||
468 | |||
469 | add sp, sp, #16 @ compensate for temp storage | ||
470 | ldmpc regs=r4-r11 | ||
471 | .size filter_process, .-filter_process | ||
184 | 472 | ||
185 | #if ARM_ARCH < 6 | 473 | #if ARM_ARCH < 6 |
186 | /**************************************************************************** | 474 | /**************************************************************************** |
187 | * void sample_output_mono(int count, struct dsp_data *data, | 475 | * void sample_output_mono(struct sample_io_data *this, |
188 | * const int32_t *src[], int16_t *dst) | 476 | * struct dsp_buffer *src, |
477 | * struct dsp_buffer *dst) | ||
189 | */ | 478 | */ |
190 | .section .icode, "ax", %progbits | 479 | .section .icode |
191 | .align 2 | ||
192 | .global sample_output_mono | 480 | .global sample_output_mono |
193 | .type sample_output_mono, %function | 481 | .type sample_output_mono, %function |
194 | sample_output_mono: | 482 | sample_output_mono: |
195 | @ input: r0 = count, r1 = data, r2 = src, r3 = dst | 483 | @ input: r0 = this, r1 = src, r2 = dst |
196 | stmfd sp!, { r4-r6, lr } | 484 | stmfd sp!, { r4-r6, lr } |
197 | 485 | ||
198 | ldr r1, [r1] @ lr = data->output_scale | 486 | ldr r0, [r0] @ r0 = this->outcount |
199 | ldr r2, [r2] @ r2 = src[0] | 487 | ldr r3, [r2, #4] @ r2 = dst->p16out |
488 | ldr r2, [r1, #4] @ r1 = src->p32[0] | ||
489 | ldrb r1, [r1, #19] @ r2 = src->format.output_scale | ||
200 | 490 | ||
201 | mov r4, #1 | 491 | mov r4, #1 |
202 | mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) | 492 | mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) |
@@ -246,19 +536,21 @@ sample_output_mono: | |||
246 | .size sample_output_mono, .-sample_output_mono | 536 | .size sample_output_mono, .-sample_output_mono |
247 | 537 | ||
248 | /**************************************************************************** | 538 | /**************************************************************************** |
249 | * void sample_output_stereo(int count, struct dsp_data *data, | 539 | * void sample_output_stereo(struct sample_io_data *this, |
250 | * const int32_t *src[], int16_t *dst) | 540 | * struct dsp_buffer *src, |
541 | * struct dsp_buffer *dst) | ||
251 | */ | 542 | */ |
252 | .section .icode, "ax", %progbits | 543 | .section .icode |
253 | .align 2 | ||
254 | .global sample_output_stereo | 544 | .global sample_output_stereo |
255 | .type sample_output_stereo, %function | 545 | .type sample_output_stereo, %function |
256 | sample_output_stereo: | 546 | sample_output_stereo: |
257 | @ input: r0 = count, r1 = data, r2 = src, r3 = dst | 547 | @ input: r0 = this, r1 = src, r2 = dst |
258 | stmfd sp!, { r4-r9, lr } | 548 | stmfd sp!, { r4-r9, lr } |
259 | 549 | ||
260 | ldr r1, [r1] @ r1 = data->output_scale | 550 | ldr r0, [r0] @ r0 = this->outcount |
261 | ldmia r2, { r2, r5 } @ r2 = src[0], r5 = src[1] | 551 | ldr r3, [r2, #4] @ r3 = dsp->p16out |
552 | ldmib r1, { r2, r5 } @ r2 = src->p32[0], r5 = src->p32[1] | ||
553 | ldrb r1, [r1, #19] @ r1 = src->format.output_scale | ||
262 | 554 | ||
263 | mov r4, #1 | 555 | mov r4, #1 |
264 | mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) | 556 | mov r4, r4, lsl r1 @ r4 = 1 << (scale-1) |
@@ -330,232 +622,3 @@ sample_output_stereo: | |||
330 | ldmpc regs=r4-r9 | 622 | ldmpc regs=r4-r9 |
331 | .size sample_output_stereo, .-sample_output_stereo | 623 | .size sample_output_stereo, .-sample_output_stereo |
332 | #endif /* ARM_ARCH < 6 */ | 624 | #endif /* ARM_ARCH < 6 */ |
333 | |||
334 | /**************************************************************************** | ||
335 | * void apply_crossfeed(int count, int32_t* src[]) | ||
336 | */ | ||
337 | .section .text | ||
338 | .global apply_crossfeed | ||
339 | apply_crossfeed: | ||
340 | @ unfortunately, we ended up in a bit of a register squeeze here, and need | ||
341 | @ to keep the count on the stack :/ | ||
342 | stmdb sp!, { r4-r11, lr } @ stack modified regs | ||
343 | ldmia r1, { r2-r3 } @ r2 = src[0], r3 = src[1] | ||
344 | |||
345 | ldr r1, =crossfeed_data | ||
346 | ldmia r1!, { r4-r11 } @ load direct gain and filter data | ||
347 | mov r12, r0 @ better to ldm delay + count later | ||
348 | add r0, r1, #13*4*2 @ calculate end of delay | ||
349 | stmdb sp!, { r0, r12 } @ stack end of delay adr and count | ||
350 | ldr r0, [r1, #13*4*2] @ fetch current delay line address | ||
351 | |||
352 | /* Register usage in loop: | ||
353 | * r0 = &delay[index][0], r1 = accumulator high, r2 = src[0], r3 = src[1], | ||
354 | * r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs), | ||
355 | * r8-r11 = filter history, r12 = temp, r14 = accumulator low | ||
356 | */ | ||
357 | .cfloop: | ||
358 | smull r14, r1, r6, r8 @ acc = b1*dr[n - 1] | ||
359 | smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1] | ||
360 | ldr r8, [r0, #4] @ r8 = dr[n] | ||
361 | smlal r14, r1, r5, r8 @ acc += b0*dr[n] | ||
362 | mov r9, r1, lsl #1 @ fix format for filter history | ||
363 | ldr r12, [r2] @ load left input | ||
364 | smlal r14, r1, r4, r12 @ acc += gain*x_l[n] | ||
365 | mov r1, r1, lsl #1 @ fix format | ||
366 | str r1, [r2], #4 @ save result | ||
367 | |||
368 | smull r14, r1, r6, r10 @ acc = b1*dl[n - 1] | ||
369 | smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1] | ||
370 | ldr r10, [r0] @ r10 = dl[n] | ||
371 | str r12, [r0], #4 @ save left input to delay line | ||
372 | smlal r14, r1, r5, r10 @ acc += b0*dl[n] | ||
373 | mov r11, r1, lsl #1 @ fix format for filter history | ||
374 | ldr r12, [r3] @ load right input | ||
375 | smlal r14, r1, r4, r12 @ acc += gain*x_r[n] | ||
376 | str r12, [r0], #4 @ save right input to delay line | ||
377 | mov r1, r1, lsl #1 @ fix format | ||
378 | ldmia sp, { r12, r14 } @ fetch delay line end addr and count from stack | ||
379 | str r1, [r3], #4 @ save result | ||
380 | |||
381 | cmp r0, r12 @ need to wrap to start of delay? | ||
382 | subeq r0, r0, #13*4*2 @ wrap back delay line ptr to start | ||
383 | |||
384 | subs r14, r14, #1 @ are we finished? | ||
385 | strne r14, [sp, #4] @ nope, save count back to stack | ||
386 | bne .cfloop | ||
387 | |||
388 | @ save data back to struct | ||
389 | ldr r12, =crossfeed_data + 4*4 | ||
390 | stmia r12, { r8-r11 } @ save filter history | ||
391 | str r0, [r12, #30*4] @ save delay line index | ||
392 | add sp, sp, #8 @ remove temp variables from stack | ||
393 | ldmpc regs=r4-r11 | ||
394 | .size apply_crossfeed, .-apply_crossfeed | ||
395 | |||
396 | /**************************************************************************** | ||
397 | * int dsp_downsample(int count, struct dsp_data *data, | ||
398 | * in32_t *src[], int32_t *dst[]) | ||
399 | */ | ||
400 | .section .text | ||
401 | .global dsp_downsample | ||
402 | dsp_downsample: | ||
403 | stmdb sp!, { r4-r11, lr } @ stack modified regs | ||
404 | ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta | ||
405 | sub r5, r5, #1 @ pre-decrement num_channels for use | ||
406 | add r4, r1, #12 @ r4 = &resample_data.phase | ||
407 | mov r12, #0xff | ||
408 | orr r12, r12, #0xff00 @ r12 = 0xffff | ||
409 | .dschannel_loop: | ||
410 | ldr r1, [r4] @ r1 = resample_data.phase | ||
411 | ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1] | ||
412 | ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1] | ||
413 | add r9, r4, #4 @ r9 = &last_sample[0] | ||
414 | ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1] | ||
415 | sub r11, r0, #1 | ||
416 | ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ... | ||
417 | str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample | ||
418 | movs r9, r1, lsr #16 @ r9 = pos = phase >> 16 | ||
419 | ldreq r11, [r7] @ if pos = 0, load src[0] and jump into loop | ||
420 | beq .dsuse_last_start | ||
421 | cmp r9, r0 @ if pos >= count, we're already done | ||
422 | bge .dsloop_skip | ||
423 | |||
424 | @ Register usage in loop: | ||
425 | @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel, | ||
426 | @ r6 = delta, r7 = s, r8 = d, r9 = pos, r10 = s[pos - 1], r11 = s[pos] | ||
427 | .dsloop: | ||
428 | add r9, r7, r9, lsl #2 @ r9 = &s[pos] | ||
429 | ldmda r9, { r10, r11 } @ r10 = s[pos - 1], r11 = s[pos] | ||
430 | .dsuse_last_start: | ||
431 | sub r11, r11, r10 @ r11 = diff = s[pos] - s[pos - 1] | ||
432 | @ keep frac in lower bits to take advantage of multiplier early termination | ||
433 | and r9, r1, r12 @ frac = phase & 0xffff | ||
434 | smull r9, r14, r11, r9 | ||
435 | add r1, r1, r6 @ phase += delta | ||
436 | add r10, r10, r9, lsr #16 @ r10 = out = s[pos - 1] + frac*diff | ||
437 | add r10, r10, r14, lsl #16 | ||
438 | str r10, [r8], #4 @ *d++ = out | ||
439 | mov r9, r1, lsr #16 @ pos = phase >> 16 | ||
440 | cmp r9, r0 @ pos < count? | ||
441 | blt .dsloop @ yup, do more samples | ||
442 | .dsloop_skip: | ||
443 | subs r5, r5, #1 | ||
444 | bpl .dschannel_loop @ if (--ch) >= 0, do another channel | ||
445 | sub r1, r1, r0, lsl #16 @ wrap phase back to start | ||
446 | str r1, [r4] @ store back | ||
447 | ldr r1, [r3] @ r1 = &dst[0] | ||
448 | sub r8, r8, r1 @ dst - &dst[0] | ||
449 | mov r0, r8, lsr #2 @ convert bytes->samples | ||
450 | ldmpc regs=r4-r11 @ ... and we're out | ||
451 | .size dsp_downsample, .-dsp_downsample | ||
452 | |||
453 | /**************************************************************************** | ||
454 | * int dsp_upsample(int count, struct dsp_data *dsp, | ||
455 | * in32_t *src[], int32_t *dst[]) | ||
456 | */ | ||
457 | .section .text | ||
458 | .global dsp_upsample | ||
459 | dsp_upsample: | ||
460 | stmfd sp!, { r4-r11, lr } @ stack modified regs | ||
461 | ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta | ||
462 | sub r5, r5, #1 @ pre-decrement num_channels for use | ||
463 | add r4, r1, #12 @ r4 = &resample_data.phase | ||
464 | mov r6, r6, lsl #16 @ we'll use carry to detect pos increments | ||
465 | stmfd sp!, { r0, r4 } @ stack count and &resample_data.phase | ||
466 | .uschannel_loop: | ||
467 | ldr r12, [r4] @ r12 = resample_data.phase | ||
468 | ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1] | ||
469 | ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1] | ||
470 | add r9, r4, #4 @ r9 = &last_sample[0] | ||
471 | mov r1, r12, lsl #16 @ we'll use carry to detect pos increments | ||
472 | sub r11, r0, #1 | ||
473 | ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ... | ||
474 | ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1] | ||
475 | str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample | ||
476 | movs r14, r12, lsr #16 @ pos = resample_data.phase >> 16 | ||
477 | beq .usstart_0 @ pos = 0 | ||
478 | cmp r14, r0 @ if pos >= count, we're already done | ||
479 | bge .usloop_skip | ||
480 | add r7, r7, r14, lsl #2 @ r7 = &s[pos] | ||
481 | ldr r10, [r7, #-4] @ r11 = s[pos - 1] | ||
482 | b .usstart_0 | ||
483 | |||
484 | @ Register usage in loop: | ||
485 | @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel, | ||
486 | @ r6 = delta, r7 = s, r8 = d, r9 = diff, r10 = s[pos - 1], r11 = s[pos] | ||
487 | .usloop_1: | ||
488 | mov r10, r11 @ r10 = previous sample | ||
489 | .usstart_0: | ||
490 | ldr r11, [r7], #4 @ r11 = next sample | ||
491 | mov r4, r1, lsr #16 @ r4 = frac = phase >> 16 | ||
492 | sub r9, r11, r10 @ r9 = diff = s[pos] - s[pos - 1] | ||
493 | .usloop_0: | ||
494 | smull r12, r14, r4, r9 | ||
495 | adds r1, r1, r6 @ phase += delta << 16 | ||
496 | mov r4, r1, lsr #16 @ r4 = frac = phase >> 16 | ||
497 | add r14, r10, r14, lsl #16 | ||
498 | add r14, r14, r12, lsr #16 @ r14 = out = s[pos - 1] + frac*diff | ||
499 | str r14, [r8], #4 @ *d++ = out | ||
500 | bcc .usloop_0 @ if carry is set, pos is incremented | ||
501 | subs r0, r0, #1 @ if count > 0, do another sample | ||
502 | bgt .usloop_1 | ||
503 | .usloop_skip: | ||
504 | subs r5, r5, #1 | ||
505 | ldmfd sp, { r0, r4 } @ reload count and &resample_data.phase | ||
506 | bpl .uschannel_loop @ if (--ch) >= 0, do another channel | ||
507 | mov r1, r1, lsr #16 @ wrap phase back to start of next frame | ||
508 | ldr r2, [r3] @ r1 = &dst[0] | ||
509 | str r1, [r4] @ store phase | ||
510 | sub r8, r8, r2 @ dst - &dst[0] | ||
511 | mov r0, r8, lsr #2 @ convert bytes->samples | ||
512 | add sp, sp, #8 @ adjust stack for temp variables | ||
513 | ldmpc regs=r4-r11 @ ... and we're out | ||
514 | .size dsp_upsample, .-dsp_upsample | ||
515 | |||
516 | /**************************************************************************** | ||
517 | * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) | ||
518 | */ | ||
519 | .section .icode, "ax", %progbits | ||
520 | .align 2 | ||
521 | .global dsp_apply_gain | ||
522 | .type dsp_apply_gain, %function | ||
523 | dsp_apply_gain: | ||
524 | @ input: r0 = count, r1 = data, r2 = buf[] | ||
525 | stmfd sp!, { r4-r8, lr } | ||
526 | |||
527 | ldr r3, [r1, #4] @ r3 = data->num_channels | ||
528 | ldr r4, [r1, #32] @ r5 = data->gain | ||
529 | |||
530 | .dag_outerloop: | ||
531 | ldr r1, [r2], #4 @ r1 = buf[0] and increment index of buf[] | ||
532 | subs r12, r0, #1 @ r12 = r0 = count - 1 | ||
533 | beq .dag_singlesample @ Zero? Only one sample! | ||
534 | |||
535 | .dag_innerloop: | ||
536 | ldmia r1, { r5, r6 } @ load r5, r6 from r1 | ||
537 | smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8) | ||
538 | smull r14, r5, r6, r4 @ r14 = FRACMUL_SHL(r6, r4, 8) | ||
539 | subs r12, r12, #2 | ||
540 | mov r7, r7, lsr #23 | ||
541 | mov r14, r14, lsr #23 | ||
542 | orr r7, r7, r8, asl #9 | ||
543 | orr r14, r14, r5, asl #9 | ||
544 | stmia r1!, { r7, r14 } @ save r7, r14 to [r1] and increment r1 | ||
545 | bgt .dag_innerloop @ end of inner loop | ||
546 | |||
547 | blt .dag_evencount @ < 0? even count | ||
548 | |||
549 | .dag_singlesample: | ||
550 | ldr r5, [r1] @ handle odd sample | ||
551 | smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8) | ||
552 | mov r7, r7, lsr #23 | ||
553 | orr r7, r7, r8, asl #9 | ||
554 | str r7, [r1] | ||
555 | |||
556 | .dag_evencount: | ||
557 | subs r3, r3, #1 | ||
558 | bgt .dag_outerloop @ end of outer loop | ||
559 | |||
560 | ldmpc regs=r4-r8 | ||
561 | .size dsp_apply_gain, .-dsp_apply_gain | ||
diff --git a/lib/rbcodec/dsp/dsp_arm_v6.S b/lib/rbcodec/dsp/dsp_arm_v6.S index a9a88ce5bf..a36760f744 100644 --- a/lib/rbcodec/dsp/dsp_arm_v6.S +++ b/lib/rbcodec/dsp/dsp_arm_v6.S | |||
@@ -20,19 +20,21 @@ | |||
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | 21 | ||
22 | /**************************************************************************** | 22 | /**************************************************************************** |
23 | * void sample_output_mono(int count, struct dsp_data *data, | 23 | * void sample_output_mono(struct sample_io_data *this, |
24 | * const int32_t *src[], int16_t *dst) | 24 | * struct dsp_buffer *src, |
25 | * struct dsp_buffer *dst) | ||
25 | */ | 26 | */ |
26 | .section .text, "ax", %progbits | 27 | .section .text |
27 | .align 2 | ||
28 | .global sample_output_mono | 28 | .global sample_output_mono |
29 | .type sample_output_mono, %function | 29 | .type sample_output_mono, %function |
30 | sample_output_mono: | 30 | sample_output_mono: |
31 | @ input: r0 = count, r1 = data, r2 = src, r3 = dst | 31 | @ input: r0 = this, r1 = src, r2 = dst |
32 | stmfd sp!, { r4, lr } @ | 32 | stmfd sp!, { r4, lr } @ |
33 | @ | 33 | @ |
34 | ldr r1, [r1] @ r1 = data->output_scale | 34 | ldr r0, [r0] @ r0 = this->outcount |
35 | ldr r2, [r2] @ r2 = src[0] | 35 | ldr r3, [r2, #4] @ r3 = dst->p16out |
36 | ldr r2, [r1, #4] @ r2 = src->p32[0] | ||
37 | ldrb r1, [r1, #19] @ r1 = src->format.output_scale | ||
36 | @ | 38 | @ |
37 | mov r4, #1 @ r4 = 1 << (scale - 1) | 39 | mov r4, #1 @ r4 = 1 << (scale - 1) |
38 | mov r4, r4, lsl r1 @ | 40 | mov r4, r4, lsl r1 @ |
@@ -68,19 +70,21 @@ sample_output_mono: | |||
68 | .size sample_output_mono, .-sample_output_mono | 70 | .size sample_output_mono, .-sample_output_mono |
69 | 71 | ||
70 | /**************************************************************************** | 72 | /**************************************************************************** |
71 | * void sample_output_stereo(int count, struct dsp_data *data, | 73 | * void sample_output_stereo(struct sample_io_data *this, |
72 | * const int32_t *src[], int16_t *dst) | 74 | * struct dsp_buffer *src, |
75 | * struct dsp_buffer *dst) | ||
73 | */ | 76 | */ |
74 | .section .text, "ax", %progbits | 77 | .section .text |
75 | .align 2 | ||
76 | .global sample_output_stereo | 78 | .global sample_output_stereo |
77 | .type sample_output_stereo, %function | 79 | .type sample_output_stereo, %function |
78 | sample_output_stereo: | 80 | sample_output_stereo: |
79 | @ input: r0 = count, r1 = data, r2 = src, r3 = dst | 81 | @ input: r0 = this, r1 = src, r2 = dst |
80 | stmfd sp!, { r4-r7, lr } @ | 82 | stmfd sp!, { r4-r7, lr } @ |
81 | @ | 83 | @ |
82 | ldr r1, [r1] @ r1 = data->output_scale | 84 | ldr r0, [r0] @ r0 = this->outcount |
83 | ldmia r2, { r2, r4 } @ r2 = src[0], r4 = src[1] | 85 | ldr r3, [r2, #4] @ r3 = dst->p16out |
86 | ldmib r1, { r2, r4 } @ r2 = src->p32[0], r4 = src->p32[1] | ||
87 | ldrb r1, [r1, #19] @ r1 = src->format.output_scale | ||
84 | @ | 88 | @ |
85 | mov r5, #1 @ r5 = 1 << (scale - 1) | 89 | mov r5, #1 @ r5 = 1 << (scale - 1) |
86 | mov r5, r5, lsl r1 @ | 90 | mov r5, r5, lsl r1 @ |
diff --git a/lib/rbcodec/dsp/dsp_asm.h b/lib/rbcodec/dsp/dsp_asm.h deleted file mode 100644 index 7bf18370a3..0000000000 --- a/lib/rbcodec/dsp/dsp_asm.h +++ /dev/null | |||
@@ -1,86 +0,0 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | #include <config.h> | ||
23 | |||
24 | #ifndef _DSP_ASM_H | ||
25 | #define _DSP_ASM_H | ||
26 | |||
27 | /* Set the appropriate #defines based on CPU or whatever matters */ | ||
28 | #if defined(CPU_ARM) | ||
29 | #define DSP_HAVE_ASM_APPLY_GAIN | ||
30 | #define DSP_HAVE_ASM_RESAMPLING | ||
31 | #define DSP_HAVE_ASM_CROSSFEED | ||
32 | #define DSP_HAVE_ASM_SOUND_CHAN_MONO | ||
33 | #define DSP_HAVE_ASM_SOUND_CHAN_CUSTOM | ||
34 | #define DSP_HAVE_ASM_SOUND_CHAN_KARAOKE | ||
35 | #define DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO | ||
36 | #define DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO | ||
37 | #elif defined (CPU_COLDFIRE) | ||
38 | #define DSP_HAVE_ASM_APPLY_GAIN | ||
39 | #define DSP_HAVE_ASM_RESAMPLING | ||
40 | #define DSP_HAVE_ASM_CROSSFEED | ||
41 | #define DSP_HAVE_ASM_SOUND_CHAN_MONO | ||
42 | #define DSP_HAVE_ASM_SOUND_CHAN_CUSTOM | ||
43 | #define DSP_HAVE_ASM_SOUND_CHAN_KARAOKE | ||
44 | #define DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO | ||
45 | #define DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO | ||
46 | #endif /* CPU_COLDFIRE */ | ||
47 | |||
48 | /* Declare prototypes based upon what's #defined above */ | ||
49 | #ifdef DSP_HAVE_ASM_CROSSFEED | ||
50 | void apply_crossfeed(int count, int32_t *buf[]); | ||
51 | #endif | ||
52 | |||
53 | #ifdef DSP_HAVE_ASM_APPLY_GAIN | ||
54 | void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]); | ||
55 | #endif /* DSP_HAVE_ASM_APPLY_GAIN* */ | ||
56 | |||
57 | #ifdef DSP_HAVE_ASM_RESAMPLING | ||
58 | int dsp_upsample(int count, struct dsp_data *data, | ||
59 | const int32_t *src[], int32_t *dst[]); | ||
60 | int dsp_downsample(int count, struct dsp_data *data, | ||
61 | const int32_t *src[], int32_t *dst[]); | ||
62 | #endif /* DSP_HAVE_ASM_RESAMPLING */ | ||
63 | |||
64 | #ifdef DSP_HAVE_ASM_SOUND_CHAN_MONO | ||
65 | void channels_process_sound_chan_mono(int count, int32_t *buf[]); | ||
66 | #endif | ||
67 | |||
68 | #ifdef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM | ||
69 | void channels_process_sound_chan_custom(int count, int32_t *buf[]); | ||
70 | #endif | ||
71 | |||
72 | #ifdef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE | ||
73 | void channels_process_sound_chan_karaoke(int count, int32_t *buf[]); | ||
74 | #endif | ||
75 | |||
76 | #ifdef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO | ||
77 | void sample_output_stereo(int count, struct dsp_data *data, | ||
78 | const int32_t *src[], int16_t *dst); | ||
79 | #endif | ||
80 | |||
81 | #ifdef DSP_HAVE_ASM_SAMPLE_OUTPUT_MONO | ||
82 | void sample_output_mono(int count, struct dsp_data *data, | ||
83 | const int32_t *src[], int16_t *dst); | ||
84 | #endif | ||
85 | |||
86 | #endif /* _DSP_ASM_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp_cf.S b/lib/rbcodec/dsp/dsp_cf.S index 15ec7eb383..c710df5177 100644 --- a/lib/rbcodec/dsp/dsp_cf.S +++ b/lib/rbcodec/dsp/dsp_cf.S | |||
@@ -19,23 +19,27 @@ | |||
19 | * KIND, either express or implied. | 19 | * KIND, either express or implied. |
20 | * | 20 | * |
21 | ****************************************************************************/ | 21 | ****************************************************************************/ |
22 | #include "config.h" | ||
22 | 23 | ||
23 | /**************************************************************************** | 24 | /**************************************************************************** |
24 | * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[]) | 25 | * void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) |
25 | */ | 26 | */ |
26 | .section .text | 27 | .section .text |
27 | .align 2 | 28 | .align 2 |
28 | .global dsp_apply_gain | 29 | .global pga_process |
29 | dsp_apply_gain: | 30 | pga_process: |
31 | | input: 4(sp) = this, 8(sp) = buf_p | ||
32 | movem.l 4(%sp), %a0-%a1 | %a0 = this, %a1 = buf_p | ||
33 | move.l (%a0), %a0 | %a0 = this->data = &pga_data | ||
34 | move.l (%a0), %a0 | %a0 = data->gain | ||
35 | move.l (%a1), %a1 | %a1 = buf = *buf_p | ||
30 | lea.l -20(%sp), %sp | save registers | 36 | lea.l -20(%sp), %sp | save registers |
31 | movem.l %d2-%d4/%a2-%a3, (%sp) | | 37 | movem.l %d2-%d4/%a2-%a3, (%sp) | |
32 | movem.l 28(%sp), %a0-%a1 | %a0 = data, | 38 | clr.l %d1 | %d1 = buf->format.num_channels |
33 | | %a1 = buf | 39 | move.b 17(%a1), %d1 | |
34 | move.l 4(%a0), %d1 | %d1 = data->num_channels | ||
35 | move.l 32(%a0), %a0 | %a0 = data->gain (in s8.23) | ||
36 | 10: | channel loop | | 40 | 10: | channel loop | |
37 | move.l 24(%sp), %d0 | %d0 = count | 41 | move.l (%a1), %d0 | %d0 = buf->remcount |
38 | move.l -4(%a1, %d1.l*4), %a2 | %a2 = s = buf[ch-1] | 42 | move.l (%a1, %d1.l*4), %a2 | %a2 = s = buf->p32[ch-1] |
39 | move.l %a2, %a3 | %a3 = d = s | 43 | move.l %a2, %a3 | %a3 = d = s |
40 | move.l (%a2)+, %d2 | %d2 = *s++, | 44 | move.l (%a2)+, %d2 | %d2 = *s++, |
41 | mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1) | 45 | mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1) |
@@ -61,25 +65,29 @@ dsp_apply_gain: | |||
61 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers | 65 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers |
62 | lea.l 20(%sp), %sp | cleanup stack | 66 | lea.l 20(%sp), %sp | cleanup stack |
63 | rts | | 67 | rts | |
64 | .size dsp_apply_gain,.-dsp_apply_gain | 68 | .size pga_process, .-pga_process |
65 | 69 | ||
66 | /**************************************************************************** | 70 | /**************************************************************************** |
67 | * void apply_crossfeed(int count, int32_t *buf[]) | 71 | * void crossfeed_process(struct dsp_proc_entry *this, |
72 | * struct dsp_buffer **buf_p) | ||
68 | */ | 73 | */ |
69 | .section .text | 74 | .section .text |
70 | .align 2 | 75 | .align 2 |
71 | .global apply_crossfeed | 76 | .global crossfeed_process |
72 | apply_crossfeed: | 77 | crossfeed_process: |
78 | | input: 4(sp) = this, 8(sp) = buf_p | ||
73 | lea.l -44(%sp), %sp | | 79 | lea.l -44(%sp), %sp | |
74 | movem.l %d2-%d7/%a2-%a6, (%sp) | save all regs | 80 | movem.l %d2-%d7/%a2-%a6, (%sp) | save all regs |
75 | movem.l 48(%sp), %d7/%a4 | %d7 = count, %a4 = src | 81 | movem.l 48(%sp), %a1/%a4 | %a1 = this, %a4 = buf_p |
76 | movem.l (%a4), %a4-%a5 | %a4 = src[0], %a5 = src[1] | 82 | move.l (%a4), %a4 | %a4 = buf = *buf_p |
77 | lea.l crossfeed_data, %a1 | %a1 = &crossfeed_data | 83 | movem.l (%a4), %d7/%a4-%a5 | %d7 = buf->remcount, %a4 = buf->p32[0], |
84 | | %a5 = buf->p32[1] | ||
85 | move.l (%a1), %a1 | %a1 = &crossfeed_state | ||
78 | move.l (%a1)+, %d6 | %d6 = direct gain | 86 | move.l (%a1)+, %d6 | %d6 = direct gain |
79 | movem.l 12(%a1), %d0-%d3 | fetch filter history samples | 87 | movem.l 12(%a1), %d0-%d3 | fetch filter history samples |
80 | move.l 132(%a1), %a0 | fetch delay line address | 88 | lea.l 132(%a1), %a6 | %a6 = delay line wrap limit |
89 | move.l (%a6), %a0 | fetch delay line address | ||
81 | movem.l (%a1), %a1-%a3 | load filter coefs | 90 | movem.l (%a1), %a1-%a3 | load filter coefs |
82 | lea.l crossfeed_data+136, %a6 | %a6 = delay line wrap limit | ||
83 | bra.b 20f | loop start | go to loop start point | 91 | bra.b 20f | loop start | go to loop start point |
84 | /* Register usage in loop: | 92 | /* Register usage in loop: |
85 | * %a0 = delay_p, %a1..%a3 = b0, b1, a1 (filter coefs), | 93 | * %a0 = delay_p, %a1..%a3 = b0, b1, a1 (filter coefs), |
@@ -109,174 +117,181 @@ apply_crossfeed: | |||
109 | mac.l %d6, %d5, %acc1 | %acc1 += gain*x_r[n] | 117 | mac.l %d6, %d5, %acc1 | %acc1 += gain*x_r[n] |
110 | cmp.l %a6, %a0 | wrap %a0 if passed end | 118 | cmp.l %a6, %a0 | wrap %a0 if passed end |
111 | bhs.b 30f | wrap buffer | | 119 | bhs.b 30f | wrap buffer | |
112 | .word 0x51fb | tpf.l | trap the buffer wrap | 120 | tpf.l | trap the buffer wrap |
113 | 30: | wrap buffer | ...fwd taken branches more costly | 121 | 30: | wrap buffer | ...fwd taken branches more costly |
114 | lea.l -104(%a0), %a0 | wrap it up | 122 | lea.l -104(%a6), %a0 | wrap it up |
115 | subq.l #1, %d7 | --count > 0 ? | 123 | subq.l #1, %d7 | --count > 0 ? |
116 | bgt.b 10b | loop | yes? do more | 124 | bgt.b 10b | loop | yes? do more |
117 | movclr.l %acc0, %d4 | write last outputs | 125 | movclr.l %acc0, %d4 | write last outputs |
118 | move.l %d4, (%a4) | . | 126 | move.l %d4, (%a4) | . |
119 | movclr.l %acc1, %d5 | . | 127 | movclr.l %acc1, %d5 | . |
120 | move.l %d5, (%a5) | . | 128 | move.l %d5, (%a5) | . |
121 | lea.l crossfeed_data+16, %a1 | save data back to struct | 129 | movem.l %d0-%d3, -120(%a6) | ...history |
122 | movem.l %d0-%d3, (%a1) | ...history | 130 | move.l %a0, (%a6) | ...delay_p |
123 | move.l %a0, 120(%a1) | ...delay_p | ||
124 | movem.l (%sp), %d2-%d7/%a2-%a6 | restore all regs | 131 | movem.l (%sp), %d2-%d7/%a2-%a6 | restore all regs |
125 | lea.l 44(%sp), %sp | | 132 | lea.l 44(%sp), %sp | |
126 | rts | | 133 | rts | |
127 | .size apply_crossfeed,.-apply_crossfeed | 134 | .size crossfeed_process,.-crossfeed_process |
128 | 135 | ||
129 | /**************************************************************************** | 136 | /**************************************************************************** |
130 | * int dsp_downsample(int count, struct dsp_data *data, | 137 | * int lin_resample_resample(struct resample_data *data, |
131 | * in32_t *src[], int32_t *dst[]) | 138 | * struct dsp_buffer *src, |
139 | * struct dsp_buffer *dst) | ||
132 | */ | 140 | */ |
133 | .section .text | 141 | .section .text |
134 | .align 2 | 142 | .align 2 |
135 | .global dsp_downsample | 143 | .global lin_resample_resample |
136 | dsp_downsample: | 144 | lin_resample_resample: |
137 | lea.l -40(%sp), %sp | save non-clobberables | 145 | | input: 4(sp) = data, 8(sp) = src, 12(sp) = dst |
138 | movem.l %d2-%d7/%a2-%a5, (%sp) | | 146 | lea.l -44(%sp), %sp | save non-volatiles |
139 | movem.l 44(%sp), %d2/%a0-%a2 | %d2 = count | 147 | movem.l %d2-%d7/%a2-%a6, (%sp) | |
140 | | %a0 = data | 148 | movem.l 48(%sp), %a0-%a2 | %a0 = data |
141 | | %a1 = src | 149 | | %a1 = src |
142 | | %a2 = dst | 150 | | %a2 = dst |
143 | movem.l 4(%a0), %d3-%d4 | %d3 = ch = data->num_channels | 151 | clr.l %d1 | %d1 = ch = src->format.num_channels |
144 | | %d4 = delta = data->resample_data.delta | 152 | move.b 17(%a1), %d1 | |
145 | moveq.l #16, %d7 | %d7 = shift | 153 | moveq.l #16, %d7 | %d7 = shift |
146 | 10: | channel loop | | 154 | .lrs_channel_loop: | |
147 | move.l 12(%a0), %d5 | %d5 = phase = data->resample_data.phase | 155 | movem.l (%a0), %d2-%d3 | %d2 = delta = data->delta, |
148 | move.l -4(%a1, %d3.l*4), %a3 | %a3 = s = src[ch-1] | 156 | | %d3 = phase = data->phase |
149 | move.l -4(%a2, %d3.l*4), %a4 | %a4 = d = dst[ch-1] | 157 | move.l (%a1), %d4 | %d4 = srcrem = src->remcount |
150 | lea.l 12(%a0, %d3.l*4), %a5 | %a5 = &data->resample_data.ast_sample[ch-1] | 158 | move.l 12(%a2), %d5 | %d5 = dstrem = dst->bufcount |
151 | move.l (%a5), %d0 | %d0 = last = data->resample_data.last_sample[ch-1] | 159 | cmp.l #0x8000, %d4 | %d4 = MIN(srcrem, 0x8000) |
152 | move.l -4(%a3, %d2.l*4), (%a5) | data->resample_data.last_sample[ch-1] = s[count-1] | 160 | ble.b 10f | |
153 | move.l %d5, %d6 | %d6 = pos = phase >> 16 | 161 | move.l #0x8000, %d4 | |
154 | lsr.l %d7, %d6 | | 162 | 10: | |
155 | cmp.l %d2, %d6 | past end of samples? | 163 | move.l (%a1, %d1.l*4), %a3 | %a3 = s = src->p32[ch] |
156 | bge.b 40f | skip resample loop| yes? skip loop | 164 | move.l (%a2, %d1.l*4), %a4 | %a4 = d = dst->p32[ch] |
157 | tst.l %d6 | need last sample of prev. frame? | 165 | move.l %d3, %d0 | %d0 = pos |
158 | bne.b 20f | resample loop | no? start main loop | 166 | lsr.l %d7, %d0 | ... |
159 | move.l (%a3, %d6.l*4), %d1 | %d1 = s[pos] | 167 | beq.b 11f | pos == 0? |
160 | bra.b 30f | resample start last | start with last (last in %d0) | 168 | cmp.l %d4, %d0 | pos = MIN(pos, srcrem) |
161 | 20: | resample loop | | 169 | blt.b 12f | |
162 | lea.l -4(%a3, %d6.l*4), %a5 | load s[pos-1] and s[pos] | 170 | move.l %d4, %d0 | pos = srcrem |
163 | movem.l (%a5), %d0-%d1 | | 171 | move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1] |
164 | 30: | resample start last | | 172 | bra.w .lrs_channel_complete | at limit; nothing to do but next |
165 | sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1] | 173 | 11: | |
166 | move.l %d0, %acc0 | %acc0 = previous sample | 174 | move.l 4(%a0, %d1.l*4), %d6 | %d6 = last = last_sample[ch] |
167 | move.l %d5, %d0 | frac = (phase << 16) >> 1 | 175 | tpf.l | trap next move.l (last = s[pos - 1]) |
176 | 12: | | ||
177 | move.l -4(%a3, %d0.l*4), %d6 | %d6 = last = s[pos - 1] | ||
178 | cmp.l #0x10000, %d2 | delta >= 1.0? | ||
179 | bhs.b .lrs_downsample | yes? downsampling | ||
180 | | | ||
181 | /** Upsampling **/ | | ||
182 | lea.l (%a3, %d0.l*4), %a3 | %a3 = &s[pos] | ||
183 | sub.l %d4, %d0 | %d0 = pos - srcrem = -dte | ||
184 | lsl.l %d7, %d2 | move delta to bits 30..15 | ||
185 | lsr.l #1, %d2 | | ||
186 | lsl.l %d7, %d3 | move phase to bits 30..15 | ||
187 | lsr.l #1, %d3 | | ||
188 | move.l (%a3)+, %a5 | %a5 = s[pos] | ||
189 | move.l %a5, %a6 | %a6 = diff = s[pos] - last | ||
190 | sub.l %d6, %a6 | | ||
191 | bra.b 22f | | ||
192 | /* Funky loop structure is to avoid emac latency stalls */ | ||
193 | 20: | | ||
194 | move.l (%a3)+, %a5 | %a5 = s[pos] | ||
195 | move.l %a5, %a6 | %a6 = diff = s[pos] - last | ||
196 | sub.l %d6, %a6 | | ||
197 | 21: | | ||
198 | movclr.l %acc0, %d7 | *d++ = %d7 = result | ||
199 | move.l %d7, (%a4)+ | | ||
200 | 22: | | ||
201 | move.l %d6, %acc0 | %acc0 = last | ||
202 | mac.l %d3, %a6, %acc0 | %acc0 += frac * diff | ||
203 | subq.l #1, %d5 | dstrem <= 0? | ||
204 | ble.b 23f | yes? stop | ||
205 | add.l %d2, %d3 | phase += delta | ||
206 | bpl.b 21b | load next values? | ||
207 | move.l %a5, %d6 | | ||
208 | bclr.l #31, %d3 | clear sign bit | ||
209 | addq.l #1, %d0 | dte > 0? | ||
210 | bmi.b 20b | yes? continue resampling | ||
211 | tpf.w | trap next add.l (phase += delta) | ||
212 | 23: | | ||
213 | add.l %d2, %d3 | phase += delta | ||
214 | lsl.l #1, %d3 | frac -> phase | ||
215 | bcs.b 24f | was sign bit set? | ||
216 | tpf.l | | ||
217 | 24: | | ||
218 | move.l %a5, %d6 | yes? was going to move to new s[pos] | ||
219 | addq.l #1, %d0 | | ||
220 | movclr.l %acc0, %d7 | *d = %d7 = result | ||
221 | move.l %d7, (%a4) | | ||
222 | add.l %d4, %d0 | %d0 = -dte + srcrem = pos | ||
223 | or.l %d0, %d3 | restore phase | ||
224 | swap.w %d3 | | ||
225 | moveq.l #16, %d7 | %d7 = shift | ||
226 | bra.b .lrs_channel_complete | | ||
227 | | | ||
228 | /** Downsampling **/ | | ||
229 | .lrs_downsample: | | ||
230 | move.l (%a3, %d0.l*4), %a5 | %a5 = s[pos] | ||
231 | bra.b 31f | | ||
232 | 30: | | ||
233 | lea.l -4(%a3, %d0.l*4), %a5 | %d6 = s[pos - 1], %a5 = s[pos] | ||
234 | movem.l (%a5), %d6/%a5 | | ||
235 | 31: | | ||
236 | move.l %d6, %acc0 | %acc0 = last | ||
237 | sub.l %d6, %a5 | %a5 = diff = s[pos] - s[pos - 1] | ||
238 | move.l %d3, %d0 | frac = (phase << 16) >> 1 | ||
168 | lsl.l %d7, %d0 | | 239 | lsl.l %d7, %d0 | |
169 | lsr.l #1, %d0 | | 240 | lsr.l #1, %d0 | |
170 | mac.l %d0, %d1, %acc0 | %acc0 += frac * diff | 241 | mac.l %d0, %a5, %acc0 | %acc0 += frac * diff |
171 | add.l %d4, %d5 | phase += delta | 242 | add.l %d2, %d3 | phase += delta |
172 | move.l %d5, %d6 | pos = phase >> 16 | 243 | move.l %d3, %d0 | pos = phase >> 16 |
173 | lsr.l %d7, %d6 | | 244 | lsr.l %d7, %d0 | |
174 | movclr.l %acc0, %d0 | | 245 | movclr.l %acc0, %a5 | |
175 | move.l %d0, (%a4)+ | *d++ = %d0 | 246 | move.l %a5, (%a4)+ | *d++ = %d0 |
176 | cmp.l %d2, %d6 | pos < count? | 247 | subq.l #1, %d5 | dst full? |
177 | blt.b 20b | resample loop | yes? continue resampling | 248 | ble.b 32f | yes? stop |
178 | 40: | skip resample loop | | 249 | cmp.l %d4, %d0 | pos < srcrem? |
179 | subq.l #1, %d3 | ch > 0? | 250 | blt.b 30b | yes? continue resampling |
180 | bgt.b 10b | channel loop | yes? process next channel | 251 | tpf.l | trap cmp.l and ble.b |
181 | lsl.l %d7, %d2 | wrap phase to start of next frame | 252 | 32: | |
182 | sub.l %d2, %d5 | data->resample_data.phase = | 253 | cmp.l %d4, %d0 | pos = MIN(pos, srcrem) |
183 | move.l %d5, 12(%a0) | ... phase - (count << 16) | 254 | ble.b 33f | |
184 | move.l %a4, %d0 | return d - d[0] | 255 | move.l %d4, %d0 | |
185 | sub.l (%a2), %d0 | | 256 | 33: | |
186 | asr.l #2, %d0 | convert bytes->samples | 257 | move.l -4(%a3, %d0.l*4), %d6 | %d6 = s[pos - 1] |
187 | movem.l (%sp), %d2-%d7/%a2-%a5 | restore non-clobberables | 258 | | |
188 | lea.l 40(%sp), %sp | cleanup stack | 259 | .lrs_channel_complete: | |
260 | move.l %d6, 4(%a0, %d1.l*4) | last_sample[ch] = last | ||
261 | subq.l #1, %d1 | ch > 0? | ||
262 | bgt.w .lrs_channel_loop | yes? process next channel | ||
263 | | | ||
264 | move.l 12(%a2), %d1 | %d1 = dst->bufcount | ||
265 | sub.l %d5, %d1 | written = dst->bufcount - dstrem | ||
266 | move.l %d1, (%a2) | dst->remcount = written | ||
267 | move.l %d0, %d1 | wrap phase to position in next frame | ||
268 | lsl.l %d7, %d1 | data->phase = phase - (pos << 16) | ||
269 | sub.l %d1, %d3 | ... | ||
270 | move.l %d3, 4(%a0) | ... | ||
271 | movem.l (%sp), %d2-%d7/%a2-%a6 | restore non-volatiles | ||
272 | lea.l 44(%sp), %sp | cleanup stack | ||
189 | rts | buh-bye | 273 | rts | buh-bye |
190 | .size dsp_downsample,.-dsp_downsample | ||
191 | 274 | ||
192 | /**************************************************************************** | 275 | .size lin_resample_resample, .-lin_resample_resample |
193 | * int dsp_upsample(int count, struct dsp_data *dsp, | 276 | |
194 | * const int32_t *src[], int32_t *dst[]) | ||
195 | */ | ||
196 | .section .text | ||
197 | .align 2 | ||
198 | .global dsp_upsample | ||
199 | dsp_upsample: | ||
200 | lea.l -40(%sp), %sp | save non-clobberables | ||
201 | movem.l %d2-%d7/%a2-%a5, (%sp) | | ||
202 | movem.l 44(%sp), %d2/%a0-%a2 | %d2 = count | ||
203 | | %a0 = data | ||
204 | | %a1 = src | ||
205 | | %a2 = dst | ||
206 | movem.l 4(%a0), %d3-%d4 | %d3 = ch = channels | ||
207 | | %d4 = delta = data->resample_data.delta | ||
208 | swap %d4 | swap delta to high word to use... | ||
209 | | ...carries to increment position | ||
210 | 10: | channel loop | | ||
211 | move.l 12(%a0), %d5 | %d5 = phase = data->resample_data.phase | ||
212 | move.l -4(%a1, %d3.l*4), %a3 | %a3 = s = src[ch-1] | ||
213 | lea.l 12(%a0, %d3.l*4), %a4 | %a4 = &data->resample_data.last_sample[ch-1] | ||
214 | lea.l -4(%a3, %d2.l*4), %a5 | %a5 = src_end = &src[count-1] | ||
215 | move.l (%a4), %d0 | %d0 = last = data->resample_data.last_sample[ch-1] | ||
216 | move.l (%a5), (%a4) | data->resample_data.last_sample[ch-1] = s[count-1] | ||
217 | move.l -4(%a2, %d3.l*4), %a4 | %a4 = d = dst[ch-1] | ||
218 | move.l (%a3)+, %d1 | fetch first sample - might throw this... | ||
219 | | ...away later but we'll be preincremented | ||
220 | move.l %d1, %d6 | save sample value | ||
221 | sub.l %d0, %d1 | %d1 = diff = s[0] - last | ||
222 | swap %d5 | swap phase to high word to use | ||
223 | | carries to increment position | ||
224 | move.l %d5, %d7 | %d7 = pos = phase >> 16 | ||
225 | clr.w %d5 | | ||
226 | eor.l %d5, %d7 | pos == 0? | ||
227 | beq.b 40f | loop start | yes? start loop | ||
228 | cmp.l %d2, %d7 | past end of samples? | ||
229 | bge.b 50f | skip resample loop| yes? go to next channel and collect info | ||
230 | lea.l (%a3, %d7.l*4), %a3 | %a3 = s = &s[pos+1] | ||
231 | movem.l -8(%a3), %d0-%d1 | %d0 = s[pos-1], %d1 = s[pos] | ||
232 | move.l %d1, %d6 | save sample value | ||
233 | sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1] | ||
234 | bra.b 40f | loop start | | ||
235 | 20: | next sample loop | | ||
236 | move.l %d6, %d0 | move previous sample to %d0 | ||
237 | move.l (%a3)+, %d1 | fetch next sample | ||
238 | move.l %d1, %d6 | save sample value | ||
239 | sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1] | ||
240 | 30: | same sample loop | | ||
241 | movclr.l %acc0, %d7 | %d7 = result | ||
242 | move.l %d7, (%a4)+ | *d++ = %d7 | ||
243 | 40: | loop start | | ||
244 | lsr.l #1, %d5 | make phase into frac | ||
245 | move.l %d0, %acc0 | %acc0 = s[pos-1] | ||
246 | mac.l %d1, %d5, %acc0 | %acc0 = diff * frac | ||
247 | lsl.l #1, %d5 | restore frac to phase | ||
248 | add.l %d4, %d5 | phase += delta | ||
249 | bcc.b 30b | same sample loop | load next values? | ||
250 | cmp.l %a5, %a3 | src <= src_end? | ||
251 | bls.b 20b | next sample loop | yes? continue resampling | ||
252 | movclr.l %acc0, %d7 | %d7 = result | ||
253 | move.l %d7, (%a4)+ | *d++ = %d7 | ||
254 | 50: | skip resample loop | | ||
255 | subq.l #1, %d3 | ch > 0? | ||
256 | bgt.b 10b | channel loop | yes? process next channel | ||
257 | swap %d5 | wrap phase to start of next frame | ||
258 | move.l %d5, 12(%a0) | ...and save in data->resample_data.phase | ||
259 | move.l %a4, %d0 | return d - d[0] | ||
260 | sub.l (%a2), %d0 | | ||
261 | movem.l (%sp), %d2-%d7/%a2-%a5 | restore non-clobberables | ||
262 | asr.l #2, %d0 | convert bytes->samples | ||
263 | lea.l 40(%sp), %sp | cleanup stack | ||
264 | rts | buh-bye | ||
265 | .size dsp_upsample,.-dsp_upsample | ||
266 | 277 | ||
267 | /**************************************************************************** | 278 | /**************************************************************************** |
268 | * void channels_process_sound_chan_mono(int count, int32_t *buf[]) | 279 | * void channel_mode_proc_mono(struct dsp_proc_entry *this, |
280 | * struct dsp_buffer **buf_p) | ||
269 | * | 281 | * |
270 | * Mix left and right channels 50/50 into a center channel. | 282 | * Mix left and right channels 50/50 into a center channel. |
271 | */ | 283 | */ |
272 | .section .text | 284 | .section .text |
273 | .align 2 | 285 | .align 2 |
274 | .global channels_process_sound_chan_mono | 286 | .global channel_mode_proc_mono |
275 | channels_process_sound_chan_mono: | 287 | channel_mode_proc_mono: |
276 | movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf | 288 | | input: 4(sp) = this, 8(sp) = buf_p |
289 | move.l 8(%sp), %a0 | %a0 = buf_p | ||
290 | move.l (%a0), %a0 | %a0 = buf = *buf_p | ||
277 | lea.l -20(%sp), %sp | save registers | 291 | lea.l -20(%sp), %sp | save registers |
278 | movem.l %d2-%d4/%a2-%a3, (%sp) | | 292 | movem.l %d2-%d4/%a2-%a3, (%sp) | |
279 | movem.l (%a0), %a0-%a1 | get channel pointers | 293 | movem.l (%a0), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0], |
294 | | %a1 = buf->p32[1] | ||
280 | move.l %a0, %a2 | use separate dst pointers since read | 295 | move.l %a0, %a2 | use separate dst pointers since read |
281 | move.l %a1, %a3 | pointers run one ahead of write | 296 | move.l %a1, %a3 | pointers run one ahead of write |
282 | move.l #0x40000000, %d3 | %d3 = 0.5 | 297 | move.l #0x40000000, %d3 | %d3 = 0.5 |
@@ -301,26 +316,29 @@ channels_process_sound_chan_mono: | |||
301 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers | 316 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers |
302 | lea.l 20(%sp), %sp | cleanup | 317 | lea.l 20(%sp), %sp | cleanup |
303 | rts | | 318 | rts | |
304 | .size channels_process_sound_chan_mono, \ | 319 | .size channel_mode_proc_mono, .-channel_mode_proc_mono |
305 | .-channels_process_sound_chan_mono | ||
306 | 320 | ||
307 | /**************************************************************************** | 321 | /**************************************************************************** |
308 | * void channels_process_sound_chan_custom(int count, int32_t *buf[]) | 322 | * void channel_mode_proc_custom(struct dsp_proc_entry *this, |
323 | * struct dsp_buffer **buf_p) | ||
309 | * | 324 | * |
310 | * Apply stereo width (narrowing/expanding) effect. | 325 | * Apply stereo width (narrowing/expanding) effect. |
311 | */ | 326 | */ |
312 | .section .text | 327 | .section .text |
313 | .align 2 | 328 | .align 2 |
314 | .global channels_process_sound_chan_custom | 329 | .global channel_mode_proc_custom |
315 | channels_process_sound_chan_custom: | 330 | channel_mode_proc_custom: |
316 | movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf | 331 | | input: 4(sp) = this, 8(sp) = buf_p |
317 | lea.l -28(%sp), %sp | save registers | 332 | lea.l -28(%sp), %sp | save registers |
318 | movem.l %d2-%d6/%a2-%a3, (%sp) | | 333 | movem.l %d2-%d6/%a2-%a3, (%sp) | |
319 | movem.l (%a0), %a0-%a1 | get channel pointers | 334 | movem.l 32(%sp), %a0-%a1 | %a0 = this, %a1 = buf_p |
335 | move.l (%a1), %a1 | %a1 = buf = *buf_p | ||
336 | move.l (%a0), %a2 | %a2 = this->data = &channel_mode_data | ||
337 | movem.l (%a1), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0], | ||
338 | | %a1 = buf->p32[1] | ||
339 | movem.l (%a2), %d3-%d4 | %d3 = sw_gain, %d4 = sw_cross | ||
320 | move.l %a0, %a2 | use separate dst pointers since read | 340 | move.l %a0, %a2 | use separate dst pointers since read |
321 | move.l %a1, %a3 | pointers run one ahead of write | 341 | move.l %a1, %a3 | pointers run one ahead of write |
322 | move.l dsp_sw_gain, %d3 | load straight (mid) gain | ||
323 | move.l dsp_sw_cross, %d4 | load cross (side) gain | ||
324 | move.l (%a0)+, %d1 | prime the input registers | 342 | move.l (%a0)+, %d1 | prime the input registers |
325 | move.l (%a1)+, %d2 | | 343 | move.l (%a1)+, %d2 | |
326 | mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross | 344 | mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross |
@@ -348,22 +366,25 @@ channels_process_sound_chan_custom: | |||
348 | movem.l (%sp), %d2-%d6/%a2-%a3 | restore registers | 366 | movem.l (%sp), %d2-%d6/%a2-%a3 | restore registers |
349 | lea.l 28(%sp), %sp | cleanup | 367 | lea.l 28(%sp), %sp | cleanup |
350 | rts | | 368 | rts | |
351 | .size channels_process_sound_chan_custom, \ | 369 | .size channel_mode_proc_custom, .-channel_mode_proc_custom |
352 | .-channels_process_sound_chan_custom | ||
353 | 370 | ||
354 | /**************************************************************************** | 371 | /**************************************************************************** |
355 | * void channels_process_sound_chan_karaoke(int count, int32_t *buf[]) | 372 | * void channel_mode_proc_karaoke(struct dsp_proc_entry *this, |
373 | * struct dsp_buffer **buf_p) | ||
356 | * | 374 | * |
357 | * Separate channels into side channels. | 375 | * Separate channels into side channels. |
358 | */ | 376 | */ |
359 | .section .text | 377 | .section .text |
360 | .align 2 | 378 | .align 2 |
361 | .global channels_process_sound_chan_karaoke | 379 | .global channel_mode_proc_karaoke |
362 | channels_process_sound_chan_karaoke: | 380 | channel_mode_proc_karaoke: |
363 | movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf | 381 | | input: 4(sp) = this, 8(sp) = buf_p |
382 | move.l 8(%sp), %a0 | %a0 = buf_p | ||
383 | move.l (%a0), %a0 | %a0 = buf = *buf_p | ||
364 | lea.l -20(%sp), %sp | save registers | 384 | lea.l -20(%sp), %sp | save registers |
365 | movem.l %d2-%d4/%a2-%a3, (%sp) | | 385 | movem.l %d2-%d4/%a2-%a3, (%sp) | |
366 | movem.l (%a0), %a0-%a1 | get channel src pointers | 386 | movem.l (%a0), %d0/%a0-%a1 | %d0 = buf->remcount, %a0 = buf->p32[0], |
387 | | %a1 = buf->p32[1] | ||
367 | move.l %a0, %a2 | use separate dst pointers since read | 388 | move.l %a0, %a2 | use separate dst pointers since read |
368 | move.l %a1, %a3 | pointers run one ahead of write | 389 | move.l %a1, %a3 | pointers run one ahead of write |
369 | move.l #0x40000000, %d3 | %d3 = 0.5 | 390 | move.l #0x40000000, %d3 | %d3 = 0.5 |
@@ -390,12 +411,90 @@ channels_process_sound_chan_karaoke: | |||
390 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers | 411 | movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers |
391 | lea.l 20(%sp), %sp | cleanup | 412 | lea.l 20(%sp), %sp | cleanup |
392 | rts | | 413 | rts | |
393 | .size channels_process_sound_chan_karaoke, \ | 414 | .size channel_mode_proc_karaoke, .-channel_mode_proc_karaoke |
394 | .-channels_process_sound_chan_karaoke | 415 | |
416 | /**************************************************************************** | ||
417 | * void filter_process(struct dsp_filter *f, int32_t *buf[], int count, | ||
418 | * unsigned int channels) | ||
419 | * | ||
420 | * define HIGH_PRECISION as '1' to make filtering calculate lower bits after | ||
421 | * shifting. without this, "shift" - 1 of the lower bits will be lost here. | ||
422 | */ | ||
423 | #define HIGH_PRECISION 0 | ||
424 | .text | ||
425 | .global filter_process | ||
426 | filter_process: | ||
427 | | input: 4(sp) = f, 8(sp) = buf, 12(sp) = count, 16(sp) = channels | ||
428 | lea.l -44(%sp), %sp | save clobbered regs | ||
429 | #if HIGH_PRECISION | ||
430 | movem.l %d2-%d7/%a2-%a6, (%sp) | . | ||
431 | #else | ||
432 | movem.l %d2-%d6/%a2-%a6, (%sp) | | ||
433 | #endif | ||
434 | move.l 48(%sp), %a5 | fetch filter structure address | ||
435 | clr.l %d6 | load shift count | ||
436 | move.b 52(%a5), %d6 | . | ||
437 | subq.l #1, %d6 | EMAC gives us one free shift | ||
438 | #if HIGH_PRECISION | ||
439 | moveq.l #8, %d7 | ||
440 | sub.l %d6, %d7 | shift for lower part of accumulator | ||
441 | #endif | ||
442 | movem.l (%a5), %a0-%a4 | load coefs | ||
443 | lea.l 20(%a5), %a5 | point to filter history | ||
444 | |||
445 | 10: | channel loop | ||
446 | move.l 52(%sp), %a6 | load input channel pointer | ||
447 | addq.l #4, 52(%sp) | point x to next channel | ||
448 | move.l (%a6), %a6 | | ||
449 | move.l 56(%sp), %d5 | number of samples | ||
450 | movem.l (%a5), %d0-%d3 | load filter history | ||
451 | |||
452 | | d0-d3 = history, d4 = temp, d5 = sample count, d6 = upper shift amount, | ||
453 | | d7 = lower shift amount,a0-a4 = coefs, a5 = history pointer, a6 = buf[ch] | ||
454 | 20: | loop | ||
455 | | Direct form 1 filtering code. We assume DSP has put EMAC in frac mode. | ||
456 | | y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | ||
457 | | where y[] is output and x[] is input. This is performed out of order | ||
458 | | to do parallel load of input value. | ||
459 | mac.l %a2, %d1, %acc0 | acc = b2*x[i - 2] | ||
460 | move.l %d0, %d1 | fix input history | ||
461 | mac.l %a1, %d0, (%a6), %d0, %acc0 | acc += b1*x[i - 1], x[i] -> d0 | ||
462 | mac.l %a0, %d0, %acc0 | acc += b0*x[i] | ||
463 | mac.l %a3, %d2, %acc0 | acc += a1*y[i - 1] | ||
464 | mac.l %a4, %d3, %acc0 | acc += a2*y[i - 2] | ||
465 | move.l %d2, %d3 | fix output history | ||
466 | #if HIGH_PRECISION | ||
467 | move.l %accext01, %d2 | fetch lower part of accumulator | ||
468 | move.b %d2, %d4 | clear upper three bytes | ||
469 | lsr.l %d7, %d4 | shift lower bits | ||
470 | #endif | ||
471 | movclr.l %acc0, %d2 | fetch upper part of result | ||
472 | asl.l %d6, %d2 | restore fixed point format | ||
473 | #if HIGH_PRECISION | ||
474 | or.l %d2, %d4 | combine lower and upper parts | ||
475 | #endif | ||
476 | move.l %d2, (%a6)+ | save result | ||
477 | subq.l #1, %d5 | are we done with this channel? | ||
478 | bgt 20b | loop | ||
479 | |||
480 | movem.l %d0-%d3, (%a5) | save history back to struct | ||
481 | lea.l 16(%a5), %a5 | point to next channel's history | ||
482 | subq.l #1, 60(%sp) | have we processed both channels? | ||
483 | bhi 10b | channel loop | ||
484 | |||
485 | #if HIGH_PRECISION | ||
486 | movem.l (%sp), %d2-%d7/%a2-%a6 | ||
487 | #else | ||
488 | movem.l (%sp), %d2-%d6/%a2-%a6 | ||
489 | #endif | ||
490 | lea.l 44(%sp), %sp | ||
491 | rts | ||
492 | .size filter_process, .-filter_process | ||
395 | 493 | ||
396 | /**************************************************************************** | 494 | /**************************************************************************** |
397 | * void sample_output_stereo(int count, struct dsp_data *data, | 495 | * void sample_output_stereo(struct sample_io_data *this, |
398 | * const int32_t *src[], int16_t *dst) | 496 | * struct dsp_buffer *src, |
497 | * struct dsp_buffer *dst) | ||
399 | * | 498 | * |
400 | * Framework based on the ubiquitous Rockbox line transfer logic for | 499 | * Framework based on the ubiquitous Rockbox line transfer logic for |
401 | * Coldfire CPUs. | 500 | * Coldfire CPUs. |
@@ -417,20 +516,24 @@ channels_process_sound_chan_karaoke: | |||
417 | .align 2 | 516 | .align 2 |
418 | .global sample_output_stereo | 517 | .global sample_output_stereo |
419 | sample_output_stereo: | 518 | sample_output_stereo: |
519 | | input: 4(sp) = count, 8(sp) = src, 12(sp) = dst | ||
420 | lea.l -48(%sp), %sp | save registers | 520 | lea.l -48(%sp), %sp | save registers |
421 | move.l %macsr, %d1 | do it now as at many lines will | 521 | move.l %macsr, %d1 | do it now as at many lines will |
422 | movem.l %d1-%d7/%a2-%a6, (%sp) | be the far more common condition | 522 | movem.l %d1-%d7/%a2-%a6, (%sp) | be the far more common condition |
423 | move.l #0x80, %macsr | put emac unit in signed int mode | 523 | move.l #0x80, %macsr | put emac unit in signed int mode |
424 | movem.l 52(%sp), %a0-%a2/%a4 | | 524 | movem.l 52(%sp), %a0-%a2 | %a0 = this, %a1 = src, %a2 = dst |
425 | lea.l (%a4, %a0.l*4), %a0 | %a0 = end address | 525 | move.l (%a0), %a0 | %a0 = this->outcount |
426 | move.l (%a1), %d1 | %a1 = multiplier: (1 << (16 - scale)) | 526 | move.l 4(%a2), %a4 | %a4 = dst->p16out |
527 | lea.l (%a4, %a0.l*4), %a0 | %a0 = count -> end address | ||
528 | movem.l 4(%a1), %a2-%a3 | %a2 = src->p32[0], %a3 = src->p32[1] | ||
529 | clr.l %d1 | %a1 = multiplier: (1 << (16 - scale)) | ||
530 | move.b 19(%a1), %d1 | %d1 = src->format.output_scale | ||
427 | sub.l #16, %d1 | | 531 | sub.l #16, %d1 | |
428 | neg.l %d1 | | 532 | neg.l %d1 | |
429 | moveq.l #1, %d0 | | 533 | moveq.l #1, %d0 | |
430 | asl.l %d1, %d0 | | 534 | asl.l %d1, %d0 | |
431 | move.l %d0, %a1 | | 535 | move.l %d0, %a1 | |
432 | move.l #0x8000, %a6 | %a6 = rounding term | 536 | move.l #0x8000, %a6 | %a6 = rounding term |
433 | movem.l (%a2), %a2-%a3 | get L/R channel pointers | ||
434 | moveq.l #28, %d0 | %d0 = second line bound | 537 | moveq.l #28, %d0 | %d0 = second line bound |
435 | add.l %a4, %d0 | | 538 | add.l %a4, %d0 | |
436 | and.l #0xfffffff0, %d0 | | 539 | and.l #0xfffffff0, %d0 | |
@@ -447,7 +550,7 @@ sample_output_stereo: | |||
447 | mac.l %d2, %a1, %acc1 | shift R to high word | 550 | mac.l %d2, %a1, %acc1 | shift R to high word |
448 | movclr.l %acc0, %d1 | get possibly saturated results | 551 | movclr.l %acc0, %d1 | get possibly saturated results |
449 | movclr.l %acc1, %d2 | | 552 | movclr.l %acc1, %d2 | |
450 | swap %d2 | move R to low word | 553 | swap.w %d2 | move R to low word |
451 | move.w %d2, %d1 | interleave MS 16 bits of each | 554 | move.w %d2, %d1 | interleave MS 16 bits of each |
452 | move.l %d1, (%a4)+ | ...and write both | 555 | move.l %d1, (%a4)+ | ...and write both |
453 | cmp.l %a4, %d0 | | 556 | cmp.l %a4, %d0 | |
@@ -477,10 +580,10 @@ sample_output_stereo: | |||
477 | mac.l %d1, %a1, (%a2)+, %d2, %acc1 | with saturation | 580 | mac.l %d1, %a1, (%a2)+, %d2, %acc1 | with saturation |
478 | mac.l %d2, %a1, (%a2)+, %d3, %acc2 | | 581 | mac.l %d2, %a1, (%a2)+, %d3, %acc2 | |
479 | mac.l %d3, %a1 , %acc3 | | 582 | mac.l %d3, %a1 , %acc3 | |
480 | swap %d4 | a) interleave most significant... | 583 | swap.w %d4 | a) interleave most significant... |
481 | swap %d5 | | 584 | swap.w %d5 | |
482 | swap %d6 | | 585 | swap.w %d6 | |
483 | swap %d7 | | 586 | swap.w %d7 | |
484 | movclr.l %acc0, %d0 | obtain L results | 587 | movclr.l %acc0, %d0 | obtain L results |
485 | movclr.l %acc1, %d1 | | 588 | movclr.l %acc1, %d1 | |
486 | movclr.l %acc2, %d2 | | 589 | movclr.l %acc2, %d2 | |
@@ -503,7 +606,7 @@ sample_output_stereo: | |||
503 | mac.l %d2, %a1, %acc1 | | 606 | mac.l %d2, %a1, %acc1 | |
504 | movclr.l %acc0, %d1 | | 607 | movclr.l %acc0, %d1 | |
505 | movclr.l %acc1, %d2 | | 608 | movclr.l %acc1, %d2 | |
506 | swap %d2 | | 609 | swap.w %d2 | |
507 | move.w %d2, %d1 | | 610 | move.w %d2, %d1 | |
508 | move.l %d1, (%a4)+ | | 611 | move.l %d1, (%a4)+ | |
509 | cmp.l %a4, %a0 | | 612 | cmp.l %a4, %a0 | |
@@ -516,8 +619,9 @@ sample_output_stereo: | |||
516 | .size sample_output_stereo, .-sample_output_stereo | 619 | .size sample_output_stereo, .-sample_output_stereo |
517 | 620 | ||
518 | /**************************************************************************** | 621 | /**************************************************************************** |
519 | * void sample_output_mono(int count, struct dsp_data *data, | 622 | * void sample_output_mono(struct sample_io_data *this, |
520 | * const int32_t *src[], int16_t *dst) | 623 | * struct dsp_buffer *src, |
624 | * struct dsp_buffer *dst) | ||
521 | * | 625 | * |
522 | * Same treatment as sample_output_stereo but for one channel. | 626 | * Same treatment as sample_output_stereo but for one channel. |
523 | */ | 627 | */ |
@@ -525,19 +629,23 @@ sample_output_stereo: | |||
525 | .align 2 | 629 | .align 2 |
526 | .global sample_output_mono | 630 | .global sample_output_mono |
527 | sample_output_mono: | 631 | sample_output_mono: |
632 | | input: 4(sp) = count, 8(sp) = src, 12(sp) = dst | ||
528 | lea.l -32(%sp), %sp | save registers | 633 | lea.l -32(%sp), %sp | save registers |
529 | move.l %macsr, %d1 | do it now as at many lines will | 634 | move.l %macsr, %d1 | do it now as at many lines will |
530 | movem.l %d1-%d5/%a2-%a4, (%sp) | be the far more common condition | 635 | movem.l %d1-%d5/%a2-%a4, (%sp) | be the far more common condition |
531 | move.l #0x80, %macsr | put emac unit in signed int mode | 636 | move.l #0x80, %macsr | put emac unit in signed int mode |
532 | movem.l 36(%sp), %a0-%a3 | | 637 | movem.l 36(%sp), %a0-%a2 | %a0 = this, %a1 = src, %a2 = dst |
533 | lea.l (%a3, %a0.l*4), %a0 | %a0 = end address | 638 | move.l (%a0), %a0 | %a0 = this->outcount |
534 | move.l (%a1), %d1 | %d5 = multiplier: (1 << (16 - scale)) | 639 | move.l 4(%a2), %a3 | %a3 = dst->p16out |
640 | movem.l 4(%a1), %a2 | %a2 = src->p32[0] | ||
641 | lea.l (%a3, %a0.l*4), %a0 | %a0 = count -> end address | ||
642 | clr.l %d1 | %d5 = multiplier: (1 << (16 - scale)) | ||
643 | move.b 19(%a1), %d1 | %d1 = src->format.output_scale | ||
535 | sub.l #16, %d1 | | 644 | sub.l #16, %d1 | |
536 | neg.l %d1 | | 645 | neg.l %d1 | |
537 | moveq.l #1, %d5 | | 646 | moveq.l #1, %d5 | |
538 | asl.l %d1, %d5 | | 647 | asl.l %d1, %d5 | |
539 | move.l #0x8000, %a4 | %a4 = rounding term | 648 | move.l #0x8000, %a4 | %a4 = rounding term |
540 | movem.l (%a2), %a2 | get source channel pointer | ||
541 | moveq.l #28, %d0 | %d0 = second line bound | 649 | moveq.l #28, %d0 | %d0 = second line bound |
542 | add.l %a3, %d0 | | 650 | add.l %a3, %d0 | |
543 | and.l #0xfffffff0, %d0 | | 651 | and.l #0xfffffff0, %d0 | |
@@ -552,7 +660,7 @@ sample_output_mono: | |||
552 | mac.l %d1, %d5, %acc0 | shift L to high word | 660 | mac.l %d1, %d5, %acc0 | shift L to high word |
553 | movclr.l %acc0, %d1 | get possibly saturated results | 661 | movclr.l %acc0, %d1 | get possibly saturated results |
554 | move.l %d1, %d2 | | 662 | move.l %d1, %d2 | |
555 | swap %d2 | move R to low word | 663 | swap.w %d2 | move R to low word |
556 | move.w %d2, %d1 | duplicate single channel into | 664 | move.w %d2, %d1 | duplicate single channel into |
557 | move.l %d1, (%a3)+ | L and R | 665 | move.l %d1, (%a3)+ | L and R |
558 | cmp.l %a3, %d0 | | 666 | cmp.l %a3, %d0 | |
@@ -575,16 +683,16 @@ sample_output_mono: | |||
575 | movclr.l %acc2, %d2 | | 683 | movclr.l %acc2, %d2 | |
576 | movclr.l %acc3, %d3 | | 684 | movclr.l %acc3, %d3 | |
577 | move.l %d0, %d4 | duplicate single channel | 685 | move.l %d0, %d4 | duplicate single channel |
578 | swap %d4 | into L and R | 686 | swap.w %d4 | into L and R |
579 | move.w %d4, %d0 | | 687 | move.w %d4, %d0 | |
580 | move.l %d1, %d4 | | 688 | move.l %d1, %d4 | |
581 | swap %d4 | | 689 | swap.w %d4 | |
582 | move.w %d4, %d1 | | 690 | move.w %d4, %d1 | |
583 | move.l %d2, %d4 | | 691 | move.l %d2, %d4 | |
584 | swap %d4 | | 692 | swap.w %d4 | |
585 | move.w %d4, %d2 | | 693 | move.w %d4, %d2 | |
586 | move.l %d3, %d4 | | 694 | move.l %d3, %d4 | |
587 | swap %d4 | | 695 | swap.w %d4 | |
588 | move.w %d4, %d3 | | 696 | move.w %d4, %d3 | |
589 | movem.l %d0-%d3, -16(%a3) | write four stereo samples | 697 | movem.l %d0-%d3, -16(%a3) | write four stereo samples |
590 | cmp.l %a3, %a1 | | 698 | cmp.l %a3, %a1 | |
@@ -598,7 +706,7 @@ sample_output_mono: | |||
598 | mac.l %d1, %d5, %acc0 | the same way as leading ones | 706 | mac.l %d1, %d5, %acc0 | the same way as leading ones |
599 | movclr.l %acc0, %d1 | | 707 | movclr.l %acc0, %d1 | |
600 | move.l %d1, %d2 | | 708 | move.l %d1, %d2 | |
601 | swap %d2 | | 709 | swap.w %d2 | |
602 | move.w %d2, %d1 | | 710 | move.w %d2, %d1 | |
603 | move.l %d1, (%a3)+ | | 711 | move.l %d1, (%a3)+ | |
604 | cmp.l %a3, %a0 | | 712 | cmp.l %a3, %a0 | |
diff --git a/lib/rbcodec/dsp/dsp_core.c b/lib/rbcodec/dsp/dsp_core.c new file mode 100644 index 0000000000..84fe64adb0 --- /dev/null +++ b/lib/rbcodec/dsp/dsp_core.c | |||
@@ -0,0 +1,554 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
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_sample_io.h" | ||
26 | #include <sys/types.h> | ||
27 | |||
28 | /* Define LOGF_ENABLE to enable logf output in this file */ | ||
29 | /*#define LOGF_ENABLE*/ | ||
30 | #include "logf.h" | ||
31 | |||
32 | /* Actually generate the database of stages */ | ||
33 | #define DSP_PROC_DB_CREATE | ||
34 | #include "dsp_proc_entry.h" | ||
35 | |||
36 | /* Linked lists give fewer loads in processing loop compared to some index | ||
37 | * list, which is more important than keeping occasionally executed code | ||
38 | * simple */ | ||
39 | |||
40 | struct dsp_config | ||
41 | { | ||
42 | /** General DSP-local data **/ | ||
43 | struct sample_io_data io_data; /* Sample input-output data (first) */ | ||
44 | uint32_t slot_free_mask; /* Mask of free slots for this DSP */ | ||
45 | uint32_t proc_masks[2]; /* Mask of active/enabled stages */ | ||
46 | struct dsp_proc_slot | ||
47 | { | ||
48 | struct dsp_proc_entry proc_entry; /* This enabled stage */ | ||
49 | struct dsp_proc_slot *next[2]; /* [0]=active next, [1]=enabled next */ | ||
50 | const struct dsp_proc_db_entry *db_entry; | ||
51 | } *proc_slots[2]; /* Pointer to first in list of | ||
52 | active/enabled stages */ | ||
53 | |||
54 | /** Misc. extra stuff **/ | ||
55 | #ifdef CPU_COLDFIRE | ||
56 | unsigned long old_macsr; /* Old macsr value to restore */ | ||
57 | #endif | ||
58 | #if 0 /* Not needed now but enable if something must know this */ | ||
59 | bool processing; /* DSP is processing (to thwart inopportune | ||
60 | buffer moves) */ | ||
61 | #endif | ||
62 | }; | ||
63 | |||
64 | /* Pool of slots for stages - supports 32 or fewer combined as-is atm. */ | ||
65 | static struct dsp_proc_slot | ||
66 | dsp_proc_slot_arr[DSP_NUM_PROC_STAGES+DSP_VOICE_NUM_PROC_STAGES] IBSS_ATTR; | ||
67 | |||
68 | /* General DSP config */ | ||
69 | static struct dsp_config dsp_conf[DSP_COUNT] IBSS_ATTR; | ||
70 | |||
71 | /** Processing stages support functions **/ | ||
72 | |||
73 | /* Find the slot for a given enabled id */ | ||
74 | static struct dsp_proc_slot * find_proc_slot(struct dsp_config *dsp, | ||
75 | unsigned int id) | ||
76 | { | ||
77 | const uint32_t mask = BIT_N(id); | ||
78 | |||
79 | if ((dsp->proc_masks[1] & mask) == 0) | ||
80 | return NULL; /* Not enabled */ | ||
81 | |||
82 | struct dsp_proc_slot *s = dsp->proc_slots[1]; | ||
83 | |||
84 | while (1) /* In proc_masks == it must be there */ | ||
85 | { | ||
86 | if (BIT_N(s->db_entry->id) == mask) | ||
87 | return s; | ||
88 | |||
89 | s = s->next[1]; | ||
90 | } | ||
91 | } | ||
92 | |||
93 | /* Broadcast to all enabled stages or to the one with the specifically | ||
94 | * crafted setting */ | ||
95 | static intptr_t proc_broadcast(struct dsp_config *dsp, unsigned int setting, | ||
96 | intptr_t value) | ||
97 | { | ||
98 | bool multi = setting < DSP_PROC_SETTING; | ||
99 | struct dsp_proc_slot *s = multi ? | ||
100 | dsp->proc_slots[1] : find_proc_slot(dsp, setting - DSP_PROC_SETTING); | ||
101 | |||
102 | while (s != NULL) | ||
103 | { | ||
104 | intptr_t ret = s->db_entry->configure(&s->proc_entry, dsp, setting, | ||
105 | value); | ||
106 | if (!multi) | ||
107 | return ret; | ||
108 | |||
109 | s = s->next[1]; | ||
110 | } | ||
111 | |||
112 | return multi ? 1 : 0; | ||
113 | } | ||
114 | |||
115 | /* Generic handler for this->process[0] */ | ||
116 | static void dsp_process_null(struct dsp_proc_entry *this, | ||
117 | struct dsp_buffer **buf_p) | ||
118 | { | ||
119 | (void)this; (void)buf_p; | ||
120 | } | ||
121 | |||
122 | /* Generic handler for this->process[1] */ | ||
123 | static void dsp_format_change_process(struct dsp_proc_entry *this, | ||
124 | struct dsp_buffer **buf_p) | ||
125 | { | ||
126 | enum dsp_proc_ids id = | ||
127 | TYPE_FROM_MEMBER(struct dsp_proc_slot, this, proc_entry)->db_entry->id; | ||
128 | |||
129 | DSP_PRINT_FORMAT(<Default Handler>, id, (*buf_p)->format); | ||
130 | |||
131 | /* We don't keep back references to the DSP, so just search for it */ | ||
132 | struct dsp_config *dsp; | ||
133 | for (int i = 0; (dsp = dsp_get_config(i)); i++) | ||
134 | { | ||
135 | struct dsp_proc_slot *slot = find_proc_slot(dsp, id); | ||
136 | /* Found one with the id, check if it's this one */ | ||
137 | if (&slot->proc_entry == this && dsp_proc_active(dsp, id)) | ||
138 | { | ||
139 | dsp_proc_call(this, buf_p, 0); | ||
140 | break; | ||
141 | } | ||
142 | } | ||
143 | } | ||
144 | |||
145 | /* Add an item to the enabled list */ | ||
146 | static struct dsp_proc_slot * | ||
147 | dsp_proc_enable_enlink(struct dsp_config *dsp, uint32_t mask) | ||
148 | { | ||
149 | /* Use the lowest-indexed available slot */ | ||
150 | int slot = find_first_set_bit(dsp->slot_free_mask); | ||
151 | |||
152 | if (slot == 32) | ||
153 | { | ||
154 | /* Should NOT happen, ever, unless called before init */ | ||
155 | DEBUGF("DSP %d: no slots!\n", (int)dsp_get_id(dsp)); | ||
156 | return NULL; | ||
157 | } | ||
158 | |||
159 | const struct dsp_proc_db_entry *db_entry_prev = NULL; | ||
160 | const struct dsp_proc_db_entry *db_entry; | ||
161 | |||
162 | /* Order of enabled list is same as DB array */ | ||
163 | for (unsigned int i = 0;; i++) | ||
164 | { | ||
165 | if (i >= DSP_NUM_PROC_STAGES) | ||
166 | return NULL; | ||
167 | |||
168 | db_entry = dsp_proc_database[i]; | ||
169 | |||
170 | uint32_t m = BIT_N(db_entry->id); | ||
171 | |||
172 | if (m == mask) | ||
173 | break; /* This is the one */ | ||
174 | |||
175 | if (dsp->proc_masks[1] & m) | ||
176 | db_entry_prev = db_entry; | ||
177 | } | ||
178 | |||
179 | struct dsp_proc_slot *s = &dsp_proc_slot_arr[slot]; | ||
180 | |||
181 | if (db_entry_prev != NULL) | ||
182 | { | ||
183 | struct dsp_proc_slot *prev = find_proc_slot(dsp, db_entry_prev->id); | ||
184 | s->next[0] = prev->next[0]; | ||
185 | s->next[1] = prev->next[1]; | ||
186 | prev->next[1] = s; | ||
187 | } | ||
188 | else | ||
189 | { | ||
190 | s->next[0] = dsp->proc_slots[0]; | ||
191 | s->next[1] = dsp->proc_slots[1]; | ||
192 | dsp->proc_slots[1] = s; | ||
193 | } | ||
194 | |||
195 | s->db_entry = db_entry; /* record DB entry */ | ||
196 | dsp->proc_masks[1] |= mask; | ||
197 | dsp->slot_free_mask &= ~BIT_N(slot); | ||
198 | |||
199 | return s; | ||
200 | } | ||
201 | |||
202 | /* Remove an item from the enabled list */ | ||
203 | static struct dsp_proc_slot * | ||
204 | dsp_proc_enable_delink(struct dsp_config *dsp, uint32_t mask) | ||
205 | { | ||
206 | struct dsp_proc_slot *s = dsp->proc_slots[1]; | ||
207 | struct dsp_proc_slot *prev = NULL; | ||
208 | |||
209 | while (1) /* In proc_masks == it must be there */ | ||
210 | { | ||
211 | if (BIT_N(s->db_entry->id) == mask) | ||
212 | { | ||
213 | if (prev) | ||
214 | prev->next[1] = s->next[1]; | ||
215 | else | ||
216 | dsp->proc_slots[1] = s->next[1]; | ||
217 | |||
218 | dsp->proc_masks[1] &= ~mask; | ||
219 | dsp->slot_free_mask |= BIT_N(s - dsp_proc_slot_arr); | ||
220 | return s; | ||
221 | } | ||
222 | |||
223 | prev = s; | ||
224 | s = s->next[1]; | ||
225 | } | ||
226 | } | ||
227 | |||
228 | void dsp_proc_enable(struct dsp_config *dsp, enum dsp_proc_ids id, | ||
229 | bool enable) | ||
230 | { | ||
231 | uint32_t mask = BIT_N(id); | ||
232 | bool enabled = dsp->proc_masks[1] & mask; | ||
233 | |||
234 | if (enable) | ||
235 | { | ||
236 | /* If enabled, just find it in list, if not, link a new one */ | ||
237 | struct dsp_proc_slot *s = enabled ? find_proc_slot(dsp, id) : | ||
238 | dsp_proc_enable_enlink(dsp, mask); | ||
239 | |||
240 | if (s == NULL) | ||
241 | { | ||
242 | DEBUGF("DSP- proc id not valid: %d\n", (int)id); | ||
243 | return; | ||
244 | } | ||
245 | |||
246 | if (!enabled) | ||
247 | { | ||
248 | /* New entry - set defaults */ | ||
249 | s->proc_entry.data = 0; | ||
250 | s->proc_entry.ip_mask = mask; | ||
251 | s->proc_entry.process[0] = dsp_process_null; | ||
252 | s->proc_entry.process[1] = dsp_format_change_process; | ||
253 | } | ||
254 | |||
255 | enabled = s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_INIT, | ||
256 | enabled) >= 0; | ||
257 | if (enabled) | ||
258 | return; | ||
259 | |||
260 | DEBUGF("DSP- proc init failed: %d\n", (int)id); | ||
261 | /* Cleanup below */ | ||
262 | } | ||
263 | else if (!enabled) | ||
264 | { | ||
265 | return; /* No change */ | ||
266 | } | ||
267 | |||
268 | dsp_proc_activate(dsp, id, false); /* Deactivate it first */ | ||
269 | struct dsp_proc_slot *s = dsp_proc_enable_delink(dsp, mask); | ||
270 | s->db_entry->configure(&s->proc_entry, dsp, DSP_PROC_CLOSE, 0); | ||
271 | } | ||
272 | |||
273 | /* Maintain the list structure for the active list where each enabled entry | ||
274 | * has a link to the next active item, even if not active which facilitates | ||
275 | * switching out of format change mode by a stage during a format change. | ||
276 | * When that happens, the iterator must jump over inactive but enabled | ||
277 | * stages after its current position. */ | ||
278 | static struct dsp_proc_slot * | ||
279 | dsp_proc_activate_link(struct dsp_config *dsp, uint32_t mask, | ||
280 | struct dsp_proc_slot *s) | ||
281 | { | ||
282 | uint32_t m = BIT_N(s->db_entry->id); | ||
283 | uint32_t mor = m | mask; | ||
284 | |||
285 | if (mor == m) /* Only if same single bit in common */ | ||
286 | { | ||
287 | dsp->proc_masks[0] |= mask; | ||
288 | return s; | ||
289 | } | ||
290 | else if (~mor == 0) /* Only if bits complement */ | ||
291 | { | ||
292 | dsp->proc_masks[0] &= mask; | ||
293 | return s->next[0]; | ||
294 | } | ||
295 | |||
296 | struct dsp_proc_slot *next = s->next[1]; | ||
297 | next = dsp_proc_activate_link(dsp, mask, next); | ||
298 | |||
299 | s->next[0] = next; | ||
300 | |||
301 | return (m & dsp->proc_masks[0]) ? s : next; | ||
302 | } | ||
303 | |||
304 | /* Activate or deactivate a stage */ | ||
305 | void dsp_proc_activate(struct dsp_config *dsp, enum dsp_proc_ids id, | ||
306 | bool activate) | ||
307 | { | ||
308 | const uint32_t mask = BIT_N(id); | ||
309 | |||
310 | if (!(dsp->proc_masks[1] & mask)) | ||
311 | return; /* Not enabled */ | ||
312 | |||
313 | if (activate != !(dsp->proc_masks[0] & mask)) | ||
314 | return; /* No change in state */ | ||
315 | |||
316 | /* Send mask bit if activating and ones complement if deactivating */ | ||
317 | dsp->proc_slots[0] = dsp_proc_activate_link( | ||
318 | dsp, activate ? mask : ~mask, dsp->proc_slots[1]); | ||
319 | } | ||
320 | |||
321 | /* Is the stage specified by the id currently active? */ | ||
322 | bool dsp_proc_active(struct dsp_config *dsp, enum dsp_proc_ids id) | ||
323 | { | ||
324 | return (dsp->proc_masks[0] & BIT_N(id)) != 0; | ||
325 | } | ||
326 | |||
327 | /* Determine by the rules if the processing function should be called */ | ||
328 | static FORCE_INLINE bool dsp_proc_should_call(struct dsp_proc_entry *this, | ||
329 | struct dsp_buffer *buf, | ||
330 | unsigned int fmt) | ||
331 | { | ||
332 | uint32_t ip_mask = this->ip_mask; | ||
333 | |||
334 | return UNLIKELY(fmt != 0) || /* Also pass override value */ | ||
335 | ip_mask == 0 || /* Not in-place */ | ||
336 | ((ip_mask & buf->proc_mask) == 0 && | ||
337 | (buf->proc_mask |= ip_mask, buf->remcount > 0)); | ||
338 | } | ||
339 | |||
340 | /* Call this->process[fmt] according to the rules (for external call) */ | ||
341 | bool dsp_proc_call(struct dsp_proc_entry *this, struct dsp_buffer **buf_p, | ||
342 | unsigned int fmt) | ||
343 | { | ||
344 | if (dsp_proc_should_call(this, *buf_p, fmt)) | ||
345 | { | ||
346 | this->process[fmt == (0u-1u) ? 0 : fmt](this, buf_p); | ||
347 | return true; | ||
348 | } | ||
349 | |||
350 | return false; | ||
351 | } | ||
352 | |||
353 | static inline void dsp_process_start(struct dsp_config *dsp) | ||
354 | { | ||
355 | #if defined(CPU_COLDFIRE) | ||
356 | /* set emac unit for dsp processing, and save old macsr, we're running in | ||
357 | codec thread context at this point, so can't clobber it */ | ||
358 | dsp->old_macsr = coldfire_get_macsr(); | ||
359 | coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE); | ||
360 | #endif | ||
361 | #if 0 /* Not needed now but enable if something must know this */ | ||
362 | dsp->processing = true; | ||
363 | #endif | ||
364 | (void)dsp; | ||
365 | } | ||
366 | |||
367 | static inline void dsp_process_end(struct dsp_config *dsp) | ||
368 | { | ||
369 | #if 0 /* Not needed now but enable if something must know this */ | ||
370 | dsp->processing = false; | ||
371 | #endif | ||
372 | #if defined(CPU_COLDFIRE) | ||
373 | /* set old macsr again */ | ||
374 | coldfire_set_macsr(dsp->old_macsr); | ||
375 | #endif | ||
376 | (void)dsp; | ||
377 | } | ||
378 | |||
379 | /** | ||
380 | * dsp_process: | ||
381 | * | ||
382 | * Process and convert src audio to dst based on the DSP configuration. | ||
383 | * dsp: the DSP instance in use | ||
384 | * | ||
385 | * src: | ||
386 | * remcount = number of input samples remaining; set to desired | ||
387 | * number of samples to be processed | ||
388 | * pin[0] = left channel if non-interleaved, audio data if | ||
389 | * interleaved or mono | ||
390 | * pin[1] = right channel if non-interleaved, ignored if | ||
391 | * interleaved or mono | ||
392 | * proc_mask = set to zero on first call, updated by this function | ||
393 | * to keep track of which in-place stages have been | ||
394 | * run on the buffers to avoid multiple applications of | ||
395 | * them | ||
396 | * format = for internal buffers, gives the relevant format | ||
397 | * details | ||
398 | * | ||
399 | * dst: | ||
400 | * remcount = number of samples placed in buffer so far; set to | ||
401 | * zero on first call | ||
402 | * p16out = current fill pointer in destination buffer; set to | ||
403 | * buffer start on first call | ||
404 | * bufcount = remaining buffer space in samples; set to maximum | ||
405 | * desired output count on first call | ||
406 | * format = ignored | ||
407 | * | ||
408 | * Processing stops when src is exhausted or dst is filled, whichever | ||
409 | * happens first. Samples can still be output when src buffer is empty | ||
410 | * if samples are held internally. Generally speaking, continue calling | ||
411 | * until no data is consumed and no data is produced to purge the DSP | ||
412 | * to the maximum extent feasible. Some internal processing stages may | ||
413 | * require more input before more output can be generated, thus there | ||
414 | * is no guarantee the DSP is free of data awaiting processing at that | ||
415 | * point. | ||
416 | * | ||
417 | * Additionally, samples consumed and samples produced do not necessarily | ||
418 | * have a direct correlation. Samples may be consumed without producing | ||
419 | * any output and samples may be produced without consuming any input. | ||
420 | * It depends on which stages are actively processing data at the time | ||
421 | * of the call and how they function internally. | ||
422 | */ | ||
423 | void dsp_process(struct dsp_config *dsp, struct dsp_buffer *src, | ||
424 | struct dsp_buffer *dst) | ||
425 | { | ||
426 | if (dst->bufcount <= 0) | ||
427 | { | ||
428 | /* No place to put anything thus nothing may be safely consumed */ | ||
429 | return; | ||
430 | } | ||
431 | |||
432 | /* At least perform one yield before starting */ | ||
433 | long last_yield = current_tick; | ||
434 | yield(); | ||
435 | |||
436 | dsp_process_start(dsp); | ||
437 | |||
438 | /* Tag input with codec-specified sample format */ | ||
439 | src->format = dsp->io_data.format; | ||
440 | |||
441 | while (1) | ||
442 | { | ||
443 | /* Out-of-place-processing stages take the current buf as input | ||
444 | * and switch the buffer to their own output buffer */ | ||
445 | struct dsp_buffer *buf = src; | ||
446 | unsigned int fmt = buf->format.changed; | ||
447 | |||
448 | /* Convert input samples to internal format */ | ||
449 | dsp->io_data.input_samples[fmt](&dsp->io_data, &buf); | ||
450 | fmt = buf->format.changed; | ||
451 | |||
452 | struct dsp_proc_slot *s = dsp->proc_slots[fmt]; | ||
453 | |||
454 | /* Call all active/enabled stages depending if format is | ||
455 | same/changed on the last output buffer */ | ||
456 | while (s != NULL) | ||
457 | { | ||
458 | if (dsp_proc_should_call(&s->proc_entry, buf, fmt)) | ||
459 | { | ||
460 | s->proc_entry.process[fmt](&s->proc_entry, &buf); | ||
461 | fmt = buf->format.changed; | ||
462 | } | ||
463 | |||
464 | /* The buffer may have changed along with the format flag */ | ||
465 | s = s->next[fmt]; | ||
466 | } | ||
467 | |||
468 | /* Don't overread/write src/destination */ | ||
469 | int outcount = MIN(dst->bufcount, buf->remcount); | ||
470 | |||
471 | if (fmt == 0 && outcount <= 0) | ||
472 | break; /* Output full or purged internal buffers */ | ||
473 | |||
474 | dsp->io_data.outcount = outcount; | ||
475 | dsp->io_data.output_samples[fmt](&dsp->io_data, buf, dst); | ||
476 | |||
477 | /* Advance buffers by what output consumed and produced */ | ||
478 | dsp_advance_buffer32(buf, outcount); | ||
479 | dsp_advance_buffer_output(dst, outcount); | ||
480 | |||
481 | /* Yield at least once each tick */ | ||
482 | long tick = current_tick; | ||
483 | if (TIME_AFTER(tick, last_yield)) | ||
484 | { | ||
485 | last_yield = tick; | ||
486 | yield(); | ||
487 | } | ||
488 | } /* while */ | ||
489 | |||
490 | dsp_process_end(dsp); | ||
491 | } | ||
492 | |||
493 | intptr_t dsp_configure(struct dsp_config *dsp, unsigned int setting, | ||
494 | intptr_t value) | ||
495 | { | ||
496 | dsp_sample_io_configure(&dsp->io_data, setting, value); | ||
497 | return proc_broadcast(dsp, setting, value); | ||
498 | } | ||
499 | |||
500 | struct dsp_config * dsp_get_config(enum dsp_ids id) | ||
501 | { | ||
502 | if (id >= DSP_COUNT) | ||
503 | return NULL; | ||
504 | |||
505 | return &dsp_conf[id]; | ||
506 | } | ||
507 | |||
508 | /* Return the id given a dsp pointer (or even via something within | ||
509 | the struct itself) */ | ||
510 | enum dsp_ids dsp_get_id(const struct dsp_config *dsp) | ||
511 | { | ||
512 | ptrdiff_t id = dsp - dsp_conf; | ||
513 | |||
514 | if (id < 0 || id >= DSP_COUNT) | ||
515 | return DSP_COUNT; /* obviously invalid */ | ||
516 | |||
517 | return (enum dsp_ids)id; | ||
518 | } | ||
519 | |||
520 | #if 0 /* Not needed now but enable if something must know this */ | ||
521 | bool dsp_is_busy(const struct dsp_config *dsp) | ||
522 | { | ||
523 | return dsp->processing; | ||
524 | } | ||
525 | #endif /* 0 */ | ||
526 | |||
527 | /* Do what needs initializing before enable/disable calls can be made. | ||
528 | * Must be done before changing settings for the first time. */ | ||
529 | void INIT_ATTR dsp_init(void) | ||
530 | { | ||
531 | static const uint8_t slot_count[DSP_COUNT] /* INITDATA_ATTR */ = | ||
532 | { | ||
533 | [CODEC_IDX_AUDIO] = DSP_NUM_PROC_STAGES, | ||
534 | [CODEC_IDX_VOICE] = DSP_VOICE_NUM_PROC_STAGES | ||
535 | }; | ||
536 | |||
537 | for (unsigned int i = 0, count, shift = 0; | ||
538 | i < DSP_COUNT; | ||
539 | i++, shift += count) | ||
540 | { | ||
541 | struct dsp_config *dsp = &dsp_conf[i]; | ||
542 | |||
543 | count = slot_count[i]; | ||
544 | dsp->slot_free_mask = MASK_N(uint32_t, count, shift); | ||
545 | |||
546 | dsp_sample_io_configure(&dsp->io_data, DSP_INIT, i); | ||
547 | |||
548 | /* Notify each db entry of global init for each DSP */ | ||
549 | for (unsigned int j = 0; j < DSP_NUM_PROC_STAGES; j++) | ||
550 | dsp_proc_database[j]->configure(NULL, dsp, DSP_INIT, i); | ||
551 | |||
552 | dsp_configure(dsp, DSP_RESET, 0); | ||
553 | } | ||
554 | } | ||
diff --git a/lib/rbcodec/dsp/dsp_filter.c b/lib/rbcodec/dsp/dsp_filter.c new file mode 100644 index 0000000000..ee0ce1b18f --- /dev/null +++ b/lib/rbcodec/dsp/dsp_filter.c | |||
@@ -0,0 +1,306 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006-2007 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include <stdbool.h> | ||
22 | #include <string.h> | ||
23 | #include "config.h" | ||
24 | #include "fixedpoint.h" | ||
25 | #include "fracmul.h" | ||
26 | #include "dsp_filter.h" | ||
27 | #include "replaygain.h" | ||
28 | |||
29 | enum filter_shift | ||
30 | { | ||
31 | FILTER_BISHELF_SHIFT = 5, /* For bishelf (bass/treble) */ | ||
32 | FILTER_PEAK_SHIFT = 4, /* Each peaking filter */ | ||
33 | FILTER_SHELF_SHIFT = 6, /* Each high/low shelving filter */ | ||
34 | }; | ||
35 | |||
36 | /** | ||
37 | * Calculate first order shelving filter. Filter is not directly usable by the | ||
38 | * filter_process() function. | ||
39 | * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format. | ||
40 | * @param A decibel value multiplied by ten, describing gain/attenuation of | ||
41 | * shelf. Max value is 24 dB. | ||
42 | * @param low true for low-shelf filter, false for high-shelf filter. | ||
43 | * @param c pointer to coefficient storage. Coefficients are s4.27 format. | ||
44 | */ | ||
45 | void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c) | ||
46 | { | ||
47 | long sin, cos; | ||
48 | int32_t b0, b1, a0, a1; /* s3.28 */ | ||
49 | const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */ | ||
50 | |||
51 | sin = fp_sincos(cutoff/2, &cos); | ||
52 | if (low) { | ||
53 | const int32_t sin_div_g = fp_div(sin, g, 25); | ||
54 | const int32_t sin_g = FRACMUL(sin, g); | ||
55 | cos >>= 3; | ||
56 | b0 = sin_g + cos; /* 0.25 .. 4.10 */ | ||
57 | b1 = sin_g - cos; /* -1 .. 3.98 */ | ||
58 | a0 = sin_div_g + cos; /* 0.25 .. 4.10 */ | ||
59 | a1 = sin_div_g - cos; /* -1 .. 3.98 */ | ||
60 | } else { | ||
61 | const int32_t cos_div_g = fp_div(cos, g, 25); | ||
62 | const int32_t cos_g = FRACMUL(cos, g); | ||
63 | sin >>= 3; | ||
64 | b0 = sin + cos_g; /* 0.25 .. 4.10 */ | ||
65 | b1 = sin - cos_g; /* -3.98 .. 1 */ | ||
66 | a0 = sin + cos_div_g; /* 0.25 .. 4.10 */ | ||
67 | a1 = sin - cos_div_g; /* -3.98 .. 1 */ | ||
68 | } | ||
69 | |||
70 | const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */ | ||
71 | *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */ | ||
72 | *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */ | ||
73 | *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */ | ||
74 | } | ||
75 | |||
76 | #ifdef HAVE_SW_TONE_CONTROLS | ||
77 | /** | ||
78 | * Calculate second order section filter consisting of one low-shelf and one | ||
79 | * high-shelf section. | ||
80 | * @param cutoff_low low-shelf midpoint frequency. See filter_pk_coefs for format. | ||
81 | * @param cutoff_high high-shelf midpoint frequency. | ||
82 | * @param A_low decibel value multiplied by ten, describing gain/attenuation of | ||
83 | * low-shelf part. Max value is 24 dB. | ||
84 | * @param A_high decibel value multiplied by ten, describing gain/attenuation of | ||
85 | * high-shelf part. Max value is 24 dB. | ||
86 | * @param A decibel value multiplied by ten, describing additional overall gain. | ||
87 | * @param c pointer to coefficient storage. Coefficients are s4.27 format. | ||
88 | */ | ||
89 | void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high, | ||
90 | long A_low, long A_high, long A, | ||
91 | struct dsp_filter *f) | ||
92 | { | ||
93 | const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */ | ||
94 | int32_t c_ls[3], c_hs[3]; | ||
95 | |||
96 | filter_shelf_coefs(cutoff_low, A_low, true, c_ls); | ||
97 | filter_shelf_coefs(cutoff_high, A_high, false, c_hs); | ||
98 | c_ls[0] = FRACMUL(g, c_ls[0]); | ||
99 | c_ls[1] = FRACMUL(g, c_ls[1]); | ||
100 | |||
101 | /* now we cascade the two first order filters to one second order filter | ||
102 | * which can be used by filter_process(). these resulting coefficients have a | ||
103 | * really wide numerical range, so we use a fixed point format which will | ||
104 | * work for the selected cutoff frequencies (in tone_controls.c) only. | ||
105 | */ | ||
106 | const int32_t b0 = c_ls[0], b1 = c_ls[1], b2 = c_hs[0], b3 = c_hs[1]; | ||
107 | const int32_t a0 = c_ls[2], a1 = c_hs[2]; | ||
108 | |||
109 | int32_t *c = f->coefs; | ||
110 | *c++ = FRACMUL_SHL(b0, b2, 4); | ||
111 | *c++ = FRACMUL_SHL(b0, b3, 4) + FRACMUL_SHL(b1, b2, 4); | ||
112 | *c++ = FRACMUL_SHL(b1, b3, 4); | ||
113 | *c++ = a0 + a1; | ||
114 | *c = -FRACMUL_SHL(a0, a1, 4); | ||
115 | |||
116 | f->shift = FILTER_BISHELF_SHIFT; | ||
117 | } | ||
118 | #endif /* HAVE_SW_TONE_CONTROLS */ | ||
119 | |||
120 | /* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson. | ||
121 | * Slightly faster calculation can be done by deriving forms which use tan() | ||
122 | * instead of cos() and sin(), but the latter are far easier to use when doing | ||
123 | * fixed point math, and performance is not a big point in the calculation part. | ||
124 | * All the 'a' filter coefficients are negated so we can use only additions | ||
125 | * in the filtering equation. | ||
126 | */ | ||
127 | |||
128 | /** | ||
129 | * Calculate second order section peaking filter coefficients. | ||
130 | * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and | ||
131 | * 0x80000000 represents the Nyquist frequency (samplerate/2). | ||
132 | * @param Q Q factor value multiplied by ten. Lower bound is artificially set | ||
133 | * at 0.5. | ||
134 | * @param db decibel value multiplied by ten, describing gain/attenuation at | ||
135 | * peak freq. Max value is 24 dB. | ||
136 | * @param c pointer to coefficient storage. Coefficients are s3.28 format. | ||
137 | */ | ||
138 | void filter_pk_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
139 | struct dsp_filter *f) | ||
140 | { | ||
141 | long cs; | ||
142 | const long one = 1 << 28; /* s3.28 */ | ||
143 | const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */ | ||
144 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
145 | int32_t a0, a1, a2; /* these are all s3.28 format */ | ||
146 | int32_t b0, b1, b2; | ||
147 | const long alphadivA = fp_div(alpha, A, 27); | ||
148 | const long alphaA = FRACMUL(alpha, A); | ||
149 | |||
150 | /* possible numerical ranges are in comments by each coef */ | ||
151 | b0 = one + alphaA; /* [1 .. 5] */ | ||
152 | b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */ | ||
153 | b2 = one - alphaA; /* [-3 .. 1] */ | ||
154 | a0 = one + alphadivA; /* [1 .. 5] */ | ||
155 | a2 = one - alphadivA; /* [-3 .. 1] */ | ||
156 | |||
157 | /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */ | ||
158 | int32_t *c = f->coefs; | ||
159 | const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */ | ||
160 | *c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */ | ||
161 | *c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */ | ||
162 | *c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */ | ||
163 | *c++ = FRACMUL(-a1, rcp_a0); /* [-2 .. 2] */ | ||
164 | *c = FRACMUL(-a2, rcp_a0); /* [-0.6 .. 1] */ | ||
165 | |||
166 | f->shift = FILTER_PEAK_SHIFT; | ||
167 | } | ||
168 | |||
169 | /** | ||
170 | * Calculate coefficients for lowshelf filter. Parameters are as for | ||
171 | * filter_pk_coefs, but the coefficient format is s5.26 fixed point. | ||
172 | */ | ||
173 | void filter_ls_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
174 | struct dsp_filter *f) | ||
175 | { | ||
176 | long cs; | ||
177 | const long one = 1 << 25; /* s6.25 */ | ||
178 | const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ | ||
179 | const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ | ||
180 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
181 | const long ap1 = (A >> 4) + one; | ||
182 | const long am1 = (A >> 4) - one; | ||
183 | const long ap1_cs = FRACMUL(ap1, cs); | ||
184 | const long am1_cs = FRACMUL(am1, cs); | ||
185 | const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); | ||
186 | int32_t a0, a1, a2; /* these are all s6.25 format */ | ||
187 | int32_t b0, b1, b2; | ||
188 | |||
189 | /* [0.1 .. 40] */ | ||
190 | b0 = FRACMUL_SHL(A, ap1 - am1_cs + twosqrtalpha, 2); | ||
191 | /* [-16 .. 63.4] */ | ||
192 | b1 = FRACMUL_SHL(A, am1 - ap1_cs, 3); | ||
193 | /* [0 .. 31.7] */ | ||
194 | b2 = FRACMUL_SHL(A, ap1 - am1_cs - twosqrtalpha, 2); | ||
195 | /* [0.5 .. 10] */ | ||
196 | a0 = ap1 + am1_cs + twosqrtalpha; | ||
197 | /* [-16 .. 4] */ | ||
198 | a1 = -2*(am1 + ap1_cs); | ||
199 | /* [0 .. 8] */ | ||
200 | a2 = ap1 + am1_cs - twosqrtalpha; | ||
201 | |||
202 | /* [0.1 .. 1.99] */ | ||
203 | int32_t *c = f->coefs; | ||
204 | const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ | ||
205 | *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */ | ||
206 | *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */ | ||
207 | *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */ | ||
208 | *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */ | ||
209 | *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */ | ||
210 | |||
211 | f->shift = FILTER_SHELF_SHIFT; | ||
212 | } | ||
213 | |||
214 | /** | ||
215 | * Calculate coefficients for highshelf filter. Parameters are as for | ||
216 | * filter_pk_coefs, but the coefficient format is s5.26 fixed point. | ||
217 | */ | ||
218 | void filter_hs_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
219 | struct dsp_filter *f) | ||
220 | { | ||
221 | long cs; | ||
222 | const long one = 1 << 25; /* s6.25 */ | ||
223 | const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ | ||
224 | const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ | ||
225 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
226 | const long ap1 = (A >> 4) + one; | ||
227 | const long am1 = (A >> 4) - one; | ||
228 | const long ap1_cs = FRACMUL(ap1, cs); | ||
229 | const long am1_cs = FRACMUL(am1, cs); | ||
230 | const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); | ||
231 | int32_t a0, a1, a2; /* these are all s6.25 format */ | ||
232 | int32_t b0, b1, b2; | ||
233 | |||
234 | /* [0.1 .. 40] */ | ||
235 | b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2); | ||
236 | /* [-63.5 .. 16] */ | ||
237 | b1 = -FRACMUL_SHL(A, am1 + ap1_cs, 3); | ||
238 | /* [0 .. 32] */ | ||
239 | b2 = FRACMUL_SHL(A, ap1 + am1_cs - twosqrtalpha, 2); | ||
240 | /* [0.5 .. 10] */ | ||
241 | a0 = ap1 - am1_cs + twosqrtalpha; | ||
242 | /* [-4 .. 16] */ | ||
243 | a1 = 2*(am1 - ap1_cs); | ||
244 | /* [0 .. 8] */ | ||
245 | a2 = ap1 - am1_cs - twosqrtalpha; | ||
246 | |||
247 | /* [0.1 .. 1.99] */ | ||
248 | int32_t *c = f->coefs; | ||
249 | const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ | ||
250 | *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */ | ||
251 | *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */ | ||
252 | *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */ | ||
253 | *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */ | ||
254 | *c = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */ | ||
255 | |||
256 | f->shift = FILTER_SHELF_SHIFT; | ||
257 | } | ||
258 | |||
259 | /** | ||
260 | * Copy filter definition without destroying dst's history | ||
261 | */ | ||
262 | void filter_copy(struct dsp_filter *dst, const struct dsp_filter *src) | ||
263 | { | ||
264 | memcpy(dst->coefs, src->coefs, sizeof (src->coefs)); | ||
265 | dst->shift = src->shift; | ||
266 | } | ||
267 | |||
268 | /** | ||
269 | * Clear filter sample history | ||
270 | */ | ||
271 | void filter_flush(struct dsp_filter *f) | ||
272 | { | ||
273 | memset(f->history, 0, sizeof (f->history)); | ||
274 | } | ||
275 | |||
276 | /** | ||
277 | * We realise the filters as a second order direct form 1 structure. Direct | ||
278 | * form 1 was chosen because of better numerical properties for fixed point | ||
279 | * implementations. | ||
280 | */ | ||
281 | #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) | ||
282 | void filter_process(struct dsp_filter *f, int32_t * const buf[], int count, | ||
283 | unsigned int channels) | ||
284 | { | ||
285 | /* Direct form 1 filtering code. | ||
286 | y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | ||
287 | where y[] is output and x[] is input. | ||
288 | */ | ||
289 | unsigned int shift = f->shift; | ||
290 | |||
291 | for (unsigned int c = 0; c < channels; c++) { | ||
292 | for (int i = 0; i < count; i++) { | ||
293 | long long acc = (long long) buf[c][i] * f->coefs[0]; | ||
294 | acc += (long long) f->history[c][0] * f->coefs[1]; | ||
295 | acc += (long long) f->history[c][1] * f->coefs[2]; | ||
296 | acc += (long long) f->history[c][2] * f->coefs[3]; | ||
297 | acc += (long long) f->history[c][3] * f->coefs[4]; | ||
298 | f->history[c][1] = f->history[c][0]; | ||
299 | f->history[c][0] = buf[c][i]; | ||
300 | f->history[c][3] = f->history[c][2]; | ||
301 | buf[c][i] = (acc << shift) >> 32; | ||
302 | f->history[c][2] = buf[c][i]; | ||
303 | } | ||
304 | } | ||
305 | } | ||
306 | #endif /* CPU */ | ||
diff --git a/lib/rbcodec/dsp/dsp_filter.h b/lib/rbcodec/dsp/dsp_filter.h new file mode 100644 index 0000000000..af6e20ce86 --- /dev/null +++ b/lib/rbcodec/dsp/dsp_filter.h | |||
@@ -0,0 +1,57 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006-2007 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef DSP_FILTER_H | ||
22 | #define DSP_FILTER_H | ||
23 | |||
24 | /** Basic filter implementations which may be used independently **/ | ||
25 | |||
26 | /* Used by: EQ, tone controls and crossfeed */ | ||
27 | |||
28 | /* These depend on the fixed point formats used by the different filter types | ||
29 | and need to be changed when they change. | ||
30 | */ | ||
31 | struct dsp_filter | ||
32 | { | ||
33 | int32_t coefs[5]; /* 00h: Order is b0, b1, b2, a1, a2 */ | ||
34 | int32_t history[2][4]; /* 14h: Order is x-1, x-2, y-1, y-2, per channel */ | ||
35 | uint8_t shift; /* 34h: Final shift after computation */ | ||
36 | /* 38h */ | ||
37 | }; | ||
38 | |||
39 | void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c); | ||
40 | #ifdef HAVE_SW_TONE_CONTROLS | ||
41 | void filter_bishelf_coefs(unsigned long cutoff_low, | ||
42 | unsigned long cutoff_high, | ||
43 | long A_low, long A_high, long A, | ||
44 | struct dsp_filter *f); | ||
45 | #endif /* HAVE_SW_TONE_CONTROLS */ | ||
46 | void filter_pk_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
47 | struct dsp_filter *f); | ||
48 | void filter_ls_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
49 | struct dsp_filter *f); | ||
50 | void filter_hs_coefs(unsigned long cutoff, unsigned long Q, long db, | ||
51 | struct dsp_filter *f); | ||
52 | void filter_copy(struct dsp_filter *dst, const struct dsp_filter *src); | ||
53 | void filter_flush(struct dsp_filter *f); | ||
54 | void filter_process(struct dsp_filter *f, int32_t * const buf[], int count, | ||
55 | unsigned int channels); | ||
56 | |||
57 | #endif /* DSP_FILTER_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp_misc.c b/lib/rbcodec/dsp/dsp_misc.c new file mode 100644 index 0000000000..7b4589151c --- /dev/null +++ b/lib/rbcodec/dsp/dsp_misc.c | |||
@@ -0,0 +1,238 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
11 | * Copyright (C) 2005 Magnus Holmgren | ||
12 | * Copyright (C) 2007 Thom Johansen | ||
13 | * Copyright (C) 2012 Michael Sevakis | ||
14 | * | ||
15 | * This program is free software; you can redistribute it and/or | ||
16 | * modify it under the terms of the GNU General Public License | ||
17 | * as published by the Free Software Foundation; either version 2 | ||
18 | * of the License, or (at your option) any later version. | ||
19 | * | ||
20 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
21 | * KIND, either express or implied. | ||
22 | * | ||
23 | ****************************************************************************/ | ||
24 | #include "config.h" | ||
25 | #include "system.h" | ||
26 | #include "dsp.h" | ||
27 | #include "dsp_sample_io.h" | ||
28 | #include "replaygain.h" | ||
29 | #include "sound.h" | ||
30 | #include "settings.h" | ||
31 | #include "fixedpoint.h" | ||
32 | #include <string.h> | ||
33 | #include "dsp_proc_entry.h" | ||
34 | |||
35 | /** Firmware callback interface **/ | ||
36 | |||
37 | /* Hook back from firmware/ part of audio, which can't/shouldn't call apps/ | ||
38 | * code directly. */ | ||
39 | int dsp_callback(int msg, intptr_t param) | ||
40 | { | ||
41 | switch (msg) | ||
42 | { | ||
43 | #ifdef HAVE_SW_TONE_CONTROLS | ||
44 | case DSP_CALLBACK_SET_PRESCALE: | ||
45 | tone_set_prescale(param); | ||
46 | break; | ||
47 | case DSP_CALLBACK_SET_BASS: | ||
48 | tone_set_bass(param); | ||
49 | break; | ||
50 | case DSP_CALLBACK_SET_TREBLE: | ||
51 | tone_set_treble(param); | ||
52 | break; | ||
53 | /* FIXME: This must be done by bottom-level PCM driver so it works with | ||
54 | all PCM, not here and not in mixer. I won't fully support it | ||
55 | here with all streams. -- jethead71 */ | ||
56 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
57 | case DSP_CALLBACK_SET_SW_VOLUME: | ||
58 | if (global_settings.volume < SW_VOLUME_MAX || | ||
59 | global_settings.volume > SW_VOLUME_MIN) | ||
60 | { | ||
61 | int vol_gain = get_replaygain_int(global_settings.volume * 100); | ||
62 | pga_set_gain(PGA_VOLUME, vol_gain); | ||
63 | } | ||
64 | break; | ||
65 | #endif /* HAVE_SW_VOLUME_CONTROL */ | ||
66 | #endif /* HAVE_SW_TONE_CONTROLS */ | ||
67 | case DSP_CALLBACK_SET_CHANNEL_CONFIG: | ||
68 | channel_mode_set_config(param); | ||
69 | break; | ||
70 | case DSP_CALLBACK_SET_STEREO_WIDTH: | ||
71 | channel_mode_custom_set_width(param); | ||
72 | break; | ||
73 | default: | ||
74 | break; | ||
75 | } | ||
76 | |||
77 | return 0; | ||
78 | } | ||
79 | |||
80 | /** Replaygain settings **/ | ||
81 | static struct dsp_replay_gains current_rpgains; | ||
82 | |||
83 | static void dsp_replaygain_update(const struct dsp_replay_gains *gains) | ||
84 | { | ||
85 | if (gains == NULL) | ||
86 | { | ||
87 | /* Use defaults */ | ||
88 | memset(¤t_rpgains, 0, sizeof (current_rpgains)); | ||
89 | gains = ¤t_rpgains; | ||
90 | } | ||
91 | else | ||
92 | { | ||
93 | current_rpgains = *gains; /* Stash settings */ | ||
94 | } | ||
95 | |||
96 | int32_t gain = PGA_UNITY; | ||
97 | |||
98 | if (global_settings.replaygain_type != REPLAYGAIN_OFF || | ||
99 | global_settings.replaygain_noclip) | ||
100 | { | ||
101 | bool track_mode = | ||
102 | get_replaygain_mode(gains->track_gain != 0, | ||
103 | gains->album_gain != 0) == REPLAYGAIN_TRACK; | ||
104 | |||
105 | int32_t peak = (track_mode || gains->album_peak == 0) ? | ||
106 | gains->track_peak : gains->album_peak; | ||
107 | |||
108 | if (global_settings.replaygain_type != REPLAYGAIN_OFF) | ||
109 | { | ||
110 | gain = (track_mode || gains->album_gain == 0) ? | ||
111 | gains->track_gain : gains->album_gain; | ||
112 | |||
113 | if (global_settings.replaygain_preamp) | ||
114 | { | ||
115 | int32_t preamp = get_replaygain_int( | ||
116 | global_settings.replaygain_preamp * 10); | ||
117 | |||
118 | gain = fp_mul(gain, preamp, 24); | ||
119 | } | ||
120 | } | ||
121 | |||
122 | if (gain == 0) | ||
123 | { | ||
124 | /* So that noclip can work even with no gain information. */ | ||
125 | gain = PGA_UNITY; | ||
126 | } | ||
127 | |||
128 | if (global_settings.replaygain_noclip && peak != 0 && | ||
129 | fp_mul(gain, peak, 24) >= PGA_UNITY) | ||
130 | { | ||
131 | gain = fp_div(PGA_UNITY, peak, 24); | ||
132 | } | ||
133 | } | ||
134 | |||
135 | pga_set_gain(PGA_REPLAYGAIN, gain); | ||
136 | pga_enable_gain(PGA_REPLAYGAIN, gain != PGA_UNITY); | ||
137 | } | ||
138 | |||
139 | int get_replaygain_mode(bool have_track_gain, bool have_album_gain) | ||
140 | { | ||
141 | bool track = false; | ||
142 | |||
143 | switch (global_settings.replaygain_type) | ||
144 | { | ||
145 | case REPLAYGAIN_TRACK: | ||
146 | track = true; | ||
147 | break; | ||
148 | |||
149 | case REPLAYGAIN_SHUFFLE: | ||
150 | track = global_settings.playlist_shuffle; | ||
151 | break; | ||
152 | } | ||
153 | |||
154 | return (!track && have_album_gain) ? | ||
155 | REPLAYGAIN_ALBUM : (have_track_gain ? REPLAYGAIN_TRACK : -1); | ||
156 | } | ||
157 | |||
158 | void dsp_set_replaygain(void) | ||
159 | { | ||
160 | dsp_replaygain_update(¤t_rpgains); | ||
161 | } | ||
162 | |||
163 | |||
164 | /** Pitch Settings **/ | ||
165 | |||
166 | #ifdef HAVE_PITCHSCREEN | ||
167 | static int32_t pitch_ratio = PITCH_SPEED_100; | ||
168 | |||
169 | static void dsp_pitch_update(struct dsp_config *dsp) | ||
170 | { | ||
171 | /* Account for playback speed adjustment when setting dsp->frequency | ||
172 | if we're called from the main audio thread. Voice playback thread | ||
173 | does not support this feature. */ | ||
174 | struct sample_io_data *data = (void *)dsp; | ||
175 | data->format.frequency = | ||
176 | (int64_t)pitch_ratio * data->format.codec_frequency / PITCH_SPEED_100; | ||
177 | } | ||
178 | |||
179 | int32_t sound_get_pitch(void) | ||
180 | { | ||
181 | return pitch_ratio; | ||
182 | } | ||
183 | |||
184 | void sound_set_pitch(int32_t percent) | ||
185 | { | ||
186 | pitch_ratio = percent > 0 ? percent : PITCH_SPEED_100; | ||
187 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
188 | struct sample_io_data *data = (void *)dsp; | ||
189 | dsp_configure(dsp, DSP_SWITCH_FREQUENCY, data->format.codec_frequency); | ||
190 | } | ||
191 | #endif /* HAVE_PITCHSCREEN */ | ||
192 | |||
193 | /* This is a null-processing stage that monitors as an enabled stage but never | ||
194 | * becomes active in processing samples. It only hooks messages. */ | ||
195 | |||
196 | /* DSP message hook */ | ||
197 | static intptr_t misc_handler_configure(struct dsp_proc_entry *this, | ||
198 | struct dsp_config *dsp, | ||
199 | unsigned setting, | ||
200 | intptr_t value) | ||
201 | { | ||
202 | switch (setting) | ||
203 | { | ||
204 | case DSP_INIT: | ||
205 | /* Enable us for the audio DSP at startup */ | ||
206 | if (value == CODEC_IDX_AUDIO) | ||
207 | dsp_proc_enable(dsp, DSP_PROC_MISC_HANDLER, true); | ||
208 | break; | ||
209 | |||
210 | case DSP_PROC_CLOSE: | ||
211 | /* This stage should be enabled at all times */ | ||
212 | DEBUGF("DSP_PROC_MISC_HANDLER - Error: Closing!\n"); | ||
213 | break; | ||
214 | |||
215 | case DSP_RESET: | ||
216 | #ifdef HAVE_PITCHSCREEN | ||
217 | dsp_pitch_update(dsp); | ||
218 | #endif | ||
219 | value = (intptr_t)NULL; /* Default gains */ | ||
220 | case REPLAYGAIN_SET_GAINS: | ||
221 | dsp_replaygain_update((void *)value); | ||
222 | break; | ||
223 | |||
224 | #ifdef HAVE_PITCHSCREEN | ||
225 | case DSP_SET_FREQUENCY: | ||
226 | dsp_pitch_update(dsp); | ||
227 | break; | ||
228 | #endif | ||
229 | } | ||
230 | |||
231 | return 1; | ||
232 | (void)this; | ||
233 | } | ||
234 | |||
235 | /* Database entry */ | ||
236 | DSP_PROC_DB_ENTRY( | ||
237 | MISC_HANDLER, | ||
238 | misc_handler_configure); | ||
diff --git a/lib/rbcodec/dsp/dsp_misc.h b/lib/rbcodec/dsp/dsp_misc.h new file mode 100644 index 0000000000..74587cbb0e --- /dev/null +++ b/lib/rbcodec/dsp/dsp_misc.h | |||
@@ -0,0 +1,50 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
11 | * Copyright (C) 2005 Magnus Holmgren | ||
12 | * Copyright (C) 2007 Thom Johansen | ||
13 | * | ||
14 | * This program is free software; you can redistribute it and/or | ||
15 | * modify it under the terms of the GNU General Public License | ||
16 | * as published by the Free Software Foundation; either version 2 | ||
17 | * of the License, or (at your option) any later version. | ||
18 | * | ||
19 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
20 | * KIND, either express or implied. | ||
21 | * | ||
22 | ****************************************************************************/ | ||
23 | #ifndef DSP_MISC_H | ||
24 | #define DSP_MISC_H | ||
25 | |||
26 | /* Set the tri-pdf dithered output */ | ||
27 | void dsp_dither_enable(bool enable); /* in dsp_sample_output.c */ | ||
28 | |||
29 | /* Structure used with REPLAYGAIN_SET_GAINS message */ | ||
30 | #define REPLAYGAIN_SET_GAINS (DSP_PROC_SETTING+DSP_PROC_MISC_HANDLER) | ||
31 | struct dsp_replay_gains | ||
32 | { | ||
33 | long track_gain; | ||
34 | long album_gain; | ||
35 | long track_peak; | ||
36 | long album_peak; | ||
37 | }; | ||
38 | |||
39 | int get_replaygain_mode(bool have_track_gain, bool have_album_gain); | ||
40 | void dsp_set_replaygain(void); | ||
41 | |||
42 | #ifdef HAVE_PITCHSCREEN | ||
43 | void sound_set_pitch(int32_t ratio); | ||
44 | int32_t sound_get_pitch(void); | ||
45 | #endif /* HAVE_PITCHSCREEN */ | ||
46 | |||
47 | /* Callback for firmware layers to interface */ | ||
48 | int dsp_callback(int msg, intptr_t param); | ||
49 | |||
50 | #endif /* DSP_MISC_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp_proc_database.h b/lib/rbcodec/dsp/dsp_proc_database.h new file mode 100644 index 0000000000..55f10e684b --- /dev/null +++ b/lib/rbcodec/dsp/dsp_proc_database.h | |||
@@ -0,0 +1,57 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | /**************************************************************************** | ||
23 | * -_-~-_-~-_-~-_-~-_-~-_- Main database of effects _-~-_-~-_-~-_-~-_-~-_-~- | ||
24 | * | ||
25 | * Order is not particularly relevant and has no intended correlation with | ||
26 | * IDs. | ||
27 | * | ||
28 | * Notable exceptions in ordering: | ||
29 | * * Sample input: which is first in line and has special responsibilities | ||
30 | * (not an effect per se). | ||
31 | * * Anything that depends on the native sample rate must go after the | ||
32 | * resampling stage. | ||
33 | * * Some bizarre dependency I didn't think of but you decided to implement. | ||
34 | * * Sample output: Naturally, this takes the final result and converts it | ||
35 | * to the target PCM format (not an effect per se). | ||
36 | */ | ||
37 | DSP_PROC_DB_START | ||
38 | DSP_PROC_DB_ITEM(MISC_HANDLER) /* misc stuff (null stage) */ | ||
39 | DSP_PROC_DB_ITEM(PGA) /* pre-gain amp */ | ||
40 | #ifdef HAVE_PITCHSCREEN | ||
41 | DSP_PROC_DB_ITEM(TIMESTRETCH) /* time-stretching */ | ||
42 | #endif | ||
43 | DSP_PROC_DB_ITEM(RESAMPLE) /* resampler providing NATIVE_FREQUENCY */ | ||
44 | DSP_PROC_DB_ITEM(CROSSFEED) /* stereo crossfeed */ | ||
45 | DSP_PROC_DB_ITEM(EQUALIZER) /* n-band equalizer */ | ||
46 | #ifdef HAVE_SW_TONE_CONTROLS | ||
47 | DSP_PROC_DB_ITEM(TONE_CONTROLS) /* bass and treble */ | ||
48 | #endif | ||
49 | DSP_PROC_DB_ITEM(CHANNEL_MODE) /* channel modes */ | ||
50 | DSP_PROC_DB_ITEM(COMPRESSOR) /* dynamic-range compressor */ | ||
51 | DSP_PROC_DB_STOP | ||
52 | |||
53 | /* This file is included multiple times with different macro definitions so | ||
54 | clean up the current ones */ | ||
55 | #undef DSP_PROC_DB_START | ||
56 | #undef DSP_PROC_DB_ITEM | ||
57 | #undef DSP_PROC_DB_STOP | ||
diff --git a/lib/rbcodec/dsp/dsp_proc_entry.h b/lib/rbcodec/dsp/dsp_proc_entry.h new file mode 100644 index 0000000000..8bdfe5e0c9 --- /dev/null +++ b/lib/rbcodec/dsp/dsp_proc_entry.h | |||
@@ -0,0 +1,153 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef DSP_PROC_ENTRY_H | ||
22 | #define DSP_PROC_ENTRY_H | ||
23 | |||
24 | #if 0 /* Set to '1' to enable local debug messages */ | ||
25 | #include <debug.h> | ||
26 | #else | ||
27 | #undef DEBUGF | ||
28 | #define DEBUGF(...) | ||
29 | #endif | ||
30 | |||
31 | /* Macros to generate the right stuff */ | ||
32 | #ifdef DSP_PROC_DB_CREATE | ||
33 | struct dsp_proc_db_entry; | ||
34 | |||
35 | #define DSP_PROC_DB_START | ||
36 | #define DSP_PROC_DB_ITEM(name) \ | ||
37 | extern const struct dsp_proc_db_entry name##_proc_db_entry; | ||
38 | #define DSP_PROC_DB_STOP | ||
39 | |||
40 | /* Create database as externs to be able to build array */ | ||
41 | #include "dsp_proc_database.h" | ||
42 | |||
43 | #define DSP_PROC_DB_START \ | ||
44 | static struct dsp_proc_db_entry const * const dsp_proc_database[] = { | ||
45 | |||
46 | #define DSP_PROC_DB_ITEM(name) \ | ||
47 | &name##_proc_db_entry, | ||
48 | |||
49 | #define DSP_PROC_DB_STOP }; | ||
50 | |||
51 | /* Create database as array */ | ||
52 | #include "dsp_proc_database.h" | ||
53 | |||
54 | /* Number of effects in database - all available in audio DSP */ | ||
55 | #define DSP_NUM_PROC_STAGES ARRAYLEN(dsp_proc_database) | ||
56 | |||
57 | /* Number of possible effects for voice DSP */ | ||
58 | #ifdef HAVE_SW_TONE_CONTROLS | ||
59 | #define DSP_VOICE_NUM_PROC_STAGES 2 /* resample, tone */ | ||
60 | #else | ||
61 | #define DSP_VOICE_NUM_PROC_STAGES 1 /* resample */ | ||
62 | #endif | ||
63 | |||
64 | #else /* !DSP_PROC_DB_CREATE */ | ||
65 | |||
66 | #ifdef DEBUG | ||
67 | #define DSP_PROC_DB_ENTRY(_name, _configure) \ | ||
68 | const struct dsp_proc_db_entry _name##_proc_db_entry = \ | ||
69 | { .id = DSP_PROC_##_name, .configure = _configure, \ | ||
70 | .name = #_name }; | ||
71 | #else /* !DEBUG */ | ||
72 | #define DSP_PROC_DB_ENTRY(_name, _configure) \ | ||
73 | const struct dsp_proc_db_entry _name##_proc_db_entry = \ | ||
74 | { .id = DSP_PROC_##_name, .configure = _configure }; | ||
75 | #endif /* DEBUG */ | ||
76 | |||
77 | #endif /* DSP_PROC_DB_CREATE */ | ||
78 | |||
79 | #define DSP_PROC_DB_START \ | ||
80 | enum dsp_proc_ids \ | ||
81 | { \ | ||
82 | ___DSP_PROC_ID_FIRST = -1, | ||
83 | |||
84 | #define DSP_PROC_DB_ITEM(name) \ | ||
85 | DSP_PROC_##name, | ||
86 | |||
87 | #define DSP_PROC_DB_STOP }; | ||
88 | |||
89 | /* Create database as enums for use as ids */ | ||
90 | #include "dsp_proc_database.h" | ||
91 | |||
92 | struct dsp_proc_entry; | ||
93 | enum dsp_proc_ids; | ||
94 | |||
95 | /* DSP sample transform function prototype */ | ||
96 | typedef void (*dsp_proc_fn_type)(struct dsp_proc_entry *this, | ||
97 | struct dsp_buffer **buf); | ||
98 | |||
99 | /** | ||
100 | * dsp_proc_entry | ||
101 | * The structure allocated to every stage when enabled. | ||
102 | * | ||
103 | * default settings: | ||
104 | * .data = 0 | ||
105 | * .ip_mask = BIT_N(dsp_proc_db_entry.id) | ||
106 | * .process[0] = dsp_process_null | ||
107 | * .process[1] = dsp_format_change_process | ||
108 | * | ||
109 | * DSP_PROC_INIT handler just has to change what it needs to change. It may | ||
110 | * also be modified at any time to implement the stage's demands. | ||
111 | */ | ||
112 | struct dsp_proc_entry | ||
113 | { | ||
114 | intptr_t data; /* 00h: any value, at beginning for easy asm use */ | ||
115 | uint32_t ip_mask; /* In-place id bit (0 or id bit flag if in-place) */ | ||
116 | dsp_proc_fn_type process[2]; /* Processing normal/format changes */ | ||
117 | }; | ||
118 | |||
119 | /* DSP transform configure function prototype */ | ||
120 | typedef intptr_t (*dsp_proc_config_fn_type)(struct dsp_proc_entry *this, | ||
121 | struct dsp_config *dsp, | ||
122 | unsigned int setting, | ||
123 | intptr_t value); | ||
124 | |||
125 | /* Enable/disable a processing stage - not to be called during processing | ||
126 | * by processing code! */ | ||
127 | void dsp_proc_enable(struct dsp_config *dsp, enum dsp_proc_ids id, | ||
128 | bool enable); | ||
129 | /* Activate/deactivate processing stage, doesn't affect enabled status | ||
130 | * thus will not enable anything - | ||
131 | * may be called during processing to activate/deactivate for format | ||
132 | * changes */ | ||
133 | void dsp_proc_activate(struct dsp_config *dsp, enum dsp_proc_ids id, | ||
134 | bool activate); | ||
135 | |||
136 | /* Is the specified stage active on the DSP? */ | ||
137 | bool dsp_proc_active(struct dsp_config *dsp, enum dsp_proc_ids id); | ||
138 | |||
139 | /* Call this->process[fmt] according to the rules | ||
140 | * pass (unsigned)-1 to call function 0 with no restriction */ | ||
141 | bool dsp_proc_call(struct dsp_proc_entry *this, struct dsp_buffer **buf_p, | ||
142 | unsigned int fmt); | ||
143 | |||
144 | struct dsp_proc_db_entry | ||
145 | { | ||
146 | enum dsp_proc_ids id; /* id of this stage */ | ||
147 | dsp_proc_config_fn_type configure; /* dsp_configure hook */ | ||
148 | #ifdef DEBUG | ||
149 | const char *name; | ||
150 | #endif | ||
151 | }; | ||
152 | |||
153 | #endif /* DSP_PROC_ENTRY_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp_proc_settings.h b/lib/rbcodec/dsp/dsp_proc_settings.h new file mode 100644 index 0000000000..769532085e --- /dev/null +++ b/lib/rbcodec/dsp/dsp_proc_settings.h | |||
@@ -0,0 +1,40 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef DSP_PROC_SETTINGS_H | ||
22 | #define DSP_PROC_SETTINGS_H | ||
23 | |||
24 | struct dsp_config; | ||
25 | |||
26 | /* Collect all headers together */ | ||
27 | #include "channel_mode.h" | ||
28 | #include "compressor.h" | ||
29 | #include "crossfeed.h" | ||
30 | #include "dsp_misc.h" | ||
31 | #include "eq.h" | ||
32 | #include "pga.h" | ||
33 | #ifdef HAVE_PITCHSCREEN | ||
34 | #include "tdspeed.h" | ||
35 | #endif | ||
36 | #ifdef HAVE_SW_TONE_CONTROLS | ||
37 | #include "tone_controls.h" | ||
38 | #endif | ||
39 | |||
40 | #endif /* DSP_PROC_SETTINGS_H */ \ No newline at end of file | ||
diff --git a/lib/rbcodec/dsp/dsp_sample_input.c b/lib/rbcodec/dsp/dsp_sample_input.c new file mode 100644 index 0000000000..84127e1f96 --- /dev/null +++ b/lib/rbcodec/dsp/dsp_sample_input.c | |||
@@ -0,0 +1,334 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
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_sample_io.h" | ||
26 | |||
27 | #if 1 | ||
28 | #include <debug.h> | ||
29 | #else | ||
30 | #undef DEBUGF | ||
31 | #define DEBUGF(...) | ||
32 | #endif | ||
33 | |||
34 | /* The internal format is 32-bit samples, non-interleaved, stereo. This | ||
35 | * format is similar to the raw output from several codecs, so no copying is | ||
36 | * needed for that case. | ||
37 | * | ||
38 | * Note that for mono, dst[0] equals dst[1], as there is no point in | ||
39 | * processing the same data twice nor should it be done when modifying | ||
40 | * samples in-place. | ||
41 | * | ||
42 | * When conversion is required: | ||
43 | * Updates source buffer to point past the samples "consumed" also consuming | ||
44 | * that portion of the input buffer and the destination is set to the buffer | ||
45 | * of samples for later stages to consume. | ||
46 | * | ||
47 | * Input operates similarly to how an out-of-place processing stage should | ||
48 | * behave. | ||
49 | */ | ||
50 | |||
51 | extern void dsp_sample_output_init(struct sample_io_data *this); | ||
52 | extern void dsp_sample_output_flush(struct sample_io_data *this); | ||
53 | |||
54 | /* convert count 16-bit mono to 32-bit mono */ | ||
55 | static void sample_input_mono16(struct sample_io_data *this, | ||
56 | struct dsp_buffer **buf_p) | ||
57 | { | ||
58 | struct dsp_buffer *src = *buf_p; | ||
59 | struct dsp_buffer *dst = &this->sample_buf; | ||
60 | |||
61 | *buf_p = dst; | ||
62 | |||
63 | if (dst->remcount > 0) | ||
64 | return; /* data still remains */ | ||
65 | |||
66 | int count = MIN(src->remcount, SAMPLE_BUF_COUNT); | ||
67 | |||
68 | dst->remcount = count; | ||
69 | dst->p32[0] = this->sample_buf_arr[0]; | ||
70 | dst->p32[1] = this->sample_buf_arr[0]; | ||
71 | dst->proc_mask = src->proc_mask; | ||
72 | |||
73 | if (count <= 0) | ||
74 | return; /* purged sample_buf */ | ||
75 | |||
76 | const int16_t *s = src->pin[0]; | ||
77 | int32_t *d = dst->p32[0]; | ||
78 | const int scale = WORD_SHIFT; | ||
79 | |||
80 | dsp_advance_buffer_input(src, count, sizeof (int16_t)); | ||
81 | |||
82 | do | ||
83 | { | ||
84 | *d++ = *s++ << scale; | ||
85 | } | ||
86 | while (--count > 0); | ||
87 | } | ||
88 | |||
89 | /* convert count 16-bit interleaved stereo to 32-bit noninterleaved */ | ||
90 | static void sample_input_i_stereo16(struct sample_io_data *this, | ||
91 | struct dsp_buffer **buf_p) | ||
92 | { | ||
93 | struct dsp_buffer *src = *buf_p; | ||
94 | struct dsp_buffer *dst = &this->sample_buf; | ||
95 | |||
96 | *buf_p = dst; | ||
97 | |||
98 | if (dst->remcount > 0) | ||
99 | return; /* data still remains */ | ||
100 | |||
101 | int count = MIN(src->remcount, SAMPLE_BUF_COUNT); | ||
102 | |||
103 | dst->remcount = count; | ||
104 | dst->p32[0] = this->sample_buf_arr[0]; | ||
105 | dst->p32[1] = this->sample_buf_arr[1]; | ||
106 | dst->proc_mask = src->proc_mask; | ||
107 | |||
108 | if (count <= 0) | ||
109 | return; /* purged sample_buf */ | ||
110 | |||
111 | const int16_t *s = src->pin[0]; | ||
112 | int32_t *dl = dst->p32[0]; | ||
113 | int32_t *dr = dst->p32[1]; | ||
114 | const int scale = WORD_SHIFT; | ||
115 | |||
116 | dsp_advance_buffer_input(src, count, 2*sizeof (int16_t)); | ||
117 | |||
118 | do | ||
119 | { | ||
120 | *dl++ = *s++ << scale; | ||
121 | *dr++ = *s++ << scale; | ||
122 | } | ||
123 | while (--count > 0); | ||
124 | } | ||
125 | |||
126 | /* convert count 16-bit noninterleaved stereo to 32-bit noninterleaved */ | ||
127 | static void sample_input_ni_stereo16(struct sample_io_data *this, | ||
128 | struct dsp_buffer **buf_p) | ||
129 | { | ||
130 | struct dsp_buffer *src = *buf_p; | ||
131 | struct dsp_buffer *dst = &this->sample_buf; | ||
132 | |||
133 | *buf_p = dst; | ||
134 | |||
135 | if (dst->remcount > 0) | ||
136 | return; /* data still remains */ | ||
137 | |||
138 | int count = MIN(src->remcount, SAMPLE_BUF_COUNT); | ||
139 | |||
140 | dst->remcount = count; | ||
141 | dst->p32[0] = this->sample_buf_arr[0]; | ||
142 | dst->p32[1] = this->sample_buf_arr[1]; | ||
143 | dst->proc_mask = src->proc_mask; | ||
144 | |||
145 | if (count <= 0) | ||
146 | return; /* purged sample_buf */ | ||
147 | |||
148 | const int16_t *sl = src->pin[0]; | ||
149 | const int16_t *sr = src->pin[1]; | ||
150 | int32_t *dl = dst->p32[0]; | ||
151 | int32_t *dr = dst->p32[1]; | ||
152 | const int scale = WORD_SHIFT; | ||
153 | |||
154 | dsp_advance_buffer_input(src, count, sizeof (int16_t)); | ||
155 | |||
156 | do | ||
157 | { | ||
158 | *dl++ = *sl++ << scale; | ||
159 | *dr++ = *sr++ << scale; | ||
160 | } | ||
161 | while (--count > 0); | ||
162 | } | ||
163 | |||
164 | /* convert count 32-bit mono to 32-bit mono */ | ||
165 | static void sample_input_mono32(struct sample_io_data *this, | ||
166 | struct dsp_buffer **buf_p) | ||
167 | { | ||
168 | struct dsp_buffer *dst = &this->sample_buf; | ||
169 | |||
170 | if (dst->remcount > 0) | ||
171 | { | ||
172 | *buf_p = dst; | ||
173 | return; /* data still remains */ | ||
174 | } | ||
175 | /* else no buffer switch */ | ||
176 | |||
177 | struct dsp_buffer *src = *buf_p; | ||
178 | src->p32[1] = src->p32[0]; | ||
179 | } | ||
180 | |||
181 | |||
182 | /* convert count 32-bit interleaved stereo to 32-bit noninterleaved stereo */ | ||
183 | static void sample_input_i_stereo32(struct sample_io_data *this, | ||
184 | struct dsp_buffer **buf_p) | ||
185 | { | ||
186 | struct dsp_buffer *src = *buf_p; | ||
187 | struct dsp_buffer *dst = &this->sample_buf; | ||
188 | |||
189 | *buf_p = dst; | ||
190 | |||
191 | if (dst->remcount > 0) | ||
192 | return; /* data still remains */ | ||
193 | |||
194 | int count = MIN(src->remcount, SAMPLE_BUF_COUNT); | ||
195 | |||
196 | dst->remcount = count; | ||
197 | dst->p32[0] = this->sample_buf_arr[0]; | ||
198 | dst->p32[1] = this->sample_buf_arr[1]; | ||
199 | dst->proc_mask = src->proc_mask; | ||
200 | |||
201 | if (count <= 0) | ||
202 | return; /* purged sample_buf */ | ||
203 | |||
204 | const int32_t *s = src->pin[0]; | ||
205 | int32_t *dl = dst->p32[0]; | ||
206 | int32_t *dr = dst->p32[1]; | ||
207 | |||
208 | dsp_advance_buffer_input(src, count, 2*sizeof (int32_t)); | ||
209 | |||
210 | do | ||
211 | { | ||
212 | *dl++ = *s++; | ||
213 | *dr++ = *s++; | ||
214 | } | ||
215 | while (--count > 0); | ||
216 | } | ||
217 | |||
218 | /* convert 32 bit-noninterleaved stereo to 32-bit noninterleaved stereo */ | ||
219 | static void sample_input_ni_stereo32(struct sample_io_data *this, | ||
220 | struct dsp_buffer **buf_p) | ||
221 | { | ||
222 | struct dsp_buffer *dst = &this->sample_buf; | ||
223 | |||
224 | if (dst->remcount > 0) | ||
225 | *buf_p = dst; /* data still remains */ | ||
226 | /* else no buffer switch */ | ||
227 | } | ||
228 | |||
229 | /* set the to-native sample conversion function based on dsp sample | ||
230 | * parameters */ | ||
231 | static void dsp_sample_input_format_change(struct sample_io_data *this, | ||
232 | struct dsp_buffer **buf_p) | ||
233 | { | ||
234 | static const sample_input_fn_type fns[STEREO_NUM_MODES][2] = | ||
235 | { | ||
236 | [STEREO_INTERLEAVED] = | ||
237 | { sample_input_i_stereo16, | ||
238 | sample_input_i_stereo32 }, | ||
239 | [STEREO_NONINTERLEAVED] = | ||
240 | { sample_input_ni_stereo16, | ||
241 | sample_input_ni_stereo32 }, | ||
242 | [STEREO_MONO] = | ||
243 | { sample_input_mono16, | ||
244 | sample_input_mono32 }, | ||
245 | }; | ||
246 | |||
247 | struct dsp_buffer *src = *buf_p; | ||
248 | struct dsp_buffer *dst = &this->sample_buf; | ||
249 | |||
250 | /* Ack configured format change */ | ||
251 | format_change_ack(&this->format); | ||
252 | |||
253 | if (dst->remcount > 0) | ||
254 | { | ||
255 | *buf_p = dst; | ||
256 | return; /* data still remains */ | ||
257 | } | ||
258 | |||
259 | DSP_PRINT_FORMAT(DSP Input, -1, src->format); | ||
260 | |||
261 | /* new format - remember it and pass it along */ | ||
262 | dst->format = src->format; | ||
263 | this->input_samples[0] = fns[this->stereo_mode] | ||
264 | [this->sample_depth > NATIVE_DEPTH ? 1 : 0]; | ||
265 | |||
266 | this->input_samples[0](this, buf_p); | ||
267 | |||
268 | if (*buf_p == dst) /* buffer switch? */ | ||
269 | format_change_ack(&src->format); | ||
270 | } | ||
271 | |||
272 | static void dsp_sample_input_init(struct sample_io_data *this) | ||
273 | { | ||
274 | this->input_samples[0] = sample_input_ni_stereo32; | ||
275 | this->input_samples[1] = dsp_sample_input_format_change; | ||
276 | } | ||
277 | |||
278 | /* discard the sample buffer */ | ||
279 | static void dsp_sample_input_flush(struct sample_io_data *this) | ||
280 | { | ||
281 | this->sample_buf.remcount = 0; | ||
282 | } | ||
283 | |||
284 | void dsp_sample_io_configure(struct sample_io_data *this, | ||
285 | unsigned int setting, | ||
286 | intptr_t value) | ||
287 | { | ||
288 | switch (setting) | ||
289 | { | ||
290 | case DSP_INIT: | ||
291 | dsp_sample_input_init(this); | ||
292 | dsp_sample_output_init(this); | ||
293 | break; | ||
294 | |||
295 | case DSP_RESET: | ||
296 | /* Reset all sample descriptions to default */ | ||
297 | format_change_set(&this->format); | ||
298 | this->format.num_channels = 2; | ||
299 | this->format.frac_bits = WORD_FRACBITS; | ||
300 | this->format.output_scale = WORD_FRACBITS + 1 - NATIVE_DEPTH; | ||
301 | this->format.frequency = NATIVE_FREQUENCY; | ||
302 | this->format.codec_frequency = NATIVE_FREQUENCY; | ||
303 | this->sample_depth = NATIVE_DEPTH; | ||
304 | this->stereo_mode = STEREO_NONINTERLEAVED; | ||
305 | break; | ||
306 | |||
307 | case DSP_SET_FREQUENCY: | ||
308 | value = value > 0 ? value : NATIVE_FREQUENCY; | ||
309 | format_change_set(&this->format); | ||
310 | this->format.frequency = value; | ||
311 | this->format.codec_frequency = value; | ||
312 | break; | ||
313 | |||
314 | case DSP_SET_SAMPLE_DEPTH: | ||
315 | format_change_set(&this->format); | ||
316 | this->format.frac_bits = | ||
317 | value <= NATIVE_DEPTH ? WORD_FRACBITS : value; | ||
318 | this->format.output_scale = | ||
319 | this->format.frac_bits + 1 - NATIVE_DEPTH; | ||
320 | this->sample_depth = value; | ||
321 | break; | ||
322 | |||
323 | case DSP_SET_STEREO_MODE: | ||
324 | format_change_set(&this->format); | ||
325 | this->format.num_channels = value == STEREO_MONO ? 1 : 2; | ||
326 | this->stereo_mode = value; | ||
327 | break; | ||
328 | |||
329 | case DSP_FLUSH: | ||
330 | dsp_sample_input_flush(this); | ||
331 | dsp_sample_output_flush(this); | ||
332 | break; | ||
333 | } | ||
334 | } | ||
diff --git a/lib/rbcodec/dsp/dsp_sample_io.h b/lib/rbcodec/dsp/dsp_sample_io.h new file mode 100644 index 0000000000..443038919d --- /dev/null +++ b/lib/rbcodec/dsp/dsp_sample_io.h | |||
@@ -0,0 +1,62 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef DSP_SAMPLE_IO_H | ||
22 | #define DSP_SAMPLE_IO_H | ||
23 | |||
24 | /* 16-bit samples are scaled based on these constants. The shift should be | ||
25 | * no more than 15. | ||
26 | */ | ||
27 | #define WORD_SHIFT 12 | ||
28 | #define WORD_FRACBITS 27 | ||
29 | #define NATIVE_DEPTH 16 | ||
30 | |||
31 | #define SAMPLE_BUF_COUNT 128 /* Per channel, per DSP */ | ||
32 | |||
33 | struct sample_io_data; | ||
34 | |||
35 | /* DSP initial buffer input function call prototype */ | ||
36 | typedef void (*sample_input_fn_type)(struct sample_io_data *this, | ||
37 | struct dsp_buffer **buf_p); | ||
38 | |||
39 | /* DSP final buffer output function call prototype */ | ||
40 | typedef void (*sample_output_fn_type)(struct sample_io_data *this, | ||
41 | struct dsp_buffer *src, | ||
42 | struct dsp_buffer *dst); | ||
43 | |||
44 | /* This becomes part of the DSP aggregate */ | ||
45 | struct sample_io_data | ||
46 | { | ||
47 | int outcount; /* 00h: Output count */ | ||
48 | struct sample_format format; /* General format info */ | ||
49 | int sample_depth; /* Codec-specified sample depth */ | ||
50 | int stereo_mode; /* Codec-specified input format */ | ||
51 | sample_input_fn_type input_samples[2]; /* input functions */ | ||
52 | struct dsp_buffer sample_buf; /* Buffer descriptor for converted samples */ | ||
53 | int32_t sample_buf_arr[2][SAMPLE_BUF_COUNT]; /* Internal format */ | ||
54 | sample_output_fn_type output_samples[2]; /* Final output functions */ | ||
55 | }; | ||
56 | |||
57 | /* Sample IO watches the format setting from the codec */ | ||
58 | void dsp_sample_io_configure(struct sample_io_data *this, | ||
59 | unsigned int setting, | ||
60 | intptr_t value); | ||
61 | |||
62 | #endif /* DSP_SAMPLE_IO_H */ | ||
diff --git a/lib/rbcodec/dsp/dsp_sample_output.c b/lib/rbcodec/dsp/dsp_sample_output.c new file mode 100644 index 0000000000..47fde0440c --- /dev/null +++ b/lib/rbcodec/dsp/dsp_sample_output.c | |||
@@ -0,0 +1,214 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
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_sample_io.h" | ||
26 | #include "dsp-util.h" | ||
27 | #include <string.h> | ||
28 | |||
29 | #if 0 | ||
30 | #include <debug.h> | ||
31 | #else | ||
32 | #undef DEBUGF | ||
33 | #define DEBUGF(...) | ||
34 | #endif | ||
35 | |||
36 | /* May be implemented in here or externally.*/ | ||
37 | void sample_output_mono(struct sample_io_data *this, | ||
38 | struct dsp_buffer *src, struct dsp_buffer *dst); | ||
39 | void sample_output_stereo(struct sample_io_data *this, | ||
40 | struct dsp_buffer *src, struct dsp_buffer *dst); | ||
41 | void sample_output_dithered(struct sample_io_data *this, | ||
42 | struct dsp_buffer *src, struct dsp_buffer *dst); | ||
43 | |||
44 | /** Sample output **/ | ||
45 | |||
46 | #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) | ||
47 | /* write mono internal format to output format */ | ||
48 | void sample_output_mono(struct sample_io_data *this, | ||
49 | struct dsp_buffer *src, struct dsp_buffer *dst) | ||
50 | { | ||
51 | int count = this->outcount; | ||
52 | const int32_t *s0 = src->p32[0]; | ||
53 | int16_t *d = dst->p16out; | ||
54 | int scale = src->format.output_scale; | ||
55 | int32_t dc_bias = 1L << (scale - 1); | ||
56 | |||
57 | do | ||
58 | { | ||
59 | int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
60 | *d++ = lr; | ||
61 | *d++ = lr; | ||
62 | } | ||
63 | while (--count > 0); | ||
64 | } | ||
65 | |||
66 | /* write stereo internal format to output format */ | ||
67 | void sample_output_stereo(struct sample_io_data *this, | ||
68 | struct dsp_buffer *src, struct dsp_buffer *dst) | ||
69 | { | ||
70 | int count = this->outcount; | ||
71 | const int32_t *s0 = src->p32[0]; | ||
72 | const int32_t *s1 = src->p32[1]; | ||
73 | int16_t *d = dst->p16out; | ||
74 | int scale = src->format.output_scale; | ||
75 | int32_t dc_bias = 1L << (scale - 1); | ||
76 | |||
77 | do | ||
78 | { | ||
79 | *d++ = clip_sample_16((*s0++ + dc_bias) >> scale); | ||
80 | *d++ = clip_sample_16((*s1++ + dc_bias) >> scale); | ||
81 | } | ||
82 | while (--count > 0); | ||
83 | } | ||
84 | #endif /* CPU */ | ||
85 | |||
86 | /** | ||
87 | * The "dither" code to convert the 24-bit samples produced by libmad was | ||
88 | * taken from the coolplayer project - coolplayer.sourceforge.net | ||
89 | * | ||
90 | * This function handles mono and stereo outputs. | ||
91 | */ | ||
92 | static struct dither_data | ||
93 | { | ||
94 | struct dither_state | ||
95 | { | ||
96 | long error[3]; /* 00h: error term history */ | ||
97 | long random; /* 0ch: last random value */ | ||
98 | } state[2]; /* 0=left, 1=right */ | ||
99 | bool enabled; /* 20h: dithered output enabled */ | ||
100 | /* 24h */ | ||
101 | } dither_data IBSS_ATTR; | ||
102 | |||
103 | void sample_output_dithered(struct sample_io_data *this, | ||
104 | struct dsp_buffer *src, struct dsp_buffer *dst) | ||
105 | { | ||
106 | int count = this->outcount; | ||
107 | int channels = src->format.num_channels; | ||
108 | int scale = src->format.output_scale; | ||
109 | int32_t dc_bias = 1L << (scale - 1); /* 1/2 bit of significance */ | ||
110 | int32_t mask = (1L << scale) - 1; /* Mask of bits quantized away */ | ||
111 | |||
112 | for (int ch = 0; ch < channels; ch++) | ||
113 | { | ||
114 | struct dither_state *dither = &dither_data.state[ch]; | ||
115 | |||
116 | const int32_t *s = src->p32[ch]; | ||
117 | int16_t *d = &dst->p16out[ch]; | ||
118 | |||
119 | for (int i = 0; i < count; i++, s++, d += 2) | ||
120 | { | ||
121 | /* Noise shape and bias (for correct rounding later) */ | ||
122 | int32_t sample = *s; | ||
123 | |||
124 | sample += dither->error[0] - dither->error[1] + dither->error[2]; | ||
125 | dither->error[2] = dither->error[1]; | ||
126 | dither->error[1] = dither->error[0] / 2; | ||
127 | |||
128 | int32_t output = sample + dc_bias; | ||
129 | |||
130 | /* Dither, highpass triangle PDF */ | ||
131 | int32_t random = dither->random*0x0019660dL + 0x3c6ef35fL; | ||
132 | output += (random & mask) - (dither->random & mask); | ||
133 | dither->random = random; | ||
134 | |||
135 | /* Quantize sample to output range */ | ||
136 | output >>= scale; | ||
137 | |||
138 | /* Error feedback of quantization */ | ||
139 | dither->error[0] = sample - (output << scale); | ||
140 | |||
141 | /* Clip and store */ | ||
142 | *d = clip_sample_16(output); | ||
143 | } | ||
144 | } | ||
145 | |||
146 | if (channels > 1) | ||
147 | return; | ||
148 | |||
149 | /* Have to duplicate left samples into the right channel since | ||
150 | output is interleaved stereo */ | ||
151 | int16_t *d = dst->p16out; | ||
152 | |||
153 | do | ||
154 | { | ||
155 | int16_t s = *d++; | ||
156 | *d++ = s; | ||
157 | } | ||
158 | while (--count > 0); | ||
159 | } | ||
160 | |||
161 | /* Initialize the output function for settings and format */ | ||
162 | static void dsp_sample_output_format_change(struct sample_io_data *this, | ||
163 | struct dsp_buffer *src, | ||
164 | struct dsp_buffer *dst) | ||
165 | { | ||
166 | static const sample_output_fn_type fns[2][2] = | ||
167 | { | ||
168 | { sample_output_mono, /* DC-biased quantizing */ | ||
169 | sample_output_stereo }, | ||
170 | { sample_output_dithered, /* Tri-PDF dithering */ | ||
171 | sample_output_dithered }, | ||
172 | }; | ||
173 | |||
174 | struct sample_format *format = &src->format; | ||
175 | bool dither = dsp_get_id((void *)this) == CODEC_IDX_AUDIO && | ||
176 | dither_data.enabled; | ||
177 | int channels = format->num_channels; | ||
178 | |||
179 | DSP_PRINT_FORMAT(DSP Output, -1, *format); | ||
180 | |||
181 | this->output_samples[0] = fns[dither ? 1 : 0][channels - 1]; | ||
182 | format_change_ack(format); /* always ack, we're last */ | ||
183 | |||
184 | /* The real function mustn't be called with no data */ | ||
185 | if (this->outcount > 0) | ||
186 | this->output_samples[0](this, src, dst); | ||
187 | } | ||
188 | |||
189 | void dsp_sample_output_init(struct sample_io_data *this) | ||
190 | { | ||
191 | this->output_samples[0] = sample_output_stereo; | ||
192 | this->output_samples[1] = dsp_sample_output_format_change; | ||
193 | } | ||
194 | |||
195 | /* Flush the dither history */ | ||
196 | void dsp_sample_output_flush(struct sample_io_data *this) | ||
197 | { | ||
198 | if (dsp_get_id((void *)this) == CODEC_IDX_AUDIO) | ||
199 | memset(dither_data.state, 0, sizeof (dither_data.state)); | ||
200 | } | ||
201 | |||
202 | /** Output settings **/ | ||
203 | |||
204 | /* Set the tri-pdf dithered output */ | ||
205 | void dsp_dither_enable(bool enable) | ||
206 | { | ||
207 | if (enable == dither_data.enabled) | ||
208 | return; | ||
209 | |||
210 | struct sample_io_data *data = (void *)dsp_get_config(CODEC_IDX_AUDIO); | ||
211 | dsp_sample_output_flush(data); | ||
212 | dither_data.enabled = enable; | ||
213 | data->output_samples[0] = dsp_sample_output_format_change; | ||
214 | } | ||
diff --git a/lib/rbcodec/dsp/eq.c b/lib/rbcodec/dsp/eq.c index 122a46a4c5..4e7df9bf5a 100644 --- a/lib/rbcodec/dsp/eq.c +++ b/lib/rbcodec/dsp/eq.c | |||
@@ -7,7 +7,8 @@ | |||
7 | * \/ \/ \/ \/ \/ | 7 | * \/ \/ \/ \/ \/ |
8 | * $Id$ | 8 | * $Id$ |
9 | * | 9 | * |
10 | * Copyright (C) 2006-2007 Thom Johansen | 10 | * Copyright (C) 2006-2007 Thom Johansen |
11 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | 12 | * |
12 | * This program is free software; you can redistribute it and/or | 13 | * This program is free software; you can redistribute it and/or |
13 | * modify it under the terms of the GNU General Public License | 14 | * modify it under the terms of the GNU General Public License |
@@ -18,251 +19,156 @@ | |||
18 | * KIND, either express or implied. | 19 | * KIND, either express or implied. |
19 | * | 20 | * |
20 | ****************************************************************************/ | 21 | ****************************************************************************/ |
21 | |||
22 | #include <inttypes.h> | ||
23 | #include "config.h" | 22 | #include "config.h" |
23 | #include "system.h" | ||
24 | #include "fixedpoint.h" | 24 | #include "fixedpoint.h" |
25 | #include "fracmul.h" | 25 | #include "fracmul.h" |
26 | #include "eq.h" | 26 | #include "dsp.h" |
27 | #include "dsp_filter.h" | ||
27 | #include "replaygain.h" | 28 | #include "replaygain.h" |
29 | #include <string.h> | ||
30 | #include "dsp_proc_entry.h" | ||
28 | 31 | ||
29 | /** | 32 | /** |
30 | * Calculate first order shelving filter. Filter is not directly usable by the | 33 | * Current setup is one lowshelf filters three peaking filters and one |
31 | * eq_filter() function. | 34 | * highshelf filter. Varying the number of shelving filters make no sense, |
32 | * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format. | 35 | * but adding peaking filters is possible. Check EQ_NUM_BANDS to have |
33 | * @param A decibel value multiplied by ten, describing gain/attenuation of | 36 | * 2 shelving filters and EQ_NUM_BANDS-2 peaking filters. |
34 | * shelf. Max value is 24 dB. | ||
35 | * @param low true for low-shelf filter, false for high-shelf filter. | ||
36 | * @param c pointer to coefficient storage. Coefficients are s4.27 format. | ||
37 | */ | 37 | */ |
38 | void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c) | 38 | |
39 | #if EQ_NUM_BANDS < 3 | ||
40 | /* No good. Expect at least 1 peaking and low/high shelving filters */ | ||
41 | #error Band count must be greater than or equal to 3 | ||
42 | #endif | ||
43 | |||
44 | static struct eq_state | ||
39 | { | 45 | { |
40 | long sin, cos; | 46 | uint32_t enabled; /* Mask of enabled bands */ |
41 | int32_t b0, b1, a0, a1; /* s3.28 */ | 47 | uint8_t bands[EQ_NUM_BANDS+1]; /* Indexes of enabled bands */ |
42 | const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */ | 48 | struct dsp_filter filters[EQ_NUM_BANDS]; /* Data for each filter */ |
49 | } eq_data IBSS_ATTR; | ||
43 | 50 | ||
44 | sin = fp_sincos(cutoff/2, &cos); | 51 | /* Clear histories of all enabled bands */ |
45 | if (low) { | 52 | static void eq_flush(void) |
46 | const int32_t sin_div_g = fp_div(sin, g, 25); | 53 | { |
47 | const int32_t sin_g = FRACMUL(sin, g); | 54 | if (eq_data.enabled == 0) |
48 | cos >>= 3; | 55 | return; /* Not initialized yet/no bands on */ |
49 | b0 = sin_g + cos; /* 0.25 .. 4.10 */ | ||
50 | b1 = sin_g - cos; /* -1 .. 3.98 */ | ||
51 | a0 = sin_div_g + cos; /* 0.25 .. 4.10 */ | ||
52 | a1 = sin_div_g - cos; /* -1 .. 3.98 */ | ||
53 | } else { | ||
54 | const int32_t cos_div_g = fp_div(cos, g, 25); | ||
55 | const int32_t cos_g = FRACMUL(cos, g); | ||
56 | sin >>= 3; | ||
57 | b0 = sin + cos_g; /* 0.25 .. 4.10 */ | ||
58 | b1 = sin - cos_g; /* -3.98 .. 1 */ | ||
59 | a0 = sin + cos_div_g; /* 0.25 .. 4.10 */ | ||
60 | a1 = sin - cos_div_g; /* -3.98 .. 1 */ | ||
61 | } | ||
62 | 56 | ||
63 | const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */ | 57 | for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++) |
64 | *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */ | 58 | filter_flush(&eq_data.filters[*b]); |
65 | *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */ | ||
66 | *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */ | ||
67 | } | 59 | } |
68 | 60 | ||
69 | #ifdef HAVE_SW_TONE_CONTROLS | 61 | /** DSP interface **/ |
70 | /** | 62 | |
71 | * Calculate second order section filter consisting of one low-shelf and one | 63 | /* Set the precut gain value */ |
72 | * high-shelf section. | 64 | void dsp_set_eq_precut(int precut) |
73 | * @param cutoff_low low-shelf midpoint frequency. See eq_pk_coefs for format. | ||
74 | * @param cutoff_high high-shelf midpoint frequency. | ||
75 | * @param A_low decibel value multiplied by ten, describing gain/attenuation of | ||
76 | * low-shelf part. Max value is 24 dB. | ||
77 | * @param A_high decibel value multiplied by ten, describing gain/attenuation of | ||
78 | * high-shelf part. Max value is 24 dB. | ||
79 | * @param A decibel value multiplied by ten, describing additional overall gain. | ||
80 | * @param c pointer to coefficient storage. Coefficients are s4.27 format. | ||
81 | */ | ||
82 | void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high, | ||
83 | long A_low, long A_high, long A, int32_t *c) | ||
84 | { | 65 | { |
85 | const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */ | 66 | pga_set_gain(PGA_EQ_PRECUT, get_replaygain_int(precut * -10)); |
86 | int32_t c_ls[3], c_hs[3]; | 67 | } |
87 | 68 | ||
88 | filter_shelf_coefs(cutoff_low, A_low, true, c_ls); | 69 | /* Update the filter configuration for the band */ |
89 | filter_shelf_coefs(cutoff_high, A_high, false, c_hs); | 70 | void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting) |
90 | c_ls[0] = FRACMUL(g, c_ls[0]); | 71 | { |
91 | c_ls[1] = FRACMUL(g, c_ls[1]); | 72 | static void (* const coef_gen[EQ_NUM_BANDS])(unsigned long cutoff, |
73 | unsigned long Q, long db, | ||
74 | struct dsp_filter *f) = | ||
75 | { | ||
76 | [0] = filter_ls_coefs, | ||
77 | [1 ... EQ_NUM_BANDS-2] = filter_pk_coefs, | ||
78 | [EQ_NUM_BANDS-1] = filter_hs_coefs, | ||
79 | }; | ||
80 | |||
81 | if (band < 0 || band >= EQ_NUM_BANDS) | ||
82 | return; | ||
83 | |||
84 | /* NOTE: The coef functions assume the EMAC unit is in fractional mode, | ||
85 | which it should be, since we're executed from the main thread. */ | ||
86 | |||
87 | uint32_t mask = eq_data.enabled; | ||
88 | struct dsp_filter *filter = &eq_data.filters[band]; | ||
89 | |||
90 | /* Assume a band is disabled if the gain is zero */ | ||
91 | mask &= ~BIT_N(band); | ||
92 | |||
93 | if (setting->gain != 0) | ||
94 | { | ||
95 | mask |= BIT_N(band); | ||
96 | |||
97 | /* Convert user settings to format required by coef generator | ||
98 | functions */ | ||
99 | coef_gen[band](0xffffffff / NATIVE_FREQUENCY * setting->cutoff, | ||
100 | setting->q ?: 1, setting->gain, filter); | ||
101 | } | ||
92 | 102 | ||
93 | /* now we cascade the two first order filters to one second order filter | 103 | if (mask == eq_data.enabled) |
94 | * which can be used by eq_filter(). these resulting coefficients have a | 104 | return; /* No change in band-enable state */ |
95 | * really wide numerical range, so we use a fixed point format which will | ||
96 | * work for the selected cutoff frequencies (in dsp.c) only. | ||
97 | */ | ||
98 | const int32_t b0 = c_ls[0], b1 = c_ls[1], b2 = c_hs[0], b3 = c_hs[1]; | ||
99 | const int32_t a0 = c_ls[2], a1 = c_hs[2]; | ||
100 | *c++ = FRACMUL_SHL(b0, b2, 4); | ||
101 | *c++ = FRACMUL_SHL(b0, b3, 4) + FRACMUL_SHL(b1, b2, 4); | ||
102 | *c++ = FRACMUL_SHL(b1, b3, 4); | ||
103 | *c++ = a0 + a1; | ||
104 | *c++ = -FRACMUL_SHL(a0, a1, 4); | ||
105 | } | ||
106 | #endif | ||
107 | 105 | ||
108 | /* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson. | 106 | if (mask & BIT_N(band)) |
109 | * Slightly faster calculation can be done by deriving forms which use tan() | 107 | filter_flush(filter); /* Coming online */ |
110 | * instead of cos() and sin(), but the latter are far easier to use when doing | ||
111 | * fixed point math, and performance is not a big point in the calculation part. | ||
112 | * All the 'a' filter coefficients are negated so we can use only additions | ||
113 | * in the filtering equation. | ||
114 | */ | ||
115 | 108 | ||
116 | /** | 109 | eq_data.enabled = mask; |
117 | * Calculate second order section peaking filter coefficients. | ||
118 | * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and | ||
119 | * 0x80000000 represents the Nyquist frequency (samplerate/2). | ||
120 | * @param Q Q factor value multiplied by ten. Lower bound is artificially set | ||
121 | * at 0.5. | ||
122 | * @param db decibel value multiplied by ten, describing gain/attenuation at | ||
123 | * peak freq. Max value is 24 dB. | ||
124 | * @param c pointer to coefficient storage. Coefficients are s3.28 format. | ||
125 | */ | ||
126 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | ||
127 | { | ||
128 | long cs; | ||
129 | const long one = 1 << 28; /* s3.28 */ | ||
130 | const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */ | ||
131 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
132 | int32_t a0, a1, a2; /* these are all s3.28 format */ | ||
133 | int32_t b0, b1, b2; | ||
134 | const long alphadivA = fp_div(alpha, A, 27); | ||
135 | const long alphaA = FRACMUL(alpha, A); | ||
136 | 110 | ||
137 | /* possible numerical ranges are in comments by each coef */ | 111 | /* Only be active if there are bands to process - if EQ is off, then |
138 | b0 = one + alphaA; /* [1 .. 5] */ | 112 | this call has no effect */ |
139 | b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */ | 113 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); |
140 | b2 = one - alphaA; /* [-3 .. 1] */ | 114 | dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, mask != 0); |
141 | a0 = one + alphadivA; /* [1 .. 5] */ | 115 | |
142 | a2 = one - alphadivA; /* [-3 .. 1] */ | 116 | /* Prepare list of enabled bands for efficient iteration */ |
117 | for (band = 0; mask != 0; mask &= mask - 1, band++) | ||
118 | eq_data.bands[band] = (uint8_t)find_first_set_bit(mask); | ||
143 | 119 | ||
144 | /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */ | 120 | eq_data.bands[band] = EQ_NUM_BANDS; |
145 | const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */ | ||
146 | *c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */ | ||
147 | *c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */ | ||
148 | *c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */ | ||
149 | *c++ = FRACMUL(-a1, rcp_a0); /* [-2 .. 2] */ | ||
150 | *c++ = FRACMUL(-a2, rcp_a0); /* [-0.6 .. 1] */ | ||
151 | } | 121 | } |
152 | 122 | ||
153 | /** | 123 | /* Enable or disable the equalizer */ |
154 | * Calculate coefficients for lowshelf filter. Parameters are as for | 124 | void dsp_eq_enable(bool enable) |
155 | * eq_pk_coefs, but the coefficient format is s5.26 fixed point. | ||
156 | */ | ||
157 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | ||
158 | { | 125 | { |
159 | long cs; | 126 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); |
160 | const long one = 1 << 25; /* s6.25 */ | 127 | dsp_proc_enable(dsp, DSP_PROC_EQUALIZER, enable); |
161 | const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ | ||
162 | const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ | ||
163 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
164 | const long ap1 = (A >> 4) + one; | ||
165 | const long am1 = (A >> 4) - one; | ||
166 | const long ap1_cs = FRACMUL(ap1, cs); | ||
167 | const long am1_cs = FRACMUL(am1, cs); | ||
168 | const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); | ||
169 | int32_t a0, a1, a2; /* these are all s6.25 format */ | ||
170 | int32_t b0, b1, b2; | ||
171 | |||
172 | /* [0.1 .. 40] */ | ||
173 | b0 = FRACMUL_SHL(A, ap1 - am1_cs + twosqrtalpha, 2); | ||
174 | /* [-16 .. 63.4] */ | ||
175 | b1 = FRACMUL_SHL(A, am1 - ap1_cs, 3); | ||
176 | /* [0 .. 31.7] */ | ||
177 | b2 = FRACMUL_SHL(A, ap1 - am1_cs - twosqrtalpha, 2); | ||
178 | /* [0.5 .. 10] */ | ||
179 | a0 = ap1 + am1_cs + twosqrtalpha; | ||
180 | /* [-16 .. 4] */ | ||
181 | a1 = -2*(am1 + ap1_cs); | ||
182 | /* [0 .. 8] */ | ||
183 | a2 = ap1 + am1_cs - twosqrtalpha; | ||
184 | 128 | ||
185 | /* [0.1 .. 1.99] */ | 129 | if (enable && eq_data.enabled != 0) |
186 | const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ | 130 | dsp_proc_activate(dsp, DSP_PROC_EQUALIZER, true); |
187 | *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */ | ||
188 | *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */ | ||
189 | *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */ | ||
190 | *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */ | ||
191 | *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */ | ||
192 | } | 131 | } |
193 | 132 | ||
194 | /** | 133 | /* Apply EQ filters to those bands that have got it switched on. */ |
195 | * Calculate coefficients for highshelf filter. Parameters are as for | 134 | static void eq_process(struct dsp_proc_entry *this, |
196 | * eq_pk_coefs, but the coefficient format is s5.26 fixed point. | 135 | struct dsp_buffer **buf_p) |
197 | */ | ||
198 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | ||
199 | { | 136 | { |
200 | long cs; | 137 | struct dsp_buffer *buf = *buf_p; |
201 | const long one = 1 << 25; /* s6.25 */ | 138 | int count = buf->remcount; |
202 | const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ | 139 | unsigned int channels = buf->format.num_channels; |
203 | const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ | ||
204 | const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ | ||
205 | const long ap1 = (A >> 4) + one; | ||
206 | const long am1 = (A >> 4) - one; | ||
207 | const long ap1_cs = FRACMUL(ap1, cs); | ||
208 | const long am1_cs = FRACMUL(am1, cs); | ||
209 | const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); | ||
210 | int32_t a0, a1, a2; /* these are all s6.25 format */ | ||
211 | int32_t b0, b1, b2; | ||
212 | 140 | ||
213 | /* [0.1 .. 40] */ | 141 | for (uint8_t *b = eq_data.bands; *b < EQ_NUM_BANDS; b++) |
214 | b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2); | 142 | filter_process(&eq_data.filters[*b], buf->p32, count, channels); |
215 | /* [-63.5 .. 16] */ | ||
216 | b1 = -FRACMUL_SHL(A, am1 + ap1_cs, 3); | ||
217 | /* [0 .. 32] */ | ||
218 | b2 = FRACMUL_SHL(A, ap1 + am1_cs - twosqrtalpha, 2); | ||
219 | /* [0.5 .. 10] */ | ||
220 | a0 = ap1 - am1_cs + twosqrtalpha; | ||
221 | /* [-4 .. 16] */ | ||
222 | a1 = 2*(am1 - ap1_cs); | ||
223 | /* [0 .. 8] */ | ||
224 | a2 = ap1 - am1_cs - twosqrtalpha; | ||
225 | 143 | ||
226 | /* [0.1 .. 1.99] */ | 144 | (void)this; |
227 | const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ | ||
228 | *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */ | ||
229 | *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */ | ||
230 | *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */ | ||
231 | *c++ = FRACMUL_SHL(-a1, rcp_a0, 2); /* [-2 .. 2] */ | ||
232 | *c++ = FRACMUL_SHL(-a2, rcp_a0, 2); /* [0 .. 1] */ | ||
233 | } | 145 | } |
234 | 146 | ||
235 | /* We realise the filters as a second order direct form 1 structure. Direct | 147 | /* DSP message hook */ |
236 | * form 1 was chosen because of better numerical properties for fixed point | 148 | static intptr_t eq_configure(struct dsp_proc_entry *this, |
237 | * implementations. | 149 | struct dsp_config *dsp, |
238 | */ | 150 | unsigned int setting, |
239 | 151 | intptr_t value) | |
240 | #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) | ||
241 | void eq_filter(int32_t **x, struct eqfilter *f, unsigned num, | ||
242 | unsigned channels, unsigned shift) | ||
243 | { | 152 | { |
244 | unsigned c, i; | 153 | switch (setting) |
245 | long long acc; | 154 | { |
246 | 155 | case DSP_PROC_INIT: | |
247 | /* Direct form 1 filtering code. | 156 | if (value != 0) |
248 | y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | 157 | break; |
249 | where y[] is output and x[] is input. | 158 | this->process[0] = eq_process; |
250 | */ | 159 | case DSP_PROC_CLOSE: |
251 | 160 | pga_enable_gain(PGA_EQ_PRECUT, setting == DSP_PROC_INIT); | |
252 | for (c = 0; c < channels; c++) { | 161 | break; |
253 | for (i = 0; i < num; i++) { | 162 | |
254 | acc = (long long) x[c][i] * f->coefs[0]; | 163 | case DSP_FLUSH: |
255 | acc += (long long) f->history[c][0] * f->coefs[1]; | 164 | eq_flush(); |
256 | acc += (long long) f->history[c][1] * f->coefs[2]; | 165 | break; |
257 | acc += (long long) f->history[c][2] * f->coefs[3]; | ||
258 | acc += (long long) f->history[c][3] * f->coefs[4]; | ||
259 | f->history[c][1] = f->history[c][0]; | ||
260 | f->history[c][0] = x[c][i]; | ||
261 | f->history[c][3] = f->history[c][2]; | ||
262 | x[c][i] = (acc << shift) >> 32; | ||
263 | f->history[c][2] = x[c][i]; | ||
264 | } | ||
265 | } | 166 | } |
167 | |||
168 | return 1; | ||
169 | (void)dsp; | ||
266 | } | 170 | } |
267 | #endif | ||
268 | 171 | ||
172 | /* Database entry */ | ||
173 | DSP_PROC_DB_ENTRY(EQUALIZER, | ||
174 | eq_configure); | ||
diff --git a/lib/rbcodec/dsp/eq.h b/lib/rbcodec/dsp/eq.h index a44e9153ac..53097beb12 100644 --- a/lib/rbcodec/dsp/eq.h +++ b/lib/rbcodec/dsp/eq.h | |||
@@ -18,33 +18,25 @@ | |||
18 | * KIND, either express or implied. | 18 | * KIND, either express or implied. |
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | |||
22 | #ifndef _EQ_H | 21 | #ifndef _EQ_H |
23 | #define _EQ_H | 22 | #define _EQ_H |
24 | 23 | ||
25 | #include <inttypes.h> | 24 | /* => support from 3 to 32 bands, inclusive |
26 | #include <stdbool.h> | 25 | * Menus and screens must be updated to support changing this from 5 |
27 | 26 | * without modifying other stuff (remove comment when this is no longer | |
28 | /* These depend on the fixed point formats used by the different filter types | 27 | * true :-) */ |
29 | and need to be changed when they change. | 28 | #define EQ_NUM_BANDS 5 |
30 | */ | ||
31 | #define FILTER_BISHELF_SHIFT 5 | ||
32 | #define EQ_PEAK_SHIFT 4 | ||
33 | #define EQ_SHELF_SHIFT 6 | ||
34 | 29 | ||
35 | struct eqfilter { | 30 | struct eq_band_setting |
36 | int32_t coefs[5]; /* Order is b0, b1, b2, a1, a2 */ | 31 | { |
37 | int32_t history[2][4]; | 32 | int cutoff; /* Hz */ |
33 | int q; | ||
34 | int gain; /* +/- dB */ | ||
38 | }; | 35 | }; |
39 | 36 | ||
40 | void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c); | 37 | /** DSP interface **/ |
41 | void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high, | 38 | void dsp_set_eq_precut(int precut); |
42 | long A_low, long A_high, long A, int32_t *c); | 39 | void dsp_set_eq_coefs(int band, const struct eq_band_setting *setting); |
43 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c); | 40 | void dsp_eq_enable(bool enable); |
44 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c); | ||
45 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c); | ||
46 | void eq_filter(int32_t **x, struct eqfilter *f, unsigned num, | ||
47 | unsigned channels, unsigned shift); | ||
48 | |||
49 | #endif | ||
50 | 41 | ||
42 | #endif /* _EQ_H */ | ||
diff --git a/lib/rbcodec/dsp/eq_arm.S b/lib/rbcodec/dsp/eq_arm.S deleted file mode 100644 index b0e1771e89..0000000000 --- a/lib/rbcodec/dsp/eq_arm.S +++ /dev/null | |||
@@ -1,89 +0,0 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006-2007 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | #include "config.h" | ||
23 | |||
24 | /* uncomment this to make filtering calculate lower bits after shifting. | ||
25 | * without this, "shift" of the lower bits will be lost here. | ||
26 | */ | ||
27 | /* #define HIGH_PRECISION */ | ||
28 | |||
29 | /* | ||
30 | * void eq_filter(int32_t **x, struct eqfilter *f, unsigned num, | ||
31 | * unsigned channels, unsigned shift) | ||
32 | */ | ||
33 | #if CONFIG_CPU == PP5002 | ||
34 | .section .icode,"ax",%progbits | ||
35 | #else | ||
36 | .text | ||
37 | #endif | ||
38 | .global eq_filter | ||
39 | eq_filter: | ||
40 | ldr r12, [sp] @ get shift parameter | ||
41 | stmdb sp!, { r0-r11, lr } @ save all params and clobbered regs | ||
42 | ldmia r1!, { r4-r8 } @ load coefs | ||
43 | mov r10, r1 @ loop prelude expects filter struct addr in r10 | ||
44 | |||
45 | .filterloop: | ||
46 | ldr r9, [sp] @ get pointer to this channels data | ||
47 | add r0, r9, #4 | ||
48 | str r0, [sp] @ save back pointer to next channels data | ||
49 | ldr r9, [r9] @ r9 = x[] | ||
50 | ldr r14, [sp, #8] @ r14 = numsamples | ||
51 | ldmia r10, { r0-r3 } @ load history, r10 should be filter struct addr | ||
52 | str r10, [sp, #4] @ save it for loop end | ||
53 | |||
54 | /* r0-r3 = history, r4-r8 = coefs, r9 = x[], r10..r11 = accumulator, | ||
55 | * r12 = shift amount, r14 = number of samples. | ||
56 | */ | ||
57 | .loop: | ||
58 | /* Direct form 1 filtering code. | ||
59 | * y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | ||
60 | * where y[] is output and x[] is input. This is performed out of order to | ||
61 | * reuse registers, we're pretty short on regs. | ||
62 | */ | ||
63 | smull r10, r11, r6, r1 @ acc = b2*x[i - 2] | ||
64 | mov r1, r0 @ fix input history | ||
65 | smlal r10, r11, r5, r0 @ acc += b1*x[i - 1] | ||
66 | ldr r0, [r9] @ load input and fix history in same operation | ||
67 | smlal r10, r11, r7, r2 @ acc += a1*y[i - 1] | ||
68 | smlal r10, r11, r8, r3 @ acc += a2*y[i - 2] | ||
69 | smlal r10, r11, r4, r0 @ acc += b0*x[i] /* avoid stall on arm9*/ | ||
70 | mov r3, r2 @ fix output history | ||
71 | mov r2, r11, asl r12 @ get upper part of result and shift left | ||
72 | #ifdef HIGH_PRECISION | ||
73 | rsb r11, r12, #32 @ get shift amount for lower part | ||
74 | orr r2, r2, r10, lsr r11 @ then mix in correctly shifted lower part | ||
75 | #endif | ||
76 | str r2, [r9], #4 @ save result | ||
77 | subs r14, r14, #1 @ are we done with this channel? | ||
78 | bne .loop | ||
79 | |||
80 | ldr r10, [sp, #4] @ load filter struct pointer | ||
81 | stmia r10!, { r0-r3 } @ save back history | ||
82 | ldr r11, [sp, #12] @ load number of channels | ||
83 | subs r11, r11, #1 @ all channels processed? | ||
84 | strne r11, [sp, #12] | ||
85 | bne .filterloop | ||
86 | |||
87 | add sp, sp, #16 @ compensate for temp storage | ||
88 | ldmpc regs=r4-r11 | ||
89 | |||
diff --git a/lib/rbcodec/dsp/eq_cf.S b/lib/rbcodec/dsp/eq_cf.S deleted file mode 100644 index 30a28b9d99..0000000000 --- a/lib/rbcodec/dsp/eq_cf.S +++ /dev/null | |||
@@ -1,91 +0,0 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006-2007 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | /* uncomment this to make filtering calculate lower bits after shifting. | ||
23 | * without this, "shift" - 1 of the lower bits will be lost here. | ||
24 | */ | ||
25 | /* #define HIGH_PRECISION */ | ||
26 | |||
27 | /* | ||
28 | * void eq_filter(int32_t **x, struct eqfilter *f, unsigned num, | ||
29 | * unsigned channels, unsigned shift) | ||
30 | */ | ||
31 | .text | ||
32 | .global eq_filter | ||
33 | eq_filter: | ||
34 | lea.l (-11*4, %sp), %sp | ||
35 | movem.l %d2-%d7/%a2-%a6, (%sp) | save clobbered regs | ||
36 | move.l (11*4+8, %sp), %a5 | fetch filter structure address | ||
37 | move.l (11*4+20, %sp), %d7 | load shift count | ||
38 | subq.l #1, %d7 | EMAC gives us one free shift | ||
39 | #ifdef HIGH_PRECISION | ||
40 | moveq.l #8, %d6 | ||
41 | sub.l %d7, %d6 | shift for lower part of accumulator | ||
42 | #endif | ||
43 | movem.l (%a5), %a0-%a4 | load coefs | ||
44 | lea.l (5*4, %a5), %a5 | point to filter history | ||
45 | |||
46 | .filterloop: | ||
47 | move.l (11*4+4, %sp), %a6 | load input channel pointer | ||
48 | addq.l #4, (11*4+4, %sp) | point x to next channel | ||
49 | move.l (%a6), %a6 | ||
50 | move.l (11*4+12, %sp), %d5 | number of samples | ||
51 | movem.l (%a5), %d0-%d3 | load filter history | ||
52 | |||
53 | /* d0-d3 = history, d4 = temp, d5 = sample count, d6 = lower shift amount, | ||
54 | * d7 = upper shift amount, a0-a4 = coefs, a5 = history pointer, a6 = x[] | ||
55 | */ | ||
56 | .loop: | ||
57 | /* Direct form 1 filtering code. We assume DSP has put EMAC in frac mode. | ||
58 | * y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2], | ||
59 | * where y[] is output and x[] is input. This is performed out of order | ||
60 | * to do parallel load of input value. | ||
61 | */ | ||
62 | mac.l %a2, %d1, %acc0 | acc = b2*x[i - 2] | ||
63 | move.l %d0, %d1 | fix input history | ||
64 | mac.l %a1, %d0, (%a6), %d0, %acc0 | acc += b1*x[i - 1], x[i] -> d0 | ||
65 | mac.l %a0, %d0, %acc0 | acc += b0*x[i] | ||
66 | mac.l %a3, %d2, %acc0 | acc += a1*y[i - 1] | ||
67 | mac.l %a4, %d3, %acc0 | acc += a2*y[i - 2] | ||
68 | move.l %d2, %d3 | fix output history | ||
69 | #ifdef HIGH_PRECISION | ||
70 | move.l %accext01, %d2 | fetch lower part of accumulator | ||
71 | move.b %d2, %d4 | clear upper three bytes | ||
72 | lsr.l %d6, %d4 | shift lower bits | ||
73 | #endif | ||
74 | movclr.l %acc0, %d2 | fetch upper part of result | ||
75 | asl.l %d7, %d2 | restore fixed point format | ||
76 | #ifdef HIGH_PRECISION | ||
77 | or.l %d2, %d4 | combine lower and upper parts | ||
78 | #endif | ||
79 | move.l %d2, (%a6)+ | save result | ||
80 | subq.l #1, %d5 | are we done with this channel? | ||
81 | jne .loop | ||
82 | |||
83 | movem.l %d0-%d3, (%a5) | save history back to struct | ||
84 | lea.l (4*4, %a5), %a5 | point to next channel's history | ||
85 | subq.l #1, (11*4+16, %sp) | have we processed both channels? | ||
86 | jne .filterloop | ||
87 | |||
88 | movem.l (%sp), %d2-%d7/%a2-%a6 | ||
89 | lea.l (11*4, %sp), %sp | ||
90 | rts | ||
91 | |||
diff --git a/lib/rbcodec/dsp/lin_resample.c b/lib/rbcodec/dsp/lin_resample.c new file mode 100644 index 0000000000..c8be3cb1ad --- /dev/null +++ b/lib/rbcodec/dsp/lin_resample.c | |||
@@ -0,0 +1,281 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Miika Pekkarinen | ||
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 "fracmul.h" | ||
26 | #include "fixedpoint.h" | ||
27 | #include "dsp_sample_io.h" | ||
28 | #include <string.h> | ||
29 | #include "dsp_proc_entry.h" | ||
30 | |||
31 | /** | ||
32 | * Linear interpolation resampling that introduces a one sample delay because | ||
33 | * of our inability to look into the future at the end of a frame. | ||
34 | */ | ||
35 | |||
36 | #if 0 /* Set to '1' to enable debug messages */ | ||
37 | #include <debug.h> | ||
38 | #else | ||
39 | #undef DEBUGF | ||
40 | #define DEBUGF(...) | ||
41 | #endif | ||
42 | |||
43 | #define RESAMPLE_BUF_COUNT 192 /* Per channel, per DSP */ | ||
44 | |||
45 | /* Data for each resampler on each DSP */ | ||
46 | static struct resample_data | ||
47 | { | ||
48 | uint32_t delta; /* 00h: Phase delta for each step */ | ||
49 | uint32_t phase; /* 04h: Current phase [pos16|frac16] */ | ||
50 | int32_t last_sample[2]; /* 08h: Last samples for interpolation (L+R) */ | ||
51 | int32_t frequency; /* 10h: Virtual samplerate */ | ||
52 | /* 14h */ | ||
53 | struct dsp_config *dsp; /* The DSP for this resampler */ | ||
54 | struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */ | ||
55 | int32_t resample_buf_arr[2][RESAMPLE_BUF_COUNT]; /* Actual output data */ | ||
56 | } resample_data[DSP_COUNT] IBSS_ATTR; | ||
57 | |||
58 | /* Actual worker function. Implemented here or in target assembly code. */ | ||
59 | int lin_resample_resample(struct resample_data *data, struct dsp_buffer *src, | ||
60 | struct dsp_buffer *dst); | ||
61 | |||
62 | static void lin_resample_flush_data(struct resample_data *data) | ||
63 | { | ||
64 | data->phase = 0; | ||
65 | data->last_sample[0] = 0; | ||
66 | data->last_sample[1] = 0; | ||
67 | } | ||
68 | |||
69 | static void lin_resample_flush(struct dsp_proc_entry *this) | ||
70 | { | ||
71 | struct resample_data *data = (void *)this->data; | ||
72 | data->resample_buf.remcount = 0; | ||
73 | lin_resample_flush_data(data); | ||
74 | } | ||
75 | |||
76 | static bool lin_resample_new_delta(struct resample_data *data, | ||
77 | struct dsp_buffer *buf) | ||
78 | { | ||
79 | int32_t frequency = buf->format.frequency; /* virtual samplerate */ | ||
80 | |||
81 | data->frequency = frequency; | ||
82 | data->delta = fp_div(frequency, NATIVE_FREQUENCY, 16); | ||
83 | |||
84 | if (frequency == NATIVE_FREQUENCY) | ||
85 | { | ||
86 | /* NOTE: If fully glitch-free transistions from no resampling to | ||
87 | resampling are desired, last_sample history should be maintained | ||
88 | even when not resampling. */ | ||
89 | lin_resample_flush_data(data); | ||
90 | return false; | ||
91 | } | ||
92 | |||
93 | return true; | ||
94 | } | ||
95 | |||
96 | #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) | ||
97 | /* Where the real work is done */ | ||
98 | int lin_resample_resample(struct resample_data *data, struct dsp_buffer *src, | ||
99 | struct dsp_buffer *dst) | ||
100 | { | ||
101 | int ch = src->format.num_channels - 1; | ||
102 | uint32_t count = MIN(src->remcount, 0x8000); | ||
103 | uint32_t delta = data->delta; | ||
104 | uint32_t phase, pos; | ||
105 | int32_t *d; | ||
106 | |||
107 | do | ||
108 | { | ||
109 | const int32_t *s = src->p32[ch]; | ||
110 | |||
111 | d = dst->p32[ch]; | ||
112 | int32_t *dmax = d + dst->bufcount; | ||
113 | |||
114 | phase = data->phase; | ||
115 | pos = phase >> 16; | ||
116 | pos = MIN(pos, count); | ||
117 | |||
118 | int32_t last = pos > 0 ? s[pos - 1] : data->last_sample[ch]; | ||
119 | |||
120 | if (pos < count) | ||
121 | { | ||
122 | while (1) | ||
123 | { | ||
124 | *d++ = last + FRACMUL((phase & 0xffff) << 15, s[pos] - last); | ||
125 | phase += delta; | ||
126 | pos = phase >> 16; | ||
127 | |||
128 | if (pos >= count || d >= dmax) | ||
129 | break; | ||
130 | |||
131 | if (pos > 0) | ||
132 | last = s[pos - 1]; | ||
133 | } | ||
134 | |||
135 | if (pos > 0) | ||
136 | { | ||
137 | pos = MIN(pos, count); | ||
138 | last = s[pos - 1]; | ||
139 | } | ||
140 | } | ||
141 | |||
142 | data->last_sample[ch] = last; | ||
143 | } | ||
144 | while (--ch >= 0); | ||
145 | |||
146 | /* Wrap phase accumulator back to start of next frame. */ | ||
147 | data->phase = phase - (pos << 16); | ||
148 | |||
149 | dst->remcount = d - dst->p32[0]; | ||
150 | |||
151 | return pos; | ||
152 | } | ||
153 | #endif /* CPU */ | ||
154 | |||
155 | /* Resample count stereo samples or stop when the destination is full. | ||
156 | * Updates the src buffer and changes to its own output buffer to refer to | ||
157 | * the resampled data. */ | ||
158 | static void lin_resample_process(struct dsp_proc_entry *this, | ||
159 | struct dsp_buffer **buf_p) | ||
160 | { | ||
161 | struct resample_data *data = (void *)this->data; | ||
162 | struct dsp_buffer *src = *buf_p; | ||
163 | struct dsp_buffer *dst = &data->resample_buf; | ||
164 | |||
165 | *buf_p = dst; | ||
166 | |||
167 | if (dst->remcount > 0) | ||
168 | return; /* data still remains */ | ||
169 | |||
170 | int channels = src->format.num_channels; | ||
171 | |||
172 | dst->remcount = 0; | ||
173 | dst->p32[0] = data->resample_buf_arr[0]; | ||
174 | dst->p32[1] = data->resample_buf_arr[channels - 1]; | ||
175 | |||
176 | if (src->remcount > 0) | ||
177 | { | ||
178 | dst->bufcount = RESAMPLE_BUF_COUNT; | ||
179 | |||
180 | int consumed = lin_resample_resample(data, src, dst); | ||
181 | |||
182 | /* Advance src by consumed amount */ | ||
183 | if (consumed > 0) | ||
184 | dsp_advance_buffer32(src, consumed); | ||
185 | } | ||
186 | /* else purged resample_buf */ | ||
187 | |||
188 | /* Inherit in-place processed mask from source buffer */ | ||
189 | dst->proc_mask = src->proc_mask; | ||
190 | } | ||
191 | |||
192 | /* Finish draining old samples then switch format or shut off */ | ||
193 | static void lin_resample_new_format(struct dsp_proc_entry *this, | ||
194 | struct dsp_buffer **buf_p) | ||
195 | { | ||
196 | struct resample_data *data = (void *)this->data; | ||
197 | struct dsp_buffer *src = *buf_p; | ||
198 | struct dsp_buffer *dst = &data->resample_buf; | ||
199 | |||
200 | if (dst->remcount > 0) | ||
201 | { | ||
202 | *buf_p = dst; | ||
203 | return; /* data still remains */ | ||
204 | } | ||
205 | |||
206 | DSP_PRINT_FORMAT(DSP_PROC_RESAMPLE, DSP_PROC_RESAMPLE, src->format); | ||
207 | |||
208 | struct dsp_config *dsp = data->dsp; | ||
209 | int32_t frequency = data->frequency; | ||
210 | bool active = dsp_proc_active(dsp, DSP_PROC_RESAMPLE); | ||
211 | |||
212 | if (src->format.frequency != frequency) | ||
213 | { | ||
214 | DEBUGF(" DSP_PROC_RESAMPLE- new delta\n"); | ||
215 | active = lin_resample_new_delta(data, src); | ||
216 | dsp_proc_activate(dsp, DSP_PROC_RESAMPLE, active); | ||
217 | } | ||
218 | |||
219 | /* Everything after us is NATIVE_FREQUENCY */ | ||
220 | struct sample_format f = src->format; | ||
221 | f.frequency = NATIVE_FREQUENCY; | ||
222 | f.codec_frequency = NATIVE_FREQUENCY; | ||
223 | |||
224 | if (!active) | ||
225 | { | ||
226 | DEBUGF(" DSP_PROC_RESAMPLE- not active\n"); | ||
227 | dst->format = f; /* Keep track */ | ||
228 | return; /* No resampling required */ | ||
229 | } | ||
230 | |||
231 | format_change_ack(&src->format); | ||
232 | |||
233 | if (EQU_SAMPLE_FORMAT(f, dst->format)) | ||
234 | { | ||
235 | DEBUGF(" DSP_PROC_RESAMPLE- same dst format\n"); | ||
236 | format_change_ack(&f); /* Nothing changed that matters downstream */ | ||
237 | } | ||
238 | |||
239 | dst->format = f; | ||
240 | dsp_proc_call(this, buf_p, 0); | ||
241 | } | ||
242 | |||
243 | /* DSP message hook */ | ||
244 | static intptr_t lin_resample_configure(struct dsp_proc_entry *this, | ||
245 | struct dsp_config *dsp, | ||
246 | unsigned int setting, | ||
247 | intptr_t value) | ||
248 | { | ||
249 | switch (setting) | ||
250 | { | ||
251 | case DSP_INIT: | ||
252 | /* Always enable resampler so that format changes may be monitored and | ||
253 | * it self-activated when required */ | ||
254 | dsp_proc_enable(dsp, DSP_PROC_RESAMPLE, true); | ||
255 | break; | ||
256 | |||
257 | case DSP_FLUSH: | ||
258 | lin_resample_flush(this); | ||
259 | break; | ||
260 | |||
261 | case DSP_PROC_INIT: | ||
262 | this->data = (intptr_t)&resample_data[dsp_get_id(dsp)]; | ||
263 | this->ip_mask = 0; /* Not in-place */ | ||
264 | this->process[0] = lin_resample_process; | ||
265 | this->process[1] = lin_resample_new_format; | ||
266 | ((struct resample_data *)this->data)->dsp = dsp; | ||
267 | break; | ||
268 | |||
269 | case DSP_PROC_CLOSE: | ||
270 | /* This stage should be enabled at all times */ | ||
271 | DEBUGF("DSP_PROC_RESAMPLE- Error: Closing!\n"); | ||
272 | break; | ||
273 | } | ||
274 | |||
275 | return 1; | ||
276 | (void)value; | ||
277 | } | ||
278 | |||
279 | /* Database entry */ | ||
280 | DSP_PROC_DB_ENTRY(RESAMPLE, | ||
281 | lin_resample_configure); | ||
diff --git a/lib/rbcodec/dsp/pga.c b/lib/rbcodec/dsp/pga.c new file mode 100644 index 0000000000..c2c29ccfc0 --- /dev/null +++ b/lib/rbcodec/dsp/pga.c | |||
@@ -0,0 +1,144 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Magnus Holmgren | ||
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-util.h" | ||
26 | #include "fixedpoint.h" | ||
27 | #include "fracmul.h" | ||
28 | #include "dsp_proc_entry.h" | ||
29 | |||
30 | /* Implemented here or in target assembly code */ | ||
31 | void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p); | ||
32 | |||
33 | #define DEFAULT_PGA_GAIN (PGA_UNITY >> 1) /* s8.23 format */ | ||
34 | |||
35 | static struct pga_data | ||
36 | { | ||
37 | int32_t gain; /* 00h: Final gain in s8.23 format */ | ||
38 | uint32_t enabled; /* Mask of enabled gains */ | ||
39 | int32_t gains[PGA_NUM_GAINS]; /* Individual gains in s7.24 format */ | ||
40 | } pga_data = | ||
41 | { | ||
42 | .gain = DEFAULT_PGA_GAIN, | ||
43 | .enabled = 0, | ||
44 | .gains[0 ... PGA_NUM_GAINS-1] = PGA_UNITY, | ||
45 | }; | ||
46 | |||
47 | /* Combine all gains to a global gain and enable/disable the amplifier if | ||
48 | the overall gain is not unity/unity */ | ||
49 | static void pga_update(void) | ||
50 | { | ||
51 | int32_t gain = PGA_UNITY; | ||
52 | |||
53 | /* Multiply all gains with one another to get overall amp gain */ | ||
54 | for (int i = 0; i < PGA_NUM_GAINS; i++) | ||
55 | { | ||
56 | if (pga_data.enabled & BIT_N(i)) /* Only enabled gains factor in */ | ||
57 | gain = fp_mul(gain, pga_data.gains[i], 24); | ||
58 | } | ||
59 | |||
60 | gain >>= 1; /* s7.24 -> s8.23 format */ | ||
61 | |||
62 | if (gain == pga_data.gain) | ||
63 | return; | ||
64 | |||
65 | struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO); | ||
66 | pga_data.gain = gain; | ||
67 | dsp_proc_enable(dsp, DSP_PROC_PGA, gain != DEFAULT_PGA_GAIN); | ||
68 | dsp_proc_activate(dsp, DSP_PROC_PGA, true); | ||
69 | } | ||
70 | |||
71 | |||
72 | /** Amp controls **/ | ||
73 | |||
74 | /* Set a particular gain value - doesn't have to be enabled */ | ||
75 | void pga_set_gain(enum pga_gain_ids id, int32_t value) | ||
76 | { | ||
77 | if (value == pga_data.gains[id]) | ||
78 | return; | ||
79 | |||
80 | pga_data.gains[id] = value; | ||
81 | |||
82 | if (BIT_N(id) & pga_data.enabled) | ||
83 | pga_update(); | ||
84 | } | ||
85 | |||
86 | /* Enable or disable the specified gain stage */ | ||
87 | void pga_enable_gain(enum pga_gain_ids id, bool enable) | ||
88 | { | ||
89 | uint32_t bit = BIT_N(id); | ||
90 | |||
91 | if (enable != !(pga_data.enabled & bit)) | ||
92 | return; | ||
93 | |||
94 | pga_data.enabled ^= bit; | ||
95 | pga_update(); | ||
96 | } | ||
97 | |||
98 | |||
99 | /** DSP interface **/ | ||
100 | |||
101 | #if !defined(CPU_COLDFIRE) && !defined(CPU_ARM) | ||
102 | /* Apply a constant gain to the samples (e.g., for ReplayGain). */ | ||
103 | void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p) | ||
104 | { | ||
105 | int32_t gain = ((struct pga_data *)this->data)->gain; | ||
106 | struct dsp_buffer *buf = *buf_p; | ||
107 | unsigned int channels = buf->format.num_channels; | ||
108 | |||
109 | for (unsigned int ch = 0; ch < channels; ch++) | ||
110 | { | ||
111 | int32_t *d = buf->p32[ch]; | ||
112 | int count = buf->remcount; | ||
113 | |||
114 | for (int i = 0; i < count; i++) | ||
115 | d[i] = FRACMUL_SHL(d[i], gain, 8); | ||
116 | } | ||
117 | |||
118 | (void)this; | ||
119 | } | ||
120 | #endif /* CPU */ | ||
121 | |||
122 | /* DSP message hook */ | ||
123 | static intptr_t pga_configure(struct dsp_proc_entry *this, | ||
124 | struct dsp_config *dsp, | ||
125 | unsigned int setting, | ||
126 | intptr_t value) | ||
127 | { | ||
128 | switch (setting) | ||
129 | { | ||
130 | case DSP_PROC_INIT: | ||
131 | if (value != 0) | ||
132 | break; /* Already initialized */ | ||
133 | this->data = (intptr_t)&pga_data; | ||
134 | this->process[0] = pga_process; | ||
135 | break; | ||
136 | } | ||
137 | |||
138 | return 1; | ||
139 | (void)dsp; | ||
140 | } | ||
141 | |||
142 | /* Database entry */ | ||
143 | DSP_PROC_DB_ENTRY(PGA, | ||
144 | pga_configure); | ||
diff --git a/lib/rbcodec/dsp/pga.h b/lib/rbcodec/dsp/pga.h new file mode 100644 index 0000000000..f0c4c4717f --- /dev/null +++ b/lib/rbcodec/dsp/pga.h | |||
@@ -0,0 +1,40 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2012 Michael Sevakis | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef PGA_H | ||
22 | #define PGA_H | ||
23 | |||
24 | #define PGA_UNITY ((int32_t)0x01000000) /* s7.24 */ | ||
25 | |||
26 | /* Various gains supported by pre-gain amp */ | ||
27 | enum pga_gain_ids | ||
28 | { | ||
29 | PGA_EQ_PRECUT = 0, | ||
30 | PGA_REPLAYGAIN, | ||
31 | #ifdef HAVE_SW_VOLUME_CONTROL | ||
32 | PGA_VOLUME, | ||
33 | #endif | ||
34 | PGA_NUM_GAINS, | ||
35 | }; | ||
36 | |||
37 | void pga_set_gain(enum pga_gain_ids id, int32_t value); | ||
38 | void pga_enable_gain(enum pga_gain_ids id, bool enable); | ||
39 | |||
40 | #endif /* PGA_H */ | ||
diff --git a/lib/rbcodec/dsp/tdspeed.c b/lib/rbcodec/dsp/tdspeed.c index c2f4a3f704..3aa8acc458 100644 --- a/lib/rbcodec/dsp/tdspeed.c +++ b/lib/rbcodec/dsp/tdspeed.c | |||
@@ -9,6 +9,7 @@ | |||
9 | * | 9 | * |
10 | * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org> | 10 | * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org> |
11 | * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca> | 11 | * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca> |
12 | * Copyright (C) 2012 Michael Sevakis | ||
12 | * | 13 | * |
13 | * This program is free software; you can redistribute it and/or | 14 | * This program is free software; you can redistribute it and/or |
14 | * modify it under the terms of the GNU General Public License | 15 | * modify it under the terms of the GNU General Public License |
@@ -19,69 +20,42 @@ | |||
19 | * KIND, either express or implied. | 20 | * KIND, either express or implied. |
20 | * | 21 | * |
21 | ****************************************************************************/ | 22 | ****************************************************************************/ |
22 | 23 | #include "config.h" | |
23 | #include <inttypes.h> | 24 | #include "system.h" |
24 | #include <stddef.h> | ||
25 | #include <stdio.h> | ||
26 | #include <string.h> | ||
27 | #include "sound.h" | 25 | #include "sound.h" |
28 | #include "core_alloc.h" | 26 | #include "core_alloc.h" |
29 | #include "system.h" | 27 | #include "system.h" |
30 | #include "tdspeed.h" | 28 | #include "tdspeed.h" |
31 | #include "settings.h" | 29 | #include "settings.h" |
32 | #include "dsp-util.h" | 30 | #include "dsp-util.h" |
31 | #include "dsp_proc_entry.h" | ||
33 | 32 | ||
34 | #define assert(cond) | 33 | #define assert(cond) |
35 | 34 | ||
35 | #define TIMESTRETCH_SET_FACTOR (DSP_PROC_SETTING+DSP_PROC_TIMESTRETCH) | ||
36 | |||
36 | #define MIN_RATE 8000 | 37 | #define MIN_RATE 8000 |
37 | #define MAX_RATE 48000 /* double buffer for double rate */ | 38 | #define MAX_RATE 48000 /* double buffer for double rate */ |
38 | #define MINFREQ 100 | 39 | #define MINFREQ 100 |
39 | 40 | ||
40 | #define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */ | 41 | #define MAX_INPUTCOUNT 512 /* Max input count so dst doesn't overflow */ |
41 | 42 | #define FIXED_BUFCOUNT 3072 /* 48KHz factor 3.0 */ | |
42 | static int32_t** dsp_src; | 43 | #define FIXED_OUTBUFCOUNT 4096 |
43 | static int handles[4]; | ||
44 | static int32_t *overlap_buffer[2] = { NULL, NULL }; | ||
45 | static int32_t *outbuf[2] = { NULL, NULL }; | ||
46 | |||
47 | static int move_callback(int handle, void* current, void* new) | ||
48 | { | ||
49 | /* TODO */ | ||
50 | (void)handle; | ||
51 | if (dsp_src) | ||
52 | { | ||
53 | int ch = (current == outbuf[0]) ? 0 : 1; | ||
54 | dsp_src[ch] = outbuf[ch] = new; | ||
55 | } | ||
56 | return BUFLIB_CB_OK; | ||
57 | } | ||
58 | |||
59 | static struct buflib_callbacks ops = { | ||
60 | .move_callback = move_callback, | ||
61 | .shrink_callback = NULL, | ||
62 | }; | ||
63 | 44 | ||
64 | static int ovl_move_callback(int handle, void* current, void* new) | 45 | enum tdspeed_ops |
65 | { | 46 | { |
66 | /* TODO */ | 47 | TDSOP_PROCESS, |
67 | (void)handle; | 48 | TDSOP_LAST, |
68 | if (dsp_src) | 49 | TDSOP_PURGE, |
69 | { | ||
70 | int ch = (current == overlap_buffer[0]) ? 0 : 1; | ||
71 | overlap_buffer[ch] = new; | ||
72 | } | ||
73 | return BUFLIB_CB_OK; | ||
74 | } | ||
75 | |||
76 | static struct buflib_callbacks ovl_ops = { | ||
77 | .move_callback = ovl_move_callback, | ||
78 | .shrink_callback = NULL, | ||
79 | }; | 50 | }; |
80 | 51 | ||
81 | |||
82 | static struct tdspeed_state_s | 52 | static struct tdspeed_state_s |
83 | { | 53 | { |
84 | bool stereo; | 54 | struct dsp_proc_entry *this; /* this stage */ |
55 | struct dsp_config *dsp; /* the DSP we use */ | ||
56 | unsigned int channels; /* flags parameter to use in call */ | ||
57 | int32_t samplerate; /* current samplerate of input data */ | ||
58 | int32_t factor; /* stretch factor (perdecimille) */ | ||
85 | int32_t shift_max; /* maximum displacement on a frame */ | 59 | int32_t shift_max; /* maximum displacement on a frame */ |
86 | int32_t src_step; /* source window pace */ | 60 | int32_t src_step; /* source window pace */ |
87 | int32_t dst_step; /* destination window pace */ | 61 | int32_t dst_step; /* destination window pace */ |
@@ -89,62 +63,132 @@ static struct tdspeed_state_s | |||
89 | int32_t ovl_shift; /* overlap buffer frame shift */ | 63 | int32_t ovl_shift; /* overlap buffer frame shift */ |
90 | int32_t ovl_size; /* overlap buffer used size */ | 64 | int32_t ovl_size; /* overlap buffer used size */ |
91 | int32_t ovl_space; /* overlap buffer size */ | 65 | int32_t ovl_space; /* overlap buffer size */ |
92 | int32_t *ovl_buff[2]; /* overlap buffer */ | 66 | int32_t *ovl_buff[2]; /* overlap buffer (L+R) */ |
93 | } tdspeed_state; | 67 | } tdspeed_state; |
94 | 68 | ||
95 | void tdspeed_init(void) | 69 | static int handles[4] = { 0, 0, 0, 0 }; |
70 | static int32_t *buffers[4] = { NULL, NULL, NULL, NULL }; | ||
71 | |||
72 | #define overlap_buffer (&buffers[0]) | ||
73 | #define outbuf (&buffers[2]) | ||
74 | #define out_size FIXED_OUTBUFCOUNT | ||
75 | |||
76 | /* Processed buffer passed out to later stages */ | ||
77 | static struct dsp_buffer dsp_outbuf; | ||
78 | |||
79 | static int move_callback(int handle, void *current, void *new) | ||
96 | { | 80 | { |
97 | if (!global_settings.timestretch_enabled) | 81 | #if 0 |
98 | return; | 82 | /* Should not currently need to block this since DSP loop completes an |
83 | iteration before yielding and begins again at its input buffer */ | ||
84 | if (dsp_is_busy(tdspeed_state.dsp)) | ||
85 | return BUFLIB_CB_CANNOT_MOVE; /* DSP processing in progress */ | ||
86 | #endif | ||
99 | 87 | ||
100 | /* Allocate buffers */ | 88 | ptrdiff_t shift = (int32_t *)new - (int32_t *)current; |
101 | if (overlap_buffer[0] == NULL) | 89 | int32_t **p32 = dsp_outbuf.p32; |
90 | |||
91 | for (unsigned int i = 0; i < ARRAYLEN(handles); i++) | ||
102 | { | 92 | { |
103 | handles[0] = core_alloc_ex("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops); | 93 | if (handle != handles[i]) |
104 | overlap_buffer[0] = core_get_data(handles[0]); | 94 | continue; |
95 | |||
96 | switch (i) | ||
97 | { | ||
98 | case 0: case 1: | ||
99 | /* moving overlap (input) buffers */ | ||
100 | tdspeed_state.ovl_buff[i] = new; | ||
101 | break; | ||
102 | |||
103 | case 2: | ||
104 | /* moving outbuf left channel and dsp_outbuf.p32[0] */ | ||
105 | if (p32[0] == p32[1]) | ||
106 | p32[1] += shift; /* mono mode */ | ||
107 | |||
108 | p32[0] += shift; | ||
109 | break; | ||
110 | |||
111 | case 3: | ||
112 | /* moving outbuf right channel and dsp_outbuf.p32[1] */ | ||
113 | p32[1] += shift; | ||
114 | break; | ||
115 | } | ||
116 | |||
117 | buffers[i] = new; | ||
118 | break; | ||
105 | } | 119 | } |
106 | if (overlap_buffer[1] == NULL) | 120 | |
121 | return BUFLIB_CB_OK; | ||
122 | } | ||
123 | |||
124 | static struct buflib_callbacks ops = | ||
125 | { | ||
126 | .move_callback = move_callback, | ||
127 | .shrink_callback = NULL, | ||
128 | }; | ||
129 | |||
130 | /* Allocate timestretch buffers */ | ||
131 | static bool tdspeed_alloc_buffers(void) | ||
132 | { | ||
133 | static const struct | ||
107 | { | 134 | { |
108 | handles[1] = core_alloc_ex("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops); | 135 | const char *name; |
109 | overlap_buffer[1] = core_get_data(handles[1]); | 136 | size_t size; |
110 | } | 137 | } bufdefs[4] = |
111 | if (outbuf[0] == NULL) | ||
112 | { | 138 | { |
113 | handles[2] = core_alloc_ex("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops); | 139 | { "tdspeed ovl L", FIXED_BUFCOUNT * sizeof(int32_t) }, |
114 | outbuf[0] = core_get_data(handles[2]); | 140 | { "tdspeed ovl R", FIXED_BUFCOUNT * sizeof(int32_t) }, |
115 | } | 141 | { "tdspeed out L", FIXED_OUTBUFCOUNT * sizeof(int32_t) }, |
116 | if (outbuf[1] == NULL) | 142 | { "tdspeed out R", FIXED_OUTBUFCOUNT * sizeof(int32_t) }, |
143 | }; | ||
144 | |||
145 | for (unsigned int i = 0; i < ARRAYLEN(bufdefs); i++) | ||
117 | { | 146 | { |
118 | handles[3] = core_alloc_ex("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops); | 147 | if (handles[i] <= 0) |
119 | outbuf[1] = core_get_data(handles[3]); | 148 | { |
149 | handles[i] = core_alloc_ex(bufdefs[i].name, bufdefs[i].size, &ops); | ||
150 | |||
151 | if (handles[i] <= 0) | ||
152 | return false; | ||
153 | } | ||
154 | |||
155 | if (buffers[i] == NULL) | ||
156 | { | ||
157 | buffers[i] = core_get_data(handles[i]); | ||
158 | |||
159 | if (buffers[i] == NULL) | ||
160 | return false; | ||
161 | } | ||
120 | } | 162 | } |
163 | |||
164 | return true; | ||
121 | } | 165 | } |
122 | 166 | ||
123 | void tdspeed_finish(void) | 167 | /* Free timestretch buffers */ |
168 | static void tdspeed_free_buffers(void) | ||
124 | { | 169 | { |
125 | for(unsigned i = 0; i < ARRAYLEN(handles); i++) | 170 | for (unsigned int i = 0; i < ARRAYLEN(handles); i++) |
126 | { | 171 | { |
127 | if (handles[i] > 0) | 172 | if (handles[i] > 0) |
128 | { | ||
129 | core_free(handles[i]); | 173 | core_free(handles[i]); |
130 | handles[i] = 0; | 174 | |
131 | } | 175 | handles[i] = 0; |
176 | buffers[i] = NULL; | ||
132 | } | 177 | } |
133 | overlap_buffer[0] = overlap_buffer[1] = NULL; | ||
134 | outbuf[0] = outbuf[1] = NULL; | ||
135 | } | 178 | } |
136 | 179 | ||
137 | bool tdspeed_config(int samplerate, bool stereo, int32_t factor) | 180 | /* Discard all data */ |
181 | static void tdspeed_flush(void) | ||
138 | { | 182 | { |
139 | struct tdspeed_state_s *st = &tdspeed_state; | 183 | struct tdspeed_state_s *st = &tdspeed_state; |
140 | int src_frame_sz; | 184 | st->ovl_size = 0; |
141 | 185 | st->ovl_shift = 0; | |
142 | /* Check buffers were allocated ok */ | 186 | dsp_outbuf.remcount = 0; /* Dump remaining output */ |
143 | if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL) | 187 | } |
144 | return false; | ||
145 | 188 | ||
146 | if (outbuf[0] == NULL || outbuf[1] == NULL) | 189 | static bool tdspeed_update(int32_t samplerate, int32_t factor) |
147 | return false; | 190 | { |
191 | struct tdspeed_state_s *st = &tdspeed_state; | ||
148 | 192 | ||
149 | /* Check parameters */ | 193 | /* Check parameters */ |
150 | if (factor == PITCH_SPEED_100) | 194 | if (factor == PITCH_SPEED_100) |
@@ -156,7 +200,10 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor) | |||
156 | if (factor < STRETCH_MIN || factor > STRETCH_MAX) | 200 | if (factor < STRETCH_MIN || factor > STRETCH_MAX) |
157 | return false; | 201 | return false; |
158 | 202 | ||
159 | st->stereo = stereo; | 203 | /* Save parameters we'll need later if format changes */ |
204 | st->samplerate = samplerate; | ||
205 | st->factor = factor; | ||
206 | |||
160 | st->dst_step = samplerate / MINFREQ; | 207 | st->dst_step = samplerate / MINFREQ; |
161 | 208 | ||
162 | if (factor > PITCH_SPEED_100) | 209 | if (factor > PITCH_SPEED_100) |
@@ -171,7 +218,7 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor) | |||
171 | st->src_step = st->dst_step * factor / PITCH_SPEED_100; | 218 | st->src_step = st->dst_step * factor / PITCH_SPEED_100; |
172 | st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step; | 219 | st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step; |
173 | 220 | ||
174 | src_frame_sz = st->shift_max + st->dst_step; | 221 | int src_frame_sz = st->shift_max + st->dst_step; |
175 | 222 | ||
176 | if (st->dst_step > st->src_step) | 223 | if (st->dst_step > st->src_step) |
177 | src_frame_sz += st->dst_step - st->src_step; | 224 | src_frame_sz += st->dst_step - st->src_step; |
@@ -182,32 +229,27 @@ bool tdspeed_config(int samplerate, bool stereo, int32_t factor) | |||
182 | if (st->src_step > st->dst_step) | 229 | if (st->src_step > st->dst_step) |
183 | st->ovl_space += 2*st->src_step - st->dst_step; | 230 | st->ovl_space += 2*st->src_step - st->dst_step; |
184 | 231 | ||
185 | if (st->ovl_space > FIXED_BUFSIZE) | 232 | if (st->ovl_space > FIXED_BUFCOUNT) |
186 | st->ovl_space = FIXED_BUFSIZE; | 233 | st->ovl_space = FIXED_BUFCOUNT; |
187 | 234 | ||
235 | /* just discard remaining input data */ | ||
188 | st->ovl_size = 0; | 236 | st->ovl_size = 0; |
189 | st->ovl_shift = 0; | 237 | st->ovl_shift = 0; |
190 | 238 | ||
191 | st->ovl_buff[0] = overlap_buffer[0]; | 239 | st->ovl_buff[0] = overlap_buffer[0]; |
192 | 240 | st->ovl_buff[1] = overlap_buffer[1]; /* ignored if mono */ | |
193 | if (stereo) | ||
194 | st->ovl_buff[1] = overlap_buffer[1]; | ||
195 | else | ||
196 | st->ovl_buff[1] = st->ovl_buff[0]; | ||
197 | 241 | ||
198 | return true; | 242 | return true; |
199 | } | 243 | } |
200 | 244 | ||
201 | static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], | 245 | static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], |
202 | int data_len, int last, int out_size) | 246 | int data_len, enum tdspeed_ops op, int *consumed) |
203 | /* data_len in samples */ | 247 | /* data_len in samples */ |
204 | { | 248 | { |
205 | struct tdspeed_state_s *st = &tdspeed_state; | 249 | struct tdspeed_state_s *st = &tdspeed_state; |
206 | int32_t *dest[2]; | 250 | int32_t *dest[2]; |
207 | int32_t next_frame, prev_frame, src_frame_sz; | 251 | int32_t next_frame, prev_frame, src_frame_sz; |
208 | bool stereo = buf_in[0] != buf_in[1]; | 252 | bool stereo = st->channels > 1; |
209 | |||
210 | assert(stereo == st->stereo); | ||
211 | 253 | ||
212 | src_frame_sz = st->shift_max + st->dst_step; | 254 | src_frame_sz = st->shift_max + st->dst_step; |
213 | 255 | ||
@@ -233,7 +275,7 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], | |||
233 | if (copy > data_len) | 275 | if (copy > data_len) |
234 | copy = data_len; | 276 | copy = data_len; |
235 | 277 | ||
236 | assert(st->ovl_size + copy <= FIXED_BUFSIZE); | 278 | assert(st->ovl_size + copy <= FIXED_BUFCOUNT); |
237 | memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0], | 279 | memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0], |
238 | copy * sizeof(int32_t)); | 280 | copy * sizeof(int32_t)); |
239 | 281 | ||
@@ -241,7 +283,9 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], | |||
241 | memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1], | 283 | memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1], |
242 | copy * sizeof(int32_t)); | 284 | copy * sizeof(int32_t)); |
243 | 285 | ||
244 | if (!last && have + copy < src_frame_sz) | 286 | *consumed += copy; |
287 | |||
288 | if (op == TDSOP_PROCESS && have + copy < src_frame_sz) | ||
245 | { | 289 | { |
246 | /* still not enough to process at least one frame */ | 290 | /* still not enough to process at least one frame */ |
247 | st->ovl_size += copy; | 291 | st->ovl_size += copy; |
@@ -254,13 +298,14 @@ static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2], | |||
254 | 298 | ||
255 | if (copy == data_len) | 299 | if (copy == data_len) |
256 | { | 300 | { |
257 | assert(have + copy <= FIXED_BUFSIZE); | 301 | assert(have + copy <= FIXED_BUFCOUNT); |
258 | return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last, | 302 | return tdspeed_apply(buf_out, st->ovl_buff, have+copy, op, |
259 | out_size); | 303 | consumed); |
260 | } | 304 | } |
261 | 305 | ||
262 | assert(have + copy <= FIXED_BUFSIZE); | 306 | assert(have + copy <= FIXED_BUFCOUNT); |
263 | int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size); | 307 | int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, |
308 | TDSOP_LAST, consumed); | ||
264 | 309 | ||
265 | dest[0] = buf_out[0] + i; | 310 | dest[0] = buf_out[0] + i; |
266 | dest[1] = buf_out[1] + i; | 311 | dest[1] = buf_out[1] + i; |
@@ -379,12 +424,12 @@ skip:; | |||
379 | } | 424 | } |
380 | 425 | ||
381 | /* now deal with remaining partial frames */ | 426 | /* now deal with remaining partial frames */ |
382 | if (last == -1) | 427 | if (op == TDSOP_LAST) |
383 | { | 428 | { |
384 | /* special overlap buffer processing: remember frame shift only */ | 429 | /* special overlap buffer processing: remember frame shift only */ |
385 | st->ovl_shift = next_frame - prev_frame; | 430 | st->ovl_shift = next_frame - prev_frame; |
386 | } | 431 | } |
387 | else if (last != 0) | 432 | else if (op == TDSOP_PURGE) |
388 | { | 433 | { |
389 | /* last call: purge all remaining data to output buffer */ | 434 | /* last call: purge all remaining data to output buffer */ |
390 | int i = data_len - prev_frame; | 435 | int i = data_len - prev_frame; |
@@ -400,6 +445,8 @@ skip:; | |||
400 | memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t)); | 445 | memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t)); |
401 | dest[1] += i; | 446 | dest[1] += i; |
402 | } | 447 | } |
448 | |||
449 | *consumed += i; | ||
403 | } | 450 | } |
404 | else | 451 | else |
405 | { | 452 | { |
@@ -408,7 +455,7 @@ skip:; | |||
408 | int i = (st->ovl_shift < 0) ? next_frame : prev_frame; | 455 | int i = (st->ovl_shift < 0) ? next_frame : prev_frame; |
409 | st->ovl_size = data_len - i; | 456 | st->ovl_size = data_len - i; |
410 | 457 | ||
411 | assert(st->ovl_size <= FIXED_BUFSIZE); | 458 | assert(st->ovl_size <= FIXED_BUFCOUNT); |
412 | memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t)); | 459 | memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t)); |
413 | 460 | ||
414 | if (stereo) | 461 | if (stereo) |
@@ -418,32 +465,223 @@ skip:; | |||
418 | return dest[0] - buf_out[0]; | 465 | return dest[0] - buf_out[0]; |
419 | } | 466 | } |
420 | 467 | ||
421 | long tdspeed_est_output_size() | 468 | |
469 | /** DSP interface **/ | ||
470 | |||
471 | static void tdspeed_process_new_format(struct dsp_proc_entry *this, | ||
472 | struct dsp_buffer **buf_p); | ||
473 | |||
474 | /* Enable or disable the availability of timestretch */ | ||
475 | void dsp_timestretch_enable(bool enabled) | ||
422 | { | 476 | { |
423 | return TDSPEED_OUTBUFSIZE; | 477 | if (enabled != !tdspeed_state.this) |
478 | return; /* No change */ | ||
479 | |||
480 | dsp_proc_enable(dsp_get_config(CODEC_IDX_AUDIO), DSP_PROC_TIMESTRETCH, | ||
481 | enabled); | ||
424 | } | 482 | } |
425 | 483 | ||
426 | long tdspeed_est_input_size(long size) | 484 | /* Set the timestretch ratio */ |
485 | void dsp_set_timestretch(int32_t percent) | ||
427 | { | 486 | { |
428 | struct tdspeed_state_s *st = &tdspeed_state; | 487 | struct tdspeed_state_s *st = &tdspeed_state; |
429 | 488 | ||
430 | size = (size - st->ovl_size) * st->src_step / st->dst_step; | 489 | if (!st->this) |
490 | return; /* not enabled */ | ||
491 | |||
492 | if (percent <= 0) | ||
493 | percent = PITCH_SPEED_100; | ||
494 | |||
495 | if (percent == st->factor) | ||
496 | return; /* no change */ | ||
497 | |||
498 | dsp_configure(st->dsp, TIMESTRETCH_SET_FACTOR, percent); | ||
499 | } | ||
500 | |||
501 | /* Return the timestretch ratio */ | ||
502 | int32_t dsp_get_timestretch(void) | ||
503 | { | ||
504 | return tdspeed_state.factor; | ||
505 | } | ||
506 | |||
507 | /* Return whether or not timestretch is enabled and initialized */ | ||
508 | bool dsp_timestretch_available(void) | ||
509 | { | ||
510 | return !!tdspeed_state.this; | ||
511 | } | ||
512 | |||
513 | /* Apply timestretch to the input buffer and switch to our output buffer */ | ||
514 | static void tdspeed_process(struct dsp_proc_entry *this, | ||
515 | struct dsp_buffer **buf_p) | ||
516 | { | ||
517 | struct dsp_buffer *src = *buf_p; | ||
518 | struct dsp_buffer *dst = &dsp_outbuf; | ||
519 | |||
520 | *buf_p = dst; /* switch to our buffer */ | ||
521 | |||
522 | int count = dst->remcount; | ||
523 | |||
524 | if (count > 0) | ||
525 | return; /* output remains from an earlier call */ | ||
431 | 526 | ||
432 | if (size < 0) | 527 | dst->p32[0] = outbuf[0]; |
433 | size = 0; | 528 | dst->p32[1] = outbuf[src->format.num_channels - 1]; |
434 | 529 | ||
435 | return size; | 530 | if (src->remcount > 0) |
531 | { | ||
532 | dst->bufcount = 0; /* use this to get consumed src */ | ||
533 | count = tdspeed_apply(dst->p32, src->p32, | ||
534 | MIN(src->remcount, MAX_INPUTCOUNT), | ||
535 | TDSOP_PROCESS, &dst->bufcount); | ||
536 | |||
537 | /* advance src by samples consumed */ | ||
538 | if (dst->bufcount > 0) | ||
539 | dsp_advance_buffer32(src, dst->bufcount); | ||
540 | } | ||
541 | /* else purged dsp_outbuf */ | ||
542 | |||
543 | dst->remcount = count; | ||
544 | |||
545 | /* inherit in-place processed mask from source buffer */ | ||
546 | dst->proc_mask = src->proc_mask; | ||
547 | |||
548 | (void)this; | ||
436 | } | 549 | } |
437 | 550 | ||
438 | int tdspeed_doit(int32_t *src[], int count) | 551 | /* Process format changes and settings changes */ |
552 | static void tdspeed_process_new_format(struct dsp_proc_entry *this, | ||
553 | struct dsp_buffer **buf_p) | ||
439 | { | 554 | { |
440 | dsp_src = src; | 555 | struct dsp_buffer *src = *buf_p; |
441 | count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] }, | 556 | struct dsp_buffer *dst = &dsp_outbuf; |
442 | src, count, 0, TDSPEED_OUTBUFSIZE); | 557 | |
558 | if (dst->remcount > 0) | ||
559 | { | ||
560 | *buf_p = dst; | ||
561 | return; /* output remains from an earlier call */ | ||
562 | } | ||
443 | 563 | ||
444 | src[0] = outbuf[0]; | 564 | DSP_PRINT_FORMAT(DSP_PROC_TIMESTRETCH, DSP_PROC_TIMESTRETCH, src->format); |
445 | src[1] = outbuf[1]; | 565 | |
566 | struct tdspeed_state_s *st = &tdspeed_state; | ||
567 | struct dsp_config *dsp = st->dsp; | ||
568 | struct sample_format *format = &src->format; | ||
569 | unsigned int channels = format->num_channels; | ||
570 | |||
571 | if (format->codec_frequency != st->samplerate) | ||
572 | { | ||
573 | /* relevent parameters are changing - all overlap will be discarded */ | ||
574 | st->channels = channels; | ||
575 | |||
576 | DEBUGF(" DSP_PROC_TIMESTRETCH- new settings: " | ||
577 | "ch:%u chz: %u, %d.%02d%%\n", | ||
578 | channels, | ||
579 | format->codec_frequency, | ||
580 | st->factor / 100, st->factor % 100); | ||
581 | bool active = tdspeed_update(format->codec_frequency, st->factor); | ||
582 | dsp_proc_activate(dsp, DSP_PROC_TIMESTRETCH, active); | ||
583 | |||
584 | if (!active) | ||
585 | { | ||
586 | DEBUGF(" DSP_PROC_RESAMPLE- not active\n"); | ||
587 | dst->format = src->format; /* Keep track */ | ||
588 | return; /* no more for now */ | ||
589 | } | ||
590 | } | ||
591 | else if (channels != st->channels) | ||
592 | { | ||
593 | /* channel count transistion - have to make old data in overlap | ||
594 | buffer compatible with new format */ | ||
595 | DEBUGF(" DSP_PROC_TIMESTRETCH- new ch count: %u=>%u\n", | ||
596 | st->channels, channels); | ||
597 | |||
598 | st->channels = channels; | ||
599 | |||
600 | if (channels > 1) | ||
601 | { | ||
602 | /* mono->stereo: Process the old mono as stereo now */ | ||
603 | memcpy(st->ovl_buff[1], st->ovl_buff[0], | ||
604 | st->ovl_size * sizeof (int32_t)); | ||
605 | } | ||
606 | else | ||
607 | { | ||
608 | /* stereo->mono: Process the old stereo as mono now */ | ||
609 | for (int i = 0; i < st->ovl_size; i++) | ||
610 | { | ||
611 | st->ovl_buff[0][i] = st->ovl_buff[0][i] / 2 + | ||
612 | st->ovl_buff[1][i] / 2; | ||
613 | } | ||
614 | } | ||
615 | } | ||
616 | |||
617 | struct sample_format f = *format; | ||
618 | format_change_ack(format); | ||
619 | |||
620 | if (EQU_SAMPLE_FORMAT(f, dst->format)) | ||
621 | { | ||
622 | DEBUGF(" DSP_PROC_TIMESTRETCH- same dst format\n"); | ||
623 | format_change_ack(&f); /* nothing changed that matters downstream */ | ||
624 | } | ||
625 | |||
626 | dst->format = f; | ||
627 | |||
628 | /* return to normal processing */ | ||
629 | this->process[0] = tdspeed_process; | ||
630 | dsp_proc_call(this, buf_p, 0); | ||
631 | } | ||
632 | |||
633 | /* DSP message hook */ | ||
634 | static intptr_t tdspeed_configure(struct dsp_proc_entry *this, | ||
635 | struct dsp_config *dsp, | ||
636 | unsigned int setting, | ||
637 | intptr_t value) | ||
638 | { | ||
639 | struct tdspeed_state_s *st = &tdspeed_state; | ||
640 | |||
641 | switch (setting) | ||
642 | { | ||
643 | case DSP_INIT: | ||
644 | /* everything is at 100% until dsp_set_timestretch is called with | ||
645 | some other value and timestretch is enabled at the time */ | ||
646 | if (value == CODEC_IDX_AUDIO) | ||
647 | st->factor = PITCH_SPEED_100; | ||
648 | break; | ||
649 | |||
650 | case DSP_FLUSH: | ||
651 | tdspeed_flush(); | ||
652 | break; | ||
653 | |||
654 | case DSP_PROC_INIT: | ||
655 | if (!tdspeed_alloc_buffers()) | ||
656 | return -1; /* fail the init */ | ||
657 | |||
658 | st->this = this; | ||
659 | st->dsp = dsp; | ||
660 | this->ip_mask = 0; /* not in-place */ | ||
661 | this->process[0] = tdspeed_process; | ||
662 | this->process[1] = tdspeed_process_new_format; | ||
663 | break; | ||
664 | |||
665 | case DSP_PROC_CLOSE: | ||
666 | st->this = NULL; | ||
667 | st->factor = PITCH_SPEED_100; | ||
668 | dsp_outbuf.remcount = 0; | ||
669 | tdspeed_free_buffers(); | ||
670 | break; | ||
671 | |||
672 | case TIMESTRETCH_SET_FACTOR: | ||
673 | /* force update as a format change */ | ||
674 | st->samplerate = 0; | ||
675 | st->factor = (int32_t)value; | ||
676 | st->this->process[0] = tdspeed_process_new_format; | ||
677 | dsp_proc_activate(st->dsp, DSP_PROC_TIMESTRETCH, true); | ||
678 | break; | ||
679 | } | ||
446 | 680 | ||
447 | return count; | 681 | return 1; |
682 | (void)value; | ||
448 | } | 683 | } |
449 | 684 | ||
685 | /* Database entry */ | ||
686 | DSP_PROC_DB_ENTRY(TIMESTRETCH, | ||
687 | tdspeed_configure); | ||
diff --git a/lib/rbcodec/dsp/tdspeed.h b/lib/rbcodec/dsp/tdspeed.h index e91eeb1701..ca8a7846a4 100644 --- a/lib/rbcodec/dsp/tdspeed.h +++ b/lib/rbcodec/dsp/tdspeed.h | |||
@@ -23,12 +23,8 @@ | |||
23 | #ifndef _TDSPEED_H | 23 | #ifndef _TDSPEED_H |
24 | #define _TDSPEED_H | 24 | #define _TDSPEED_H |
25 | 25 | ||
26 | #include "dsp.h" | 26 | /* some #define functions to get the pitch, stretch and speed values based |
27 | 27 | * upon two known values. Remember that params are alphabetical. */ | |
28 | #define TDSPEED_OUTBUFSIZE 4096 | ||
29 | |||
30 | /* some #define functions to get the pitch, stretch and speed values based on */ | ||
31 | /* two known values. Remember that params are alphabetical. */ | ||
32 | #define GET_SPEED(pitch, stretch) \ | 28 | #define GET_SPEED(pitch, stretch) \ |
33 | ((pitch * stretch + PITCH_SPEED_100 / 2L) / PITCH_SPEED_100) | 29 | ((pitch * stretch + PITCH_SPEED_100 / 2L) / PITCH_SPEED_100) |
34 | #define GET_PITCH(speed, stretch) \ | 30 | #define GET_PITCH(speed, stretch) \ |
@@ -36,14 +32,12 @@ | |||
36 | #define GET_STRETCH(pitch, speed) \ | 32 | #define GET_STRETCH(pitch, speed) \ |
37 | ((speed * PITCH_SPEED_100 + pitch / 2L) / pitch) | 33 | ((speed * PITCH_SPEED_100 + pitch / 2L) / pitch) |
38 | 34 | ||
39 | void tdspeed_init(void); | ||
40 | void tdspeed_finish(void); | ||
41 | bool tdspeed_config(int samplerate, bool stereo, int32_t factor); | ||
42 | long tdspeed_est_output_size(void); | ||
43 | long tdspeed_est_input_size(long size); | ||
44 | int tdspeed_doit(int32_t *src[], int count); | ||
45 | |||
46 | #define STRETCH_MAX (250L * PITCH_SPEED_PRECISION) /* 250% */ | 35 | #define STRETCH_MAX (250L * PITCH_SPEED_PRECISION) /* 250% */ |
47 | #define STRETCH_MIN (35L * PITCH_SPEED_PRECISION) /* 35% */ | 36 | #define STRETCH_MIN (35L * PITCH_SPEED_PRECISION) /* 35% */ |
48 | 37 | ||
49 | #endif | 38 | void dsp_timestretch_enable(bool enable); |
39 | void dsp_set_timestretch(int32_t percent); | ||
40 | int32_t dsp_get_timestretch(void); | ||
41 | bool dsp_timestretch_available(void); | ||
42 | |||
43 | #endif /* _TDSPEED_H */ | ||
diff --git a/lib/rbcodec/dsp/tone_controls.c b/lib/rbcodec/dsp/tone_controls.c new file mode 100644 index 0000000000..0bd4a447d7 --- /dev/null +++ b/lib/rbcodec/dsp/tone_controls.c | |||
@@ -0,0 +1,118 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2007 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 <string.h> | ||
26 | #include "dsp_proc_entry.h" | ||
27 | #include "dsp_filter.h" | ||
28 | |||
29 | /* These apply to all DSP streams to remain as consistant as possible with | ||
30 | * the behavior of hardware tone controls */ | ||
31 | |||
32 | /* Cutoffs in HZ - not adjustable for now */ | ||
33 | static const unsigned int tone_bass_cutoff = 200; | ||
34 | static const unsigned int tone_treble_cutoff = 3500; | ||
35 | |||
36 | /* Current bass and treble gain values */ | ||
37 | static int tone_bass = 0; | ||
38 | static int tone_treble = 0; | ||
39 | |||
40 | /* Data for each DSP */ | ||
41 | static struct dsp_filter tone_filters[DSP_COUNT] IBSS_ATTR; | ||
42 | |||
43 | /* Update the filters' coefficients based upon the bass/treble settings */ | ||
44 | void tone_set_prescale(int prescale) | ||
45 | { | ||
46 | int bass = tone_bass; | ||
47 | int treble = tone_treble; | ||
48 | |||
49 | struct dsp_filter tone_filter; /* Temp to hold new version */ | ||
50 | filter_bishelf_coefs(0xffffffff / NATIVE_FREQUENCY * tone_bass_cutoff, | ||
51 | 0xffffffff / NATIVE_FREQUENCY * tone_treble_cutoff, | ||
52 | bass, treble, -prescale, &tone_filter); | ||
53 | |||
54 | struct dsp_config *dsp; | ||
55 | for (int i = 0; (dsp = dsp_get_config(i)); i++) | ||
56 | { | ||
57 | struct dsp_filter *filter = &tone_filters[i]; | ||
58 | filter_copy(filter, &tone_filter); | ||
59 | |||
60 | bool enable = bass != 0 || treble != 0; | ||
61 | dsp_proc_enable(dsp, DSP_PROC_TONE_CONTROLS, enable); | ||
62 | |||
63 | if (!dsp_proc_active(dsp, DSP_PROC_TONE_CONTROLS)) | ||
64 | { | ||
65 | filter_flush(filter); /* Going online */ | ||
66 | dsp_proc_activate(dsp, DSP_PROC_TONE_CONTROLS, true); | ||
67 | } | ||
68 | } | ||
69 | } | ||
70 | |||
71 | /* Prescaler is always set after setting bass/treble, so we wait with | ||
72 | * calculating coefs until such time. */ | ||
73 | |||
74 | /* Change the bass setting */ | ||
75 | void tone_set_bass(int bass) | ||
76 | { | ||
77 | tone_bass = bass; | ||
78 | } | ||
79 | |||
80 | /* Change the treble setting */ | ||
81 | void tone_set_treble(int treble) | ||
82 | { | ||
83 | tone_treble = treble; | ||
84 | } | ||
85 | |||
86 | /* Apply the tone control filter in-place */ | ||
87 | static void tone_process(struct dsp_proc_entry *this, | ||
88 | struct dsp_buffer **buf_p) | ||
89 | { | ||
90 | struct dsp_buffer *buf = *buf_p; | ||
91 | filter_process((void *)this->data, buf->p32, buf->remcount, | ||
92 | buf->format.num_channels); | ||
93 | } | ||
94 | |||
95 | /* DSP message hook */ | ||
96 | static intptr_t tone_configure(struct dsp_proc_entry *this, | ||
97 | struct dsp_config *dsp, | ||
98 | unsigned int setting, | ||
99 | intptr_t value) | ||
100 | { | ||
101 | switch (setting) | ||
102 | { | ||
103 | case DSP_PROC_INIT: | ||
104 | if (value != 0) | ||
105 | break; | ||
106 | this->data = (intptr_t)&tone_filters[dsp_get_id(dsp)]; | ||
107 | this->process[0] = tone_process; | ||
108 | case DSP_FLUSH: | ||
109 | filter_flush((struct dsp_filter *)this->data); | ||
110 | break; | ||
111 | } | ||
112 | |||
113 | return 1; | ||
114 | } | ||
115 | |||
116 | /* Database entry */ | ||
117 | DSP_PROC_DB_ENTRY(TONE_CONTROLS, | ||
118 | tone_configure); | ||
diff --git a/lib/rbcodec/dsp/tone_controls.h b/lib/rbcodec/dsp/tone_controls.h new file mode 100644 index 0000000000..1e27ecf800 --- /dev/null +++ b/lib/rbcodec/dsp/tone_controls.h | |||
@@ -0,0 +1,28 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2007 Thom Johansen | ||
11 | * | ||
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. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef TONE_CONTROLS_H | ||
22 | #define TONE_CONTROLS_H | ||
23 | |||
24 | void tone_set_prescale(int prescale); | ||
25 | void tone_set_bass(int bass); | ||
26 | void tone_set_treble(int treble); | ||
27 | |||
28 | #endif /* TONE_CONTROLS_H */ | ||