#include <math.h>
#if defined __SOFTFP__
#include <stdint.h>
#include <limits.h>
typedef float fp_t;
typedef uint32_t rep_t;
static const int significandBits = 23;
#define REP_C UINT32_C
static inline int rep_clz(rep_t a) {
return __builtin_clz(a);
}
static inline rep_t toRep(fp_t x) {
const union { rep_t i; fp_t f; } rep = { .f = x };
return rep.i;
}
static inline fp_t fromRep(rep_t x) {
const union { rep_t i; fp_t f; } rep = { .i = x };
return rep.f;
}
static inline uint32_t mulhi(uint32_t a, uint32_t b) {
return (uint64_t)a*b >> 32;
}
fp_t sqrtf(fp_t x) {
static const int typeWidth = sizeof(rep_t) * CHAR_BIT;
static const int exponentBits = typeWidth - significandBits - 1;
static const int exponentBias = (1 << (exponentBits - 1)) - 1;
static const rep_t minNormal = REP_C(1) << significandBits;
static const rep_t significandMask = minNormal - 1;
static const rep_t signBit = REP_C(1) << (typeWidth - 1);
static const rep_t absMask = signBit - 1;
static const rep_t infRep = absMask ^ significandMask;
static const rep_t qnan = infRep | REP_C(1) << (significandBits - 1);
const rep_t xRep = toRep(x);
rep_t significand = xRep & significandMask;
int exponent = (xRep >> significandBits) - exponentBias;
if (xRep - minNormal >= infRep - minNormal) {
const rep_t xAbs = xRep & absMask;
if (xAbs == 0) return x;
if (xAbs > infRep) return fromRep(qnan | xRep);
if (xRep > signBit) return fromRep(qnan);
if (xRep == infRep) return x;
const int shift = rep_clz(significand) - rep_clz(minNormal);
significand <<= shift;
exponent += 1 - shift;
}
significand |= minNormal;
const int resultExponent = exponent >> 1;
uint32_t xQ30 = significand << (7 + (exponent & 1));
const uint32_t oneSixthQ34 = UINT32_C(0xaaaaaaaa);
uint32_t recipQ32 = UINT32_C(0x1a756d3b) - mulhi(oneSixthQ34, xQ30);
const uint32_t threeQ30 = UINT32_C(0xc0000000);
uint32_t residualQ30 = mulhi(xQ30, mulhi(recipQ32, recipQ32));
recipQ32 = mulhi(recipQ32, threeQ30 - residualQ30) << 1;
residualQ30 = mulhi(xQ30, mulhi(recipQ32, recipQ32));
recipQ32 = mulhi(recipQ32, threeQ30 - residualQ30) << 1;
residualQ30 = mulhi(xQ30, mulhi(recipQ32, recipQ32));
recipQ32 = mulhi(recipQ32, threeQ30 - residualQ30) << 1;
residualQ30 = mulhi(xQ30, mulhi(recipQ32, recipQ32));
recipQ32 = mulhi(recipQ32, threeQ30 - residualQ30) << 1;
rep_t result = (mulhi(recipQ32, xQ30) - 2) >> 7;
rep_t residual = (xQ30 << 16) - result*result;
result += residual > result;
result &= significandMask;
result |= (rep_t)(resultExponent + exponentBias) << significandBits;
return fromRep(result);
}
#else // __SOFTFP__
float sqrtf(float x) {
return __builtin_sqrtf(x);
}
#endif // __SOFTFP__