From d0d9f38d074cd796e9b58f93b8ce6d820fcd0e46 Mon Sep 17 00:00:00 2001 From: mcuelenaere Date: Sat, 4 Jul 2009 12:26:45 +0000 Subject: [PATCH] Consolidate all fixed point math routines in one library (FS#10400) by Jeffrey Goode git-svn-id: svn://svn.rockbox.org/rockbox/trunk@21633 a1c6a512-1295-4272-9138-f99709370657 --- apps/SOURCES | 1 + apps/codecs/adx.c | 119 +--------------- apps/codecs/lib/SOURCES | 2 +- apps/{dsp.h => codecs/lib/fixedpoint.h} | 94 ++++--------- apps/codecs/spc.c | 1 + apps/dsp.c | 1 + apps/dsp.h | 80 ----------- apps/eq.c | 97 +------------ apps/eq.h | 1 + apps/{plugins/lib => }/fixedpoint.c | 220 +++++++++++++++++++++++++++-- apps/fixedpoint.h | 197 ++++++++++++++++++++++++++ apps/plugins/lib/SOURCES | 2 +- apps/plugins/lib/fixedpoint.c | 238 -------------------------------- apps/replaygain.c | 181 +----------------------- 14 files changed, 442 insertions(+), 792 deletions(-) copy apps/{dsp.h => codecs/lib/fixedpoint.h} (66%) copy apps/{plugins/lib => }/fixedpoint.c (56%) create mode 100644 apps/fixedpoint.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..e23b3d4f8 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 "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) { diff --git a/apps/codecs/lib/SOURCES b/apps/codecs/lib/SOURCES index cbb8e6037..a1730f656 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/dsp.h b/apps/codecs/lib/fixedpoint.h similarity index 66% copy from apps/dsp.h copy to apps/codecs/lib/fixedpoint.h index 8c23c3053..54ece2708 100644 --- a/apps/dsp.h +++ b/apps/codecs/lib/fixedpoint.h @@ -5,9 +5,11 @@ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ - * $Id$ + * $Id: fixedpoint.h -1 $ * - * Copyright (C) 2005 Miika Pekkarinen + * 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 @@ -19,51 +21,12 @@ * ****************************************************************************/ -#ifndef _DSP_H -#define _DSP_H - -#include -#include - -#define NATIVE_FREQUENCY 44100 -enum -{ - STEREO_INTERLEAVED = 0, - STEREO_NONINTERLEAVED, - STEREO_MONO, - STEREO_NUM_MODES, -}; - -enum -{ - CODEC_IDX_AUDIO = 0, - CODEC_IDX_VOICE, -}; - -enum -{ - DSP_MYDSP = 1, - DSP_SET_FREQUENCY, - DSP_SWITCH_FREQUENCY, - DSP_SET_SAMPLE_DEPTH, - DSP_SET_STEREO_MODE, - DSP_RESET, - DSP_FLUSH, - DSP_SET_TRACK_GAIN, - DSP_SET_ALBUM_GAIN, - DSP_SET_TRACK_PEAK, - DSP_SET_ALBUM_PEAK, - DSP_CROSSFEED -}; - -enum { - DSP_CALLBACK_SET_PRESCALE = 0, - DSP_CALLBACK_SET_BASS, - DSP_CALLBACK_SET_TREBLE, - DSP_CALLBACK_SET_CHANNEL_CONFIG, - DSP_CALLBACK_SET_STEREO_WIDTH -}; +#ifndef _FIXEDPOINT_H +#define _FIXEDPOINT_H + +#include +/** TAKEN FROM apps/dsp.h */ /* 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 @@ -144,29 +107,20 @@ enum { #define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y)) -struct dsp_config; - -int dsp_process(struct dsp_config *dsp, char *dest, - const char *src[], int count); -int dsp_input_count(struct dsp_config *dsp, int count); -int dsp_output_count(struct dsp_config *dsp, int count); -intptr_t dsp_configure(struct dsp_config *dsp, int setting, - intptr_t value); -void dsp_set_replaygain(void); -void dsp_set_crossfeed(bool enable); -void dsp_set_crossfeed_direct_gain(int gain); -void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, - long cutoff); -void dsp_set_eq(bool enable); -void dsp_set_eq_precut(int precut); -void dsp_set_eq_coefs(int band); -void dsp_dither_enable(bool enable); -void dsp_timestretch_enable(bool enable); -bool dsp_timestretch_available(void); -void sound_set_pitch(int r); -int sound_get_pitch(void); -void dsp_set_timestretch(int percent); -int dsp_get_timestretch(void); -int dsp_callback(int msg, intptr_t param); + +/** TAKEN FROM ORIGINAL fixedpoint.h */ +/* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit, + * whichever is faster for the architecture) */ +#ifdef CPU_ARM +#define FMULU(a, b) ((uint32_t) (((uint32_t) (a)) * ((uint32_t) (b)))) +#else /* SH1, coldfire */ +#define FMULU(a, b) ((uint32_t) (((uint16_t) (a)) * ((uint16_t) (b)))) +#endif + +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); #endif diff --git a/apps/codecs/spc.c b/apps/codecs/spc.c index 6ceb704c7..d5313bfa4 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 "fixedpoint.h" #include "libspc/spc_codec.h" #include "libspc/spc_profiler.h" diff --git a/apps/dsp.c b/apps/dsp.c index a760865af..66469304b 100644 --- a/apps/dsp.c +++ b/apps/dsp.c @@ -33,6 +33,7 @@ #include "misc.h" #include "tdspeed.h" #include "buffer.h" +#include "fixedpoint.h" /* 16-bit samples are scaled based on these constants. The shift should be * no more than 15. 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..7b7fba341 100644 --- a/apps/eq.c +++ b/apps/eq.c @@ -21,105 +21,10 @@ #include #include "config.h" -#include "dsp.h" +#include "fixedpoint.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. 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 56% copy from apps/plugins/lib/fixedpoint.c copy to apps/fixedpoint.c index 0ae2cded6..b65070e34 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,15 @@ * ****************************************************************************/ -#include -#include "plugin.h" #include "fixedpoint.h" +#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 */ @@ -139,18 +144,27 @@ long fsincos(unsigned long phase, long *cos) /** * 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 fsqrt(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 = DIV64(x, b, fracbits); + if (c == b) break; + b = (b + c)/2; + } return b; } @@ -236,3 +250,191 @@ long flog(int x) { y-=x>>15; return y; } + +/** 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); +} + + +/** 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; +} + + +/** 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; +} diff --git a/apps/fixedpoint.h b/apps/fixedpoint.h new file mode 100644 index 000000000..a3ca6ee6e --- /dev/null +++ b/apps/fixedpoint.h @@ -0,0 +1,197 @@ +/*************************************************************************** + * __________ __ ___. + * 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. + * + ****************************************************************************/ + +/** 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) + * + * Shortcut: Multiply two fixed point numbers with 31 fractional bits: + * fp31_mul(x, y) + * + * Shortcut: Multiply two fixed point numbers with 31 fractional bits, + * then shift left by z bits: + * fp31_mulshl(x, y, z) + * NOTE: z must be in the range 1-8 on Coldfire targets. + * + * Divide two fixed point numbers: + * fp_div(x, y, fracbits) + * + * Take square root of a fixed point number: + * fp_sqrt(x, fracbits) + * + * 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. + * + * 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 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 +#define _FIXEDPOINT_H + +#include + +/* Redefine function names, making sure legacy code is usable */ +#define fp31_mul(x, y) FRACMUL(x, y) +#define fp31_mulshl(x, y, z) FRACMUL_SHL(x, y, z) +#define fp_div(x, y, z) DIV64(x, y, z) +#define fp_sqrt(x, y) fsqrt(x, y) +#define fp_sincos(x, y) fsincos(x, y) +#define fp14_sin(x) sin_int(x) +#define fp14_cos(x) cos_int(x) +#define fp16_log(x) flog(x) + + +#define fp_mul(x, y, z) (long)((((long long)(x)) * ((long long)(y))) >> (z)) +#define DIV64(x, y, z) (long)((((long long)(x)) << (z)) / ((long long)(y))) + +/** TAKEN FROM apps/dsp.h */ +/* 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 + +/** TAKEN FROM ORIGINAL fixedpoint.h */ +/* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit, + * whichever is faster for the architecture) */ +#ifdef CPU_ARM +#define FMULU(a, b) ((uint32_t) (((uint32_t) (a)) * ((uint32_t) (b)))) +#else /* SH1, coldfire */ +#define FMULU(a, b) ((uint32_t) (((uint16_t) (a)) * ((uint16_t) (b)))) +#endif + +long fsincos(unsigned long phase, long *cos); +long fsqrt(long x, unsigned int fracbits); +long sin_int(int val); +long cos_int(int val); +long flog(int x); + + +/** 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/plugins/lib/SOURCES b/apps/plugins/lib/SOURCES index 02adb7089..bcce3f296 100644 --- a/apps/plugins/lib/SOURCES +++ b/apps/plugins/lib/SOURCES @@ -1,7 +1,7 @@ gcc-support.c jhash.c configfile.c -fixedpoint.c +../../fixedpoint.c playback_control.c rgb_hsv.c buflib.c diff --git a/apps/plugins/lib/fixedpoint.c b/apps/plugins/lib/fixedpoint.c dissimilarity index 100% index 0ae2cded6..e69de29bb 100644 --- a/apps/plugins/lib/fixedpoint.c +++ b/apps/plugins/lib/fixedpoint.c @@ -1,238 +0,0 @@ -/*************************************************************************** - * __________ __ ___. - * 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; -} 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