diff --git a/artiq/runtime/ksupport.c b/artiq/runtime/ksupport.c index a3421c528..d6c087de9 100644 --- a/artiq/runtime/ksupport.c +++ b/artiq/runtime/ksupport.c @@ -18,6 +18,7 @@ #include "i2c.h" double round(double x); +double sqrt(double x); void ksupport_abort(void); static void attribute_writeback(void *); @@ -87,6 +88,7 @@ static const struct symbol runtime_exports[] = { /* libm */ {"round", &round}, + {"sqrt", &sqrt}, /* exceptions */ {"_Unwind_Resume", &_Unwind_Resume}, @@ -161,6 +163,107 @@ double round(double x) return y; } +double sqrt(double x) +{ + static const double one = 1.0, tiny = 1.0e-300; + double z; + int32_t sign = (int)0x80000000; + int32_t ix0,s0,q,m,t,i; + uint32_t r,t1,s1,ix1,q1; + + union {double f; struct{uint32_t msw; uint32_t lsw;};} u = {x}; + ix0 = u.msw; + ix1 = u.lsw; + + /* take care of Inf and NaN */ + if((ix0&0x7ff00000)==0x7ff00000) { + return x*x+x; /* sqrt(NaN)=NaN, sqrt(+inf)=+inf + sqrt(-inf)=sNaN */ + } + /* take care of zero */ + if(ix0<=0) { + if(((ix0&(~sign))|ix1)==0) return x;/* sqrt(+-0) = +-0 */ + else if(ix0<0) + return (x-x)/(x-x); /* sqrt(-ve) = sNaN */ + } + /* normalize x */ + m = (ix0>>20); + if(m==0) { /* subnormal x */ + while(ix0==0) { + m -= 21; + ix0 |= (ix1>>11); ix1 <<= 21; + } + for(i=0;(ix0&0x00100000)==0;i++) ix0<<=1; + m -= i-1; + ix0 |= (ix1>>(32-i)); + ix1 <<= i; + } + m -= 1023; /* unbias exponent */ + ix0 = (ix0&0x000fffff)|0x00100000; + if(m&1){ /* odd m, double x to make it even */ + ix0 += ix0 + ((ix1&sign)>>31); + ix1 += ix1; + } + m >>= 1; /* m = [m/2] */ + + /* generate sqrt(x) bit by bit */ + ix0 += ix0 + ((ix1&sign)>>31); + ix1 += ix1; + q = q1 = s0 = s1 = 0; /* [q,q1] = sqrt(x) */ + r = 0x00200000; /* r = moving bit from right to left */ + + while(r!=0) { + t = s0+r; + if(t<=ix0) { + s0 = t+r; + ix0 -= t; + q += r; + } + ix0 += ix0 + ((ix1&sign)>>31); + ix1 += ix1; + r>>=1; + } + + r = sign; + while(r!=0) { + t1 = s1+r; + t = s0; + if((t>31); + ix1 += ix1; + r>>=1; + } + + /* use floating add to find out rounding direction */ + if((ix0|ix1)!=0) { + z = one-tiny; /* trigger inexact flag */ + if (z>=one) { + z = one+tiny; + if (q1==(uint32_t)0xffffffff) { q1=0; q += 1;} + else if (z>one) { + if (q1==(uint32_t)0xfffffffe) q+=1; + q1+=2; + } else + q1 += (q1&1); + } + } + ix0 = (q>>1)+0x3fe00000; + ix1 = q1>>1; + if ((q&1)==1) ix1 |= sign; + ix0 += (m <<20); + + u.msw = ix0; + u.lsw = ix1; + return u.f; +} + /* called by libunwind */ int fprintf(FILE *stream, const char *fmt, ...) {