mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-17 08:40:52 +00:00
fixes to fp_16 library
- add saturating operations - division was broken fp_12 library might also be sus...
This commit is contained in:
parent
5b60713c02
commit
75e7937301
1 changed files with 108 additions and 42 deletions
|
|
@ -50,16 +50,59 @@ typedef int32_t fp_t;
|
||||||
#define FP_FROM_FLOAT(x) ((fp_t)((x) * FP_ONE + 0.5f))
|
#define FP_FROM_FLOAT(x) ((fp_t)((x) * FP_ONE + 0.5f))
|
||||||
#define FP_TO_FLOAT(x) ((float)(x) / FP_ONE)
|
#define FP_TO_FLOAT(x) ((float)(x) / FP_ONE)
|
||||||
|
|
||||||
|
#define FIX16_OVERFLOW 0x80000000
|
||||||
|
#define FIX16_MAXIMUM 0x7FFFFFFF /*!< the maximum value of fp_t */
|
||||||
|
#define FIX16_MINIMUM 0x80000000 /*!< the minimum value of fp_t */
|
||||||
|
|
||||||
/* --- Arithmetic --- */
|
/* --- Arithmetic --- */
|
||||||
|
|
||||||
static inline fp_t fp_add(fp_t a, fp_t b)
|
static inline fp_t fp_add(fp_t a, fp_t b)
|
||||||
{
|
{
|
||||||
return a + b;
|
// Use unsigned integers because overflow with signed integers is
|
||||||
|
// an undefined operation (http://www.airs.com/blog/archives/120).
|
||||||
|
uint32_t _a = a, _b = b;
|
||||||
|
uint32_t sum = _a + _b;
|
||||||
|
|
||||||
|
// Overflow can only happen if sign of a == sign of b, and then
|
||||||
|
// it causes sign of sum != sign of a.
|
||||||
|
if (!((_a ^ _b) & 0x80000000) && ((_a ^ sum) & 0x80000000))
|
||||||
|
return (fp_t) FIX16_OVERFLOW;
|
||||||
|
|
||||||
|
return sum;
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline fp_t fp_sub(fp_t a, fp_t b)
|
static inline fp_t fp_sub(fp_t a, fp_t b)
|
||||||
{
|
{
|
||||||
return a - b;
|
uint32_t _a = a, _b = b;
|
||||||
|
uint32_t diff = _a - _b;
|
||||||
|
|
||||||
|
// Overflow can only happen if sign of a != sign of b, and then
|
||||||
|
// it causes sign of diff != sign of a.
|
||||||
|
if (((_a ^ _b) & 0x80000000) && ((_a ^ diff) & 0x80000000))
|
||||||
|
return (fp_t) FIX16_OVERFLOW;
|
||||||
|
|
||||||
|
return diff;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Saturating arithmetic */
|
||||||
|
static inline fp_t fp_sadd(fp_t a, fp_t b)
|
||||||
|
{
|
||||||
|
fp_t result = fp_add(a, b);
|
||||||
|
|
||||||
|
if (result == (fp_t) FIX16_OVERFLOW)
|
||||||
|
return (a >= 0) ? FIX16_MAXIMUM : FIX16_MINIMUM;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline fp_t fp_ssub(fp_t a, fp_t b)
|
||||||
|
{
|
||||||
|
fp_t result = fp_sub(a, b);
|
||||||
|
|
||||||
|
if (result == (fp_t) FIX16_OVERFLOW)
|
||||||
|
return (a >= 0) ? FIX16_MAXIMUM : FIX16_MINIMUM;
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -77,52 +120,75 @@ static inline fp_t fp_mul(fp_t a, fp_t b)
|
||||||
return (fp_t)(result >> FP_FRAC_BITS);
|
return (fp_t)(result >> FP_FRAC_BITS);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
fp_t fp_div(fp_t a, fp_t b)
|
||||||
* u32_divrem - unsigned 32-bit shift-and-subtract divider.
|
|
||||||
*
|
|
||||||
* 32 iterations of: shift one bit of the dividend into the partial
|
|
||||||
* remainder, subtract the divisor if it fits, record the quotient bit.
|
|
||||||
* No hardware divide instruction required.
|
|
||||||
*/
|
|
||||||
static inline uint32_t u32_divrem(uint32_t dividend, uint32_t divisor,
|
|
||||||
uint32_t *remainder)
|
|
||||||
{
|
{
|
||||||
uint32_t quotient = 0;
|
// This uses the basic binary restoring division algorithm.
|
||||||
uint32_t rem = 0;
|
// It appears to be faster to do the whole division manually than
|
||||||
int i;
|
// trying to compose a 64-bit divide out of 32-bit divisions on
|
||||||
|
// platforms without hardware divide.
|
||||||
|
|
||||||
for (i = 31; i >= 0; i--) {
|
if (b == 0)
|
||||||
rem = (rem << 1) | ((dividend >> i) & 1);
|
return FIX16_MINIMUM;
|
||||||
if (rem >= divisor) {
|
|
||||||
rem -= divisor;
|
|
||||||
quotient |= (1u << i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (remainder) *remainder = rem;
|
uint32_t remainder = (a >= 0) ? a : (-a);
|
||||||
return quotient;
|
uint32_t divider = (b >= 0) ? b : (-b);
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
uint32_t quotient = 0;
|
||||||
* fp_div - signed fixed-point division, no hardware divide.
|
uint32_t bit = 0x10000;
|
||||||
*
|
|
||||||
* Works in two passes through u32_divrem:
|
|
||||||
* pass 1: integer quotient and remainder of |a| / |b|
|
|
||||||
* pass 2: fractional bits from (remainder << FP_FRAC_BITS) / |b|
|
|
||||||
* Sign is restored at the end.
|
|
||||||
*/
|
|
||||||
static inline fp_t fp_div(fp_t a, fp_t b)
|
|
||||||
{
|
|
||||||
int32_t sign = ((a ^ b) < 0) ? -1 : 1;
|
|
||||||
uint32_t ua = (uint32_t)(a < 0 ? -a : a);
|
|
||||||
uint32_t ub = (uint32_t)(b < 0 ? -b : b);
|
|
||||||
uint32_t q, r;
|
|
||||||
fp_t res;
|
|
||||||
|
|
||||||
q = u32_divrem(ua, ub, &r);
|
/* The algorithm requires D >= R */
|
||||||
res = (fp_t)((q << FP_FRAC_BITS) | u32_divrem(r << FP_FRAC_BITS, ub, NULL));
|
while (divider < remainder)
|
||||||
|
{
|
||||||
|
divider <<= 1;
|
||||||
|
bit <<= 1;
|
||||||
|
}
|
||||||
|
|
||||||
return sign < 0 ? -res : res;
|
if (!bit)
|
||||||
|
return FIX16_OVERFLOW;
|
||||||
|
|
||||||
|
if (divider & 0x80000000)
|
||||||
|
{
|
||||||
|
// Perform one step manually to avoid overflows later.
|
||||||
|
// We know that divider's bottom bit is 0 here.
|
||||||
|
if (remainder >= divider)
|
||||||
|
{
|
||||||
|
quotient |= bit;
|
||||||
|
remainder -= divider;
|
||||||
|
}
|
||||||
|
divider >>= 1;
|
||||||
|
bit >>= 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Main division loop */
|
||||||
|
while (bit && remainder)
|
||||||
|
{
|
||||||
|
if (remainder >= divider)
|
||||||
|
{
|
||||||
|
quotient |= bit;
|
||||||
|
remainder -= divider;
|
||||||
|
}
|
||||||
|
|
||||||
|
remainder <<= 1;
|
||||||
|
bit >>= 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (remainder >= divider)
|
||||||
|
{
|
||||||
|
quotient++;
|
||||||
|
}
|
||||||
|
|
||||||
|
fp_t result = quotient;
|
||||||
|
|
||||||
|
/* Figure out the sign of result */
|
||||||
|
if ((a ^ b) & 0x80000000)
|
||||||
|
{
|
||||||
|
if (result == (fp_t) FIX16_MINIMUM)
|
||||||
|
return FIX16_OVERFLOW;
|
||||||
|
|
||||||
|
result = -result;
|
||||||
|
}
|
||||||
|
|
||||||
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* --- CORDIC cosine --- */
|
/* --- CORDIC cosine --- */
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue