A modern Music Player Daemon based on Rockbox open source high quality audio player
tsiry-sandratraina.com
libadwaita
audio
rust
zig
deno
mpris
rockbox
mpd
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Jens Arnold
11 *
12 * Fixed point library for plugins
13 *
14 * This program is free software; you can redistribute it and/or
15 * modify it under the terms of the GNU General Public License
16 * as published by the Free Software Foundation; either version 2
17 * of the License, or (at your option) any later version.
18 *
19 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
20 * KIND, either express or implied.
21 *
22 ****************************************************************************/
23#include "fixedpoint.h"
24#include <stdlib.h>
25#include <stdbool.h>
26#include <inttypes.h>
27#include <limits.h>
28
29#define ULONG_BITS (sizeof (unsigned long)*CHAR_BIT)
30
31/** TAKEN FROM ORIGINAL fixedpoint.h */
32/* Inverse gain of circular cordic rotation in s0.31 format. */
33static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */
34
35/* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */
36static const unsigned long atan_table[] = {
37 0x1fffffff, /* +0.785398163 (or pi/4) */
38 0x12e4051d, /* +0.463647609 */
39 0x09fb385b, /* +0.244978663 */
40 0x051111d4, /* +0.124354995 */
41 0x028b0d43, /* +0.062418810 */
42 0x0145d7e1, /* +0.031239833 */
43 0x00a2f61e, /* +0.015623729 */
44 0x00517c55, /* +0.007812341 */
45 0x0028be53, /* +0.003906230 */
46 0x00145f2e, /* +0.001953123 */
47 0x000a2f98, /* +0.000976562 */
48 0x000517cc, /* +0.000488281 */
49 0x00028be6, /* +0.000244141 */
50 0x000145f3, /* +0.000122070 */
51 0x0000a2f9, /* +0.000061035 */
52 0x0000517c, /* +0.000030518 */
53 0x000028be, /* +0.000015259 */
54 0x0000145f, /* +0.000007629 */
55 0x00000a2f, /* +0.000003815 */
56 0x00000517, /* +0.000001907 */
57 0x0000028b, /* +0.000000954 */
58 0x00000145, /* +0.000000477 */
59 0x000000a2, /* +0.000000238 */
60 0x00000051, /* +0.000000119 */
61 0x00000028, /* +0.000000060 */
62 0x00000014, /* +0.000000030 */
63 0x0000000a, /* +0.000000015 */
64 0x00000005, /* +0.000000007 */
65 0x00000002, /* +0.000000004 */
66 0x00000001, /* +0.000000002 */
67 0x00000000, /* +0.000000001 */
68 0x00000000, /* +0.000000000 */
69};
70
71/* Precalculated sine and cosine * 16384 (2^14) (fixed point 18.14) */
72static const short sin_table[91] =
73{
74 0, 285, 571, 857, 1142, 1427, 1712, 1996, 2280, 2563,
75 2845, 3126, 3406, 3685, 3963, 4240, 4516, 4790, 5062, 5334,
76 5603, 5871, 6137, 6401, 6663, 6924, 7182, 7438, 7691, 7943,
77 8191, 8438, 8682, 8923, 9161, 9397, 9630, 9860, 10086, 10310,
78 10531, 10748, 10963, 11173, 11381, 11585, 11785, 11982, 12175, 12365,
79 12550, 12732, 12910, 13084, 13254, 13420, 13582, 13740, 13894, 14043,
80 14188, 14329, 14466, 14598, 14725, 14848, 14967, 15081, 15190, 15295,
81 15395, 15491, 15582, 15668, 15749, 15825, 15897, 15964, 16025, 16082,
82 16135, 16182, 16224, 16261, 16294, 16321, 16344, 16361, 16374, 16381,
83 16384
84};
85
86/**
87 * Implements sin and cos using CORDIC rotation.
88 *
89 * @param phase has range from 0 to 0xffffffff, representing 0 and
90 * 2*pi respectively.
91 * @param cos return address for cos
92 * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX,
93 * representing -1 and 1 respectively.
94 */
95long fp_sincos(unsigned long phase, long *cos)
96{
97 int32_t x, x1, y, y1;
98 unsigned long z, z1;
99 int i;
100
101 /* Setup initial vector */
102 x = cordic_circular_gain;
103 y = 0;
104 z = phase;
105
106 /* The phase has to be somewhere between 0..pi for this to work right */
107 if (z < 0xffffffff / 4) {
108 /* z in first quadrant, z += pi/2 to correct */
109 x = -x;
110 z += 0xffffffff / 4;
111 } else if (z < 3 * (0xffffffff / 4)) {
112 /* z in third quadrant, z -= pi/2 to correct */
113 z -= 0xffffffff / 4;
114 } else {
115 /* z in fourth quadrant, z -= 3pi/2 to correct */
116 x = -x;
117 z -= 3 * (0xffffffff / 4);
118 }
119
120 /* Each iteration adds roughly 1-bit of extra precision */
121 for (i = 0; i < 31; i++) {
122 x1 = x >> i;
123 y1 = y >> i;
124 z1 = atan_table[i];
125
126 /* Decided which direction to rotate vector. Pivot point is pi/2 */
127 if (z >= 0xffffffff / 4) {
128 x -= y1;
129 y += x1;
130 z -= z1;
131 } else {
132 x += y1;
133 y -= x1;
134 z += z1;
135 }
136 }
137
138 if (cos)
139 *cos = x;
140
141 return y;
142}
143
144/* Accurate sqrt with only elementary operations.
145 * Snagged from:
146 * http://www.devmaster.net/articles/fixed-point-optimizations/
147 *
148 * Extension to fractions and initial estimate improvement by jethead71
149 */
150long fp_sqrt(long x, unsigned int fracbits)
151{
152 if (x <= 0) {
153 return 0; /* no sqrt(neg), or just sqrt(0) = 0 */
154 }
155
156 unsigned long g = 0;
157 unsigned long e = x;
158
159 int intwidth = ULONG_BITS - fracbits;
160 int bshift = __builtin_clzl(e);
161
162 if (bshift >= intwidth) {
163 bshift = -1;
164 }
165 else {
166 bshift = (intwidth - bshift - 1) >> 1;
167 }
168
169 unsigned long b = 1ul << (bshift + fracbits);
170
171 /* integer part */
172 while (e && bshift >= 0) {
173 unsigned long t = ((g << 1) | b) << bshift--;
174
175 if (e >= t) {
176 g |= b;
177 e -= t;
178 }
179
180 b >>= 1;
181 }
182
183 /* fractional part */
184 while (e && b) {
185 unsigned long t = (g << 1) | b;
186 unsigned long c = e; /* detect carry */
187
188 e <<= 1;
189
190 if (e < c || e >= t) {
191 g |= b;
192 e -= t;
193 }
194
195 b >>= 1;
196 }
197
198#if 0
199 /* round up if the next bit would be a '1' */
200 if (e) {
201 unsigned long c = e; /* detect carry */
202
203 e <<= 1;
204
205 if (e < c || e >= ((g << 1) | 1)) {
206 g++;
207 }
208 }
209#endif
210
211 return g;
212}
213
214/* raise an integer to an integer power */
215long ipow(long x, long y)
216{
217 /* y[k] = bit k of y, 0 or 1; k=0...n; n=|_ lg(y) _|
218 *
219 * x^y = x^(y[0]*2^0 + y[1]*2^1 + ... + y[n]*2^n)
220 * = x^(y[0]*2^0) * x^(y[1]*2^1) * ... * x^(y[n]*2^n)
221 */
222 long a = 1;
223
224 if (y < 0 && x != -1)
225 {
226 a = 0; /* would be < 1 or +inf if x == 0 */
227 }
228 else
229 {
230 while (y)
231 {
232 if (y & 1)
233 a *= x;
234
235 y /= 2;
236 x *= x;
237 }
238 }
239
240 return a;
241}
242
243/**
244 * Fixed point sinus using a lookup table
245 * don't forget to divide the result by 16384 to get the actual sinus value
246 * @param val sinus argument in degree
247 * @return sin(val)*16384
248 */
249long fp14_sin(int val)
250{
251 val = (val+360)%360;
252 if (val < 181)
253 {
254 if (val < 91)/* phase 0-90 degree */
255 return (long)sin_table[val];
256 else/* phase 91-180 degree */
257 return (long)sin_table[180-val];
258 }
259 else
260 {
261 if (val < 271)/* phase 181-270 degree */
262 return -(long)sin_table[val-180];
263 else/* phase 270-359 degree */
264 return -(long)sin_table[360-val];
265 }
266 return 0;
267}
268
269/**
270 * Fixed point cosinus using a lookup table
271 * don't forget to divide the result by 16384 to get the actual cosinus value
272 * @param val sinus argument in degree
273 * @return cos(val)*16384
274 */
275long fp14_cos(int val)
276{
277 val = (val+360)%360;
278 if (val < 181)
279 {
280 if (val < 91)/* phase 0-90 degree */
281 return (long)sin_table[90-val];
282 else/* phase 91-180 degree */
283 return -(long)sin_table[val-90];
284 }
285 else
286 {
287 if (val < 271)/* phase 181-270 degree */
288 return -(long)sin_table[270-val];
289 else/* phase 270-359 degree */
290 return (long)sin_table[val-270];
291 }
292 return 0;
293}
294
295/**
296 * Fixed-point natural log
297 * taken from http://www.quinapalus.com/efunc.html
298 * "The code assumes integers are at least 32 bits long. The (positive)
299 * argument and the result of the function are both expressed as fixed-point
300 * values with 16 fractional bits, although intermediates are kept with 28
301 * bits of precision to avoid loss of accuracy during shifts."
302 */
303long fp16_log(int x)
304{
305 int t;
306 int y = 0xa65af;
307
308 if (x < 0x00008000) x <<=16, y -= 0xb1721;
309 if (x < 0x00800000) x <<= 8, y -= 0x58b91;
310 if (x < 0x08000000) x <<= 4, y -= 0x2c5c8;
311 if (x < 0x20000000) x <<= 2, y -= 0x162e4;
312 if (x < 0x40000000) x <<= 1, y -= 0x0b172;
313 t = x + (x >> 1); if ((t & 0x80000000) == 0) x = t, y -= 0x067cd;
314 t = x + (x >> 2); if ((t & 0x80000000) == 0) x = t, y -= 0x03920;
315 t = x + (x >> 3); if ((t & 0x80000000) == 0) x = t, y -= 0x01e27;
316 t = x + (x >> 4); if ((t & 0x80000000) == 0) x = t, y -= 0x00f85;
317 t = x + (x >> 5); if ((t & 0x80000000) == 0) x = t, y -= 0x007e1;
318 t = x + (x >> 6); if ((t & 0x80000000) == 0) x = t, y -= 0x003f8;
319 t = x + (x >> 7); if ((t & 0x80000000) == 0) x = t, y -= 0x001fe;
320 x = 0x80000000 - x;
321 y -= x >> 15;
322
323 return y;
324}
325
326/**
327 * Fixed-point exponential
328 * taken from http://www.quinapalus.com/efunc.html
329 * "The code assumes integers are at least 32 bits long. The (non-negative)
330 * argument and the result of the function are both expressed as fixed-point
331 * values with 16 fractional bits. Notice that after 11 steps of the
332 * algorithm the constants involved become such that the code is simply
333 * doing a multiplication: this is explained in the note below.
334 * The extension to negative arguments is left as an exercise."
335 */
336long fp16_exp(int x)
337{
338 int t;
339 int y = 0x00010000;
340
341 if (x < 0) x += 0xb1721, y >>= 16;
342 t = x - 0x58b91; if (t >= 0) x = t, y <<= 8;
343 t = x - 0x2c5c8; if (t >= 0) x = t, y <<= 4;
344 t = x - 0x162e4; if (t >= 0) x = t, y <<= 2;
345 t = x - 0x0b172; if (t >= 0) x = t, y <<= 1;
346 t = x - 0x067cd; if (t >= 0) x = t, y += y >> 1;
347 t = x - 0x03920; if (t >= 0) x = t, y += y >> 2;
348 t = x - 0x01e27; if (t >= 0) x = t, y += y >> 3;
349 t = x - 0x00f85; if (t >= 0) x = t, y += y >> 4;
350 t = x - 0x007e1; if (t >= 0) x = t, y += y >> 5;
351 t = x - 0x003f8; if (t >= 0) x = t, y += y >> 6;
352 t = x - 0x001fe; if (t >= 0) x = t, y += y >> 7;
353 y += ((y >> 8) * x) >> 8;
354
355 return y;
356}
357
358/** MODIFIED FROM replaygain.c */
359
360#define FP_MUL_FRAC(x, y) fp_mul(x, y, fracbits)
361#define FP_DIV_FRAC(x, y) fp_div(x, y, fracbits)
362
363/* constants in fixed point format, 28 fractional bits */
364#define FP28_LN2 (186065279L) /* ln(2) */
365#define FP28_LN2_INV (387270501L) /* 1/ln(2) */
366#define FP28_EXP_ZERO (44739243L) /* 1/6 */
367#define FP28_EXP_ONE (-745654L) /* -1/360 */
368#define FP28_EXP_TWO (12428L) /* 1/21600 */
369#define FP28_LN10 (618095479L) /* ln(10) */
370#define FP28_LOG10OF2 (80807124L) /* log10(2) */
371
372#define TOL_BITS 2 /* log calculation tolerance */
373
374
375/* The fpexp10 fixed point math routine is based
376 * on oMathFP by Dan Carter (http://orbisstudios.com).
377 */
378
379/** FIXED POINT EXP10
380 * Return 10^x as FP integer. Argument is FP integer.
381 */
382long fp_exp10(long x, unsigned int fracbits)
383{
384 long k;
385 long z;
386 long R;
387 long xp;
388
389 /* scale constants */
390 const long fp_one = (1 << fracbits);
391 const long fp_half = (1 << (fracbits - 1));
392 const long fp_two = (2 << fracbits);
393 const long fp_mask = (fp_one - 1);
394 const long fp_ln2_inv = (FP28_LN2_INV >> (28 - fracbits));
395 const long fp_ln2 = (FP28_LN2 >> (28 - fracbits));
396 const long fp_ln10 = (FP28_LN10 >> (28 - fracbits));
397 const long fp_exp_zero = (FP28_EXP_ZERO >> (28 - fracbits));
398 const long fp_exp_one = (FP28_EXP_ONE >> (28 - fracbits));
399 const long fp_exp_two = (FP28_EXP_TWO >> (28 - fracbits));
400
401 /* exp(0) = 1 */
402 if (x == 0)
403 {
404 return fp_one;
405 }
406
407 /* convert from base 10 to base e */
408 x = FP_MUL_FRAC(x, fp_ln10);
409
410 /* calculate exp(x) */
411 k = (FP_MUL_FRAC(abs(x), fp_ln2_inv) + fp_half) & ~fp_mask;
412
413 if (x < 0)
414 {
415 k = -k;
416 }
417
418 x -= FP_MUL_FRAC(k, fp_ln2);
419 z = FP_MUL_FRAC(x, x);
420 R = fp_two + FP_MUL_FRAC(z, fp_exp_zero + FP_MUL_FRAC(z, fp_exp_one
421 + FP_MUL_FRAC(z, fp_exp_two)));
422 xp = fp_one + FP_DIV_FRAC(FP_MUL_FRAC(fp_two, x), R - x);
423
424 if (k < 0)
425 {
426 k = fp_one >> (-k >> fracbits);
427 }
428 else
429 {
430 k = fp_one << (k >> fracbits);
431 }
432
433 return FP_MUL_FRAC(k, xp);
434}
435
436/** FIXED POINT LOG10
437 * Return log10(x) as FP integer. Argument is FP integer.
438 */
439long fp_log10(long n, unsigned int fracbits)
440{
441 /* Calculate log2 of argument */
442
443 long log2, frac;
444 const long fp_one = (1 << fracbits);
445 const long fp_two = (2 << fracbits);
446 const long tolerance = (1 << ((fracbits / 2) + 2));
447
448 if (n <=0) return FP_NEGINF;
449 log2 = 0;
450
451 /* integer part */
452 while (n < fp_one)
453 {
454 log2 -= fp_one;
455 n <<= 1;
456 }
457 while (n >= fp_two)
458 {
459 log2 += fp_one;
460 n >>= 1;
461 }
462
463 /* fractional part */
464 frac = fp_one;
465 while (frac > tolerance)
466 {
467 frac >>= 1;
468 n = FP_MUL_FRAC(n, n);
469 if (n >= fp_two)
470 {
471 n >>= 1;
472 log2 += frac;
473 }
474 }
475
476 /* convert log2 to log10 */
477 return FP_MUL_FRAC(log2, (FP28_LOG10OF2 >> (28 - fracbits)));
478}
479
480/** CONVERT FACTOR TO DECIBELS */
481long fp_decibels(unsigned long factor, unsigned int fracbits)
482{
483 /* decibels = 20 * log10(factor) */
484 return FP_MUL_FRAC((20L << fracbits), fp_log10(factor, fracbits));
485}
486
487/** CONVERT DECIBELS TO FACTOR */
488long fp_factor(long decibels, unsigned int fracbits)
489{
490 /* factor = 10 ^ (decibels / 20) */
491 return fp_exp10(FP_DIV_FRAC(decibels, (20L << fracbits)), fracbits);
492}