diff options
Diffstat (limited to 'firmware/replaygain.c')
-rw-r--r-- | firmware/replaygain.c | 328 |
1 files changed, 328 insertions, 0 deletions
diff --git a/firmware/replaygain.c b/firmware/replaygain.c new file mode 100644 index 0000000000..23a25cc310 --- /dev/null +++ b/firmware/replaygain.c | |||
@@ -0,0 +1,328 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2005 Magnus Holmgren | ||
11 | * | ||
12 | * All files in this archive are subject to the GNU General Public License. | ||
13 | * See the file COPYING in the source tree root for full license agreement. | ||
14 | * | ||
15 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
16 | * KIND, either express or implied. | ||
17 | * | ||
18 | ****************************************************************************/ | ||
19 | |||
20 | #include <ctype.h> | ||
21 | #include <inttypes.h> | ||
22 | #include <math.h> | ||
23 | #include <stdio.h> | ||
24 | #include <stdlib.h> | ||
25 | #include <stdbool.h> | ||
26 | #include "debug.h" | ||
27 | |||
28 | /* The fixed point math routines (with the exception of fp_atof) are based | ||
29 | * on oMathFP by Dan Carter (http://orbisstudios.com). | ||
30 | */ | ||
31 | |||
32 | /* 12 bits of precision gives fairly accurate result, but still allows a | ||
33 | * compact implementation. The math code supports up to 13... | ||
34 | */ | ||
35 | |||
36 | #define FP_BITS (12) | ||
37 | #define FP_MASK ((1 << FP_BITS) - 1) | ||
38 | #define FP_ONE (1 << FP_BITS) | ||
39 | #define FP_TWO (2 << FP_BITS) | ||
40 | #define FP_HALF (1 << (FP_BITS - 1)) | ||
41 | #define FP_LN2 ( 45426 >> (16 - FP_BITS)) | ||
42 | #define FP_LN2_INV ( 94548 >> (16 - FP_BITS)) | ||
43 | #define FP_EXP_ZERO ( 10922 >> (16 - FP_BITS)) | ||
44 | #define FP_EXP_ONE ( -182 >> (16 - FP_BITS)) | ||
45 | #define FP_EXP_TWO ( 4 >> (16 - FP_BITS)) | ||
46 | #define FP_INF (0x7fffffff) | ||
47 | #define FP_LN10 (150902 >> (16 - FP_BITS)) | ||
48 | |||
49 | #define FP_MAX_DIGITS (4) | ||
50 | #define FP_MAX_DIGITS_INT (10000) | ||
51 | |||
52 | #define FP_FAST_MUL_DIV | ||
53 | |||
54 | #ifdef FP_FAST_MUL_DIV | ||
55 | |||
56 | /* These macros can easily overflow, but they are good enough for our uses, | ||
57 | * and saves some code. | ||
58 | */ | ||
59 | #define fp_mul(x, y) (((x) * (y)) >> FP_BITS) | ||
60 | #define fp_div(x, y) (((x) << FP_BITS) / (y)) | ||
61 | |||
62 | #else | ||
63 | |||
64 | static long fp_mul(long x, long y) | ||
65 | { | ||
66 | long x_neg = 0; | ||
67 | long y_neg = 0; | ||
68 | long rc; | ||
69 | |||
70 | if ((x == 0) || (y == 0)) | ||
71 | { | ||
72 | return 0; | ||
73 | } | ||
74 | |||
75 | if (x < 0) | ||
76 | { | ||
77 | x_neg = 1; | ||
78 | x = -x; | ||
79 | } | ||
80 | |||
81 | if (y < 0) | ||
82 | { | ||
83 | y_neg = 1; | ||
84 | y = -y; | ||
85 | } | ||
86 | |||
87 | rc = (((x >> FP_BITS) * (y >> FP_BITS)) << FP_BITS) | ||
88 | + (((x & FP_MASK) * (y & FP_MASK)) >> FP_BITS) | ||
89 | + ((x & FP_MASK) * (y >> FP_BITS)) | ||
90 | + ((x >> FP_BITS) * (y & FP_MASK)); | ||
91 | |||
92 | if ((x_neg ^ y_neg) == 1) | ||
93 | { | ||
94 | rc = -rc; | ||
95 | } | ||
96 | |||
97 | return rc; | ||
98 | } | ||
99 | |||
100 | static long fp_div(long x, long y) | ||
101 | { | ||
102 | long x_neg = 0; | ||
103 | long y_neg = 0; | ||
104 | long shifty; | ||
105 | long rc; | ||
106 | int msb = 0; | ||
107 | int lsb = 0; | ||
108 | |||
109 | if (x == 0) | ||
110 | { | ||
111 | return 0; | ||
112 | } | ||
113 | |||
114 | if (y == 0) | ||
115 | { | ||
116 | return (x < 0) ? -FP_INF : FP_INF; | ||
117 | } | ||
118 | |||
119 | if (x < 0) | ||
120 | { | ||
121 | x_neg = 1; | ||
122 | x = -x; | ||
123 | } | ||
124 | |||
125 | if (y < 0) | ||
126 | { | ||
127 | y_neg = 1; | ||
128 | y = -y; | ||
129 | } | ||
130 | |||
131 | while ((x & (1 << (30 - msb))) == 0) | ||
132 | { | ||
133 | msb++; | ||
134 | } | ||
135 | |||
136 | while ((y & (1 << lsb)) == 0) | ||
137 | { | ||
138 | lsb++; | ||
139 | } | ||
140 | |||
141 | shifty = FP_BITS - (msb + lsb); | ||
142 | rc = ((x << msb) / (y >> lsb)); | ||
143 | |||
144 | if (shifty > 0) | ||
145 | { | ||
146 | rc <<= shifty; | ||
147 | } | ||
148 | else | ||
149 | { | ||
150 | rc >>= -shifty; | ||
151 | } | ||
152 | |||
153 | if ((x_neg ^ y_neg) == 1) | ||
154 | { | ||
155 | rc = -rc; | ||
156 | } | ||
157 | |||
158 | return rc; | ||
159 | } | ||
160 | |||
161 | #endif /* FP_FAST_MUL_DIV */ | ||
162 | |||
163 | static long fp_exp(long x) | ||
164 | { | ||
165 | long k; | ||
166 | long z; | ||
167 | long R; | ||
168 | long xp; | ||
169 | |||
170 | if (x == 0) | ||
171 | { | ||
172 | return FP_ONE; | ||
173 | } | ||
174 | |||
175 | k = (fp_mul(abs(x), FP_LN2_INV) + FP_HALF) & ~FP_MASK; | ||
176 | |||
177 | if (x < 0) | ||
178 | { | ||
179 | k = -k; | ||
180 | } | ||
181 | |||
182 | x -= fp_mul(k, FP_LN2); | ||
183 | z = fp_mul(x, x); | ||
184 | R = FP_TWO + fp_mul(z, FP_EXP_ZERO + fp_mul(z, FP_EXP_ONE | ||
185 | + fp_mul(z, FP_EXP_TWO))); | ||
186 | xp = FP_ONE + fp_div(fp_mul(FP_TWO, x), R - x); | ||
187 | |||
188 | if (k < 0) | ||
189 | { | ||
190 | k = FP_ONE >> (-k >> FP_BITS); | ||
191 | } | ||
192 | else | ||
193 | { | ||
194 | k = FP_ONE << (k >> FP_BITS); | ||
195 | } | ||
196 | |||
197 | return fp_mul(k, xp); | ||
198 | } | ||
199 | |||
200 | static long fp_exp10(long x) | ||
201 | { | ||
202 | if (x == 0) | ||
203 | { | ||
204 | return FP_ONE; | ||
205 | } | ||
206 | |||
207 | return fp_exp(fp_mul(FP_LN10, x)); | ||
208 | } | ||
209 | |||
210 | static long fp_atof(const char* s, int precision) | ||
211 | { | ||
212 | long int_part = 0; | ||
213 | long int_one = 1 << precision; | ||
214 | long frac_part = 0; | ||
215 | long frac_count = 0; | ||
216 | long frac_max = ((precision * 4) + 12) / 13; | ||
217 | long frac_max_int = 1; | ||
218 | long sign = 1; | ||
219 | bool point = false; | ||
220 | |||
221 | while ((*s != '\0') && isspace(*s)) | ||
222 | { | ||
223 | s++; | ||
224 | } | ||
225 | |||
226 | if (*s == '-') | ||
227 | { | ||
228 | sign = -1; | ||
229 | s++; | ||
230 | } | ||
231 | else if (*s == '+') | ||
232 | { | ||
233 | s++; | ||
234 | } | ||
235 | |||
236 | while (*s != '\0') | ||
237 | { | ||
238 | if (*s == '.') | ||
239 | { | ||
240 | if (point) | ||
241 | { | ||
242 | break; | ||
243 | } | ||
244 | |||
245 | point = true; | ||
246 | } | ||
247 | else if (isdigit(*s)) | ||
248 | { | ||
249 | if (point) | ||
250 | { | ||
251 | if (frac_count < frac_max) | ||
252 | { | ||
253 | frac_part = frac_part * 10 + (*s - '0'); | ||
254 | frac_count++; | ||
255 | frac_max_int *= 10; | ||
256 | } | ||
257 | } | ||
258 | else | ||
259 | { | ||
260 | int_part = int_part * 10 + (*s - '0'); | ||
261 | } | ||
262 | } | ||
263 | else | ||
264 | { | ||
265 | break; | ||
266 | } | ||
267 | |||
268 | s++; | ||
269 | } | ||
270 | |||
271 | while (frac_count < frac_max) | ||
272 | { | ||
273 | frac_part *= 10; | ||
274 | frac_count++; | ||
275 | frac_max_int *= 10; | ||
276 | } | ||
277 | |||
278 | return sign * ((int_part * int_one) | ||
279 | + (((int64_t) frac_part * int_one) / frac_max_int)); | ||
280 | } | ||
281 | |||
282 | static long convert_gain(long gain) | ||
283 | { | ||
284 | if (gain != 0) | ||
285 | { | ||
286 | /* Don't allow unreasonably low or high gain changes. | ||
287 | * Our math code can't handle it properly anyway. :) | ||
288 | */ | ||
289 | if (gain < (-23 * FP_ONE)) | ||
290 | { | ||
291 | gain = -23 * FP_ONE; | ||
292 | } | ||
293 | |||
294 | if (gain > (17 * FP_ONE)) | ||
295 | { | ||
296 | gain = 17 * FP_ONE; | ||
297 | } | ||
298 | |||
299 | gain = fp_exp10(gain / 20) << (24 - FP_BITS); | ||
300 | } | ||
301 | |||
302 | return gain; | ||
303 | } | ||
304 | |||
305 | long get_replaygain(const char* str) | ||
306 | { | ||
307 | long gain = 0; | ||
308 | |||
309 | if (str) | ||
310 | { | ||
311 | gain = fp_atof(str, FP_BITS); | ||
312 | gain = convert_gain(gain); | ||
313 | } | ||
314 | |||
315 | return gain; | ||
316 | } | ||
317 | |||
318 | long get_replaypeak(const char* str) | ||
319 | { | ||
320 | long peak = 0; | ||
321 | |||
322 | if (str) | ||
323 | { | ||
324 | peak = fp_atof(str, 24); | ||
325 | } | ||
326 | |||
327 | return peak; | ||
328 | } | ||