summaryrefslogtreecommitdiff
path: root/lib/rbcodec/dsp
diff options
context:
space:
mode:
authorSean Bartell <wingedtachikoma@gmail.com>2011-06-24 01:25:21 -0400
committerNils Wallménius <nils@rockbox.org>2012-03-18 12:00:39 +0100
commitb5716df4cb2837bbbc42195cf1aefcf03e21d6a6 (patch)
tree130cd712e2e00893b6df9959a375a8d9523a1aca /lib/rbcodec/dsp
parent24bd9d5393dbe39a5c6194877bc00ede669b1d5d (diff)
downloadrockbox-b5716df4cb2837bbbc42195cf1aefcf03e21d6a6.tar.gz
rockbox-b5716df4cb2837bbbc42195cf1aefcf03e21d6a6.zip
Build librbcodec with DSP and metadata.
All associated files are moved to /lib/rbcodec. Change-Id: I572ddd2b8a996aae1e98c081d06b1ed356dce222
Diffstat (limited to 'lib/rbcodec/dsp')
-rw-r--r--lib/rbcodec/dsp/compressor.c363
-rw-r--r--lib/rbcodec/dsp/compressor.h29
-rw-r--r--lib/rbcodec/dsp/dsp.c1573
-rw-r--r--lib/rbcodec/dsp/dsp.h125
-rw-r--r--lib/rbcodec/dsp/dsp_arm.S561
-rw-r--r--lib/rbcodec/dsp/dsp_arm_v6.S127
-rw-r--r--lib/rbcodec/dsp/dsp_asm.h86
-rw-r--r--lib/rbcodec/dsp/dsp_cf.S611
-rw-r--r--lib/rbcodec/dsp/eq.c268
-rw-r--r--lib/rbcodec/dsp/eq.h50
-rw-r--r--lib/rbcodec/dsp/eq_arm.S89
-rw-r--r--lib/rbcodec/dsp/eq_cf.S91
-rw-r--r--lib/rbcodec/dsp/eqs/Acoustic.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Bass.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Classical.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Default.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Disco.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Electronic.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Hip-Hop.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Jazz.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Lounge.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Pop.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/R&B.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Rock.cfg17
-rw-r--r--lib/rbcodec/dsp/eqs/Vocal.cfg17
-rw-r--r--lib/rbcodec/dsp/tdspeed.c450
-rw-r--r--lib/rbcodec/dsp/tdspeed.h49
27 files changed, 4693 insertions, 0 deletions
diff --git a/lib/rbcodec/dsp/compressor.c b/lib/rbcodec/dsp/compressor.c
new file mode 100644
index 0000000000..3a8d52e4da
--- /dev/null
+++ b/lib/rbcodec/dsp/compressor.c
@@ -0,0 +1,363 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2009 Jeffrey Goode
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 "fixedpoint.h"
23#include "fracmul.h"
24#include "settings.h"
25#include "dsp.h"
26#include "compressor.h"
27
28/* Define LOGF_ENABLE to enable logf output in this file */
29/*#define LOGF_ENABLE*/
30#include "logf.h"
31
32static int32_t comp_rel_slope IBSS_ATTR; /* S7.24 format */
33static int32_t comp_makeup_gain IBSS_ATTR; /* S7.24 format */
34static int32_t comp_curve[66] IBSS_ATTR; /* S7.24 format */
35static int32_t release_gain IBSS_ATTR; /* S7.24 format */
36
37#define UNITY (1L << 24) /* unity gain in S7.24 format */
38
39/** COMPRESSOR UPDATE
40 * Called via the menu system to configure the compressor process */
41bool compressor_update(void)
42{
43 static int curr_set[5];
44 int new_set[5] = {
45 global_settings.compressor_threshold,
46 global_settings.compressor_makeup_gain,
47 global_settings.compressor_ratio,
48 global_settings.compressor_knee,
49 global_settings.compressor_release_time};
50
51 /* make menu values useful */
52 int threshold = new_set[0];
53 bool auto_gain = (new_set[1] == 1);
54 const int comp_ratios[] = {2, 4, 6, 10, 0};
55 int ratio = comp_ratios[new_set[2]];
56 bool soft_knee = (new_set[3] == 1);
57 int release = new_set[4] * NATIVE_FREQUENCY / 1000;
58
59 bool changed = false;
60 bool active = (threshold < 0);
61
62 for (int i = 0; i < 5; i++)
63 {
64 if (curr_set[i] != new_set[i])
65 {
66 changed = true;
67 curr_set[i] = new_set[i];
68
69#if defined(ROCKBOX_HAS_LOGF) && defined(LOGF_ENABLE)
70 switch (i)
71 {
72 case 0:
73 logf(" Compressor Threshold: %d dB\tEnabled: %s",
74 threshold, active ? "Yes" : "No");
75 break;
76 case 1:
77 logf(" Compressor Makeup Gain: %s",
78 auto_gain ? "Auto" : "Off");
79 break;
80 case 2:
81 if (ratio)
82 { logf(" Compressor Ratio: %d:1", ratio); }
83 else
84 { logf(" Compressor Ratio: Limit"); }
85 break;
86 case 3:
87 logf(" Compressor Knee: %s", soft_knee?"Soft":"Hard");
88 break;
89 case 4:
90 logf(" Compressor Release: %d", release);
91 break;
92 }
93#endif
94 }
95 }
96
97 if (changed && active)
98 {
99 /* configure variables for compressor operation */
100 static const int32_t db[] = {
101 /* positive db equivalents in S15.16 format */
102 0x000000, 0x241FA4, 0x1E1A5E, 0x1A94C8,
103 0x181518, 0x1624EA, 0x148F82, 0x1338BD,
104 0x120FD2, 0x1109EB, 0x101FA4, 0x0F4BB6,
105 0x0E8A3C, 0x0DD840, 0x0D3377, 0x0C9A0E,
106 0x0C0A8C, 0x0B83BE, 0x0B04A5, 0x0A8C6C,
107 0x0A1A5E, 0x09ADE1, 0x094670, 0x08E398,
108 0x0884F6, 0x082A30, 0x07D2FA, 0x077F0F,
109 0x072E31, 0x06E02A, 0x0694C8, 0x064BDF,
110 0x060546, 0x05C0DA, 0x057E78, 0x053E03,
111 0x04FF5F, 0x04C273, 0x048726, 0x044D64,
112 0x041518, 0x03DE30, 0x03A89B, 0x037448,
113 0x03412A, 0x030F32, 0x02DE52, 0x02AE80,
114 0x027FB0, 0x0251D6, 0x0224EA, 0x01F8E2,
115 0x01CDB4, 0x01A359, 0x0179C9, 0x0150FC,
116 0x0128EB, 0x010190, 0x00DAE4, 0x00B4E1,
117 0x008F82, 0x006AC1, 0x004699, 0x002305};
118
119 struct curve_point
120 {
121 int32_t db; /* S15.16 format */
122 int32_t offset; /* S15.16 format */
123 } db_curve[5];
124
125 /** Set up the shape of the compression curve first as decibel
126 values */
127 /* db_curve[0] = bottom of knee
128 [1] = threshold
129 [2] = top of knee
130 [3] = 0 db input
131 [4] = ~+12db input (2 bits clipping overhead) */
132
133 db_curve[1].db = threshold << 16;
134 if (soft_knee)
135 {
136 /* bottom of knee is 3dB below the threshold for soft knee*/
137 db_curve[0].db = db_curve[1].db - (3 << 16);
138 /* top of knee is 3dB above the threshold for soft knee */
139 db_curve[2].db = db_curve[1].db + (3 << 16);
140 if (ratio)
141 /* offset = -3db * (ratio - 1) / ratio */
142 db_curve[2].offset = (int32_t)((long long)(-3 << 16)
143 * (ratio - 1) / ratio);
144 else
145 /* offset = -3db for hard limit */
146 db_curve[2].offset = (-3 << 16);
147 }
148 else
149 {
150 /* bottom of knee is at the threshold for hard knee */
151 db_curve[0].db = threshold << 16;
152 /* top of knee is at the threshold for hard knee */
153 db_curve[2].db = threshold << 16;
154 db_curve[2].offset = 0;
155 }
156
157 /* Calculate 0db and ~+12db offsets */
158 db_curve[4].db = 0xC0A8C; /* db of 2 bits clipping */
159 if (ratio)
160 {
161 /* offset = threshold * (ratio - 1) / ratio */
162 db_curve[3].offset = (int32_t)((long long)(threshold << 16)
163 * (ratio - 1) / ratio);
164 db_curve[4].offset = (int32_t)((long long)-db_curve[4].db
165 * (ratio - 1) / ratio) + db_curve[3].offset;
166 }
167 else
168 {
169 /* offset = threshold for hard limit */
170 db_curve[3].offset = (threshold << 16);
171 db_curve[4].offset = -db_curve[4].db + db_curve[3].offset;
172 }
173
174 /** Now set up the comp_curve table with compression offsets in the
175 form of gain factors in S7.24 format */
176 /* comp_curve[0] is 0 (-infinity db) input */
177 comp_curve[0] = UNITY;
178 /* comp_curve[1 to 63] are intermediate compression values
179 corresponding to the 6 MSB of the input values of a non-clipped
180 signal */
181 for (int i = 1; i < 64; i++)
182 {
183 /* db constants are stored as positive numbers;
184 make them negative here */
185 int32_t this_db = -db[i];
186
187 /* no compression below the knee */
188 if (this_db <= db_curve[0].db)
189 comp_curve[i] = UNITY;
190
191 /* if soft knee and below top of knee,
192 interpolate along soft knee slope */
193 else if (soft_knee && (this_db <= db_curve[2].db))
194 comp_curve[i] = fp_factor(fp_mul(
195 ((this_db - db_curve[0].db) / 6),
196 db_curve[2].offset, 16), 16) << 8;
197
198 /* interpolate along ratio slope above the knee */
199 else
200 comp_curve[i] = fp_factor(fp_mul(
201 fp_div((db_curve[1].db - this_db), db_curve[1].db, 16),
202 db_curve[3].offset, 16), 16) << 8;
203 }
204 /* comp_curve[64] is the compression level of a maximum level,
205 non-clipped signal */
206 comp_curve[64] = fp_factor(db_curve[3].offset, 16) << 8;
207
208 /* comp_curve[65] is the compression level of a maximum level,
209 clipped signal */
210 comp_curve[65] = fp_factor(db_curve[4].offset, 16) << 8;
211
212#if defined(ROCKBOX_HAS_LOGF) && defined(LOGF_ENABLE)
213 logf("\n *** Compression Offsets ***");
214 /* some settings for display only, not used in calculations */
215 db_curve[0].offset = 0;
216 db_curve[1].offset = 0;
217 db_curve[3].db = 0;
218
219 for (int i = 0; i <= 4; i++)
220 {
221 logf("Curve[%d]: db: % 6.2f\toffset: % 6.2f", i,
222 (float)db_curve[i].db / (1 << 16),
223 (float)db_curve[i].offset / (1 << 16));
224 }
225
226 logf("\nGain factors:");
227 for (int i = 1; i <= 65; i++)
228 {
229 debugf("%02d: %.6f ", i, (float)comp_curve[i] / UNITY);
230 if (i % 4 == 0) debugf("\n");
231 }
232 debugf("\n");
233#endif
234
235 /* if using auto peak, then makeup gain is max offset -
236 .1dB headroom */
237 comp_makeup_gain = auto_gain ?
238 fp_factor(-(db_curve[3].offset) - 0x199A, 16) << 8 : UNITY;
239 logf("Makeup gain:\t%.6f", (float)comp_makeup_gain / UNITY);
240
241 /* calculate per-sample gain change a rate of 10db over release time
242 */
243 comp_rel_slope = 0xAF0BB2 / release;
244 logf("Release slope:\t%.6f", (float)comp_rel_slope / UNITY);
245
246 release_gain = UNITY;
247 }
248
249 return active;
250}
251
252/** GET COMPRESSION GAIN
253 * Returns the required gain factor in S7.24 format in order to compress the
254 * sample in accordance with the compression curve. Always 1 or less.
255 */
256static inline int32_t get_compression_gain(struct dsp_data *data,
257 int32_t sample)
258{
259 const int frac_bits_offset = data->frac_bits - 15;
260
261 /* sample must be positive */
262 if (sample < 0)
263 sample = -(sample + 1);
264
265 /* shift sample into 15 frac bit range */
266 if (frac_bits_offset > 0)
267 sample >>= frac_bits_offset;
268 if (frac_bits_offset < 0)
269 sample <<= -frac_bits_offset;
270
271 /* normal case: sample isn't clipped */
272 if (sample < (1 << 15))
273 {
274 /* index is 6 MSB, rem is 9 LSB */
275 int index = sample >> 9;
276 int32_t rem = (sample & 0x1FF) << 22;
277
278 /* interpolate from the compression curve:
279 higher gain - ((rem / (1 << 31)) * (higher gain - lower gain)) */
280 return comp_curve[index] - (FRACMUL(rem,
281 (comp_curve[index] - comp_curve[index + 1])));
282 }
283 /* sample is somewhat clipped, up to 2 bits of overhead */
284 if (sample < (1 << 17))
285 {
286 /* straight interpolation:
287 higher gain - ((clipped portion of sample * 4/3
288 / (1 << 31)) * (higher gain - lower gain)) */
289 return comp_curve[64] - (FRACMUL(((sample - (1 << 15)) / 3) << 16,
290 (comp_curve[64] - comp_curve[65])));
291 }
292
293 /* sample is too clipped, return invalid value */
294 return -1;
295}
296
297/** COMPRESSOR PROCESS
298 * Changes the gain of the samples according to the compressor curve
299 */
300void compressor_process(int count, struct dsp_data *data, int32_t *buf[])
301{
302 const int num_chan = data->num_channels;
303 int32_t *in_buf[2] = {buf[0], buf[1]};
304
305 while (count-- > 0)
306 {
307 int ch;
308 /* use lowest (most compressed) gain factor of the output buffer
309 sample pair for both samples (mono is also handled correctly here)
310 */
311 int32_t sample_gain = UNITY;
312 for (ch = 0; ch < num_chan; ch++)
313 {
314 int32_t this_gain = get_compression_gain(data, *in_buf[ch]);
315 if (this_gain < sample_gain)
316 sample_gain = this_gain;
317 }
318
319 /* perform release slope; skip if no compression and no release slope
320 */
321 if ((sample_gain != UNITY) || (release_gain != UNITY))
322 {
323 /* if larger offset than previous slope, start new release slope
324 */
325 if ((sample_gain <= release_gain) && (sample_gain > 0))
326 {
327 release_gain = sample_gain;
328 }
329 else
330 /* keep sloping towards unity gain (and ignore invalid value) */
331 {
332 release_gain += comp_rel_slope;
333 if (release_gain > UNITY)
334 {
335 release_gain = UNITY;
336 }
337 }
338 }
339
340 /* total gain factor is the product of release gain and makeup gain,
341 but avoid computation if possible */
342 int32_t total_gain = ((release_gain == UNITY) ? comp_makeup_gain :
343 (comp_makeup_gain == UNITY) ? release_gain :
344 FRACMUL_SHL(release_gain, comp_makeup_gain, 7));
345
346 /* Implement the compressor: apply total gain factor (if any) to the
347 output buffer sample pair/mono sample */
348 if (total_gain != UNITY)
349 {
350 for (ch = 0; ch < num_chan; ch++)
351 {
352 *in_buf[ch] = FRACMUL_SHL(total_gain, *in_buf[ch], 7);
353 }
354 }
355 in_buf[0]++;
356 in_buf[1]++;
357 }
358}
359
360void compressor_reset(void)
361{
362 release_gain = UNITY;
363}
diff --git a/lib/rbcodec/dsp/compressor.h b/lib/rbcodec/dsp/compressor.h
new file mode 100644
index 0000000000..6154372e05
--- /dev/null
+++ b/lib/rbcodec/dsp/compressor.h
@@ -0,0 +1,29 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2009 Jeffrey Goode
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#ifndef COMPRESSOR_H
23#define COMPRESSOR_H
24
25void compressor_process(int count, struct dsp_data *data, int32_t *buf[]);
26bool compressor_update(void);
27void compressor_reset(void);
28
29#endif /* COMPRESSOR_H */
diff --git a/lib/rbcodec/dsp/dsp.c b/lib/rbcodec/dsp/dsp.c
new file mode 100644
index 0000000000..4da555747b
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp.c
@@ -0,0 +1,1573 @@
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 */
51enum
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
62enum
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 */
71struct dither_data
72{
73 long error[3]; /* 00h */
74 long random; /* 0ch */
75 /* 10h */
76};
77
78struct 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 */
92struct 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 */
104typedef void (*sample_input_fn_type)(int count, const char *src[],
105 int32_t *dst[]);
106typedef int (*resample_fn_type)(int count, struct dsp_data *data,
107 const int32_t *src[], int32_t *dst[]);
108typedef 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 */
112typedef void (*channels_process_fn_type)(int count, int32_t *buf[]);
113/* DSP local channel processing in place */
114typedef void (*channels_process_dsp_fn_type)(int count, struct dsp_data *data,
115 int32_t *buf[]);
116
117/*
118 ***************************************************************************/
119
120struct 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 */
151static struct dsp_config dsp_conf[2] IBSS_ATTR; /* 0=A, 1=V */
152/* Dithering */
153static struct dither_data dither_data[2] IBSS_ATTR; /* 0=left, 1=right */
154static long dither_mask IBSS_ATTR;
155static long dither_bias IBSS_ATTR;
156/* Crossfeed */
157struct crossfeed_data crossfeed_data IDATA_ATTR = /* A */
158{
159 .index = (int32_t *)crossfeed_data.delay
160};
161
162/* Equalizer */
163static struct eq_state eq_data; /* A */
164
165/* Software tone controls */
166#ifdef HAVE_SW_TONE_CONTROLS
167static int prescale; /* A/V */
168static int bass; /* A/V */
169static int treble; /* A/V */
170#endif
171
172/* Settings applicable to audio codec only */
173#ifdef HAVE_PITCHSCREEN
174static int32_t pitch_ratio = PITCH_SPEED_100;
175static int big_sample_locks;
176#endif
177static int channels_mode;
178 long dsp_sw_gain;
179 long dsp_sw_cross;
180static bool dither_enabled;
181static long eq_precut;
182static long track_gain;
183static bool new_gain;
184static long album_gain;
185static long track_peak;
186static long album_peak;
187static long replaygain;
188static 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
203static int32_t small_sample_buf[2][SMALL_SAMPLE_BUF_COUNT] IBSS_ATTR;
204static int32_t small_resample_buf[2][SMALL_RESAMPLE_BUF_COUNT] IBSS_ATTR;
205
206#ifdef HAVE_PITCHSCREEN
207static int32_t (* big_sample_buf)[BIG_SAMPLE_BUF_COUNT] = NULL;
208static int32_t (* big_resample_buf)[BIG_RESAMPLE_BUF_COUNT] = NULL;
209#endif
210
211static int sample_buf_count = SMALL_SAMPLE_BUF_COUNT;
212static int32_t *sample_buf[2] = { small_sample_buf[0], small_sample_buf[1] };
213static int resample_buf_count = SMALL_RESAMPLE_BUF_COUNT;
214static int32_t *resample_buf[2] = { small_resample_buf[0], small_resample_buf[1] };
215
216#ifdef HAVE_PITCHSCREEN
217int32_t sound_get_pitch(void)
218{
219 return pitch_ratio;
220}
221
222void 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
229static 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
251static 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
277static 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
291static void lock_sample_buf( bool lock )
292{
293 if ( lock )
294 big_sample_locks++;
295 else
296 big_sample_locks--;
297}
298
299static struct buflib_callbacks ops = {
300 .move_callback = move_callback,
301 .shrink_callback = NULL,
302};
303
304
305void 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
345void dsp_set_timestretch(int32_t percent)
346{
347 AUDIO_DSP.tdspeed_percent = percent;
348 tdspeed_setup(&AUDIO_DSP);
349}
350
351int32_t dsp_get_timestretch()
352{
353 return AUDIO_DSP.tdspeed_percent;
354}
355
356bool 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 */
369static 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 */
386static 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 */
411static 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 */
432static 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 */
440static 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 */
458static 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 */
477static 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 */
500static 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
518static 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 */
540static 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 */
620static 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
643static 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
684static 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
727static 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 */
752static 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
768static 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
775void 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
788static 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 */
842void 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
849void 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 */
858void 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
884static 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. */
901static 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 */
943void 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 */
954void dsp_set_eq_coefs(int band)
955{
956 /* Adjust setting pointer to the band we actually want to change */
957 struct eq_band_setting *setting = &global_settings.eq_band_settings[band];
958
959 /* Convert user settings to format required by coef generator functions */
960 unsigned long cutoff = 0xffffffff / NATIVE_FREQUENCY * setting->cutoff;
961 unsigned long q = setting->q;
962 int gain = setting->gain;
963
964 if (q == 0)
965 q = 1;
966
967 /* NOTE: The coef functions assume the EMAC unit is in fractional mode,
968 which it should be, since we're executed from the main thread. */
969
970 /* Assume a band is disabled if the gain is zero */
971 if (gain == 0)
972 {
973 eq_data.enabled[band] = 0;
974 }
975 else
976 {
977 if (band == 0)
978 eq_ls_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
979 else if (band == 4)
980 eq_hs_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
981 else
982 eq_pk_coefs(cutoff, q, gain, eq_data.filters[band].coefs);
983
984 eq_data.enabled[band] = 1;
985 }
986}
987
988/* Apply EQ filters to those bands that have got it switched on. */
989static void eq_process(int count, int32_t *buf[])
990{
991 static const int shifts[] =
992 {
993 EQ_SHELF_SHIFT, /* low shelf */
994 EQ_PEAK_SHIFT, /* peaking */
995 EQ_PEAK_SHIFT, /* peaking */
996 EQ_PEAK_SHIFT, /* peaking */
997 EQ_SHELF_SHIFT, /* high shelf */
998 };
999 unsigned int channels = AUDIO_DSP.data.num_channels;
1000 int i;
1001
1002 /* filter configuration currently is 1 low shelf filter, 3 band peaking
1003 filters and 1 high shelf filter, in that order. we need to know this
1004 so we can choose the correct shift factor.
1005 */
1006 for (i = 0; i < 5; i++)
1007 {
1008 if (!eq_data.enabled[i])
1009 continue;
1010 eq_filter(buf, &eq_data.filters[i], count, channels, shifts[i]);
1011 }
1012}
1013
1014/**
1015 * Use to enable the equalizer.
1016 *
1017 * @param enable true to enable the equalizer
1018 */
1019void dsp_set_eq(bool enable)
1020{
1021 AUDIO_DSP.eq_process = enable ? eq_process : NULL;
1022 set_gain(&AUDIO_DSP);
1023}
1024
1025static void dsp_set_stereo_width(int value)
1026{
1027 long width, straight, cross;
1028
1029 width = value * 0x7fffff / 100;
1030
1031 if (value <= 100)
1032 {
1033 straight = (0x7fffff + width) / 2;
1034 cross = straight - width;
1035 }
1036 else
1037 {
1038 /* straight = (1 + width) / (2 * width) */
1039 straight = ((int64_t)(0x7fffff + width) << 22) / width;
1040 cross = straight - 0x7fffff;
1041 }
1042
1043 dsp_sw_gain = straight << 8;
1044 dsp_sw_cross = cross << 8;
1045}
1046
1047/**
1048 * Implements the different channel configurations and stereo width.
1049 */
1050
1051/* SOUND_CHAN_STEREO mode is a noop so has no function - just outline one for
1052 * completeness. */
1053#if 0
1054static void channels_process_sound_chan_stereo(int count, int32_t *buf[])
1055{
1056 /* The channels are each just themselves */
1057 (void)count; (void)buf;
1058}
1059#endif
1060
1061#ifndef DSP_HAVE_ASM_SOUND_CHAN_MONO
1062static void channels_process_sound_chan_mono(int count, int32_t *buf[])
1063{
1064 int32_t *sl = buf[0], *sr = buf[1];
1065
1066 while (count-- > 0)
1067 {
1068 int32_t lr = *sl/2 + *sr/2;
1069 *sl++ = lr;
1070 *sr++ = lr;
1071 }
1072}
1073#endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
1074
1075#ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
1076static void channels_process_sound_chan_custom(int count, int32_t *buf[])
1077{
1078 const int32_t gain = dsp_sw_gain;
1079 const int32_t cross = dsp_sw_cross;
1080 int32_t *sl = buf[0], *sr = buf[1];
1081
1082 while (count-- > 0)
1083 {
1084 int32_t l = *sl;
1085 int32_t r = *sr;
1086 *sl++ = FRACMUL(l, gain) + FRACMUL(r, cross);
1087 *sr++ = FRACMUL(r, gain) + FRACMUL(l, cross);
1088 }
1089}
1090#endif /* DSP_HAVE_ASM_SOUND_CHAN_CUSTOM */
1091
1092static void channels_process_sound_chan_mono_left(int count, int32_t *buf[])
1093{
1094 /* Just copy over the other channel */
1095 memcpy(buf[1], buf[0], count * sizeof (*buf));
1096}
1097
1098static void channels_process_sound_chan_mono_right(int count, int32_t *buf[])
1099{
1100 /* Just copy over the other channel */
1101 memcpy(buf[0], buf[1], count * sizeof (*buf));
1102}
1103
1104#ifndef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
1105static void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
1106{
1107 int32_t *sl = buf[0], *sr = buf[1];
1108
1109 while (count-- > 0)
1110 {
1111 int32_t ch = *sl/2 - *sr/2;
1112 *sl++ = ch;
1113 *sr++ = -ch;
1114 }
1115}
1116#endif /* DSP_HAVE_ASM_SOUND_CHAN_KARAOKE */
1117
1118static void dsp_set_channel_config(int value)
1119{
1120 static const channels_process_fn_type channels_process_functions[] =
1121 {
1122 /* SOUND_CHAN_STEREO = All-purpose index for no channel processing */
1123 [SOUND_CHAN_STEREO] = NULL,
1124 [SOUND_CHAN_MONO] = channels_process_sound_chan_mono,
1125 [SOUND_CHAN_CUSTOM] = channels_process_sound_chan_custom,
1126 [SOUND_CHAN_MONO_LEFT] = channels_process_sound_chan_mono_left,
1127 [SOUND_CHAN_MONO_RIGHT] = channels_process_sound_chan_mono_right,
1128 [SOUND_CHAN_KARAOKE] = channels_process_sound_chan_karaoke,
1129 };
1130
1131 if ((unsigned)value >= ARRAYLEN(channels_process_functions) ||
1132 AUDIO_DSP.stereo_mode == STEREO_MONO)
1133 {
1134 value = SOUND_CHAN_STEREO;
1135 }
1136
1137 /* This doesn't apply to voice */
1138 channels_mode = value;
1139 AUDIO_DSP.channels_process = channels_process_functions[value];
1140}
1141
1142#if CONFIG_CODEC == SWCODEC
1143
1144#ifdef HAVE_SW_TONE_CONTROLS
1145static void set_tone_controls(void)
1146{
1147 filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
1148 0xffffffff/NATIVE_FREQUENCY*3500,
1149 bass, treble, -prescale,
1150 AUDIO_DSP.tone_filter.coefs);
1151 /* Sync the voice dsp coefficients */
1152 memcpy(&VOICE_DSP.tone_filter.coefs, AUDIO_DSP.tone_filter.coefs,
1153 sizeof (VOICE_DSP.tone_filter.coefs));
1154}
1155#endif
1156
1157/* Hook back from firmware/ part of audio, which can't/shouldn't call apps/
1158 * code directly.
1159 */
1160int dsp_callback(int msg, intptr_t param)
1161{
1162 switch (msg)
1163 {
1164#ifdef HAVE_SW_TONE_CONTROLS
1165 case DSP_CALLBACK_SET_PRESCALE:
1166 prescale = param;
1167 set_tone_controls();
1168 break;
1169 /* prescaler is always set after calling any of these, so we wait with
1170 * calculating coefs until the above case is hit.
1171 */
1172 case DSP_CALLBACK_SET_BASS:
1173 bass = param;
1174 break;
1175 case DSP_CALLBACK_SET_TREBLE:
1176 treble = param;
1177 break;
1178#ifdef HAVE_SW_VOLUME_CONTROL
1179 case DSP_CALLBACK_SET_SW_VOLUME:
1180 set_gain(&AUDIO_DSP);
1181 break;
1182#endif
1183#endif
1184 case DSP_CALLBACK_SET_CHANNEL_CONFIG:
1185 dsp_set_channel_config(param);
1186 break;
1187 case DSP_CALLBACK_SET_STEREO_WIDTH:
1188 dsp_set_stereo_width(param);
1189 break;
1190 default:
1191 break;
1192 }
1193 return 0;
1194}
1195#endif
1196
1197/* Process and convert src audio to dst based on the DSP configuration,
1198 * reading count number of audio samples. dst is assumed to be large
1199 * enough; use dsp_output_count() to get the required number. src is an
1200 * array of pointers; for mono and interleaved stereo, it contains one
1201 * pointer to the start of the audio data and the other is ignored; for
1202 * non-interleaved stereo, it contains two pointers, one for each audio
1203 * channel. Returns number of bytes written to dst.
1204 */
1205int dsp_process(struct dsp_config *dsp, char *dst, const char *src[], int count)
1206{
1207 static int32_t *tmp[2]; /* tdspeed_doit() needs it static */
1208 static long last_yield;
1209 long tick;
1210 int written = 0;
1211
1212#if defined(CPU_COLDFIRE)
1213 /* set emac unit for dsp processing, and save old macsr, we're running in
1214 codec thread context at this point, so can't clobber it */
1215 unsigned long old_macsr = coldfire_get_macsr();
1216 coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
1217#endif
1218
1219 if (new_gain)
1220 dsp_set_replaygain(); /* Gain has changed */
1221
1222 /* Perform at least one yield before starting */
1223 last_yield = current_tick;
1224 yield();
1225
1226 /* Testing function pointers for NULL is preferred since the pointer
1227 will be preloaded to be used for the call if not. */
1228 while (count > 0)
1229 {
1230 int samples = MIN(sample_buf_count, count);
1231 count -= samples;
1232
1233 dsp->input_samples(samples, src, tmp);
1234
1235#ifdef HAVE_PITCHSCREEN
1236 if (dsp->tdspeed_active)
1237 samples = tdspeed_doit(tmp, samples);
1238#endif
1239
1240 int chunk_offset = 0;
1241 while (samples > 0)
1242 {
1243 int32_t *t2[2];
1244 t2[0] = tmp[0]+chunk_offset;
1245 t2[1] = tmp[1]+chunk_offset;
1246
1247 int chunk = MIN(sample_buf_count, samples);
1248 chunk_offset += chunk;
1249 samples -= chunk;
1250
1251 if (dsp->apply_gain)
1252 dsp->apply_gain(chunk, &dsp->data, t2);
1253
1254 if (dsp->resample && (chunk = resample(dsp, chunk, t2)) <= 0)
1255 break; /* I'm pretty sure we're downsampling here */
1256
1257 if (dsp->apply_crossfeed)
1258 dsp->apply_crossfeed(chunk, t2);
1259
1260 if (dsp->eq_process)
1261 dsp->eq_process(chunk, t2);
1262
1263#ifdef HAVE_SW_TONE_CONTROLS
1264 if ((bass | treble) != 0)
1265 eq_filter(t2, &dsp->tone_filter, chunk,
1266 dsp->data.num_channels, FILTER_BISHELF_SHIFT);
1267#endif
1268
1269 if (dsp->channels_process)
1270 dsp->channels_process(chunk, t2);
1271
1272 if (dsp->compressor_process)
1273 dsp->compressor_process(chunk, &dsp->data, t2);
1274
1275 dsp->output_samples(chunk, &dsp->data, (const int32_t **)t2, (int16_t *)dst);
1276
1277 written += chunk;
1278 dst += chunk * sizeof (int16_t) * 2;
1279
1280 /* yield at least once each tick */
1281 tick = current_tick;
1282 if (TIME_AFTER(tick, last_yield))
1283 {
1284 last_yield = tick;
1285 yield();
1286 }
1287 }
1288 }
1289
1290#if defined(CPU_COLDFIRE)
1291 /* set old macsr again */
1292 coldfire_set_macsr(old_macsr);
1293#endif
1294 return written;
1295}
1296
1297/* Given count number of input samples, calculate the maximum number of
1298 * samples of output data that would be generated (the calculation is not
1299 * entirely exact and rounds upwards to be on the safe side; during
1300 * resampling, the number of samples generated depends on the current state
1301 * of the resampler).
1302 */
1303/* dsp_input_size MUST be called afterwards */
1304int dsp_output_count(struct dsp_config *dsp, int count)
1305{
1306#ifdef HAVE_PITCHSCREEN
1307 if (dsp->tdspeed_active)
1308 count = tdspeed_est_output_size();
1309#endif
1310 if (dsp->resample)
1311 {
1312 count = (int)(((unsigned long)count * NATIVE_FREQUENCY
1313 + (dsp->frequency - 1)) / dsp->frequency);
1314 }
1315
1316 /* Now we have the resampled sample count which must not exceed
1317 * resample_buf_count to avoid resample buffer overflow. One
1318 * must call dsp_input_count() to get the correct input sample
1319 * count.
1320 */
1321 if (count > resample_buf_count)
1322 count = resample_buf_count;
1323
1324 return count;
1325}
1326
1327/* Given count output samples, calculate number of input samples
1328 * that would be consumed in order to fill the output buffer.
1329 */
1330int dsp_input_count(struct dsp_config *dsp, int count)
1331{
1332 /* count is now the number of resampled input samples. Convert to
1333 original input samples. */
1334 if (dsp->resample)
1335 {
1336 /* Use the real resampling delta =
1337 * dsp->frequency * 65536 / NATIVE_FREQUENCY, and
1338 * round towards zero to avoid buffer overflows. */
1339 count = (int)(((unsigned long)count *
1340 dsp->data.resample_data.delta) >> 16);
1341 }
1342
1343#ifdef HAVE_PITCHSCREEN
1344 if (dsp->tdspeed_active)
1345 count = tdspeed_est_input_size(count);
1346#endif
1347
1348 return count;
1349}
1350
1351static void dsp_set_gain_var(long *var, long value)
1352{
1353 *var = value;
1354 new_gain = true;
1355}
1356
1357static void dsp_update_functions(struct dsp_config *dsp)
1358{
1359 sample_input_new_format(dsp);
1360 sample_output_new_format(dsp);
1361 if (dsp == &AUDIO_DSP)
1362 dsp_set_crossfeed(crossfeed_enabled);
1363}
1364
1365intptr_t dsp_configure(struct dsp_config *dsp, int setting, intptr_t value)
1366{
1367 switch (setting)
1368 {
1369 case DSP_MYDSP:
1370 switch (value)
1371 {
1372 case CODEC_IDX_AUDIO:
1373 return (intptr_t)&AUDIO_DSP;
1374 case CODEC_IDX_VOICE:
1375 return (intptr_t)&VOICE_DSP;
1376 default:
1377 return (intptr_t)NULL;
1378 }
1379
1380 case DSP_SET_FREQUENCY:
1381 memset(&dsp->data.resample_data, 0, sizeof (dsp->data.resample_data));
1382 /* Fall through!!! */
1383 case DSP_SWITCH_FREQUENCY:
1384 dsp->codec_frequency = (value == 0) ? NATIVE_FREQUENCY : value;
1385 /* Account for playback speed adjustment when setting dsp->frequency
1386 if we're called from the main audio thread. Voice UI thread should
1387 not need this feature.
1388 */
1389#ifdef HAVE_PITCHSCREEN
1390 if (dsp == &AUDIO_DSP)
1391 dsp->frequency = pitch_ratio * dsp->codec_frequency / PITCH_SPEED_100;
1392 else
1393#endif
1394 dsp->frequency = dsp->codec_frequency;
1395
1396 resampler_new_delta(dsp);
1397#ifdef HAVE_PITCHSCREEN
1398 tdspeed_setup(dsp);
1399#endif
1400 break;
1401
1402 case DSP_SET_SAMPLE_DEPTH:
1403 dsp->sample_depth = value;
1404
1405 if (dsp->sample_depth <= NATIVE_DEPTH)
1406 {
1407 dsp->data.frac_bits = WORD_FRACBITS;
1408 dsp->sample_bytes = sizeof (int16_t); /* samples are 16 bits */
1409 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1410 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1411 }
1412 else
1413 {
1414 dsp->data.frac_bits = value;
1415 dsp->sample_bytes = sizeof (int32_t); /* samples are 32 bits */
1416 dsp->data.clip_max = (1 << value) - 1;
1417 dsp->data.clip_min = -(1 << value);
1418 }
1419
1420 dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH;
1421 sample_input_new_format(dsp);
1422 dither_init(dsp);
1423 break;
1424
1425 case DSP_SET_STEREO_MODE:
1426 dsp->stereo_mode = value;
1427 dsp->data.num_channels = value == STEREO_MONO ? 1 : 2;
1428 dsp_update_functions(dsp);
1429#ifdef HAVE_PITCHSCREEN
1430 tdspeed_setup(dsp);
1431#endif
1432 break;
1433
1434 case DSP_RESET:
1435 dsp->stereo_mode = STEREO_NONINTERLEAVED;
1436 dsp->data.num_channels = 2;
1437 dsp->sample_depth = NATIVE_DEPTH;
1438 dsp->data.frac_bits = WORD_FRACBITS;
1439 dsp->sample_bytes = sizeof (int16_t);
1440 dsp->data.output_scale = dsp->data.frac_bits + 1 - NATIVE_DEPTH;
1441 dsp->data.clip_max = ((1 << WORD_FRACBITS) - 1);
1442 dsp->data.clip_min = -((1 << WORD_FRACBITS));
1443 dsp->codec_frequency = dsp->frequency = NATIVE_FREQUENCY;
1444
1445 if (dsp == &AUDIO_DSP)
1446 {
1447 track_gain = 0;
1448 album_gain = 0;
1449 track_peak = 0;
1450 album_peak = 0;
1451 new_gain = true;
1452 }
1453
1454 dsp_update_functions(dsp);
1455 resampler_new_delta(dsp);
1456#ifdef HAVE_PITCHSCREEN
1457 tdspeed_setup(dsp);
1458#endif
1459 if (dsp == &AUDIO_DSP)
1460 compressor_reset();
1461 break;
1462
1463 case DSP_FLUSH:
1464 memset(&dsp->data.resample_data, 0,
1465 sizeof (dsp->data.resample_data));
1466 resampler_new_delta(dsp);
1467 dither_init(dsp);
1468#ifdef HAVE_PITCHSCREEN
1469 tdspeed_setup(dsp);
1470#endif
1471 if (dsp == &AUDIO_DSP)
1472 compressor_reset();
1473 break;
1474
1475 case DSP_SET_TRACK_GAIN:
1476 if (dsp == &AUDIO_DSP)
1477 dsp_set_gain_var(&track_gain, value);
1478 break;
1479
1480 case DSP_SET_ALBUM_GAIN:
1481 if (dsp == &AUDIO_DSP)
1482 dsp_set_gain_var(&album_gain, value);
1483 break;
1484
1485 case DSP_SET_TRACK_PEAK:
1486 if (dsp == &AUDIO_DSP)
1487 dsp_set_gain_var(&track_peak, value);
1488 break;
1489
1490 case DSP_SET_ALBUM_PEAK:
1491 if (dsp == &AUDIO_DSP)
1492 dsp_set_gain_var(&album_peak, value);
1493 break;
1494
1495 default:
1496 return 0;
1497 }
1498
1499 return 1;
1500}
1501
1502int get_replaygain_mode(bool have_track_gain, bool have_album_gain)
1503{
1504 int type;
1505
1506 bool track = ((global_settings.replaygain_type == REPLAYGAIN_TRACK)
1507 || ((global_settings.replaygain_type == REPLAYGAIN_SHUFFLE)
1508 && global_settings.playlist_shuffle));
1509
1510 type = (!track && have_album_gain) ? REPLAYGAIN_ALBUM
1511 : have_track_gain ? REPLAYGAIN_TRACK : -1;
1512
1513 return type;
1514}
1515
1516void dsp_set_replaygain(void)
1517{
1518 long gain = 0;
1519
1520 new_gain = false;
1521
1522 if ((global_settings.replaygain_type != REPLAYGAIN_OFF) ||
1523 global_settings.replaygain_noclip)
1524 {
1525 bool track_mode = get_replaygain_mode(track_gain != 0,
1526 album_gain != 0) == REPLAYGAIN_TRACK;
1527 long peak = (track_mode || !album_peak) ? track_peak : album_peak;
1528
1529 if (global_settings.replaygain_type != REPLAYGAIN_OFF)
1530 {
1531 gain = (track_mode || !album_gain) ? track_gain : album_gain;
1532
1533 if (global_settings.replaygain_preamp)
1534 {
1535 long preamp = get_replaygain_int(
1536 global_settings.replaygain_preamp * 10);
1537
1538 gain = (long) (((int64_t) gain * preamp) >> 24);
1539 }
1540 }
1541
1542 if (gain == 0)
1543 {
1544 /* So that noclip can work even with no gain information. */
1545 gain = DEFAULT_GAIN;
1546 }
1547
1548 if (global_settings.replaygain_noclip && (peak != 0)
1549 && ((((int64_t) gain * peak) >> 24) >= DEFAULT_GAIN))
1550 {
1551 gain = (((int64_t) DEFAULT_GAIN << 24) / peak);
1552 }
1553
1554 if (gain == DEFAULT_GAIN)
1555 {
1556 /* Nothing to do, disable processing. */
1557 gain = 0;
1558 }
1559 }
1560
1561 /* Store in S7.24 format to simplify calculations. */
1562 replaygain = gain;
1563 set_gain(&AUDIO_DSP);
1564}
1565
1566/** SET COMPRESSOR
1567 * Called by the menu system to configure the compressor process */
1568void dsp_set_compressor(void)
1569{
1570 /* enable/disable the compressor */
1571 AUDIO_DSP.compressor_process = compressor_update() ?
1572 compressor_process : NULL;
1573}
diff --git a/lib/rbcodec/dsp/dsp.h b/lib/rbcodec/dsp/dsp.h
new file mode 100644
index 0000000000..2a00f649f8
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp.h
@@ -0,0 +1,125 @@
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
22#ifndef _DSP_H
23#define _DSP_H
24
25#include <stdlib.h>
26#include <stdbool.h>
27
28#define NATIVE_FREQUENCY 44100
29
30enum
31{
32 STEREO_INTERLEAVED = 0,
33 STEREO_NONINTERLEAVED,
34 STEREO_MONO,
35 STEREO_NUM_MODES,
36};
37
38enum
39{
40 CODEC_IDX_AUDIO = 0,
41 CODEC_IDX_VOICE,
42};
43
44enum
45{
46 DSP_MYDSP = 1,
47 DSP_SET_FREQUENCY,
48 DSP_SWITCH_FREQUENCY,
49 DSP_SET_SAMPLE_DEPTH,
50 DSP_SET_STEREO_MODE,
51 DSP_RESET,
52 DSP_FLUSH,
53 DSP_SET_TRACK_GAIN,
54 DSP_SET_ALBUM_GAIN,
55 DSP_SET_TRACK_PEAK,
56 DSP_SET_ALBUM_PEAK,
57 DSP_CROSSFEED
58};
59
60
61/****************************************************************************
62 * NOTE: Any assembly routines that use these structures must be updated
63 * if current data members are moved or changed.
64 */
65struct resample_data
66{
67 uint32_t delta; /* 00h */
68 uint32_t phase; /* 04h */
69 int32_t last_sample[2]; /* 08h */
70 /* 10h */
71};
72
73/* This is for passing needed data to external dsp routines. If another
74 * dsp parameter needs to be passed, add to the end of the structure
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 */
86struct dsp_data
87{
88 int output_scale; /* 00h */
89 int num_channels; /* 04h */
90 struct resample_data resample_data; /* 08h */
91 int32_t clip_min; /* 18h */
92 int32_t clip_max; /* 1ch */
93 int32_t gain; /* 20h - Note that this is in S8.23 format. */
94 int frac_bits; /* 24h */
95 /* 28h */
96};
97
98struct dsp_config;
99
100int dsp_process(struct dsp_config *dsp, char *dest,
101 const char *src[], int count);
102int dsp_input_count(struct dsp_config *dsp, int count);
103int dsp_output_count(struct dsp_config *dsp, int count);
104intptr_t dsp_configure(struct dsp_config *dsp, int setting,
105 intptr_t value);
106int get_replaygain_mode(bool have_track_gain, bool have_album_gain);
107void dsp_set_replaygain(void);
108void dsp_set_crossfeed(bool enable);
109void dsp_set_crossfeed_direct_gain(int gain);
110void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain,
111 long cutoff);
112void dsp_set_eq(bool enable);
113void dsp_set_eq_precut(int precut);
114void dsp_set_eq_coefs(int band);
115void dsp_dither_enable(bool enable);
116void dsp_timestretch_enable(bool enable);
117bool dsp_timestretch_available(void);
118void sound_set_pitch(int32_t r);
119int32_t sound_get_pitch(void);
120void dsp_set_timestretch(int32_t percent);
121int32_t dsp_get_timestretch(void);
122int dsp_callback(int msg, intptr_t param);
123void dsp_set_compressor(void);
124
125#endif
diff --git a/lib/rbcodec/dsp/dsp_arm.S b/lib/rbcodec/dsp/dsp_arm.S
new file mode 100644
index 0000000000..7e360749a3
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_arm.S
@@ -0,0 +1,561 @@
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 "config.h"
22
23/****************************************************************************
24 * void channels_process_sound_chan_mono(int count, int32_t *buf[])
25 */
26
27#include "config.h"
28
29 .section .icode, "ax", %progbits
30 .align 2
31 .global channels_process_sound_chan_mono
32 .type channels_process_sound_chan_mono, %function
33channels_process_sound_chan_mono:
34 @ input: r0 = count, r1 = buf
35 stmfd sp!, { r4, lr } @
36 @
37 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1]
38 subs r0, r0, #1 @ odd: end at 0; even: end at -1
39 beq .mono_singlesample @ Zero? Only one sample!
40 @
41.monoloop: @
42 ldmia r1, { r3, r4 } @ r3, r4 = Li0, Li1
43 ldmia r2, { r12, r14 } @ r12, r14 = Ri0, Ri1
44 mov r3, r3, asr #1 @ Mo0 = Li0 / 2 + Ri0 / 2
45 mov r4, r4, asr #1 @ Mo1 = Li1 / 2 + Ri1 / 2
46 add r12, r3, r12, asr #1 @
47 add r14, r4, r14, asr #1 @
48 subs r0, r0, #2 @
49 stmia r1!, { r12, r14 } @ store Mo0, Mo1
50 stmia r2!, { r12, r14 } @ store Mo0, Mo1
51 bgt .monoloop @
52 @
53 ldmpc cond=lt, regs=r4 @ if count was even, we're done
54 @
55.mono_singlesample: @
56 ldr r3, [r1] @ r3 = Ls
57 ldr r12, [r2] @ r12 = Rs
58 mov r3, r3, asr #1 @ Mo = Ls / 2 + Rs / 2
59 add r12, r3, r12, asr #1 @
60 str r12, [r1] @ store Mo
61 str r12, [r2] @ store Mo
62 @
63 ldmpc regs=r4 @
64 .size channels_process_sound_chan_mono, \
65 .-channels_process_sound_chan_mono
66
67/****************************************************************************
68 * void channels_process_sound_chan_custom(int count, int32_t *buf[])
69 */
70 .section .icode, "ax", %progbits
71 .align 2
72 .global channels_process_sound_chan_custom
73 .type channels_process_sound_chan_custom, %function
74channels_process_sound_chan_custom:
75 stmfd sp!, { r4-r10, lr }
76
77 ldr r3, =dsp_sw_gain
78 ldr r4, =dsp_sw_cross
79
80 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1]
81 ldr r3, [r3] @ r3 = dsp_sw_gain
82 ldr r4, [r4] @ r4 = dsp_sw_cross
83
84 subs r0, r0, #1
85 beq .custom_single_sample @ Zero? Only one sample!
86
87.custom_loop:
88 ldmia r1, { r5, r6 } @ r5 = Li0, r6 = Li1
89 ldmia r2, { r7, r8 } @ r7 = Ri0, r8 = Ri1
90
91 subs r0, r0, #2
92
93 smull r9, r10, r5, r3 @ Lc0 = Li0*gain
94 smull r12, r14, r7, r3 @ Rc0 = Ri0*gain
95 smlal r9, r10, r7, r4 @ Lc0 += Ri0*cross
96 smlal r12, r14, r5, r4 @ Rc0 += Li0*cross
97
98 mov r9, r9, lsr #31 @ Convert to s0.31
99 mov r12, r12, lsr #31
100 orr r5, r9, r10, asl #1
101 orr r7, r12, r14, asl #1
102
103 smull r9, r10, r6, r3 @ Lc1 = Li1*gain
104 smull r12, r14, r8, r3 @ Rc1 = Ri1*gain
105 smlal r9, r10, r8, r4 @ Lc1 += Ri1*cross
106 smlal r12, r14, r6, r4 @ Rc1 += Li1*cross
107
108 mov r9, r9, lsr #31 @ Convert to s0.31
109 mov r12, r12, lsr #31
110 orr r6, r9, r10, asl #1
111 orr r8, r12, r14, asl #1
112
113 stmia r1!, { r5, r6 } @ Store Lc0, Lc1
114 stmia r2!, { r7, r8 } @ Store Rc0, Rc1
115
116 bgt .custom_loop
117
118 ldmpc cond=lt, regs=r4-r10 @ < 0? even count
119
120.custom_single_sample:
121 ldr r5, [r1] @ handle odd sample
122 ldr r7, [r2]
123
124 smull r9, r10, r5, r3 @ Lc0 = Li0*gain
125 smull r12, r14, r7, r3 @ Rc0 = Ri0*gain
126 smlal r9, r10, r7, r4 @ Lc0 += Ri0*cross
127 smlal r12, r14, r5, r4 @ Rc0 += Li0*cross
128
129 mov r9, r9, lsr #31 @ Convert to s0.31
130 mov r12, r12, lsr #31
131 orr r5, r9, r10, asl #1
132 orr r7, r12, r14, asl #1
133
134 str r5, [r1] @ Store Lc0
135 str r7, [r2] @ Store Rc0
136
137 ldmpc regs=r4-r10
138 .size channels_process_sound_chan_custom, \
139 .-channels_process_sound_chan_custom
140
141/****************************************************************************
142 * void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
143 */
144 .section .icode, "ax", %progbits
145 .align 2
146 .global channels_process_sound_chan_karaoke
147 .type channels_process_sound_chan_karaoke, %function
148channels_process_sound_chan_karaoke:
149 @ input: r0 = count, r1 = buf
150 stmfd sp!, { r4, lr } @
151 @
152 ldmia r1, { r1, r2 } @ r1 = buf[0], r2 = buf[1]
153 subs r0, r0, #1 @ odd: end at 0; even: end at -1
154 beq .karaoke_singlesample @ Zero? Only one sample!
155 @
156.karaokeloop: @
157 ldmia r1, { r3, r4 } @ r3, r4 = Li0, Li1
158 ldmia r2, { r12, r14 } @ r12, r14 = Ri0, Ri1
159 mov r3, r3, asr #1 @ Lo0 = Li0 / 2 - Ri0 / 2
160 mov r4, r4, asr #1 @ Lo1 = Li1 / 2 - Ri1 / 2
161 sub r3, r3, r12, asr #1 @
162 sub r4, r4, r14, asr #1 @
163 rsb r12, r3, #0 @ Ro0 = -Lk0 = Rs0 / 2 - Ls0 / 2
164 rsb r14, r4, #0 @ Ro1 = -Lk1 = Ri1 / 2 - Li1 / 2
165 subs r0, r0, #2 @
166 stmia r1!, { r3, r4 } @ store Lo0, Lo1
167 stmia r2!, { r12, r14 } @ store Ro0, Ro1
168 bgt .karaokeloop @
169 @
170 ldmpc cond=lt, regs=r4 @ if count was even, we're done
171 @
172.karaoke_singlesample: @
173 ldr r3, [r1] @ r3 = Li
174 ldr r12, [r2] @ r12 = Ri
175 mov r3, r3, asr #1 @ Lk = Li / 2 - Ri /2
176 sub r3, r3, r12, asr #1 @
177 rsb r12, r3, #0 @ Rk = -Lo = Ri / 2 - Li / 2
178 str r3, [r1] @ store Lo
179 str r12, [r2] @ store Ro
180 @
181 ldmpc regs=r4 @
182 .size channels_process_sound_chan_karaoke, \
183 .-channels_process_sound_chan_karaoke
184
185#if ARM_ARCH < 6
186/****************************************************************************
187 * void sample_output_mono(int count, struct dsp_data *data,
188 * const int32_t *src[], int16_t *dst)
189 */
190 .section .icode, "ax", %progbits
191 .align 2
192 .global sample_output_mono
193 .type sample_output_mono, %function
194sample_output_mono:
195 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
196 stmfd sp!, { r4-r6, lr }
197
198 ldr r1, [r1] @ lr = data->output_scale
199 ldr r2, [r2] @ r2 = src[0]
200
201 mov r4, #1
202 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
203 mov r4, r4, lsr #1
204 mvn r14, #0x8000 @ r14 = 0xffff7fff, needed for
205 @ clipping and masking
206 subs r0, r0, #1 @
207 beq .som_singlesample @ Zero? Only one sample!
208
209.somloop:
210 ldmia r2!, { r5, r6 }
211 add r5, r5, r4 @ r6 = (r6 + 1<<(scale-1)) >> scale
212 mov r5, r5, asr r1
213 mov r12, r5, asr #15
214 teq r12, r12, asr #31
215 eorne r5, r14, r5, asr #31 @ Clip (-32768...+32767)
216 add r6, r6, r4
217 mov r6, r6, asr r1 @ r7 = (r7 + 1<<(scale-1)) >> scale
218 mov r12, r6, asr #15
219 teq r12, r12, asr #31
220 eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
221
222 and r5, r5, r14, lsr #16
223 and r6, r6, r14, lsr #16
224 orr r5, r5, r5, lsl #16 @ pack first 2 halfwords into 1 word
225 orr r6, r6, r6, lsl #16 @ pack last 2 halfwords into 1 word
226 stmia r3!, { r5, r6 }
227
228 subs r0, r0, #2
229 bgt .somloop
230
231 ldmpc cond=lt, regs=r4-r6 @ even 'count'? return
232
233.som_singlesample:
234 ldr r5, [r2] @ do odd sample
235 add r5, r5, r4
236 mov r5, r5, asr r1
237 mov r12, r5, asr #15
238 teq r12, r12, asr #31
239 eorne r5, r14, r5, asr #31
240
241 and r5, r5, r14, lsr #16 @ pack 2 halfwords into 1 word
242 orr r5, r5, r5, lsl #16
243 str r5, [r3]
244
245 ldmpc regs=r4-r6
246 .size sample_output_mono, .-sample_output_mono
247
248/****************************************************************************
249 * void sample_output_stereo(int count, struct dsp_data *data,
250 * const int32_t *src[], int16_t *dst)
251 */
252 .section .icode, "ax", %progbits
253 .align 2
254 .global sample_output_stereo
255 .type sample_output_stereo, %function
256sample_output_stereo:
257 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
258 stmfd sp!, { r4-r9, lr }
259
260 ldr r1, [r1] @ r1 = data->output_scale
261 ldmia r2, { r2, r5 } @ r2 = src[0], r5 = src[1]
262
263 mov r4, #1
264 mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
265 mov r4, r4, lsr #1 @
266
267 mvn r14, #0x8000 @ r14 = 0xffff7fff, needed for
268 @ clipping and masking
269 subs r0, r0, #1 @
270 beq .sos_singlesample @ Zero? Only one sample!
271
272.sosloop:
273 ldmia r2!, { r6, r7 } @ 2 left
274 ldmia r5!, { r8, r9 } @ 2 right
275
276 add r6, r6, r4 @ r6 = (r6 + 1<<(scale-1)) >> scale
277 mov r6, r6, asr r1
278 mov r12, r6, asr #15
279 teq r12, r12, asr #31
280 eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
281 add r7, r7, r4
282 mov r7, r7, asr r1 @ r7 = (r7 + 1<<(scale-1)) >> scale
283 mov r12, r7, asr #15
284 teq r12, r12, asr #31
285 eorne r7, r14, r7, asr #31 @ Clip (-32768...+32767)
286
287 add r8, r8, r4 @ r8 = (r8 + 1<<(scale-1)) >> scale
288 mov r8, r8, asr r1
289 mov r12, r8, asr #15
290 teq r12, r12, asr #31
291 eorne r8, r14, r8, asr #31 @ Clip (-32768...+32767)
292 add r9, r9, r4 @ r9 = (r9 + 1<<(scale-1)) >> scale
293 mov r9, r9, asr r1
294 mov r12, r9, asr #15
295 teq r12, r12, asr #31
296 eorne r9, r14, r9, asr #31 @ Clip (-32768...+32767)
297
298 and r6, r6, r14, lsr #16 @ pack first 2 halfwords into 1 word
299 orr r8, r6, r8, asl #16
300 and r7, r7, r14, lsr #16 @ pack last 2 halfwords into 1 word
301 orr r9, r7, r9, asl #16
302
303 stmia r3!, { r8, r9 }
304
305 subs r0, r0, #2
306 bgt .sosloop
307
308 ldmpc cond=lt, regs=r4-r9 @ even 'count'? return
309
310.sos_singlesample:
311 ldr r6, [r2] @ left odd sample
312 ldr r8, [r5] @ right odd sample
313
314 add r6, r6, r4 @ r6 = (r7 + 1<<(scale-1)) >> scale
315 mov r6, r6, asr r1
316 mov r12, r6, asr #15
317 teq r12, r12, asr #31
318 eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
319 add r8, r8, r4 @ r8 = (r8 + 1<<(scale-1)) >> scale
320 mov r8, r8, asr r1
321 mov r12, r8, asr #15
322 teq r12, r12, asr #31
323 eorne r8, r14, r8, asr #31 @ Clip (-32768...+32767)
324
325 and r6, r6, r14, lsr #16 @ pack 2 halfwords into 1 word
326 orr r8, r6, r8, asl #16
327
328 str r8, [r3]
329
330 ldmpc regs=r4-r9
331 .size sample_output_stereo, .-sample_output_stereo
332#endif /* ARM_ARCH < 6 */
333
334/****************************************************************************
335 * void apply_crossfeed(int count, int32_t* src[])
336 */
337 .section .text
338 .global apply_crossfeed
339apply_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
402dsp_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
459dsp_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
523dsp_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
new file mode 100644
index 0000000000..39949498ea
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_arm_v6.S
@@ -0,0 +1,127 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2010 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 * void sample_output_mono(int count, struct dsp_data *data,
24 * const int32_t *src[], int16_t *dst)
25 */
26 .section .text, "ax", %progbits
27 .align 2
28 .global sample_output_mono
29 .type sample_output_mono, %function
30sample_output_mono:
31 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
32 stmfd sp!, { r4, lr } @
33 @
34 ldr r1, [r1] @ r1 = data->output_scale
35 ldr r2, [r2] @ r2 = src[0]
36 @
37 mov r4, #1 @ r4 = 1 << (scale - 1)
38 mov r4, r4, lsl r1 @
39 subs r0, r0, #1 @ odd: end at 0; even: end at -1
40 mov r4, r4, lsr #1 @
41 beq 2f @ Zero? Only one sample!
42 @
431: @
44 ldmia r2!, { r12, r14 } @ load Mi0, Mi1
45 qadd r12, r12, r4 @ round, scale, saturate and
46 qadd r14, r14, r4 @ pack Mi0 to So0, Mi1 to So1
47 mov r12, r12, asr r1 @
48 mov r14, r14, asr r1 @
49 ssat r12, #16, r12 @
50 ssat r14, #16, r14 @
51 pkhbt r12, r12, r12, asl #16 @
52 pkhbt r14, r14, r14, asl #16 @
53 subs r0, r0, #2 @
54 stmia r3!, { r12, r14 } @ store So0, So1
55 bgt 1b @
56 @
57 ldmltfd sp!, { r4, pc } @ if count was even, we're done
58 @
592: @
60 ldr r12, [r2] @ round, scale, saturate
61 qadd r12, r12, r4 @ and pack Mi to So
62 mov r12, r12, asr r1 @
63 ssat r12, #16, r12 @
64 pkhbt r12, r12, r12, asl #16 @
65 str r12, [r3] @ store So
66 @
67 ldmfd sp!, { r4, pc } @
68 .size sample_output_mono, .-sample_output_mono
69
70/****************************************************************************
71 * void sample_output_stereo(int count, struct dsp_data *data,
72 * const int32_t *src[], int16_t *dst)
73 */
74 .section .text, "ax", %progbits
75 .align 2
76 .global sample_output_stereo
77 .type sample_output_stereo, %function
78sample_output_stereo:
79 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
80 stmfd sp!, { r4-r7, lr } @
81 @
82 ldr r1, [r1] @ r1 = data->output_scale
83 ldmia r2, { r2, r4 } @ r2 = src[0], r4 = src[1]
84 @
85 mov r5, #1 @ r5 = 1 << (scale - 1)
86 mov r5, r5, lsl r1 @
87 subs r0, r0, #1 @ odd: end at 0; even: end at -1
88 mov r5, r5, lsr #1 @
89 beq 2f @ Zero? Only one sample!
90 @
911: @
92 ldmia r2!, { r6, r7 } @ r6, r7 = Li0, Li1
93 ldmia r4!, { r12, r14 } @ r12, r14 = Ri0, Ri1
94 qadd r6, r6, r5 @ round, scale, saturate and pack
95 qadd r7, r7, r5 @ Li0+Ri0 to So0, Li1+Ri1 to So1
96 qadd r12, r12, r5 @
97 qadd r14, r14, r5 @
98 mov r6, r6, asr r1 @
99 mov r7, r7, asr r1 @
100 mov r12, r12, asr r1 @
101 mov r14, r14, asr r1 @
102 ssat r6, #16, r6 @
103 ssat r12, #16, r12 @
104 ssat r7, #16, r7 @
105 ssat r14, #16, r14 @
106 pkhbt r6, r6, r12, asl #16 @
107 pkhbt r7, r7, r14, asl #16 @
108 subs r0, r0, #2 @
109 stmia r3!, { r6, r7 } @ store So0, So1
110 bgt 1b @
111 @
112 ldmltfd sp!, { r4-r7, pc } @ if count was even, we're done
113 @
1142: @
115 ldr r6, [r2] @ r6 = Li
116 ldr r12, [r4] @ r12 = Ri
117 qadd r6, r6, r5 @ round, scale, saturate
118 qadd r12, r12, r5 @ and pack Li+Ri to So
119 mov r6, r6, asr r1 @
120 mov r12, r12, asr r1 @
121 ssat r6, #16, r6 @
122 ssat r12, #16, r12 @
123 pkhbt r6, r6, r12, asl #16 @
124 str r6, [r3] @ store So
125 @
126 ldmfd sp!, { r4-r7, pc } @
127 .size sample_output_stereo, .-sample_output_stereo
diff --git a/lib/rbcodec/dsp/dsp_asm.h b/lib/rbcodec/dsp/dsp_asm.h
new file mode 100644
index 0000000000..7bf18370a3
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_asm.h
@@ -0,0 +1,86 @@
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
50void apply_crossfeed(int count, int32_t *buf[]);
51#endif
52
53#ifdef DSP_HAVE_ASM_APPLY_GAIN
54void 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
58int dsp_upsample(int count, struct dsp_data *data,
59 const int32_t *src[], int32_t *dst[]);
60int 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
65void channels_process_sound_chan_mono(int count, int32_t *buf[]);
66#endif
67
68#ifdef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
69void channels_process_sound_chan_custom(int count, int32_t *buf[]);
70#endif
71
72#ifdef DSP_HAVE_ASM_SOUND_CHAN_KARAOKE
73void channels_process_sound_chan_karaoke(int count, int32_t *buf[]);
74#endif
75
76#ifdef DSP_HAVE_ASM_SAMPLE_OUTPUT_STEREO
77void 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
82void 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
new file mode 100644
index 0000000000..cda811a7d5
--- /dev/null
+++ b/lib/rbcodec/dsp/dsp_cf.S
@@ -0,0 +1,611 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Thom Johansen
11 * Portions Copyright (C) 2007 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
23/****************************************************************************
24 * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
25 */
26 .section .text
27 .align 2
28 .global dsp_apply_gain
29dsp_apply_gain:
30 lea.l -20(%sp), %sp | save registers
31 movem.l %d2-%d4/%a2-%a3, (%sp) |
32 movem.l 28(%sp), %a0-%a1 | %a0 = data,
33 | %a1 = buf
34 move.l 4(%a0), %d1 | %d1 = data->num_channels
35 move.l 32(%a0), %a0 | %a0 = data->gain (in s8.23)
3610: | channel loop |
37 move.l 24(%sp), %d0 | %d0 = count
38 move.l -4(%a1, %d1.l*4), %a2 | %a2 = s = buf[ch-1]
39 move.l %a2, %a3 | %a3 = d = s
40 move.l (%a2)+, %d2 | %d2 = *s++,
41 mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1)
42 subq.l #1, %d0 | --count > 0 ? : effectively n++
43 ble.b 30f | loop done | no? finish up
4420: | loop |
45 move.l %accext01, %d4 | fetch S(n-1)[7:0]
46 movclr.l %acc0, %d3 | fetch S(n-1)[40:8] in %d5[31:0]
47 asl.l #8, %d3 | *s++ = (S(n-1)[40:8] << 8) | S(n-1)[7:0]
48 mac.l %a0, %d2, (%a2)+, %d2, %acc0 | %acc0 = S(n)*gain, load S(n+1)
49 move.b %d4, %d3 |
50 move.l %d3, (%a3)+ |
51 subq.l #1, %d0 | --count > 0 ? : effectively n++
52 bgt.b 20b | loop | yes? do more samples
5330: | loop done |
54 move.l %accext01, %d4 | fetch S(n-1)[7:0]
55 movclr.l %acc0, %d3 | fetch S(n-1)[40:8] in %d5[31:0]
56 asl.l #8, %d3 | *s = (S(n-1)[40:8] << 8) | S(n-1)[7:0]
57 move.b %d4, %d3 |
58 move.l %d3, (%a3) |
59 subq.l #1, %d1 | next channel
60 bgt.b 10b | channel loop |
61 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
62 lea.l 20(%sp), %sp | cleanup stack
63 rts |
64 .size dsp_apply_gain,.-dsp_apply_gain
65
66/****************************************************************************
67 * void apply_crossfeed(int count, int32_t *buf[])
68 */
69 .section .text
70 .align 2
71 .global apply_crossfeed
72apply_crossfeed:
73 lea.l -44(%sp), %sp |
74 movem.l %d2-%d7/%a2-%a6, (%sp) | save all regs
75 movem.l 48(%sp), %d7/%a4 | %d7 = count, %a4 = src
76 movem.l (%a4), %a4-%a5 | %a4 = src[0], %a5 = src[1]
77 lea.l crossfeed_data, %a1 | %a1 = &crossfeed_data
78 move.l (%a1)+, %d6 | %d6 = direct gain
79 movem.l 12(%a1), %d0-%d3 | fetch filter history samples
80 move.l 132(%a1), %a0 | fetch delay line address
81 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
84 /* Register usage in loop:
85 * %a0 = delay_p, %a1..%a3 = b0, b1, a1 (filter coefs),
86 * %a4 = buf[0], %a5 = buf[1],
87 * %a6 = delay line pointer wrap limit,
88 * %d0..%d3 = history
89 * %d4..%d5 = temp.
90 * %d6 = direct gain,
91 * %d7 = count
92 */
9310: | loop |
94 movclr.l %acc0, %d4 | write outputs
95 move.l %d4, (%a4)+ | .
96 movclr.l %acc1, %d5 | .
97 move.l %d5, (%a5)+ | .
9820: | loop start |
99 mac.l %a2, %d0, (%a0)+, %d0, %acc0 | %acc0 = b1*dl[n - 1], %d0 = dl[n]
100 mac.l %a1, %d0 , %acc0 | %acc0 += b0*dl[n]
101 mac.l %a3, %d1, (%a5), %d5, %acc0 | %acc0 += a1*y_r[n - 1], load R
102 mac.l %a2, %d2, (%a0)+, %d2, %acc1 | %acc1 = b1*dr[n - 1], %d2 = dr[n]
103 mac.l %a1, %d2 , %acc1 | %acc1 += b0*dr[n]
104 mac.l %a3, %d3, (%a4), %d4, %acc1 | %acc1 += a1*y_l[n - 1], load L
105 movem.l %d4-%d5, -8(%a0) | save left & right inputs to delay line
106 move.l %acc0, %d3 | get filtered delayed left sample (y_l[n])
107 move.l %acc1, %d1 | get filtered delayed right sample (y_r[n])
108 mac.l %d6, %d4, %acc0 | %acc0 += gain*x_l[n]
109 mac.l %d6, %d5, %acc1 | %acc1 += gain*x_r[n]
110 cmp.l %a6, %a0 | wrap %a0 if passed end
111 bhs.b 30f | wrap buffer |
112 .word 0x51fb | tpf.l | trap the buffer wrap
11330: | wrap buffer | ...fwd taken branches more costly
114 lea.l -104(%a0), %a0 | wrap it up
115 subq.l #1, %d7 | --count > 0 ?
116 bgt.b 10b | loop | yes? do more
117 movclr.l %acc0, %d4 | write last outputs
118 move.l %d4, (%a4) | .
119 movclr.l %acc1, %d5 | .
120 move.l %d5, (%a5) | .
121 lea.l crossfeed_data+16, %a1 | save data back to struct
122 movem.l %d0-%d3, (%a1) | ...history
123 move.l %a0, 120(%a1) | ...delay_p
124 movem.l (%sp), %d2-%d7/%a2-%a6 | restore all regs
125 lea.l 44(%sp), %sp |
126 rts |
127 .size apply_crossfeed,.-apply_crossfeed
128
129/****************************************************************************
130 * int dsp_downsample(int count, struct dsp_data *data,
131 * in32_t *src[], int32_t *dst[])
132 */
133 .section .text
134 .align 2
135 .global dsp_downsample
136dsp_downsample:
137 lea.l -40(%sp), %sp | save non-clobberables
138 movem.l %d2-%d7/%a2-%a5, (%sp) |
139 movem.l 44(%sp), %d2/%a0-%a2 | %d2 = count
140 | %a0 = data
141 | %a1 = src
142 | %a2 = dst
143 movem.l 4(%a0), %d3-%d4 | %d3 = ch = data->num_channels
144 | %d4 = delta = data->resample_data.delta
145 moveq.l #16, %d7 | %d7 = shift
14610: | channel loop |
147 move.l 12(%a0), %d5 | %d5 = phase = data->resample_data.phase
148 move.l -4(%a1, %d3.l*4), %a3 | %a3 = s = src[ch-1]
149 move.l -4(%a2, %d3.l*4), %a4 | %a4 = d = dst[ch-1]
150 lea.l 12(%a0, %d3.l*4), %a5 | %a5 = &data->resample_data.ast_sample[ch-1]
151 move.l (%a5), %d0 | %d0 = last = data->resample_data.last_sample[ch-1]
152 move.l -4(%a3, %d2.l*4), (%a5) | data->resample_data.last_sample[ch-1] = s[count-1]
153 move.l %d5, %d6 | %d6 = pos = phase >> 16
154 lsr.l %d7, %d6 |
155 cmp.l %d2, %d6 | past end of samples?
156 bge.b 40f | skip resample loop| yes? skip loop
157 tst.l %d6 | need last sample of prev. frame?
158 bne.b 20f | resample loop | no? start main loop
159 move.l (%a3, %d6.l*4), %d1 | %d1 = s[pos]
160 bra.b 30f | resample start last | start with last (last in %d0)
16120: | resample loop |
162 lea.l -4(%a3, %d6.l*4), %a5 | load s[pos-1] and s[pos]
163 movem.l (%a5), %d0-%d1 |
16430: | resample start last |
165 sub.l %d0, %d1 | %d1 = diff = s[pos] - s[pos-1]
166 move.l %d0, %acc0 | %acc0 = previous sample
167 move.l %d5, %d0 | frac = (phase << 16) >> 1
168 lsl.l %d7, %d0 |
169 lsr.l #1, %d0 |
170 mac.l %d0, %d1, %acc0 | %acc0 += frac * diff
171 add.l %d4, %d5 | phase += delta
172 move.l %d5, %d6 | pos = phase >> 16
173 lsr.l %d7, %d6 |
174 movclr.l %acc0, %d0 |
175 move.l %d0, (%a4)+ | *d++ = %d0
176 cmp.l %d2, %d6 | pos < count?
177 blt.b 20b | resample loop | yes? continue resampling
17840: | skip resample loop |
179 subq.l #1, %d3 | ch > 0?
180 bgt.b 10b | channel loop | yes? process next channel
181 lsl.l %d7, %d2 | wrap phase to start of next frame
182 sub.l %d2, %d5 | data->resample_data.phase =
183 move.l %d5, 12(%a0) | ... phase - (count << 16)
184 move.l %a4, %d0 | return d - d[0]
185 sub.l (%a2), %d0 |
186 asr.l #2, %d0 | convert bytes->samples
187 movem.l (%sp), %d2-%d7/%a2-%a5 | restore non-clobberables
188 lea.l 40(%sp), %sp | cleanup stack
189 rts | buh-bye
190 .size dsp_downsample,.-dsp_downsample
191
192/****************************************************************************
193 * int dsp_upsample(int count, struct dsp_data *dsp,
194 * const int32_t *src[], int32_t *dst[])
195 */
196 .section .text
197 .align 2
198 .global dsp_upsample
199dsp_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
21010: | 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 |
23520: | 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]
24030: | same sample loop |
241 movclr.l %acc0, %d7 | %d7 = result
242 move.l %d7, (%a4)+ | *d++ = %d7
24340: | 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
25450: | 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
267/****************************************************************************
268 * void channels_process_sound_chan_mono(int count, int32_t *buf[])
269 *
270 * Mix left and right channels 50/50 into a center channel.
271 */
272 .section .text
273 .align 2
274 .global channels_process_sound_chan_mono
275channels_process_sound_chan_mono:
276 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf
277 lea.l -20(%sp), %sp | save registers
278 movem.l %d2-%d4/%a2-%a3, (%sp) |
279 movem.l (%a0), %a0-%a1 | get channel pointers
280 move.l %a0, %a2 | use separate dst pointers since read
281 move.l %a1, %a3 | pointers run one ahead of write
282 move.l #0x40000000, %d3 | %d3 = 0.5
283 move.l (%a0)+, %d1 | prime the input registers
284 move.l (%a1)+, %d2 |
285 mac.l %d1, %d3, (%a0)+, %d1, %acc0 |
286 mac.l %d2, %d3, (%a1)+, %d2, %acc0 |
287 subq.l #1, %d0 |
288 ble.s 20f | loop done |
28910: | loop |
290 movclr.l %acc0, %d4 | L = R = l/2 + r/2
291 mac.l %d1, %d3, (%a0)+, %d1, %acc0 |
292 mac.l %d2, %d3, (%a1)+, %d2, %acc0 |
293 move.l %d4, (%a2)+ | output to original buffer
294 move.l %d4, (%a3)+ |
295 subq.l #1, %d0 |
296 bgt.s 10b | loop |
29720: | loop done |
298 movclr.l %acc0, %d4 | output last sample
299 move.l %d4, (%a2) |
300 move.l %d4, (%a3) |
301 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
302 lea.l 20(%sp), %sp | cleanup
303 rts |
304 .size channels_process_sound_chan_mono, \
305 .-channels_process_sound_chan_mono
306
307/****************************************************************************
308 * void channels_process_sound_chan_custom(int count, int32_t *buf[])
309 *
310 * Apply stereo width (narrowing/expanding) effect.
311 */
312 .section .text
313 .align 2
314 .global channels_process_sound_chan_custom
315channels_process_sound_chan_custom:
316 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf
317 lea.l -28(%sp), %sp | save registers
318 movem.l %d2-%d6/%a2-%a3, (%sp) |
319 movem.l (%a0), %a0-%a1 | get channel pointers
320 move.l %a0, %a2 | use separate dst pointers since read
321 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
325 move.l (%a1)+, %d2 |
326 mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross
327 mac.l %d1, %d4, (%a0)+, %d1, %acc1 | R = r*gain + l*cross
328 mac.l %d2, %d4 , %acc0 |
329 mac.l %d2, %d3, (%a1)+, %d2, %acc1 |
330 subq.l #1, %d0 |
331 ble.b 20f | loop done |
33210: | loop |
333 movclr.l %acc0, %d5 |
334 movclr.l %acc1, %d6 |
335 mac.l %d1, %d3 , %acc0 | L = l*gain + r*cross
336 mac.l %d1, %d4, (%a0)+, %d1, %acc1 | R = r*gain + l*cross
337 mac.l %d2, %d4 , %acc0 |
338 mac.l %d2, %d3, (%a1)+, %d2, %acc1 |
339 move.l %d5, (%a2)+ |
340 move.l %d6, (%a3)+ |
341 subq.l #1, %d0 |
342 bgt.s 10b | loop |
34320: | loop done |
344 movclr.l %acc0, %d5 | output last sample
345 movclr.l %acc1, %d6 |
346 move.l %d5, (%a2) |
347 move.l %d6, (%a3) |
348 movem.l (%sp), %d2-%d6/%a2-%a3 | restore registers
349 lea.l 28(%sp), %sp | cleanup
350 rts |
351 .size channels_process_sound_chan_custom, \
352 .-channels_process_sound_chan_custom
353
354/****************************************************************************
355 * void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
356 *
357 * Separate channels into side channels.
358 */
359 .section .text
360 .align 2
361 .global channels_process_sound_chan_karaoke
362channels_process_sound_chan_karaoke:
363 movem.l 4(%sp), %d0/%a0 | %d0 = count, %a0 = buf
364 lea.l -20(%sp), %sp | save registers
365 movem.l %d2-%d4/%a2-%a3, (%sp) |
366 movem.l (%a0), %a0-%a1 | get channel src pointers
367 move.l %a0, %a2 | use separate dst pointers since read
368 move.l %a1, %a3 | pointers run one ahead of write
369 move.l #0x40000000, %d3 | %d3 = 0.5
370 move.l (%a0)+, %d1 | prime the input registers
371 move.l (%a1)+, %d2 |
372 mac.l %d1, %d3, (%a0)+, %d1, %acc0 | L = l/2 - r/2
373 msac.l %d2, %d3, (%a1)+, %d2, %acc0 |
374 subq.l #1, %d0 |
375 ble.b 20f | loop done |
37610: | loop |
377 movclr.l %acc0, %d4 |
378 mac.l %d1, %d3, (%a0)+, %d1, %acc0 | L = l/2 - r/2
379 msac.l %d2, %d3, (%a1)+, %d2, %acc0 |
380 move.l %d4, (%a2)+ |
381 neg.l %d4 | R = -L = -(l/2 - r/2) = r/2 - l/2
382 move.l %d4, (%a3)+ |
383 subq.l #1, %d0 |
384 bgt.s 10b | loop |
38520: | loop done |
386 movclr.l %acc0, %d4 | output last sample
387 move.l %d4, (%a2) |
388 neg.l %d4 | R = -L = -(l/2 - r/2) = r/2 - l/2
389 move.l %d4, (%a3) |
390 movem.l (%sp), %d2-%d4/%a2-%a3 | restore registers
391 lea.l 20(%sp), %sp | cleanup
392 rts |
393 .size channels_process_sound_chan_karaoke, \
394 .-channels_process_sound_chan_karaoke
395
396/****************************************************************************
397 * void sample_output_stereo(int count, struct dsp_data *data,
398 * const int32_t *src[], int16_t *dst)
399 *
400 * Framework based on the ubiquitous Rockbox line transfer logic for
401 * Coldfire CPUs.
402 *
403 * Does emac clamping and scaling (which proved faster than the usual
404 * checks and branches - even single test clamping) and writes using
405 * line burst transfers. Also better than writing a single L-R pair per
406 * loop but a good deal more code.
407 *
408 * Attemping bursting during reads is rather futile since the source and
409 * destination alignments rarely agree and too much complication will
410 * slow us up. The parallel loads seem to do a bit better at least until
411 * a pcm buffer can always give line aligned chunk and then aligning the
412 * dest can then imply the source is aligned if the source buffers are.
413 * For now longword alignment is assumed of both the source and dest.
414 *
415 */
416 .section .text
417 .align 2
418 .global sample_output_stereo
419sample_output_stereo:
420 lea.l -48(%sp), %sp | save registers
421 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
423 move.l #0x80, %macsr | put emac unit in signed int mode
424 movem.l 52(%sp), %a0-%a2/%a4 |
425 lea.l (%a4, %a0.l*4), %a0 | %a0 = end address
426 move.l (%a1), %d1 | %a1 = multiplier: (1 << (16 - scale))
427 sub.l #16, %d1 |
428 neg.l %d1 |
429 moveq.l #1, %d0 |
430 asl.l %d1, %d0 |
431 move.l %d0, %a1 |
432 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
435 add.l %a4, %d0 |
436 and.l #0xfffffff0, %d0 |
437 cmp.l %a0, %d0 | at least a full line?
438 bhi.w 40f | long loop 1 start | no? do as trailing longwords
439 sub.l #16, %d0 | %d1 = first line bound
440 cmp.l %a4, %d0 | any leading longwords?
441 bls.b 20f | line loop start | no? start line loop
44210: | long loop 0 |
443 move.l (%a2)+, %d1 | read longword from L and R
444 move.l %a6, %acc0 |
445 move.l %acc0, %acc1 |
446 mac.l %d1, %a1, (%a3)+, %d2, %acc0 | shift L to high word
447 mac.l %d2, %a1, %acc1 | shift R to high word
448 movclr.l %acc0, %d1 | get possibly saturated results
449 movclr.l %acc1, %d2 |
450 swap %d2 | move R to low word
451 move.w %d2, %d1 | interleave MS 16 bits of each
452 move.l %d1, (%a4)+ | ...and write both
453 cmp.l %a4, %d0 |
454 bhi.b 10b | long loop 0 |
45520: | line loop start |
456 lea.l -12(%a0), %a5 | %a5 = at or just before last line bound
45730: | line loop |
458 move.l (%a3)+, %d4 | get next 4 R samples and scale
459 move.l %a6, %acc0 |
460 move.l %acc0, %acc1 |
461 move.l %acc1, %acc2 |
462 move.l %acc2, %acc3 |
463 mac.l %d4, %a1, (%a3)+, %d5, %acc0 | with saturation
464 mac.l %d5, %a1, (%a3)+, %d6, %acc1 |
465 mac.l %d6, %a1, (%a3)+, %d7, %acc2 |
466 mac.l %d7, %a1, (%a2)+, %d0, %acc3 |
467 lea.l 16(%a4), %a4 | increment dest here, mitigate stalls
468 movclr.l %acc0, %d4 | obtain R results
469 movclr.l %acc1, %d5 |
470 movclr.l %acc2, %d6 |
471 movclr.l %acc3, %d7 |
472 move.l %a6, %acc0 |
473 move.l %acc0, %acc1 |
474 move.l %acc1, %acc2 |
475 move.l %acc2, %acc3 |
476 mac.l %d0, %a1, (%a2)+, %d1, %acc0 | get next 4 L samples and scale
477 mac.l %d1, %a1, (%a2)+, %d2, %acc1 | with saturation
478 mac.l %d2, %a1, (%a2)+, %d3, %acc2 |
479 mac.l %d3, %a1 , %acc3 |
480 swap %d4 | a) interleave most significant...
481 swap %d5 |
482 swap %d6 |
483 swap %d7 |
484 movclr.l %acc0, %d0 | obtain L results
485 movclr.l %acc1, %d1 |
486 movclr.l %acc2, %d2 |
487 movclr.l %acc3, %d3 |
488 move.w %d4, %d0 | a) ... 16 bits of L and R
489 move.w %d5, %d1 |
490 move.w %d6, %d2 |
491 move.w %d7, %d3 |
492 movem.l %d0-%d3, -16(%a4) | write four stereo samples
493 cmp.l %a4, %a5 |
494 bhi.b 30b | line loop |
49540: | long loop 1 start |
496 cmp.l %a4, %a0 | any longwords left?
497 bls.b 60f | output end | no? stop
49850: | long loop 1 |
499 move.l (%a2)+, %d1 | handle trailing longwords
500 move.l %a6, %acc0 |
501 move.l %acc0, %acc1 |
502 mac.l %d1, %a1, (%a3)+, %d2, %acc0 | the same way as leading ones
503 mac.l %d2, %a1, %acc1 |
504 movclr.l %acc0, %d1 |
505 movclr.l %acc1, %d2 |
506 swap %d2 |
507 move.w %d2, %d1 |
508 move.l %d1, (%a4)+ |
509 cmp.l %a4, %a0 |
510 bhi.b 50b | long loop 1
51160: | output end |
512 movem.l (%sp), %d1-%d7/%a2-%a6 | restore registers
513 move.l %d1, %macsr |
514 lea.l 48(%sp), %sp | cleanup
515 rts |
516 .size sample_output_stereo, .-sample_output_stereo
517
518/****************************************************************************
519 * void sample_output_mono(int count, struct dsp_data *data,
520 * const int32_t *src[], int16_t *dst)
521 *
522 * Same treatment as sample_output_stereo but for one channel.
523 */
524 .section .text
525 .align 2
526 .global sample_output_mono
527sample_output_mono:
528 lea.l -32(%sp), %sp | save registers
529 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
531 move.l #0x80, %macsr | put emac unit in signed int mode
532 movem.l 36(%sp), %a0-%a3 |
533 lea.l (%a3, %a0.l*4), %a0 | %a0 = end address
534 move.l (%a1), %d1 | %d5 = multiplier: (1 << (16 - scale))
535 sub.l #16, %d1 |
536 neg.l %d1 |
537 moveq.l #1, %d5 |
538 asl.l %d1, %d5 |
539 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
542 add.l %a3, %d0 |
543 and.l #0xfffffff0, %d0 |
544 cmp.l %a0, %d0 | at least a full line?
545 bhi.w 40f | long loop 1 start | no? do as trailing longwords
546 sub.l #16, %d0 | %d1 = first line bound
547 cmp.l %a3, %d0 | any leading longwords?
548 bls.b 20f | line loop start | no? start line loop
54910: | long loop 0 |
550 move.l (%a2)+, %d1 | read longword from L and R
551 move.l %a4, %acc0 |
552 mac.l %d1, %d5, %acc0 | shift L to high word
553 movclr.l %acc0, %d1 | get possibly saturated results
554 move.l %d1, %d2 |
555 swap %d2 | move R to low word
556 move.w %d2, %d1 | duplicate single channel into
557 move.l %d1, (%a3)+ | L and R
558 cmp.l %a3, %d0 |
559 bhi.b 10b | long loop 0 |
56020: | line loop start |
561 lea.l -12(%a0), %a1 | %a1 = at or just before last line bound
56230: | line loop |
563 move.l (%a2)+, %d0 | get next 4 L samples and scale
564 move.l %a4, %acc0 |
565 move.l %acc0, %acc1 |
566 move.l %acc1, %acc2 |
567 move.l %acc2, %acc3 |
568 mac.l %d0, %d5, (%a2)+, %d1, %acc0 | with saturation
569 mac.l %d1, %d5, (%a2)+, %d2, %acc1 |
570 mac.l %d2, %d5, (%a2)+, %d3, %acc2 |
571 mac.l %d3, %d5 , %acc3 |
572 lea.l 16(%a3), %a3 | increment dest here, mitigate stalls
573 movclr.l %acc0, %d0 | obtain results
574 movclr.l %acc1, %d1 |
575 movclr.l %acc2, %d2 |
576 movclr.l %acc3, %d3 |
577 move.l %d0, %d4 | duplicate single channel
578 swap %d4 | into L and R
579 move.w %d4, %d0 |
580 move.l %d1, %d4 |
581 swap %d4 |
582 move.w %d4, %d1 |
583 move.l %d2, %d4 |
584 swap %d4 |
585 move.w %d4, %d2 |
586 move.l %d3, %d4 |
587 swap %d4 |
588 move.w %d4, %d3 |
589 movem.l %d0-%d3, -16(%a3) | write four stereo samples
590 cmp.l %a3, %a1 |
591 bhi.b 30b | line loop |
59240: | long loop 1 start |
593 cmp.l %a3, %a0 | any longwords left?
594 bls.b 60f | output end | no? stop
59550: | loop loop 1 |
596 move.l (%a2)+, %d1 | handle trailing longwords
597 move.l %a4, %acc0 |
598 mac.l %d1, %d5, %acc0 | the same way as leading ones
599 movclr.l %acc0, %d1 |
600 move.l %d1, %d2 |
601 swap %d2 |
602 move.w %d2, %d1 |
603 move.l %d1, (%a3)+ |
604 cmp.l %a3, %a0 |
605 bhi.b 50b | long loop 1 |
60660: | output end |
607 movem.l (%sp), %d1-%d5/%a2-%a4 | restore registers
608 move.l %d1, %macsr |
609 lea.l 32(%sp), %sp | cleanup
610 rts |
611 .size sample_output_mono, .-sample_output_mono
diff --git a/lib/rbcodec/dsp/eq.c b/lib/rbcodec/dsp/eq.c
new file mode 100644
index 0000000000..122a46a4c5
--- /dev/null
+++ b/lib/rbcodec/dsp/eq.c
@@ -0,0 +1,268 @@
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 <inttypes.h>
23#include "config.h"
24#include "fixedpoint.h"
25#include "fracmul.h"
26#include "eq.h"
27#include "replaygain.h"
28
29/**
30 * Calculate first order shelving filter. Filter is not directly usable by the
31 * eq_filter() function.
32 * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
33 * @param A decibel value multiplied by ten, describing gain/attenuation of
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 */
38void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c)
39{
40 long sin, cos;
41 int32_t b0, b1, a0, a1; /* s3.28 */
42 const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */
43
44 sin = fp_sincos(cutoff/2, &cos);
45 if (low) {
46 const int32_t sin_div_g = fp_div(sin, g, 25);
47 const int32_t sin_g = FRACMUL(sin, g);
48 cos >>= 3;
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
63 const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */
64 *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */
65 *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */
66 *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */
67}
68
69#ifdef HAVE_SW_TONE_CONTROLS
70/**
71 * Calculate second order section filter consisting of one low-shelf and one
72 * high-shelf section.
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 */
82void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
83 long A_low, long A_high, long A, int32_t *c)
84{
85 const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */
86 int32_t c_ls[3], c_hs[3];
87
88 filter_shelf_coefs(cutoff_low, A_low, true, c_ls);
89 filter_shelf_coefs(cutoff_high, A_high, false, c_hs);
90 c_ls[0] = FRACMUL(g, c_ls[0]);
91 c_ls[1] = FRACMUL(g, c_ls[1]);
92
93 /* now we cascade the two first order filters to one second order filter
94 * which can be used by eq_filter(). these resulting coefficients have a
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
108/* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
109 * Slightly faster calculation can be done by deriving forms which use tan()
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
116/**
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 */
126void 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
137 /* possible numerical ranges are in comments by each coef */
138 b0 = one + alphaA; /* [1 .. 5] */
139 b1 = a1 = -2*(cs >> 3); /* [-2 .. 2] */
140 b2 = one - alphaA; /* [-3 .. 1] */
141 a0 = one + alphadivA; /* [1 .. 5] */
142 a2 = one - alphadivA; /* [-3 .. 1] */
143
144 /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */
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}
152
153/**
154 * Calculate coefficients for lowshelf filter. Parameters are as for
155 * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
156 */
157void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
158{
159 long cs;
160 const long one = 1 << 25; /* s6.25 */
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
185 /* [0.1 .. 1.99] */
186 const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */
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}
193
194/**
195 * Calculate coefficients for highshelf filter. Parameters are as for
196 * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
197 */
198void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
199{
200 long cs;
201 const long one = 1 << 25; /* s6.25 */
202 const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */
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
213 /* [0.1 .. 40] */
214 b0 = FRACMUL_SHL(A, ap1 + am1_cs + twosqrtalpha, 2);
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
226 /* [0.1 .. 1.99] */
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}
234
235/* We realise the filters as a second order direct form 1 structure. Direct
236 * form 1 was chosen because of better numerical properties for fixed point
237 * implementations.
238 */
239
240#if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM))
241void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
242 unsigned channels, unsigned shift)
243{
244 unsigned c, i;
245 long long acc;
246
247 /* Direct form 1 filtering code.
248 y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
249 where y[] is output and x[] is input.
250 */
251
252 for (c = 0; c < channels; c++) {
253 for (i = 0; i < num; i++) {
254 acc = (long long) x[c][i] * f->coefs[0];
255 acc += (long long) f->history[c][0] * f->coefs[1];
256 acc += (long long) f->history[c][1] * f->coefs[2];
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 }
266}
267#endif
268
diff --git a/lib/rbcodec/dsp/eq.h b/lib/rbcodec/dsp/eq.h
new file mode 100644
index 0000000000..a44e9153ac
--- /dev/null
+++ b/lib/rbcodec/dsp/eq.h
@@ -0,0 +1,50 @@
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#ifndef _EQ_H
23#define _EQ_H
24
25#include <inttypes.h>
26#include <stdbool.h>
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#define FILTER_BISHELF_SHIFT 5
32#define EQ_PEAK_SHIFT 4
33#define EQ_SHELF_SHIFT 6
34
35struct eqfilter {
36 int32_t coefs[5]; /* Order is b0, b1, b2, a1, a2 */
37 int32_t history[2][4];
38};
39
40void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c);
41void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
42 long A_low, long A_high, long A, int32_t *c);
43void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
44void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
45void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
46void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
47 unsigned channels, unsigned shift);
48
49#endif
50
diff --git a/lib/rbcodec/dsp/eq_arm.S b/lib/rbcodec/dsp/eq_arm.S
new file mode 100644
index 0000000000..b0e1771e89
--- /dev/null
+++ b/lib/rbcodec/dsp/eq_arm.S
@@ -0,0 +1,89 @@
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
39eq_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
new file mode 100644
index 0000000000..30a28b9d99
--- /dev/null
+++ b/lib/rbcodec/dsp/eq_cf.S
@@ -0,0 +1,91 @@
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
33eq_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/eqs/Acoustic.cfg b/lib/rbcodec/dsp/eqs/Acoustic.cfg
new file mode 100644
index 0000000000..34b5ed8a2b
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Acoustic.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 45
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 45
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 10
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 15
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 30
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 20
diff --git a/lib/rbcodec/dsp/eqs/Bass.cfg b/lib/rbcodec/dsp/eqs/Bass.cfg
new file mode 100644
index 0000000000..2742459081
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Bass.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 50
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 50
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 35
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 15
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 5
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: -5
diff --git a/lib/rbcodec/dsp/eqs/Classical.cfg b/lib/rbcodec/dsp/eqs/Classical.cfg
new file mode 100644
index 0000000000..bf2f9f9566
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Classical.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 50
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 50
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 40
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: -20
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 10
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 20
diff --git a/lib/rbcodec/dsp/eqs/Default.cfg b/lib/rbcodec/dsp/eqs/Default.cfg
new file mode 100644
index 0000000000..d6f345fa9e
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Default.cfg
@@ -0,0 +1,17 @@
1eq enabled: off
2eq precut: 0
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 0
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 0
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 0
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 0
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 0
diff --git a/lib/rbcodec/dsp/eqs/Disco.cfg b/lib/rbcodec/dsp/eqs/Disco.cfg
new file mode 100644
index 0000000000..f894f26da1
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Disco.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 45
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 30
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 10
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 45
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 25
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 10
diff --git a/lib/rbcodec/dsp/eqs/Electronic.cfg b/lib/rbcodec/dsp/eqs/Electronic.cfg
new file mode 100644
index 0000000000..e70c911272
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Electronic.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 55
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 45
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 5
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 25
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 15
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 55
diff --git a/lib/rbcodec/dsp/eqs/Hip-Hop.cfg b/lib/rbcodec/dsp/eqs/Hip-Hop.cfg
new file mode 100644
index 0000000000..2d38425dc4
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Hip-Hop.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 65
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 65
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 25
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: -10
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 15
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 35
diff --git a/lib/rbcodec/dsp/eqs/Jazz.cfg b/lib/rbcodec/dsp/eqs/Jazz.cfg
new file mode 100644
index 0000000000..f576f9fcc1
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Jazz.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 60
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 40
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 15
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: -25
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 5
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 60
diff --git a/lib/rbcodec/dsp/eqs/Lounge.cfg b/lib/rbcodec/dsp/eqs/Lounge.cfg
new file mode 100644
index 0000000000..39ae23a7e7
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Lounge.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 20
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: -25
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 5
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 20
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: -15
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 15
diff --git a/lib/rbcodec/dsp/eqs/Pop.cfg b/lib/rbcodec/dsp/eqs/Pop.cfg
new file mode 100644
index 0000000000..1d8cefe173
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Pop.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 50
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: -10
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 5
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 50
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 15
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: -10
diff --git a/lib/rbcodec/dsp/eqs/R&B.cfg b/lib/rbcodec/dsp/eqs/R&B.cfg
new file mode 100644
index 0000000000..a460b587f5
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/R&B.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 45
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 35
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 45
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 5
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 25
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 30
diff --git a/lib/rbcodec/dsp/eqs/Rock.cfg b/lib/rbcodec/dsp/eqs/Rock.cfg
new file mode 100644
index 0000000000..ec4f0356a8
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Rock.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 45
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: 25
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 10
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 0
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 20
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 45
diff --git a/lib/rbcodec/dsp/eqs/Vocal.cfg b/lib/rbcodec/dsp/eqs/Vocal.cfg
new file mode 100644
index 0000000000..1de754f07c
--- /dev/null
+++ b/lib/rbcodec/dsp/eqs/Vocal.cfg
@@ -0,0 +1,17 @@
1eq enabled: on
2eq precut: 45
3eq band 0 cutoff: 60
4eq band 0 q: 7
5eq band 0 gain: -45
6eq band 1 cutoff: 200
7eq band 1 q: 10
8eq band 1 gain: 5
9eq band 2 cutoff: 800
10eq band 2 q: 10
11eq band 2 gain: 45
12eq band 3 cutoff: 4000
13eq band 3 q: 10
14eq band 3 gain: 20
15eq band 4 cutoff: 12000
16eq band 4 q: 7
17eq band 4 gain: 0
diff --git a/lib/rbcodec/dsp/tdspeed.c b/lib/rbcodec/dsp/tdspeed.c
new file mode 100644
index 0000000000..731be12621
--- /dev/null
+++ b/lib/rbcodec/dsp/tdspeed.c
@@ -0,0 +1,450 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
11 * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
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
23#include <inttypes.h>
24#include <stddef.h>
25#include <stdio.h>
26#include <string.h>
27#include "sound.h"
28#include "core_alloc.h"
29#include "system.h"
30#include "tdspeed.h"
31#include "settings.h"
32
33#define assert(cond)
34
35#define MIN_RATE 8000
36#define MAX_RATE 48000 /* double buffer for double rate */
37#define MINFREQ 100
38
39#define FIXED_BUFSIZE 3072 /* 48KHz factor 3.0 */
40
41static int32_t** dsp_src;
42static int handles[4];
43static int32_t *overlap_buffer[2] = { NULL, NULL };
44static int32_t *outbuf[2] = { NULL, NULL };
45
46static int move_callback(int handle, void* current, void* new)
47{
48 /* TODO */
49 (void)handle;
50 if (dsp_src)
51 {
52 int ch = (current == outbuf[0]) ? 0 : 1;
53 dsp_src[ch] = outbuf[ch] = new;
54 }
55 return BUFLIB_CB_OK;
56}
57
58static struct buflib_callbacks ops = {
59 .move_callback = move_callback,
60 .shrink_callback = NULL,
61};
62
63static int ovl_move_callback(int handle, void* current, void* new)
64{
65 /* TODO */
66 (void)handle;
67 if (dsp_src)
68 {
69 int ch = (current == overlap_buffer[0]) ? 0 : 1;
70 overlap_buffer[ch] = new;
71 }
72 return BUFLIB_CB_OK;
73}
74
75static struct buflib_callbacks ovl_ops = {
76 .move_callback = ovl_move_callback,
77 .shrink_callback = NULL,
78};
79
80
81static struct tdspeed_state_s
82{
83 bool stereo;
84 int32_t shift_max; /* maximum displacement on a frame */
85 int32_t src_step; /* source window pace */
86 int32_t dst_step; /* destination window pace */
87 int32_t dst_order; /* power of two for dst_step */
88 int32_t ovl_shift; /* overlap buffer frame shift */
89 int32_t ovl_size; /* overlap buffer used size */
90 int32_t ovl_space; /* overlap buffer size */
91 int32_t *ovl_buff[2]; /* overlap buffer */
92} tdspeed_state;
93
94void tdspeed_init(void)
95{
96 if (!global_settings.timestretch_enabled)
97 return;
98
99 /* Allocate buffers */
100 if (overlap_buffer[0] == NULL)
101 {
102 handles[0] = core_alloc_ex("tdspeed ovl left", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
103 overlap_buffer[0] = core_get_data(handles[0]);
104 }
105 if (overlap_buffer[1] == NULL)
106 {
107 handles[1] = core_alloc_ex("tdspeed ovl right", FIXED_BUFSIZE * sizeof(int32_t), &ovl_ops);
108 overlap_buffer[1] = core_get_data(handles[1]);
109 }
110 if (outbuf[0] == NULL)
111 {
112 handles[2] = core_alloc_ex("tdspeed left", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
113 outbuf[0] = core_get_data(handles[2]);
114 }
115 if (outbuf[1] == NULL)
116 {
117 handles[3] = core_alloc_ex("tdspeed right", TDSPEED_OUTBUFSIZE * sizeof(int32_t), &ops);
118 outbuf[1] = core_get_data(handles[3]);
119 }
120}
121
122void tdspeed_finish(void)
123{
124 for(unsigned i = 0; i < ARRAYLEN(handles); i++)
125 {
126 if (handles[i] > 0)
127 {
128 core_free(handles[i]);
129 handles[i] = 0;
130 }
131 }
132 overlap_buffer[0] = overlap_buffer[1] = NULL;
133 outbuf[0] = outbuf[1] = NULL;
134}
135
136bool tdspeed_config(int samplerate, bool stereo, int32_t factor)
137{
138 struct tdspeed_state_s *st = &tdspeed_state;
139 int src_frame_sz;
140
141 /* Check buffers were allocated ok */
142 if (overlap_buffer[0] == NULL || overlap_buffer[1] == NULL)
143 return false;
144
145 if (outbuf[0] == NULL || outbuf[1] == NULL)
146 return false;
147
148 /* Check parameters */
149 if (factor == PITCH_SPEED_100)
150 return false;
151
152 if (samplerate < MIN_RATE || samplerate > MAX_RATE)
153 return false;
154
155 if (factor < STRETCH_MIN || factor > STRETCH_MAX)
156 return false;
157
158 st->stereo = stereo;
159 st->dst_step = samplerate / MINFREQ;
160
161 if (factor > PITCH_SPEED_100)
162 st->dst_step = st->dst_step * PITCH_SPEED_100 / factor;
163
164 st->dst_order = 1;
165
166 while (st->dst_step >>= 1)
167 st->dst_order++;
168
169 st->dst_step = (1 << st->dst_order);
170 st->src_step = st->dst_step * factor / PITCH_SPEED_100;
171 st->shift_max = (st->dst_step > st->src_step) ? st->dst_step : st->src_step;
172
173 src_frame_sz = st->shift_max + st->dst_step;
174
175 if (st->dst_step > st->src_step)
176 src_frame_sz += st->dst_step - st->src_step;
177
178 st->ovl_space = ((src_frame_sz - 2) / st->src_step) * st->src_step
179 + src_frame_sz;
180
181 if (st->src_step > st->dst_step)
182 st->ovl_space += 2*st->src_step - st->dst_step;
183
184 if (st->ovl_space > FIXED_BUFSIZE)
185 st->ovl_space = FIXED_BUFSIZE;
186
187 st->ovl_size = 0;
188 st->ovl_shift = 0;
189
190 st->ovl_buff[0] = overlap_buffer[0];
191
192 if (stereo)
193 st->ovl_buff[1] = overlap_buffer[1];
194 else
195 st->ovl_buff[1] = st->ovl_buff[0];
196
197 return true;
198}
199
200static int tdspeed_apply(int32_t *buf_out[2], int32_t *buf_in[2],
201 int data_len, int last, int out_size)
202/* data_len in samples */
203{
204 struct tdspeed_state_s *st = &tdspeed_state;
205 int32_t *dest[2];
206 int32_t next_frame, prev_frame, src_frame_sz;
207 bool stereo = buf_in[0] != buf_in[1];
208
209 assert(stereo == st->stereo);
210
211 src_frame_sz = st->shift_max + st->dst_step;
212
213 if (st->dst_step > st->src_step)
214 src_frame_sz += st->dst_step - st->src_step;
215
216 /* deal with overlap data first, if any */
217 if (st->ovl_size)
218 {
219 int32_t have, copy, steps;
220 have = st->ovl_size;
221
222 if (st->ovl_shift > 0)
223 have -= st->ovl_shift;
224
225 /* append just enough data to have all of the overlap buffer consumed */
226 steps = (have - 1) / st->src_step;
227 copy = steps * st->src_step + src_frame_sz - have;
228
229 if (copy < src_frame_sz - st->dst_step)
230 copy += st->src_step; /* one more step to allow for pregap data */
231
232 if (copy > data_len)
233 copy = data_len;
234
235 assert(st->ovl_size + copy <= FIXED_BUFSIZE);
236 memcpy(st->ovl_buff[0] + st->ovl_size, buf_in[0],
237 copy * sizeof(int32_t));
238
239 if (stereo)
240 memcpy(st->ovl_buff[1] + st->ovl_size, buf_in[1],
241 copy * sizeof(int32_t));
242
243 if (!last && have + copy < src_frame_sz)
244 {
245 /* still not enough to process at least one frame */
246 st->ovl_size += copy;
247 return 0;
248 }
249
250 /* recursively call ourselves to process the overlap buffer */
251 have = st->ovl_size;
252 st->ovl_size = 0;
253
254 if (copy == data_len)
255 {
256 assert(have + copy <= FIXED_BUFSIZE);
257 return tdspeed_apply(buf_out, st->ovl_buff, have+copy, last,
258 out_size);
259 }
260
261 assert(have + copy <= FIXED_BUFSIZE);
262 int i = tdspeed_apply(buf_out, st->ovl_buff, have+copy, -1, out_size);
263
264 dest[0] = buf_out[0] + i;
265 dest[1] = buf_out[1] + i;
266
267 /* readjust pointers to account for data already consumed */
268 next_frame = copy - src_frame_sz + st->src_step;
269 prev_frame = next_frame - st->ovl_shift;
270 }
271 else
272 {
273 dest[0] = buf_out[0];
274 dest[1] = buf_out[1];
275
276 next_frame = prev_frame = 0;
277
278 if (st->ovl_shift > 0)
279 next_frame += st->ovl_shift;
280 else
281 prev_frame += -st->ovl_shift;
282 }
283
284 st->ovl_shift = 0;
285
286 /* process all complete frames */
287 while (data_len - next_frame >= src_frame_sz)
288 {
289 /* find frame overlap by autocorelation */
290 int const INC1 = 8;
291 int const INC2 = 32;
292
293 int64_t min_delta = INT64_MAX; /* most positive */
294 int shift = 0;
295
296 /* Power of 2 of a 28bit number requires 56bits, can accumulate
297 256times in a 64bit variable. */
298 assert(st->dst_step / INC2 <= 256);
299 assert(next_frame + st->shift_max - 1 + st->dst_step - 1 < data_len);
300 assert(prev_frame + st->dst_step - 1 < data_len);
301
302 for (int i = 0; i < st->shift_max; i += INC1)
303 {
304 int64_t delta = 0;
305
306 int32_t *curr = buf_in[0] + next_frame + i;
307 int32_t *prev = buf_in[0] + prev_frame;
308
309 for (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
310 {
311 int32_t diff = *curr - *prev;
312 delta += abs(diff);
313
314 if (delta >= min_delta)
315 goto skip;
316 }
317
318 if (stereo)
319 {
320 curr = buf_in[1] + next_frame + i;
321 prev = buf_in[1] + prev_frame;
322
323 for (int j = 0; j < st->dst_step; j += INC2, curr += INC2, prev += INC2)
324 {
325 int32_t diff = *curr - *prev;
326 delta += abs(diff);
327
328 if (delta >= min_delta)
329 goto skip;
330 }
331 }
332
333 min_delta = delta;
334 shift = i;
335skip:;
336 }
337
338 /* overlap fading-out previous frame with fading-in current frame */
339 int32_t *curr = buf_in[0] + next_frame + shift;
340 int32_t *prev = buf_in[0] + prev_frame;
341
342 int32_t *d = dest[0];
343
344 assert(next_frame + shift + st->dst_step - 1 < data_len);
345 assert(prev_frame + st->dst_step - 1 < data_len);
346 assert(dest[0] - buf_out[0] + st->dst_step - 1 < out_size);
347
348 for (int i = 0, j = st->dst_step; j; i++, j--)
349 {
350 *d++ = (*curr++ * (int64_t)i +
351 *prev++ * (int64_t)j) >> st->dst_order;
352 }
353
354 dest[0] = d;
355
356 if (stereo)
357 {
358 curr = buf_in[1] + next_frame + shift;
359 prev = buf_in[1] + prev_frame;
360
361 d = dest[1];
362
363 for (int i = 0, j = st->dst_step; j; i++, j--)
364 {
365 assert(d < buf_out[1] + out_size);
366
367 *d++ = (*curr++ * (int64_t)i +
368 *prev++ * (int64_t)j) >> st->dst_order;
369 }
370
371 dest[1] = d;
372 }
373
374 /* adjust pointers for next frame */
375 prev_frame = next_frame + shift + st->dst_step;
376 next_frame += st->src_step;
377
378 /* here next_frame - prev_frame = src_step - dst_step - shift */
379 assert(next_frame - prev_frame == st->src_step - st->dst_step - shift);
380 }
381
382 /* now deal with remaining partial frames */
383 if (last == -1)
384 {
385 /* special overlap buffer processing: remember frame shift only */
386 st->ovl_shift = next_frame - prev_frame;
387 }
388 else if (last != 0)
389 {
390 /* last call: purge all remaining data to output buffer */
391 int i = data_len - prev_frame;
392
393 assert(dest[0] + i <= buf_out[0] + out_size);
394 memcpy(dest[0], buf_in[0] + prev_frame, i * sizeof(int32_t));
395
396 dest[0] += i;
397
398 if (stereo)
399 {
400 assert(dest[1] + i <= buf_out[1] + out_size);
401 memcpy(dest[1], buf_in[1] + prev_frame, i * sizeof(int32_t));
402 dest[1] += i;
403 }
404 }
405 else
406 {
407 /* preserve remaining data + needed overlap data for next call */
408 st->ovl_shift = next_frame - prev_frame;
409 int i = (st->ovl_shift < 0) ? next_frame : prev_frame;
410 st->ovl_size = data_len - i;
411
412 assert(st->ovl_size <= FIXED_BUFSIZE);
413 memcpy(st->ovl_buff[0], buf_in[0] + i, st->ovl_size * sizeof(int32_t));
414
415 if (stereo)
416 memcpy(st->ovl_buff[1], buf_in[1] + i, st->ovl_size * sizeof(int32_t));
417 }
418
419 return dest[0] - buf_out[0];
420}
421
422long tdspeed_est_output_size()
423{
424 return TDSPEED_OUTBUFSIZE;
425}
426
427long tdspeed_est_input_size(long size)
428{
429 struct tdspeed_state_s *st = &tdspeed_state;
430
431 size = (size - st->ovl_size) * st->src_step / st->dst_step;
432
433 if (size < 0)
434 size = 0;
435
436 return size;
437}
438
439int tdspeed_doit(int32_t *src[], int count)
440{
441 dsp_src = src;
442 count = tdspeed_apply( (int32_t *[2]) { outbuf[0], outbuf[1] },
443 src, count, 0, TDSPEED_OUTBUFSIZE);
444
445 src[0] = outbuf[0];
446 src[1] = outbuf[1];
447
448 return count;
449}
450
diff --git a/lib/rbcodec/dsp/tdspeed.h b/lib/rbcodec/dsp/tdspeed.h
new file mode 100644
index 0000000000..e91eeb1701
--- /dev/null
+++ b/lib/rbcodec/dsp/tdspeed.h
@@ -0,0 +1,49 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 by Nicolas Pitre <nico@cam.org>
11 * Copyright (C) 2006-2007 by Stéphane Doyon <s.doyon@videotron.ca>
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
23#ifndef _TDSPEED_H
24#define _TDSPEED_H
25
26#include "dsp.h"
27
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) \
33 ((pitch * stretch + PITCH_SPEED_100 / 2L) / PITCH_SPEED_100)
34#define GET_PITCH(speed, stretch) \
35 ((speed * PITCH_SPEED_100 + stretch / 2L) / stretch)
36#define GET_STRETCH(pitch, speed) \
37 ((speed * PITCH_SPEED_100 + pitch / 2L) / pitch)
38
39void tdspeed_init(void);
40void tdspeed_finish(void);
41bool tdspeed_config(int samplerate, bool stereo, int32_t factor);
42long tdspeed_est_output_size(void);
43long tdspeed_est_input_size(long size);
44int tdspeed_doit(int32_t *src[], int count);
45
46#define STRETCH_MAX (250L * PITCH_SPEED_PRECISION) /* 250% */
47#define STRETCH_MIN (35L * PITCH_SPEED_PRECISION) /* 35% */
48
49#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-28 09:28:42 +0000