#ifndef lib_fixed_h #define lib_fixed_h /* Cesar project {{{ * * Copyright (C) 2009 Spidcom * * <<>> * * }}} */ /** * \file lib/fixed.h * \brief Fixed point math. * \ingroup lib */ /** * Convert a floating point number to fixed point. * \param x floating point constant * \param shift fixed point fraction part size * \return fixed point number * * Rounds to nearest, halfway away from 0. */ #define FIXED(x, shift) \ ((s32) ((x) * (1 << (shift)) + ((x) > 0 ? 0.5 : -0.5))) /** The famous number. */ #define FIXED_PI(shift) FIXED (3.14159265358979323846, (shift)) #define FIXED_PI_Q24 FIXED_PI (24) /** Union used for fast access to 64 bit number parts. */ union fixed_dwu { u64 dw; s64 sdw; struct { #if DEFS_BIG_ENDIAN u32 high, low; #else u32 low, high; #endif } w; }; /** * Reduce fixed number precision, round to nearest. * \param x number to round * \param shift precision reduction (1 to 31) * \return number rounded * * Rounds to nearest, halfway up. */ extern inline s32 fixed_round (s32 x, uint shift) { return (x + (1 << (shift - 1))) >> shift; } /** * Reduce fixed number precision, round to nearest, 64 bits version. * \param x number to round * \param shift precision reduction (1 to 63) * \return number rounded * * Rounds to nearest, halfway up. */ extern inline s64 fixed_round_64 (s64 x, uint shift) { return (x + (1ll << (shift - 1))) >> shift; } /** * Fixed point product. * \param a first operand * \param b second operand * \param shift shift to fix format * \return multiplication result * * When doing a fixed point product, one usually wants to get the same format * for output as the one used for the given operand. This means that for a * format Qm.n, result of the integer multiplication should be shifted by n. * * However, this is not always the case, you can use a different shift to get * a different format. For example, to get a Q24 from two Q16, use a shift of * 16 + 16 - 24 = 8. * * This function only accept shift between 1 and 31 included and produce * faster code if shift is a constant known by the compiler.. * * Rounds to nearest, halfway up. */ extern inline s32 fixed_mul (s32 a, s32 b, uint shift) { /* Would assert (shift > 0 && shift < 32) if not in a hurry. */ union fixed_dwu u; /* Get a 64 bit result of 32 bit multiplication, */ u.sdw = (s64) a * b /* add 0.5 for rounding. */ + (1u << (shift - 1)); /* Compiler shift do not know that shift is in [1:31], make my own. */ return u.w.high << (32 - shift) | u.w.low >> shift; } /** * Fixed point division. * \param a dividend * \param b divisor * \param shift shift to fix format * \return quotient * * See fixed_mul for discussion about shift. * * Rounds to nearest, halfway away from 0. */ extern inline s32 fixed_div (s32 a, s32 b, uint shift) { #ifndef __sparc__ /* Would assert (shift > 0 && shift < 32) if not in a hurry. */ union fixed_dwu al; /* I know shift is in [1:31], compiler don't. */ al.w.high = a >> (32 - shift); al.w.low = a << shift; /* As compiler and/or microprocessor rounds toward zero, care must be * taken when rounding because of negative inputs. */ s32 bh = b / 2; if ((a ^ b) < 0) bh = -bh; al.sdw += bh; /* OK, proceed with the division. */ al.sdw /= b; return al.w.low; #else /* __sparc__ */ /* I can not make the compiler use the 64/32=32 bit division, so assembly * has to be used. */ u32 lo, hi, t1, t2; s32 r; __asm__ ( "srl %[b], 31, %[t1]" /* b < 0 ? 1 : 0 */ "\n\t" "neg %[sh], %[hi]" /* 32 - shift (mod 32) */ "\n\t" "add %[b], %[t1], %[t1]" /* b < 0 ? b + 1 : b */ "\n\t" "sra %[t1], 1, %[t1]" /* b / 2 (rounded to 0) */ "\n\t" "xorcc %[a], %[b], %%g0" /* a ^ b */ "\n\t" "sra %[a], %[hi], %[hi]" /* hi (a << shift) */ "\n\t" "bge 1f" /* if (a ^ b >= 0) */ "\n\t" " sll %[a], %[sh], %[lo]" /* (delay slot) lo (a << shift) */ "\n\t" "neg %[t1]" /* (if) -b / 2 */ "\n\t" "1: " "sra %[t1], 31, %[t2]" /* sign extend */ "\n\t" "addcc %[lo], %[t1], %[lo]" /* add... */ "\n\t" "addx %[hi], %[t2], %[hi]" /* ...with carry */ "\n\t" "mov %[hi], %%y" /* division... */ "\n\t" "nop" /* ...need 3... */ "\n\t" "nop" /* ...instructions... */ "\n\t" "nop" /* ...after y is set... */ "\n\t" "sdiv %[lo], %[b], %[r]" /* ...before div */ : [r] "=r" (r), [lo] "=&r" (lo), [hi] "=&r" (hi), [t1] "=&r" (t1), [t2] "=&r" (t2) : [a] "r" (a), [b] "r" (b), [sh] "r" (shift) : "cc" /* "y" */ ); return r; #endif } /** * Compute sinus on [-pi/4:pi/4]. * \param a angle divided by pi, q24 * \return sinus, q24 */ s32 fixed_sin_limited_q24 (s32 a); /** * Compute cosinus on [-pi/4:pi/4]. * \param a angle divided by pi, q24 * \return cosinus, q24 */ s32 fixed_cos_limited_q24 (s32 a); /** * Compute sinus. * \param a angle divided by pi, q24 * \return sinus, q24 */ s32 fixed_sin_q24 (s32 a); /** * Compute cosinus. * \param a angle divided by pi, q24 * \return cosinus, q24 */ s32 fixed_cos_q24 (s32 a); /** * Compute square root. * \param x radicant * \param shift fixed format, should be even * \return radical */ s32 fixed_sqrt (s32 x, uint shift); #endif /* lib_fixed_h */