From 78e228cf84408622d4ce0feadd49bf4d97fa86bc Mon Sep 17 00:00:00 2001 From: mcuelenaere Date: Sun, 5 Jul 2009 18:06:07 +0000 Subject: [PATCH] Take 2 at 'Consolidate all fixed point math routines in one library' (FS#10400) by Jeffrey Goode git-svn-id: svn://svn.rockbox.org/rockbox/trunk@21664 a1c6a512-1295-4272-9138-f99709370657 --- apps/SOURCES | 1 + apps/codecs/adx.c | 123 +---------------- apps/codecs/lib/SOURCES | 2 +- apps/codecs/lib/fixedpoint.c | 1 + apps/codecs/lib/fixedpoint.h | 49 +++++++ apps/codecs/spc.c | 1 + apps/dsp.c | 4 +- apps/dsp.h | 80 ----------- apps/eq.c | 120 ++--------------- apps/eq.h | 1 + apps/{plugins/lib => }/fixedpoint.c | 240 +++++++++++++++++++++++++++++++-- apps/fixedpoint.h | 69 ++++++++++ apps/fracmul.h | 93 +++++++++++++ apps/plugins/bounce.c | 4 +- apps/plugins/bubbles.c | 20 +-- apps/plugins/clock/clock_draw_analog.c | 6 +- apps/plugins/cube.c | 12 +- apps/plugins/lib/fixedpoint.c | 239 +------------------------------- apps/plugins/lib/fixedpoint.h | 45 ++++++- apps/plugins/plasma.c | 2 +- apps/plugins/vu_meter.c | 4 +- apps/replaygain.c | 181 +------------------------ 22 files changed, 529 insertions(+), 768 deletions(-) create mode 100644 apps/codecs/lib/fixedpoint.c create mode 100644 apps/codecs/lib/fixedpoint.h copy apps/{plugins/lib => }/fixedpoint.c (54%) create mode 100644 apps/fixedpoint.h create mode 100644 apps/fracmul.h rewrite apps/plugins/lib/fixedpoint.c (100%) diff --git a/apps/SOURCES b/apps/SOURCES index 747582601..f3acef173 100644 --- a/apps/SOURCES +++ b/apps/SOURCES @@ -125,6 +125,7 @@ recorder/recording.c #if INPUT_SRC_CAPS != 0 audio_path.c #endif /* INPUT_SRC_CAPS != 0 */ +fixedpoint.c pcmbuf.c playback.c codecs.c diff --git a/apps/codecs/adx.c b/apps/codecs/adx.c index cc36f6a90..0e5005475 100644 --- a/apps/codecs/adx.c +++ b/apps/codecs/adx.c @@ -21,6 +21,7 @@ #include "codeclib.h" #include "inttypes.h" #include "math.h" +#include "lib/fixedpoint.h" CODEC_HEADER @@ -41,124 +42,6 @@ const long cutoff = 500; static int16_t samples[WAV_CHUNK_SIZE] IBSS_ATTR; -/* fixed point stuff from apps/plugins/lib/fixedpoint.c */ - -/* Inverse gain of circular cordic rotation in s0.31 format. */ -static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ - -/* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */ -static const unsigned long atan_table[] = { - 0x1fffffff, /* +0.785398163 (or pi/4) */ - 0x12e4051d, /* +0.463647609 */ - 0x09fb385b, /* +0.244978663 */ - 0x051111d4, /* +0.124354995 */ - 0x028b0d43, /* +0.062418810 */ - 0x0145d7e1, /* +0.031239833 */ - 0x00a2f61e, /* +0.015623729 */ - 0x00517c55, /* +0.007812341 */ - 0x0028be53, /* +0.003906230 */ - 0x00145f2e, /* +0.001953123 */ - 0x000a2f98, /* +0.000976562 */ - 0x000517cc, /* +0.000488281 */ - 0x00028be6, /* +0.000244141 */ - 0x000145f3, /* +0.000122070 */ - 0x0000a2f9, /* +0.000061035 */ - 0x0000517c, /* +0.000030518 */ - 0x000028be, /* +0.000015259 */ - 0x0000145f, /* +0.000007629 */ - 0x00000a2f, /* +0.000003815 */ - 0x00000517, /* +0.000001907 */ - 0x0000028b, /* +0.000000954 */ - 0x00000145, /* +0.000000477 */ - 0x000000a2, /* +0.000000238 */ - 0x00000051, /* +0.000000119 */ - 0x00000028, /* +0.000000060 */ - 0x00000014, /* +0.000000030 */ - 0x0000000a, /* +0.000000015 */ - 0x00000005, /* +0.000000007 */ - 0x00000002, /* +0.000000004 */ - 0x00000001, /* +0.000000002 */ - 0x00000000, /* +0.000000001 */ - 0x00000000, /* +0.000000000 */ -}; - -/** - * Implements sin and cos using CORDIC rotation. - * - * @param phase has range from 0 to 0xffffffff, representing 0 and - * 2*pi respectively. - * @param cos return address for cos - * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, - * representing -1 and 1 respectively. - */ -static long fsincos(unsigned long phase, long *cos) -{ - int32_t x, x1, y, y1; - unsigned long z, z1; - int i; - - /* Setup initial vector */ - x = cordic_circular_gain; - y = 0; - z = phase; - - /* The phase has to be somewhere between 0..pi for this to work right */ - if (z < 0xffffffff / 4) { - /* z in first quadrant, z += pi/2 to correct */ - x = -x; - z += 0xffffffff / 4; - } else if (z < 3 * (0xffffffff / 4)) { - /* z in third quadrant, z -= pi/2 to correct */ - z -= 0xffffffff / 4; - } else { - /* z in fourth quadrant, z -= 3pi/2 to correct */ - x = -x; - z -= 3 * (0xffffffff / 4); - } - - /* Each iteration adds roughly 1-bit of extra precision */ - for (i = 0; i < 31; i++) { - x1 = x >> i; - y1 = y >> i; - z1 = atan_table[i]; - - /* Decided which direction to rotate vector. Pivot point is pi/2 */ - if (z >= 0xffffffff / 4) { - x -= y1; - y += x1; - z -= z1; - } else { - x += y1; - y -= x1; - z += z1; - } - } - - if (cos) - *cos = x; - - return y; -} - -/** - * Fixed point square root via Newton-Raphson. - * @param a square root argument. - * @param fracbits specifies number of fractional bits in argument. - * @return Square root of argument in same fixed point format as input. - */ -static long fsqrt(long a, unsigned int fracbits) -{ - long b = a/2 + (1 << fracbits); /* initial approximation */ - unsigned n; - const unsigned iterations = 8; /* bumped up from 4 as it wasn't - nearly enough for 28 fractional bits */ - - for (n = 0; n < iterations; ++n) - b = (b + (long)(((long long)(a) << fracbits)/b))/2; - - return b; -} - /* this is the codec entry point */ enum codec_status codec_main(void) { @@ -238,7 +121,7 @@ next_track: int64_t c; int64_t d; - fsincos((unsigned long)phasemultiple,&z); + fp_sincos((unsigned long)phasemultiple,&z); a = (M_SQRT2*big28)-(z*big28/LONG_MAX); @@ -247,7 +130,7 @@ next_track: * which is sufficient here, but this is the only reason why I don't * use 32 fractional bits everywhere. */ - d = fsqrt((a+b)*(a-b)/big28,28); + d = fp_sqrt((a+b)*(a-b)/big28,28); c = (a-d)*big28/b; coef1 = (c*8192) >> 28; diff --git a/apps/codecs/lib/SOURCES b/apps/codecs/lib/SOURCES index cbb8e6037..0141af21d 100644 --- a/apps/codecs/lib/SOURCES +++ b/apps/codecs/lib/SOURCES @@ -1,6 +1,6 @@ #if CONFIG_CODEC == SWCODEC /* software codec platforms */ codeclib.c - +fixedpoint.c mdct2.c #ifdef CPU_ARM diff --git a/apps/codecs/lib/fixedpoint.c b/apps/codecs/lib/fixedpoint.c new file mode 100644 index 000000000..352e24667 --- /dev/null +++ b/apps/codecs/lib/fixedpoint.c @@ -0,0 +1 @@ +#include "../../fixedpoint.c" diff --git a/apps/codecs/lib/fixedpoint.h b/apps/codecs/lib/fixedpoint.h new file mode 100644 index 000000000..e6ed5208c --- /dev/null +++ b/apps/codecs/lib/fixedpoint.h @@ -0,0 +1,49 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id: fixedpoint.h -1 $ + * + * Copyright (C) 2006 Jens Arnold + * + * Fixed point library for plugins + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ + + /** CODECS - FIXED POINT MATH ROUTINES - USAGE + * + * - x and y arguments are fixed point integers + * - fracbits is the number of fractional bits in the argument(s) + * - functions return long fixed point integers with the specified number + * of fractional bits unless otherwise specified + * + * Calculate sin and cos of an angle: + * fp_sincos(phase, *cos) + * where phase is a 32 bit unsigned integer with 0 representing 0 + * and 0xFFFFFFFF representing 2*pi, and *cos is the address to + * a long signed integer. Value returned is a long signed integer + * from LONG_MIN to LONG_MAX, representing -1 to 1 respectively. + * That is, value is a fixed point integer with 31 fractional bits. + * + * Take square root of a fixed point number: + * fp_sqrt(x, fracbits) + * + */ +#ifndef _FIXEDPOINT_H_CODECS +#define _FIXEDPOINT_H_CODECS + +long fp_sincos(unsigned long phase, long *cos); +long fp_sqrt(long a, unsigned int fracbits); + +#endif diff --git a/apps/codecs/spc.c b/apps/codecs/spc.c index 6ceb704c7..3a06a228e 100644 --- a/apps/codecs/spc.c +++ b/apps/codecs/spc.c @@ -26,6 +26,7 @@ /* DSP Based on Brad Martin's OpenSPC DSP emulator */ /* tag reading from sexyspc by John Brawn (John_Brawn@yahoo.com) and others */ #include "codeclib.h" +#include "../fracmul.h" #include "libspc/spc_codec.h" #include "libspc/spc_profiler.h" diff --git a/apps/dsp.c b/apps/dsp.c index a760865af..30b4ed357 100644 --- a/apps/dsp.c +++ b/apps/dsp.c @@ -33,6 +33,8 @@ #include "misc.h" #include "tdspeed.h" #include "buffer.h" +#include "fixedpoint.h" +#include "fracmul.h" /* 16-bit samples are scaled based on these constants. The shift should be * no more than 15. @@ -841,7 +843,7 @@ void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff) * crossfeed shelf filter and should be removed if crossfeed settings are * ever made incompatible for any other good reason. */ - cutoff = DIV64(cutoff, get_replaygain_int(hf_gain*5), 24); + cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24); filter_shelf_coefs(cutoff, hf_gain, false, c); /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains * over 1 and can do this safely diff --git a/apps/dsp.h b/apps/dsp.h index 8c23c3053..3d24b2424 100644 --- a/apps/dsp.h +++ b/apps/dsp.h @@ -64,86 +64,6 @@ enum { DSP_CALLBACK_SET_STEREO_WIDTH }; -/* A bunch of fixed point assembler helper macros */ -#if defined(CPU_COLDFIRE) -/* These macros use the Coldfire EMAC extension and need the MACSR flags set - * to fractional mode with no rounding. - */ - -/* Multiply two S.31 fractional integers and return the sign bit and the - * 31 most significant bits of the result. - */ -#define FRACMUL(x, y) \ -({ \ - long t; \ - asm ("mac.l %[a], %[b], %%acc0\n\t" \ - "movclr.l %%acc0, %[t]\n\t" \ - : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \ - t; \ -}) - -/* Multiply two S.31 fractional integers, and return the 32 most significant - * bits after a shift left by the constant z. NOTE: Only works for shifts of - * 1 to 8 on Coldfire! - */ -#define FRACMUL_SHL(x, y, z) \ -({ \ - long t, t2; \ - asm ("mac.l %[a], %[b], %%acc0\n\t" \ - "moveq.l %[d], %[t]\n\t" \ - "move.l %%accext01, %[t2]\n\t" \ - "and.l %[mask], %[t2]\n\t" \ - "lsr.l %[t], %[t2]\n\t" \ - "movclr.l %%acc0, %[t]\n\t" \ - "asl.l %[c], %[t]\n\t" \ - "or.l %[t2], %[t]\n\t" \ - : [t] "=&d" (t), [t2] "=&d" (t2) \ - : [a] "r" (x), [b] "r" (y), [mask] "d" (0xff), \ - [c] "i" ((z)), [d] "i" (8 - (z))); \ - t; \ -}) - -#elif defined(CPU_ARM) - -/* Multiply two S.31 fractional integers and return the sign bit and the - * 31 most significant bits of the result. - */ -#define FRACMUL(x, y) \ -({ \ - long t, t2; \ - asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ - "mov %[t2], %[t2], asl #1\n\t" \ - "orr %[t], %[t2], %[t], lsr #31\n\t" \ - : [t] "=&r" (t), [t2] "=&r" (t2) \ - : [a] "r" (x), [b] "r" (y)); \ - t; \ -}) - -/* Multiply two S.31 fractional integers, and return the 32 most significant - * bits after a shift left by the constant z. - */ -#define FRACMUL_SHL(x, y, z) \ -({ \ - long t, t2; \ - asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ - "mov %[t2], %[t2], asl %[c]\n\t" \ - "orr %[t], %[t2], %[t], lsr %[d]\n\t" \ - : [t] "=&r" (t), [t2] "=&r" (t2) \ - : [a] "r" (x), [b] "r" (y), \ - [c] "M" ((z) + 1), [d] "M" (31 - (z))); \ - t; \ -}) - -#else - -#define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 31)) -#define FRACMUL_SHL(x, y, z) \ -((long)(((((long long) (x)) * ((long long) (y))) >> (31 - (z))))) - -#endif - -#define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y)) - struct dsp_config; int dsp_process(struct dsp_config *dsp, char *dest, diff --git a/apps/eq.c b/apps/eq.c index 5977200c9..6437fed90 100644 --- a/apps/eq.c +++ b/apps/eq.c @@ -21,105 +21,11 @@ #include #include "config.h" -#include "dsp.h" +#include "fixedpoint.h" +#include "fracmul.h" #include "eq.h" #include "replaygain.h" -/* Inverse gain of circular cordic rotation in s0.31 format. */ -static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ - -/* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */ -static const unsigned long atan_table[] = { - 0x1fffffff, /* +0.785398163 (or pi/4) */ - 0x12e4051d, /* +0.463647609 */ - 0x09fb385b, /* +0.244978663 */ - 0x051111d4, /* +0.124354995 */ - 0x028b0d43, /* +0.062418810 */ - 0x0145d7e1, /* +0.031239833 */ - 0x00a2f61e, /* +0.015623729 */ - 0x00517c55, /* +0.007812341 */ - 0x0028be53, /* +0.003906230 */ - 0x00145f2e, /* +0.001953123 */ - 0x000a2f98, /* +0.000976562 */ - 0x000517cc, /* +0.000488281 */ - 0x00028be6, /* +0.000244141 */ - 0x000145f3, /* +0.000122070 */ - 0x0000a2f9, /* +0.000061035 */ - 0x0000517c, /* +0.000030518 */ - 0x000028be, /* +0.000015259 */ - 0x0000145f, /* +0.000007629 */ - 0x00000a2f, /* +0.000003815 */ - 0x00000517, /* +0.000001907 */ - 0x0000028b, /* +0.000000954 */ - 0x00000145, /* +0.000000477 */ - 0x000000a2, /* +0.000000238 */ - 0x00000051, /* +0.000000119 */ - 0x00000028, /* +0.000000060 */ - 0x00000014, /* +0.000000030 */ - 0x0000000a, /* +0.000000015 */ - 0x00000005, /* +0.000000007 */ - 0x00000002, /* +0.000000004 */ - 0x00000001, /* +0.000000002 */ - 0x00000000, /* +0.000000001 */ - 0x00000000, /* +0.000000000 */ -}; - -/** - * Implements sin and cos using CORDIC rotation. - * - * @param phase has range from 0 to 0xffffffff, representing 0 and - * 2*pi respectively. - * @param cos return address for cos - * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, - * representing -1 and 1 respectively. - */ -static long fsincos(unsigned long phase, long *cos) { - int32_t x, x1, y, y1; - unsigned long z, z1; - int i; - - /* Setup initial vector */ - x = cordic_circular_gain; - y = 0; - z = phase; - - /* The phase has to be somewhere between 0..pi for this to work right */ - if (z < 0xffffffff / 4) { - /* z in first quadrant, z += pi/2 to correct */ - x = -x; - z += 0xffffffff / 4; - } else if (z < 3 * (0xffffffff / 4)) { - /* z in third quadrant, z -= pi/2 to correct */ - z -= 0xffffffff / 4; - } else { - /* z in fourth quadrant, z -= 3pi/2 to correct */ - x = -x; - z -= 3 * (0xffffffff / 4); - } - - /* Each iteration adds roughly 1-bit of extra precision */ - for (i = 0; i < 31; i++) { - x1 = x >> i; - y1 = y >> i; - z1 = atan_table[i]; - - /* Decided which direction to rotate vector. Pivot point is pi/2 */ - if (z >= 0xffffffff / 4) { - x -= y1; - y += x1; - z -= z1; - } else { - x += y1; - y -= x1; - z += z1; - } - } - - *cos = x; - - return y; -} - /** * Calculate first order shelving filter. Filter is not directly usable by the * eq_filter() function. @@ -135,16 +41,16 @@ void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c) int32_t b0, b1, a0, a1; /* s3.28 */ const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */ - sin = fsincos(cutoff/2, &cos); + sin = fp_sincos(cutoff/2, &cos); if (low) { - const int32_t sin_div_g = DIV64(sin, g, 25); + const int32_t sin_div_g = fp_div(sin, g, 25); cos >>= 3; b0 = FRACMUL(sin, g) + cos; /* 0.25 .. 4.10 */ b1 = FRACMUL(sin, g) - cos; /* -1 .. 3.98 */ a0 = sin_div_g + cos; /* 0.25 .. 4.10 */ a1 = sin_div_g - cos; /* -1 .. 3.98 */ } else { - const int32_t cos_div_g = DIV64(cos, g, 25); + const int32_t cos_div_g = fp_div(cos, g, 25); sin >>= 3; b0 = sin + FRACMUL(cos, g); /* 0.25 .. 4.10 */ b1 = sin - FRACMUL(cos, g); /* -3.98 .. 1 */ @@ -152,7 +58,7 @@ void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c) a1 = sin - cos_div_g; /* -3.98 .. 1 */ } - const int32_t rcp_a0 = DIV64(1, a0, 57); /* 0.24 .. 3.98, s2.29 */ + const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */ *c++ = FRACMUL_SHL(b0, rcp_a0, 1); /* 0.063 .. 15.85 */ *c++ = FRACMUL_SHL(b1, rcp_a0, 1); /* -15.85 .. 15.85 */ *c++ = -FRACMUL_SHL(a1, rcp_a0, 1); /* -1 .. 1 */ @@ -220,10 +126,10 @@ void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) long cs; const long one = 1 << 28; /* s3.28 */ const long A = get_replaygain_int(db*5) << 5; /* 10^(db/40), s2.29 */ - const long alpha = fsincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ + const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ int32_t a0, a1, a2; /* these are all s3.28 format */ int32_t b0, b1, b2; - const long alphadivA = DIV64(alpha, A, 27); + const long alphadivA = fp_div(alpha, A, 27); /* possible numerical ranges are in comments by each coef */ b0 = one + FRACMUL(alpha, A); /* [1 .. 5] */ @@ -233,7 +139,7 @@ void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) a2 = one - alphadivA; /* [-3 .. 1] */ /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */ - const long rcp_a0 = DIV64(1, a0, 59); /* s0.31 */ + const long rcp_a0 = fp_div(1, a0, 59); /* s0.31 */ *c++ = FRACMUL(b0, rcp_a0); /* [0.25 .. 4] */ *c++ = FRACMUL(b1, rcp_a0); /* [-2 .. 2] */ *c++ = FRACMUL(b2, rcp_a0); /* [-2.4 .. 1] */ @@ -251,7 +157,7 @@ void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) const long one = 1 << 25; /* s6.25 */ const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ - const long alpha = fsincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ + const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ const long ap1 = (A >> 4) + one; const long am1 = (A >> 4) - one; const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); @@ -272,7 +178,7 @@ void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha; /* [0.1 .. 1.99] */ - const long rcp_a0 = DIV64(1, a0, 55); /* s1.30 */ + const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0.06 .. 15.9] */ *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-2 .. 31.7] */ *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 15.9] */ @@ -290,7 +196,7 @@ void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) const long one = 1 << 25; /* s6.25 */ const long sqrtA = get_replaygain_int(db*5/2) << 2; /* 10^(db/80), s5.26 */ const long A = FRACMUL_SHL(sqrtA, sqrtA, 8); /* s2.29 */ - const long alpha = fsincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ + const long alpha = fp_sincos(cutoff, &cs)/(2*Q)*10 >> 1; /* s1.30 */ const long ap1 = (A >> 4) + one; const long am1 = (A >> 4) - one; const long twosqrtalpha = 2*FRACMUL(sqrtA, alpha); @@ -311,7 +217,7 @@ void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha; /* [0.1 .. 1.99] */ - const long rcp_a0 = DIV64(1, a0, 55); /* s1.30 */ + const long rcp_a0 = fp_div(1, a0, 55); /* s1.30 */ *c++ = FRACMUL_SHL(b0, rcp_a0, 2); /* [0 .. 16] */ *c++ = FRACMUL_SHL(b1, rcp_a0, 2); /* [-31.7 .. 2] */ *c++ = FRACMUL_SHL(b2, rcp_a0, 2); /* [0 .. 16] */ diff --git a/apps/eq.h b/apps/eq.h index 1c3efe50e..a44e9153a 100644 --- a/apps/eq.h +++ b/apps/eq.h @@ -23,6 +23,7 @@ #define _EQ_H #include +#include /* These depend on the fixed point formats used by the different filter types and need to be changed when they change. diff --git a/apps/plugins/lib/fixedpoint.c b/apps/fixedpoint.c similarity index 54% copy from apps/plugins/lib/fixedpoint.c copy to apps/fixedpoint.c index 0ae2cded6..917f62425 100644 --- a/apps/plugins/lib/fixedpoint.c +++ b/apps/fixedpoint.c @@ -5,7 +5,7 @@ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ - * $Id$ + * $Id: fixedpoint.c -1 $ * * Copyright (C) 2006 Jens Arnold * @@ -21,10 +21,16 @@ * ****************************************************************************/ -#include -#include "plugin.h" #include "fixedpoint.h" +#include +#include +#include + +#ifndef BIT_N +#define BIT_N(n) (1U << (n)) +#endif +/** TAKEN FROM ORIGINAL fixedpoint.h */ /* Inverse gain of circular cordic rotation in s0.31 format. */ static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ @@ -88,7 +94,7 @@ static const short sin_table[91] = * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, * representing -1 and 1 respectively. */ -long fsincos(unsigned long phase, long *cos) +long fp_sincos(unsigned long phase, long *cos) { int32_t x, x1, y, y1; unsigned long z, z1; @@ -137,31 +143,45 @@ long fsincos(unsigned long phase, long *cos) return y; } + +#if defined(PLUGIN) || defined(CODEC) /** * Fixed point square root via Newton-Raphson. - * @param a square root argument. + * @param x square root argument. * @param fracbits specifies number of fractional bits in argument. - * @return Square root of argument in same fixed point format as input. + * @return Square root of argument in same fixed point format as input. + * + * This routine has been modified to run longer for greater precision, + * but cuts calculation short if the answer is reached sooner. In + * general, the closer x is to 1, the quicker the calculation. */ -long fsqrt(long a, unsigned int fracbits) +long fp_sqrt(long x, unsigned int fracbits) { - long b = a/2 + BIT_N(fracbits); /* initial approximation */ + long b = x/2 + BIT_N(fracbits); /* initial approximation */ + long c; unsigned n; - const unsigned iterations = 4; + const unsigned iterations = 8; for (n = 0; n < iterations; ++n) - b = (b + (long)(((long long)(a) << fracbits)/b))/2; + { + c = fp_div(x, b, fracbits); + if (c == b) break; + b = (b + c)/2; + } return b; } +#endif /* PLUGIN or CODEC */ + +#if defined(PLUGIN) /** * Fixed point sinus using a lookup table * don't forget to divide the result by 16384 to get the actual sinus value * @param val sinus argument in degree * @return sin(val)*16384 */ -long sin_int(int val) +long fp14_sin(int val) { val = (val+360)%360; if (val < 181) @@ -187,7 +207,7 @@ long sin_int(int val) * @param val sinus argument in degree * @return cos(val)*16384 */ -long cos_int(int val) +long fp14_cos(int val) { val = (val+360)%360; if (val < 181) @@ -216,7 +236,7 @@ long cos_int(int val) * bits of precision to avoid loss of accuracy during shifts." */ -long flog(int x) { +long fp16_log(int x) { long t,y; y=0xa65af; @@ -236,3 +256,197 @@ long flog(int x) { y-=x>>15; return y; } +#endif /* PLUGIN */ + + +#if (!defined(PLUGIN) && !defined(CODEC)) +/** MODIFIED FROM replaygain.c */ +/* These math routines have 64-bit internal precision to avoid overflows. + * Arguments and return values are 32-bit (long) precision. + */ + +#define FP_MUL64(x, y) (((x) * (y)) >> (fracbits)) +#define FP_DIV64(x, y) (((x) << (fracbits)) / (y)) + +static long long fp_exp10(long long x, unsigned int fracbits); +/* static long long fp_log10(long long n, unsigned int fracbits); */ + +/* constants in fixed point format, 28 fractional bits */ +#define FP28_LN2 (186065279LL) /* ln(2) */ +#define FP28_LN2_INV (387270501LL) /* 1/ln(2) */ +#define FP28_EXP_ZERO (44739243LL) /* 1/6 */ +#define FP28_EXP_ONE (-745654LL) /* -1/360 */ +#define FP28_EXP_TWO (12428LL) /* 1/21600 */ +#define FP28_LN10 (618095479LL) /* ln(10) */ +#define FP28_LOG10OF2 (80807124LL) /* log10(2) */ + +#define TOL_BITS 2 /* log calculation tolerance */ + + +/* The fpexp10 fixed point math routine is based + * on oMathFP by Dan Carter (http://orbisstudios.com). + */ + +/** FIXED POINT EXP10 + * Return 10^x as FP integer. Argument is FP integer. + */ +static long long fp_exp10(long long x, unsigned int fracbits) +{ + long long k; + long long z; + long long R; + long long xp; + + /* scale constants */ + const long long fp_one = (1 << fracbits); + const long long fp_half = (1 << (fracbits - 1)); + const long long fp_two = (2 << fracbits); + const long long fp_mask = (fp_one - 1); + const long long fp_ln2_inv = (FP28_LN2_INV >> (28 - fracbits)); + const long long fp_ln2 = (FP28_LN2 >> (28 - fracbits)); + const long long fp_ln10 = (FP28_LN10 >> (28 - fracbits)); + const long long fp_exp_zero = (FP28_EXP_ZERO >> (28 - fracbits)); + const long long fp_exp_one = (FP28_EXP_ONE >> (28 - fracbits)); + const long long fp_exp_two = (FP28_EXP_TWO >> (28 - fracbits)); + + /* exp(0) = 1 */ + if (x == 0) + { + return fp_one; + } + + /* convert from base 10 to base e */ + x = FP_MUL64(x, fp_ln10); + + /* calculate exp(x) */ + k = (FP_MUL64(abs(x), fp_ln2_inv) + fp_half) & ~fp_mask; + + if (x < 0) + { + k = -k; + } + + x -= FP_MUL64(k, fp_ln2); + z = FP_MUL64(x, x); + R = fp_two + FP_MUL64(z, fp_exp_zero + FP_MUL64(z, fp_exp_one + + FP_MUL64(z, fp_exp_two))); + xp = fp_one + FP_DIV64(FP_MUL64(fp_two, x), R - x); + + if (k < 0) + { + k = fp_one >> (-k >> fracbits); + } + else + { + k = fp_one << (k >> fracbits); + } + + return FP_MUL64(k, xp); +} + + +#if 0 /* useful code, but not currently used */ +/** FIXED POINT LOG10 + * Return log10(x) as FP integer. Argument is FP integer. + */ +static long long fp_log10(long long n, unsigned int fracbits) +{ + /* Calculate log2 of argument */ + + long long log2, frac; + const long long fp_one = (1 << fracbits); + const long long fp_two = (2 << fracbits); + const long tolerance = (1 << ((fracbits / 2) + 2)); + + if (n <=0) return FP_NEGINF; + log2 = 0; + + /* integer part */ + while (n < fp_one) + { + log2 -= fp_one; + n <<= 1; + } + while (n >= fp_two) + { + log2 += fp_one; + n >>= 1; + } + + /* fractional part */ + frac = fp_one; + while (frac > tolerance) + { + frac >>= 1; + n = FP_MUL64(n, n); + if (n >= fp_two) + { + n >>= 1; + log2 += frac; + } + } + + /* convert log2 to log10 */ + return FP_MUL64(log2, (FP28_LOG10OF2 >> (28 - fracbits))); +} + + +/** CONVERT FACTOR TO DECIBELS */ +long fp_decibels(unsigned long factor, unsigned int fracbits) +{ + long long decibels; + long long f = (long long)factor; + bool neg; + + /* keep factor in signed long range */ + if (f >= (1LL << 31)) + f = (1LL << 31) - 1; + + /* decibels = 20 * log10(factor) */ + decibels = FP_MUL64((20LL << fracbits), fp_log10(f, fracbits)); + + /* keep result in signed long range */ + if ((neg = (decibels < 0))) + decibels = -decibels; + if (decibels >= (1LL << 31)) + return neg ? FP_NEGINF : FP_INF; + + return neg ? (long)-decibels : (long)decibels; +} +#endif /* unused code */ + + +/** CONVERT DECIBELS TO FACTOR */ +long fp_factor(long decibels, unsigned int fracbits) +{ + bool neg; + long long factor; + long long db = (long long)decibels; + + /* if decibels is 0, factor is 1 */ + if (db == 0) + return (1L << fracbits); + + /* calculate for positive decibels only */ + if ((neg = (db < 0))) + db = -db; + + /* factor = 10 ^ (decibels / 20) */ + factor = fp_exp10(FP_DIV64(db, (20LL << fracbits)), fracbits); + + /* keep result in signed long range, return 0 if very small */ + if (factor >= (1LL << 31)) + { + if (neg) + return 0; + else + return FP_INF; + } + + /* if negative argument, factor is 1 / result */ + if (neg) + factor = FP_DIV64((1LL << fracbits), factor); + + return (long)factor; +} +#endif /* !PLUGIN and !CODEC */ diff --git a/apps/fixedpoint.h b/apps/fixedpoint.h new file mode 100644 index 000000000..49292f341 --- /dev/null +++ b/apps/fixedpoint.h @@ -0,0 +1,69 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id: fixedpoint.h -1 $ + * + * Copyright (C) 2006 Jens Arnold + * + * Fixed point library for plugins + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ + +/** APPS - FIXED POINT MATH ROUTINES - USAGE + * + * - x and y arguments are fixed point integers + * - fracbits is the number of fractional bits in the argument(s) + * - functions return long fixed point integers with the specified number + * of fractional bits unless otherwise specified + * + * Multiply two fixed point numbers: + * fp_mul(x, y, fracbits) + * + * Divide two fixed point numbers: + * fp_div(x, y, fracbits) + * + * Calculate decibel equivalent of a gain factor: + * fp_decibels(factor, fracbits) + * where fracbits is in the range 12 to 22 (higher is better), + * and factor is a positive fixed point integer. + * + * Calculate factor equivalent of a decibel value: + * fp_factor(decibels, fracbits) + * where fracbits is in the range 12 to 22 (lower is better), + * and decibels is a fixed point integer. + */ + +#ifndef _FIXEDPOINT_H_APPS +#define _FIXEDPOINT_H_APPS + +#define fp_mul(x, y, z) (long)((((long long)(x)) * ((long long)(y))) >> (z)) +#define fp_div(x, y, z) (long)((((long long)(x)) << (z)) / ((long long)(y))) + + +/** TAKEN FROM ORIGINAL fixedpoint.h */ +long fp_sincos(unsigned long phase, long *cos); + + +/** MODIFIED FROM replaygain.c */ +#define FP_INF (0x7fffffff) +#define FP_NEGINF -(0x7fffffff) + +/* fracbits in range 12 - 22 work well. Higher is better for + * calculating dB, lower is better for calculating ratio. + */ +/* long fp_decibels(unsigned long factor, unsigned int fracbits); */ +long fp_factor(long decibels, unsigned int fracbits); + +#endif diff --git a/apps/fracmul.h b/apps/fracmul.h new file mode 100644 index 000000000..5cc83af62 --- /dev/null +++ b/apps/fracmul.h @@ -0,0 +1,93 @@ +#ifndef _FRACMUL_H +#define _FRACMUL_H + +/** FRACTIONAL MULTIPLICATION - TAKEN FROM apps/dsp.h + * Multiply two fixed point numbers with 31 fractional bits: + * FRACMUL(x, y) + * + * Multiply two fixed point numbers with 31 fractional bits, + * then shift left by z bits: + * FRACMUL_SHL(x, y, z) + * NOTE: z must be in the range 1-8 on Coldfire targets. + */ + + +/* A bunch of fixed point assembler helper macros */ +#if defined(CPU_COLDFIRE) +/* These macros use the Coldfire EMAC extension and need the MACSR flags set + * to fractional mode with no rounding. + */ + +/* Multiply two S.31 fractional integers and return the sign bit and the + * 31 most significant bits of the result. + */ +#define FRACMUL(x, y) \ +({ \ + long t; \ + asm ("mac.l %[a], %[b], %%acc0\n\t" \ + "movclr.l %%acc0, %[t]\n\t" \ + : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \ + t; \ +}) + +/* Multiply two S.31 fractional integers, and return the 32 most significant + * bits after a shift left by the constant z. NOTE: Only works for shifts of + * 1 to 8 on Coldfire! + */ +#define FRACMUL_SHL(x, y, z) \ +({ \ + long t, t2; \ + asm ("mac.l %[a], %[b], %%acc0\n\t" \ + "moveq.l %[d], %[t]\n\t" \ + "move.l %%accext01, %[t2]\n\t" \ + "and.l %[mask], %[t2]\n\t" \ + "lsr.l %[t], %[t2]\n\t" \ + "movclr.l %%acc0, %[t]\n\t" \ + "asl.l %[c], %[t]\n\t" \ + "or.l %[t2], %[t]\n\t" \ + : [t] "=&d" (t), [t2] "=&d" (t2) \ + : [a] "r" (x), [b] "r" (y), [mask] "d" (0xff), \ + [c] "i" ((z)), [d] "i" (8 - (z))); \ + t; \ +}) + +#elif defined(CPU_ARM) + +/* Multiply two S.31 fractional integers and return the sign bit and the + * 31 most significant bits of the result. + */ +#define FRACMUL(x, y) \ +({ \ + long t, t2; \ + asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ + "mov %[t2], %[t2], asl #1\n\t" \ + "orr %[t], %[t2], %[t], lsr #31\n\t" \ + : [t] "=&r" (t), [t2] "=&r" (t2) \ + : [a] "r" (x), [b] "r" (y)); \ + t; \ +}) + +/* Multiply two S.31 fractional integers, and return the 32 most significant + * bits after a shift left by the constant z. + */ +#define FRACMUL_SHL(x, y, z) \ +({ \ + long t, t2; \ + asm ("smull %[t], %[t2], %[a], %[b]\n\t" \ + "mov %[t2], %[t2], asl %[c]\n\t" \ + "orr %[t], %[t2], %[t], lsr %[d]\n\t" \ + : [t] "=&r" (t), [t2] "=&r" (t2) \ + : [a] "r" (x), [b] "r" (y), \ + [c] "M" ((z) + 1), [d] "M" (31 - (z))); \ + t; \ +}) + +#else + +#define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 31)) +#define FRACMUL_SHL(x, y, z) \ +((long)(((((long long) (x)) * ((long long) (y))) >> (31 - (z))))) + +#endif + +#endif diff --git a/apps/plugins/bounce.c b/apps/plugins/bounce.c index ee4c3e443..14bc7dea9 100644 --- a/apps/plugins/bounce.c +++ b/apps/plugins/bounce.c @@ -344,7 +344,7 @@ static void init_tables(void) phase = pfrac = 0; for (i = 0; i < TABLE_SIZE; i++) { - sin = fsincos(phase, NULL); + sin = fp_sincos(phase, NULL); xtable[i] = RADIUS_X + sin / DIV_X; ytable[i] = RADIUS_Y + sin / DIV_Y; @@ -411,7 +411,7 @@ static void init_clock(void) phase = pfrac = 0; for (i = 0; i < 60; i++) { - sin = fsincos(phase, &cos); + sin = fp_sincos(phase, &cos); xminute[i] = LCD_WIDTH/2 + sin / DIV_MX; yminute[i] = LCD_HEIGHT/2 - cos / DIV_MY; xhour[i] = LCD_WIDTH/2 + sin / DIV_HX; diff --git a/apps/plugins/bubbles.c b/apps/plugins/bubbles.c index 4146b45b3..44d172c4e 100644 --- a/apps/plugins/bubbles.c +++ b/apps/plugins/bubbles.c @@ -1469,17 +1469,17 @@ static void bubbles_drawboard(struct game_context* bb) { ROW_HEIGHT*(BB_HEIGHT-2)+BUBBLE_HEIGHT); /* draw arrow */ - tipx = SHOTX+BUBBLE_WIDTH/2+(((sin_int(bb->angle)>>4)*BUBBLE_WIDTH*3/2)>>10); - tipy = SHOTY+BUBBLE_HEIGHT/2-(((cos_int(bb->angle)>>4)*BUBBLE_HEIGHT*3/2)>>10); + tipx = SHOTX+BUBBLE_WIDTH/2+(((fp14_sin(bb->angle)>>4)*BUBBLE_WIDTH*3/2)>>10); + tipy = SHOTY+BUBBLE_HEIGHT/2-(((fp14_cos(bb->angle)>>4)*BUBBLE_HEIGHT*3/2)>>10); - rb->lcd_drawline(SHOTX+BUBBLE_WIDTH/2+(((sin_int(bb->angle)>>4)*BUBBLE_WIDTH/2)>>10), - SHOTY+BUBBLE_HEIGHT/2-(((cos_int(bb->angle)>>4)*BUBBLE_HEIGHT/2)>>10), + rb->lcd_drawline(SHOTX+BUBBLE_WIDTH/2+(((fp14_sin(bb->angle)>>4)*BUBBLE_WIDTH/2)>>10), + SHOTY+BUBBLE_HEIGHT/2-(((fp14_cos(bb->angle)>>4)*BUBBLE_HEIGHT/2)>>10), tipx, tipy); xlcd_filltriangle(tipx, tipy, - tipx+(((sin_int(bb->angle-135)>>4)*BUBBLE_WIDTH/3)>>10), - tipy-(((cos_int(bb->angle-135)>>4)*BUBBLE_HEIGHT/3)>>10), - tipx+(((sin_int(bb->angle+135)>>4)*BUBBLE_WIDTH/3)>>10), - tipy-(((cos_int(bb->angle+135)>>4)*BUBBLE_HEIGHT/3)>>10)); + tipx+(((fp14_sin(bb->angle-135)>>4)*BUBBLE_WIDTH/3)>>10), + tipy-(((fp14_cos(bb->angle-135)>>4)*BUBBLE_HEIGHT/3)>>10), + tipx+(((fp14_sin(bb->angle+135)>>4)*BUBBLE_WIDTH/3)>>10), + tipy-(((fp14_cos(bb->angle+135)>>4)*BUBBLE_HEIGHT/3)>>10)); /* draw text */ rb->lcd_getstringsize(level, &w, &h); @@ -1524,8 +1524,8 @@ static int bubbles_fire(struct game_context* bb) { /* get current bubble */ bubblecur = bb->queue[bb->nextinq]; - shotxinc = ((sin_int(bb->angle)>>4)*BUBBLE_WIDTH)/3; - shotyinc = ((-1*(cos_int(bb->angle)>>4))*BUBBLE_HEIGHT)/3; + shotxinc = ((fp14_sin(bb->angle)>>4)*BUBBLE_WIDTH)/3; + shotyinc = ((-1*(fp14_cos(bb->angle)>>4))*BUBBLE_HEIGHT)/3; shotxofs = shotyofs = 0; /* advance the queue */ diff --git a/apps/plugins/clock/clock_draw_analog.c b/apps/plugins/clock/clock_draw_analog.c index c41ec3b24..9efe3623a 100644 --- a/apps/plugins/clock/clock_draw_analog.c +++ b/apps/plugins/clock/clock_draw_analog.c @@ -41,11 +41,11 @@ void polar_to_cartesian(int a, int r, int* x, int* y) { #if CONFIG_LCD == LCD_SSD1815 /* Correct non-square pixel aspect of archos recorder LCD */ - *x = (sin_int(a) * 5 / 4 * r) >> 14; + *x = (fp14_sin(a) * 5 / 4 * r) >> 14; #else - *x = (sin_int(a) * r) >> 14; + *x = (fp14_sin(a) * r) >> 14; #endif - *y = (sin_int(a-90) * r) >> 14; + *y = (fp14_sin(a-90) * r) >> 14; } void polar_to_cartesian_screen_centered(struct screen * display, diff --git a/apps/plugins/cube.c b/apps/plugins/cube.c index 55219e5a5..c77021470 100644 --- a/apps/plugins/cube.c +++ b/apps/plugins/cube.c @@ -433,12 +433,12 @@ static void cube_rotate(int xa, int ya, int za) /* Just to prevent unnecessary lookups */ long sxa, cxa, sya, cya, sza, cza; - sxa = sin_int(xa); - cxa = cos_int(xa); - sya = sin_int(ya); - cya = cos_int(ya); - sza = sin_int(za); - cza = cos_int(za); + sxa = fp14_sin(xa); + cxa = fp14_cos(xa); + sya = fp14_sin(ya); + cya = fp14_cos(ya); + sza = fp14_sin(za); + cza = fp14_cos(za); /* calculate overall translation matrix */ matrice[0][0] = (cza * cya) >> 14; diff --git a/apps/plugins/lib/fixedpoint.c b/apps/plugins/lib/fixedpoint.c dissimilarity index 100% index 0ae2cded6..352e24667 100644 --- a/apps/plugins/lib/fixedpoint.c +++ b/apps/plugins/lib/fixedpoint.c @@ -1,238 +1 @@ -/*************************************************************************** - * __________ __ ___. - * Open \______ \ ____ ____ | | _\_ |__ _______ ___ - * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / - * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < - * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ - * \/ \/ \/ \/ \/ - * $Id$ - * - * Copyright (C) 2006 Jens Arnold - * - * Fixed point library for plugins - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY - * KIND, either express or implied. - * - ****************************************************************************/ - -#include -#include "plugin.h" -#include "fixedpoint.h" - -/* Inverse gain of circular cordic rotation in s0.31 format. */ -static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ - -/* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */ -static const unsigned long atan_table[] = { - 0x1fffffff, /* +0.785398163 (or pi/4) */ - 0x12e4051d, /* +0.463647609 */ - 0x09fb385b, /* +0.244978663 */ - 0x051111d4, /* +0.124354995 */ - 0x028b0d43, /* +0.062418810 */ - 0x0145d7e1, /* +0.031239833 */ - 0x00a2f61e, /* +0.015623729 */ - 0x00517c55, /* +0.007812341 */ - 0x0028be53, /* +0.003906230 */ - 0x00145f2e, /* +0.001953123 */ - 0x000a2f98, /* +0.000976562 */ - 0x000517cc, /* +0.000488281 */ - 0x00028be6, /* +0.000244141 */ - 0x000145f3, /* +0.000122070 */ - 0x0000a2f9, /* +0.000061035 */ - 0x0000517c, /* +0.000030518 */ - 0x000028be, /* +0.000015259 */ - 0x0000145f, /* +0.000007629 */ - 0x00000a2f, /* +0.000003815 */ - 0x00000517, /* +0.000001907 */ - 0x0000028b, /* +0.000000954 */ - 0x00000145, /* +0.000000477 */ - 0x000000a2, /* +0.000000238 */ - 0x00000051, /* +0.000000119 */ - 0x00000028, /* +0.000000060 */ - 0x00000014, /* +0.000000030 */ - 0x0000000a, /* +0.000000015 */ - 0x00000005, /* +0.000000007 */ - 0x00000002, /* +0.000000004 */ - 0x00000001, /* +0.000000002 */ - 0x00000000, /* +0.000000001 */ - 0x00000000, /* +0.000000000 */ -}; - -/* Precalculated sine and cosine * 16384 (2^14) (fixed point 18.14) */ -static const short sin_table[91] = -{ - 0, 285, 571, 857, 1142, 1427, 1712, 1996, 2280, 2563, - 2845, 3126, 3406, 3685, 3963, 4240, 4516, 4790, 5062, 5334, - 5603, 5871, 6137, 6401, 6663, 6924, 7182, 7438, 7691, 7943, - 8191, 8438, 8682, 8923, 9161, 9397, 9630, 9860, 10086, 10310, - 10531, 10748, 10963, 11173, 11381, 11585, 11785, 11982, 12175, 12365, - 12550, 12732, 12910, 13084, 13254, 13420, 13582, 13740, 13894, 14043, - 14188, 14329, 14466, 14598, 14725, 14848, 14967, 15081, 15190, 15295, - 15395, 15491, 15582, 15668, 15749, 15825, 15897, 15964, 16025, 16082, - 16135, 16182, 16224, 16261, 16294, 16321, 16344, 16361, 16374, 16381, - 16384 -}; - -/** - * Implements sin and cos using CORDIC rotation. - * - * @param phase has range from 0 to 0xffffffff, representing 0 and - * 2*pi respectively. - * @param cos return address for cos - * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, - * representing -1 and 1 respectively. - */ -long fsincos(unsigned long phase, long *cos) -{ - int32_t x, x1, y, y1; - unsigned long z, z1; - int i; - - /* Setup initial vector */ - x = cordic_circular_gain; - y = 0; - z = phase; - - /* The phase has to be somewhere between 0..pi for this to work right */ - if (z < 0xffffffff / 4) { - /* z in first quadrant, z += pi/2 to correct */ - x = -x; - z += 0xffffffff / 4; - } else if (z < 3 * (0xffffffff / 4)) { - /* z in third quadrant, z -= pi/2 to correct */ - z -= 0xffffffff / 4; - } else { - /* z in fourth quadrant, z -= 3pi/2 to correct */ - x = -x; - z -= 3 * (0xffffffff / 4); - } - - /* Each iteration adds roughly 1-bit of extra precision */ - for (i = 0; i < 31; i++) { - x1 = x >> i; - y1 = y >> i; - z1 = atan_table[i]; - - /* Decided which direction to rotate vector. Pivot point is pi/2 */ - if (z >= 0xffffffff / 4) { - x -= y1; - y += x1; - z -= z1; - } else { - x += y1; - y -= x1; - z += z1; - } - } - - if (cos) - *cos = x; - - return y; -} - -/** - * Fixed point square root via Newton-Raphson. - * @param a square root argument. - * @param fracbits specifies number of fractional bits in argument. - * @return Square root of argument in same fixed point format as input. - */ -long fsqrt(long a, unsigned int fracbits) -{ - long b = a/2 + BIT_N(fracbits); /* initial approximation */ - unsigned n; - const unsigned iterations = 4; - - for (n = 0; n < iterations; ++n) - b = (b + (long)(((long long)(a) << fracbits)/b))/2; - - return b; -} - -/** - * Fixed point sinus using a lookup table - * don't forget to divide the result by 16384 to get the actual sinus value - * @param val sinus argument in degree - * @return sin(val)*16384 - */ -long sin_int(int val) -{ - val = (val+360)%360; - if (val < 181) - { - if (val < 91)/* phase 0-90 degree */ - return (long)sin_table[val]; - else/* phase 91-180 degree */ - return (long)sin_table[180-val]; - } - else - { - if (val < 271)/* phase 181-270 degree */ - return -(long)sin_table[val-180]; - else/* phase 270-359 degree */ - return -(long)sin_table[360-val]; - } - return 0; -} - -/** - * Fixed point cosinus using a lookup table - * don't forget to divide the result by 16384 to get the actual cosinus value - * @param val sinus argument in degree - * @return cos(val)*16384 - */ -long cos_int(int val) -{ - val = (val+360)%360; - if (val < 181) - { - if (val < 91)/* phase 0-90 degree */ - return (long)sin_table[90-val]; - else/* phase 91-180 degree */ - return -(long)sin_table[val-90]; - } - else - { - if (val < 271)/* phase 181-270 degree */ - return -(long)sin_table[270-val]; - else/* phase 270-359 degree */ - return (long)sin_table[val-270]; - } - return 0; -} - -/** - * Fixed-point natural log - * taken from http://www.quinapalus.com/efunc.html - * "The code assumes integers are at least 32 bits long. The (positive) - * argument and the result of the function are both expressed as fixed-point - * values with 16 fractional bits, although intermediates are kept with 28 - * bits of precision to avoid loss of accuracy during shifts." - */ - -long flog(int x) { - long t,y; - - y=0xa65af; - if(x<0x00008000) x<<=16, y-=0xb1721; - if(x<0x00800000) x<<= 8, y-=0x58b91; - if(x<0x08000000) x<<= 4, y-=0x2c5c8; - if(x<0x20000000) x<<= 2, y-=0x162e4; - if(x<0x40000000) x<<= 1, y-=0x0b172; - t=x+(x>>1); if((t&0x80000000)==0) x=t,y-=0x067cd; - t=x+(x>>2); if((t&0x80000000)==0) x=t,y-=0x03920; - t=x+(x>>3); if((t&0x80000000)==0) x=t,y-=0x01e27; - t=x+(x>>4); if((t&0x80000000)==0) x=t,y-=0x00f85; - t=x+(x>>5); if((t&0x80000000)==0) x=t,y-=0x007e1; - t=x+(x>>6); if((t&0x80000000)==0) x=t,y-=0x003f8; - t=x+(x>>7); if((t&0x80000000)==0) x=t,y-=0x001fe; - x=0x80000000-x; - y-=x>>15; - return y; -} +#include "../../fixedpoint.c" diff --git a/apps/plugins/lib/fixedpoint.h b/apps/plugins/lib/fixedpoint.h index dfabbad8d..ef50dd008 100644 --- a/apps/plugins/lib/fixedpoint.h +++ b/apps/plugins/lib/fixedpoint.h @@ -21,11 +21,44 @@ * ****************************************************************************/ -long fsincos(unsigned long phase, long *cos); -long fsqrt(long a, unsigned int fracbits); -long cos_int(int val); -long sin_int(int val); -long flog(int x); +/** PLUGINS - FIXED POINT MATH ROUTINES - USAGE + * + * - x and y arguments are fixed point integers + * - fracbits is the number of fractional bits in the argument(s) + * - functions return long fixed point integers with the specified number + * of fractional bits unless otherwise specified + * + * Calculate sin and cos of an angle: + * fp_sincos(phase, *cos) + * where phase is a 32 bit unsigned integer with 0 representing 0 + * and 0xFFFFFFFF representing 2*pi, and *cos is the address to + * a long signed integer. Value returned is a long signed integer + * from LONG_MIN to LONG_MAX, representing -1 to 1 respectively. + * That is, value is a fixed point integer with 31 fractional bits. + * + * Take square root of a fixed point number: + * fp_sqrt(x, fracbits) + * + * Calculate sin or cos of an angle (very fast, from a table): + * fp14_sin(angle) + * fp14_cos(angle) + * where angle is a non-fixed point integer in degrees. Value + * returned is a fixed point integer with 14 fractional bits. + * + * Calculate the natural log of a positive fixed point integer + * fp16_log(x) + * where x and the value returned are fixed point integers + * with 16 fractional bits. + */ + +#ifndef _FIXEDPOINT_H_PLUGINS +#define _FIXEDPOINT_H_PLUGINS + +long fp_sincos(unsigned long phase, long *cos); +long fp_sqrt(long a, unsigned int fracbits); +long fp14_cos(int val); +long fp14_sin(int val); +long fp16_log(int x); /* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit, * whichever is faster for the architecture) */ @@ -34,3 +67,5 @@ long flog(int x); #else /* SH1, coldfire */ #define FMULU(a, b) ((uint32_t) (((uint16_t) (a)) * ((uint16_t) (b)))) #endif + +#endif diff --git a/apps/plugins/plasma.c b/apps/plugins/plasma.c index 2a3e43e6b..00287eb0b 100644 --- a/apps/plugins/plasma.c +++ b/apps/plugins/plasma.c @@ -198,7 +198,7 @@ static void wave_table_generate(void) for (i=0;i<256;++i) { wave_array[i] = (unsigned char)((WAV_AMP - * (sin_int((i * 360 * plasma_frequency) / 256))) / 16384); + * (fp14_sin((i * 360 * plasma_frequency) / 256))) / 16384); } } diff --git a/apps/plugins/vu_meter.c b/apps/plugins/vu_meter.c index 16aac3a01..74c3b1cf9 100644 --- a/apps/plugins/vu_meter.c +++ b/apps/plugins/vu_meter.c @@ -415,7 +415,7 @@ void calc_scales(void) for (i=1; i <= half_width; i++) { /* analog scale */ - y = (half_width/5)*flog(i*fx_log_factor); + y = (half_width/5)*fp16_log(i*fx_log_factor); /* better way of checking for negative values? */ z = y>>16; @@ -431,7 +431,7 @@ void calc_scales(void) k = nh2 - ( j * j ); /* fsqrt+1 seems to give a closer approximation */ - y_values[i-1] = LCD_HEIGHT - (fsqrt(k, 16)>>8) - 1; + y_values[i-1] = LCD_HEIGHT - (fp_sqrt(k, 16)>>8) - 1; rb->yield(); } } diff --git a/apps/replaygain.c b/apps/replaygain.c index 90944f91d..b398afc29 100644 --- a/apps/replaygain.c +++ b/apps/replaygain.c @@ -30,188 +30,11 @@ #include "metadata.h" #include "debug.h" #include "replaygain.h" - -/* The fixed point math routines (with the exception of fp_atof) are based - * on oMathFP by Dan Carter (http://orbisstudios.com). - */ - -/* 12 bits of precision gives fairly accurate result, but still allows a - * compact implementation. The math code supports up to 13... - */ +#include "fixedpoint.h" #define FP_BITS (12) -#define FP_MASK ((1 << FP_BITS) - 1) #define FP_ONE (1 << FP_BITS) -#define FP_TWO (2 << FP_BITS) -#define FP_HALF (1 << (FP_BITS - 1)) -#define FP_LN2 ( 45426 >> (16 - FP_BITS)) -#define FP_LN2_INV ( 94548 >> (16 - FP_BITS)) -#define FP_EXP_ZERO ( 10922 >> (16 - FP_BITS)) -#define FP_EXP_ONE ( -182 >> (16 - FP_BITS)) -#define FP_EXP_TWO ( 4 >> (16 - FP_BITS)) -#define FP_INF (0x7fffffff) -#define FP_LN10 (150902 >> (16 - FP_BITS)) - -#define FP_MAX_DIGITS (4) -#define FP_MAX_DIGITS_INT (10000) - -#define FP_FAST_MUL_DIV - -#ifdef FP_FAST_MUL_DIV - -/* These macros can easily overflow, but they are good enough for our uses, - * and saves some code. - */ -#define fp_mul(x, y) (((x) * (y)) >> FP_BITS) -#define fp_div(x, y) (((x) << FP_BITS) / (y)) - -#else - -static long fp_mul(long x, long y) -{ - long x_neg = 0; - long y_neg = 0; - long rc; - - if ((x == 0) || (y == 0)) - { - return 0; - } - - if (x < 0) - { - x_neg = 1; - x = -x; - } - - if (y < 0) - { - y_neg = 1; - y = -y; - } - - rc = (((x >> FP_BITS) * (y >> FP_BITS)) << FP_BITS) - + (((x & FP_MASK) * (y & FP_MASK)) >> FP_BITS) - + ((x & FP_MASK) * (y >> FP_BITS)) - + ((x >> FP_BITS) * (y & FP_MASK)); - - if ((x_neg ^ y_neg) == 1) - { - rc = -rc; - } - - return rc; -} - -static long fp_div(long x, long y) -{ - long x_neg = 0; - long y_neg = 0; - long shifty; - long rc; - int msb = 0; - int lsb = 0; - - if (x == 0) - { - return 0; - } - - if (y == 0) - { - return (x < 0) ? -FP_INF : FP_INF; - } - - if (x < 0) - { - x_neg = 1; - x = -x; - } - - if (y < 0) - { - y_neg = 1; - y = -y; - } - - while ((x & BIT_N(30 - msb)) == 0) - { - msb++; - } - - while ((y & BIT_N(lsb)) == 0) - { - lsb++; - } - - shifty = FP_BITS - (msb + lsb); - rc = ((x << msb) / (y >> lsb)); - if (shifty > 0) - { - rc <<= shifty; - } - else - { - rc >>= -shifty; - } - - if ((x_neg ^ y_neg) == 1) - { - rc = -rc; - } - - return rc; -} - -#endif /* FP_FAST_MUL_DIV */ - -static long fp_exp(long x) -{ - long k; - long z; - long R; - long xp; - - if (x == 0) - { - return FP_ONE; - } - - k = (fp_mul(abs(x), FP_LN2_INV) + FP_HALF) & ~FP_MASK; - - if (x < 0) - { - k = -k; - } - - x -= fp_mul(k, FP_LN2); - z = fp_mul(x, x); - R = FP_TWO + fp_mul(z, FP_EXP_ZERO + fp_mul(z, FP_EXP_ONE - + fp_mul(z, FP_EXP_TWO))); - xp = FP_ONE + fp_div(fp_mul(FP_TWO, x), R - x); - - if (k < 0) - { - k = FP_ONE >> (-k >> FP_BITS); - } - else - { - k = FP_ONE << (k >> FP_BITS); - } - - return fp_mul(k, xp); -} - -static long fp_exp10(long x) -{ - if (x == 0) - { - return FP_ONE; - } - - return fp_exp(fp_mul(FP_LN10, x)); -} static long fp_atof(const char* s, int precision) { @@ -300,7 +123,7 @@ static long convert_gain(long gain) gain = 17 * FP_ONE; } - gain = fp_exp10(gain / 20) << (24 - FP_BITS); + gain = fp_factor(gain, FP_BITS) << (24 - FP_BITS); return gain; } -- 2.11.4.GIT