1 /*
2  * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #include <math.h>
8 #include "pico/types.h"
9 #include "pico/double.h"
10 #include "pico/platform.h"
11 
12 typedef uint64_t ui64;
13 typedef uint32_t ui32;
14 typedef int64_t i64;
15 
16 #define PINF ( HUGE_VAL)
17 #define MINF (-HUGE_VAL)
18 #define PZERO (+0.0)
19 #define MZERO (-0.0)
20 
21 
22 #define PI       3.14159265358979323846
23 #define LOG2     0.69314718055994530941
24 // Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
25 #define LOG10    2.30258509299404568401
26 #define LOG2E    1.44269504088896340737
27 #define LOG10E   0.43429448190325182765
28 #define ONETHIRD 0.33333333333333333333
29 
30 #define PIf       3.14159265358979323846f
31 #define LOG2f     0.69314718055994530941f
32 #define LOG2Ef    1.44269504088896340737f
33 #define LOG10Ef   0.43429448190325182765f
34 #define ONETHIRDf 0.33333333333333333333f
35 
36 #define DUNPACK(x,e,m) e=((x)>>52)&0x7ff,m=((x)&0x000fffffffffffffULL)|0x0010000000000000ULL
37 #define DUNPACKS(x,s,e,m) s=((x)>>63),DUNPACK((x),(e),(m))
38 
39 _Pragma("GCC diagnostic push")
40 _Pragma("GCC diagnostic ignored \"-Wstrict-aliasing\"")
41 
disnan(double x)42 static inline bool disnan(double x) {
43     ui64 ix=*(i64*)&x;
44     // checks the top bit of the low 32 bit of the NAN, but it I think that is ok
45     return ((uint32_t)(ix >> 31)) > 0xffe00000u;
46 }
47 
48 #if PICO_DOUBLE_PROPAGATE_NANS
49 #define check_nan_d1(x) if (disnan((x))) return (x)
50 #define check_nan_d2(x,y) if (disnan((x))) return (x); else if (disnan((y))) return (y);
51 #else
52 #define check_nan_d1(x) ((void)0)
53 #define check_nan_d2(x,y) ((void)0)
54 #endif
55 
dgetsignexp(double x)56 static inline int dgetsignexp(double x) {
57     ui64 ix=*(ui64*)&x;
58     return (ix>>52)&0xfff;
59 }
60 
dgetexp(double x)61 static inline int dgetexp(double x) {
62     ui64 ix=*(ui64*)&x;
63     return (ix>>52)&0x7ff;
64 }
65 
dldexp(double x,int de)66 static inline double dldexp(double x,int de) {
67     ui64 ix=*(ui64*)&x,iy;
68     int e;
69     e=dgetexp(x);
70     if(e==0||e==0x7ff) return x;
71     e+=de;
72     if(e<=0) iy=ix&0x8000000000000000ULL; // signed zero for underflow
73     else if(e>=0x7ff) iy=(ix&0x8000000000000000ULL)|0x7ff0000000000000ULL; // signed infinity on overflow
74     else iy=ix+((ui64)de<<52);
75     return *(double*)&iy;
76 }
77 
WRAPPER_FUNC(ldexp)78 double WRAPPER_FUNC(ldexp)(double x, int de) {
79     check_nan_d1(x);
80     return dldexp(x, de);
81 }
82 
83 
dcopysign(double x,double y)84 static inline double dcopysign(double x,double y) {
85     ui64 ix=*(ui64*)&x,iy=*(ui64*)&y;
86     ix=((ix&0x7fffffffffffffffULL)|(iy&0x8000000000000000ULL));
87     return *(double*)&ix;
88 }
89 
WRAPPER_FUNC(copysign)90 double WRAPPER_FUNC(copysign)(double x, double y) {
91     check_nan_d2(x,y);
92     return dcopysign(x, y);
93 }
diszero(double x)94 static inline int diszero(double x)  { return dgetexp    (x)==0; }
dispzero(double x)95 static inline int dispzero(double x) { return dgetsignexp(x)==0; }
dismzero(double x)96 static inline int dismzero(double x) { return dgetsignexp(x)==0x800; }
disinf(double x)97 static inline int disinf(double x)   { return dgetexp    (x)==0x7ff; }
dispinf(double x)98 static inline int dispinf(double x)  { return dgetsignexp(x)==0x7ff; }
disminf(double x)99 static inline int disminf(double x)  { return dgetsignexp(x)==0xfff; }
100 
disint(double x)101 static inline int disint(double x) {
102     ui64 ix=*(ui64*)&x,m;
103     int e=dgetexp(x);
104     if(e==0) return 1;       // 0 is an integer
105     e-=0x3ff;                // remove exponent bias
106     if(e<0) return 0;        // |x|<1
107     e=52-e;                  // bit position in mantissa with significance 1
108     if(e<=0) return 1;       // |x| large, so must be an integer
109     m=(1ULL<<e)-1;           // mask for bits of significance <1
110     if(ix&m) return 0;       // not an integer
111     return 1;
112 }
113 
disoddint(double x)114 static inline int disoddint(double x) {
115     ui64 ix=*(ui64*)&x,m;
116     int e=dgetexp(x);
117     e-=0x3ff;                // remove exponent bias
118     if(e<0) return 0;        // |x|<1; 0 is not odd
119     e=52-e;                  // bit position in mantissa with significance 1
120     if(e<0) return 0;        // |x| large, so must be even
121     m=(1ULL<<e)-1;           // mask for bits of significance <1 (if any)
122     if(ix&m) return 0;       // not an integer
123     if(e==52) return 1;      // value is exactly 1
124     return (ix>>e)&1;
125 }
126 
disstrictneg(double x)127 static inline int disstrictneg(double x) {
128     ui64 ix=*(ui64*)&x;
129     if(diszero(x)) return 0;
130     return ix>>63;
131 }
132 
disneg(double x)133 static inline int disneg(double x) {
134     ui64 ix=*(ui64*)&x;
135     return ix>>63;
136 }
137 
dneg(double x)138 static inline double dneg(double x) {
139     ui64 ix=*(ui64*)&x;
140     ix^=0x8000000000000000ULL;
141     return *(double*)&ix;
142 }
143 
dispo2(double x)144 static inline int dispo2(double x) {
145     ui64 ix=*(ui64*)&x;
146     if(diszero(x)) return 0;
147     if(disinf(x)) return 0;
148     ix&=0x000fffffffffffffULL;
149     return ix==0;
150 }
151 
dnan_or(double x)152 static inline double dnan_or(double x) {
153 #if PICO_DOUBLE_PROPAGATE_NANS
154     return NAN;
155 #else
156     return x;
157 #endif
158 }
159 
WRAPPER_FUNC(trunc)160 double WRAPPER_FUNC(trunc)(double x) {
161     check_nan_d1(x);
162     ui64 ix=*(ui64*)&x,m;
163     int e=dgetexp(x);
164     e-=0x3ff;                // remove exponent bias
165     if(e<0) {                // |x|<1
166         ix&=0x8000000000000000ULL;
167         return *(double*)&ix;
168     }
169     e=52-e;                  // bit position in mantissa with significance 1
170     if(e<=0) return x;       // |x| large, so must be an integer
171     m=(1ULL<<e)-1;           // mask for bits of significance <1
172     ix&=~m;
173     return *(double*)&ix;
174 }
175 
WRAPPER_FUNC(round)176 double WRAPPER_FUNC(round)(double x) {
177     check_nan_d1(x);
178     ui64 ix=*(ui64*)&x,m;
179     int e=dgetexp(x);
180     e-=0x3ff;                // remove exponent bias
181     if(e<-1) {               // |x|<0.5
182         ix&=0x8000000000000000ULL;
183         return *(double*)&ix;
184     }
185     if(e==-1) {              // 0.5<=|x|<1
186         ix&=0x8000000000000000ULL;
187         ix|=0x3ff0000000000000ULL;        // ±1
188         return *(double*)&ix;
189     }
190     e=52-e;                  // bit position in mantissa with significance 1, <=52
191     if(e<=0) return x;       // |x| large, so must be an integer
192     m=1ULL<<(e-1);           // mask for bit of significance 0.5
193     ix+=m;
194     m=m+m-1;                 // mask for bits of significance <1
195     ix&=~m;
196     return *(double*)&ix;
197 }
198 
WRAPPER_FUNC(floor)199 double WRAPPER_FUNC(floor)(double x) {
200     check_nan_d1(x);
201     ui64 ix=*(ui64*)&x,m;
202     int e=dgetexp(x);
203     if(e==0) {               // x==0
204         ix&=0x8000000000000000ULL;
205         return *(double*)&ix;
206     }
207     e-=0x3ff;                // remove exponent bias
208     if(e<0) {                // |x|<1, not zero
209         if(disneg(x)) return -1;
210         return PZERO;
211     }
212     e=52-e;                  // bit position in mantissa with significance 1
213     if(e<=0) return x;       // |x| large, so must be an integer
214     m=(1ULL<<e)-1;           // mask for bit of significance <1
215     if(disneg(x)) ix+=m;     // add 1-ε to magnitude if negative
216     ix&=~m;                  // truncate
217     return *(double*)&ix;
218 }
219 
WRAPPER_FUNC(ceil)220 double WRAPPER_FUNC(ceil)(double x) {
221     check_nan_d1(x);
222     ui64 ix=*(ui64*)&x,m;
223     int e=dgetexp(x);
224     if(e==0) {               // x==0
225         ix&=0x8000000000000000ULL;
226         return *(double*)&ix;
227     }
228     e-=0x3ff;                 // remove exponent bias
229     if(e<0) {                // |x|<1, not zero
230         if(disneg(x)) return MZERO;
231         return 1;
232     }
233     e=52-e;                  // bit position in mantissa with significance 1
234     if(e<=0) return x;       // |x| large, so must be an integer
235     m=(1ULL<<e)-1;           // mask for bit of significance <1
236     if(!disneg(x)) ix+=m;    // add 1-ε to magnitude if positive
237     ix&=~m;                  // truncate
238     return *(double*)&ix;
239 }
240 
WRAPPER_FUNC(asin)241 double WRAPPER_FUNC(asin)(double x) {
242     check_nan_d1(x);
243     double u;
244     u=(1-x)*(1+x);
245     if(disstrictneg(u)) return dnan_or(PINF);
246     return atan2(x,sqrt(u));
247 }
248 
WRAPPER_FUNC(acos)249 double WRAPPER_FUNC(acos)(double x) {
250     check_nan_d1(x);
251     double u;
252     u=(1-x)*(1+x);
253     if(disstrictneg(u)) return dnan_or(PINF);
254     return atan2(sqrt(u),x);
255 }
256 
WRAPPER_FUNC(atan)257 double WRAPPER_FUNC(atan)(double x) {
258     check_nan_d1(x);
259     if(dispinf(x)) return  PI/2;
260     if(disminf(x)) return -PI/2;
261     return atan2(x,1);
262 }
263 
WRAPPER_FUNC(sinh)264 double WRAPPER_FUNC(sinh)(double x) {
265     check_nan_d1(x);
266     return dldexp((exp(x)-exp(dneg(x))),-1);
267 }
268 
WRAPPER_FUNC(cosh)269 double WRAPPER_FUNC(cosh)(double x) {
270     check_nan_d1(x);
271     return dldexp((exp(x)+exp(dneg(x))),-1);
272 }
273 
WRAPPER_FUNC(tanh)274 double WRAPPER_FUNC(tanh)(double x) {
275     check_nan_d1(x);
276     double u;
277     int e;
278     e=dgetexp(x);
279     if(e>=5+0x3ff) {             // |x|>=32?
280         if(!disneg(x)) return  1;  // 1 << exp 2x; avoid generating infinities later
281         else           return -1;  // 1 >> exp 2x
282     }
283     u=exp(dldexp(x,1));
284     return (u-1)/(u+1);
285 }
286 
WRAPPER_FUNC(asinh)287 double WRAPPER_FUNC(asinh)(double x) {
288     check_nan_d1(x);
289     int e;
290     e=dgetexp(x);
291     if(e>=32+0x3ff) {                                // |x|>=2^32?
292         if(!disneg(x)) return      log(     x )+LOG2;  // 1/x^2 << 1
293         else           return dneg(log(dneg(x))+LOG2); // 1/x^2 << 1
294     }
295     if(x>0) return      log(sqrt(x*x+1)+x);
296     else    return dneg(log(sqrt(x*x+1)-x));
297 }
298 
WRAPPER_FUNC(acosh)299 double WRAPPER_FUNC(acosh)(double x) {
300     check_nan_d1(x);
301     int e;
302     if(disneg(x)) x=dneg(x);
303     e=dgetexp(x);
304     if(e>=32+0x3ff) return log(x)+LOG2;           // |x|>=2^32?
305     return log(sqrt((x-1)*(x+1))+x);
306 }
307 
WRAPPER_FUNC(atanh)308 double WRAPPER_FUNC(atanh)(double x) {
309     check_nan_d1(x);
310     return dldexp(log((1+x)/(1-x)),-1);
311 }
312 
WRAPPER_FUNC(exp2)313 double WRAPPER_FUNC(exp2)(double x) {
314     check_nan_d1(x);
315     int e;
316     // extra check for disminf as this catches -Nan, and x<=-4096 doesn't.
317     if (disminf(x) || x<=-4096) return 0;    // easily underflows
318     else if (x>=4096)           return PINF; // easily overflows
319     e=(int)round(x);
320     x-=e;
321     return dldexp(exp(x*LOG2),e);
322 }
WRAPPER_FUNC(log2)323 double WRAPPER_FUNC(log2)(double x) { check_nan_d1(x); return log(x)*LOG2E;  }
WRAPPER_FUNC(exp10)324 double WRAPPER_FUNC(exp10)(double x) { check_nan_d1(x); return pow(10,x); }
WRAPPER_FUNC(log10)325 double WRAPPER_FUNC(log10)(double x) { check_nan_d1(x); return log(x)*LOG10E; }
326 
327 // todo these are marked as lofi
328 double WRAPPER_FUNC(expm1(double x) { check_nan_d1(x); return exp)(x)-1; }
329 double WRAPPER_FUNC(log1p(double x) { check_nan_d1(x); return log)(1+x); }
330 double WRAPPER_FUNC(fma)(double x,double y,double z) { check_nan_d1(x); return x*y+z; }
331 
332 // general power, x>0, finite
333 static double dpow_1(double x,double y) {
334     int a,b,c;
335     double t,rt,u,v,v0,v1,w,ry;
336     a=dgetexp(x)-0x3ff;
337     u=log2(dldexp(x,-a)); // now log_2 x = a+u
338     if(u>0.5) u-=1,a++;              // |u|<=~0.5
339     if(a==0) return exp2(u*y);
340     // here |log_2 x| >~0.5
341     if(y>= 4096) { // then easily over/underflows
342         if(a<0) return 0;
343         return PINF;
344     }
345     if(y<=-4096) { // then easily over/underflows
346         if(a<0) return PINF;
347         return 0;
348     }
349     ry=round(y);
350     v=y-ry;
351     v0=dldexp(round(ldexp(v,26)),-26);
352     v1=v-v0;
353     b=(int)ry; // guaranteed to fit in an int; y=b+v0+v1
354     // now the result is exp2( (a+u) * (b+v0+v1) )
355     c=a*b;      // integer
356     t=a*v0;
357     rt=round(t);
358     c+=(int)rt;
359     w=t-rt;
360     t=a*v1;
361     w+=t;
362     t=u*b;
363     rt=round(t);
364     c+=(int)rt;
365     w+=t-rt;
366     w+=u*v;
367     return dldexp(exp2(w),c);
368 }
369 
370 static double dpow_int2(double x,int y) {
371     double u;
372     if(y==1) return x;
373     u=dpow_int2(x,y/2);
374     u*=u;
375     if(y&1) u*=x;
376     return u;
377 }
378 
379 // for the case where x not zero or infinity, y small and not zero
380 static inline double dpowint_1(double x,int y) {
381     if(y<0) x=1/x,y=-y;
382     return dpow_int2(x,y);
383 }
384 
385 // for the case where x not zero or infinity
386 static double dpowint_0(double x,int y) {
387     int e;
388     if(disneg(x)) {
389         if(disoddint(y)) return dneg(dpowint_0(dneg(x),y));
390         else             return      dpowint_0(dneg(x),y);
391     }
392     if(dispo2(x)) {
393         e=dgetexp(x)-0x3ff;
394         if(y>=2048) y= 2047;  // avoid overflow
395         if(y<-2048) y=-2048;
396         y*=e;
397         return dldexp(1,y);
398     }
399     if(y==0) return 1;
400     if(y>=-32&&y<=32) return dpowint_1(x,y);
401     return dpow_1(x,y);
402 }
403 
404 double WRAPPER_FUNC(powint)(double x,int y) {
405     _Pragma("GCC diagnostic push")
406     _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
407     if(x==1.0||y==0) return 1;
408     _Pragma("GCC diagnostic pop")
409     check_nan_d1(x);
410     if(diszero(x)) {
411         if(y>0) {
412             if(y&1) return x;
413             else    return 0;
414         }
415         if((y&1)) return dcopysign(PINF,x);
416         return PINF;
417     }
418     if(dispinf(x)) {
419         if(y<0) return 0;
420         else    return PINF;
421     }
422     if(disminf(x)) {
423         if(y>0) {
424             if((y&1)) return MINF;
425             else      return PINF;
426         }
427         if((y&1)) return MZERO;
428         else      return PZERO;
429     }
430     return dpowint_0(x,y);
431 }
432 
433 // for the case where y is guaranteed a finite integer, x not zero or infinity
434 static double dpow_0(double x,double y) {
435     int e,p;
436     if(disneg(x)) {
437         if(disoddint(y)) return dneg(dpow_0(dneg(x),y));
438         else             return      dpow_0(dneg(x),y);
439     }
440     p=(int)y;
441     if(dispo2(x)) {
442         e=dgetexp(x)-0x3ff;
443         if(p>=2048) p= 2047;  // avoid overflow
444         if(p<-2048) p=-2048;
445         p*=e;
446         return dldexp(1,p);
447     }
448     if(p==0) return 1;
449     if(p>=-32&&p<=32) return dpowint_1(x,p);
450     return dpow_1(x,y);
451 }
452 
453 double WRAPPER_FUNC(pow)(double x,double y) {
454     _Pragma("GCC diagnostic push")
455     _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
456 
457     if(x==1.0||diszero(y)) return 1;
458     check_nan_d2(x, y);
459     if(x==-1.0&&disinf(y)) return 1;
460     _Pragma("GCC diagnostic pop")
461 
462     if(diszero(x)) {
463         if(!disneg(y)) {
464             if(disoddint(y)) return x;
465             else             return 0;
466         }
467         if(disoddint(y)) return dcopysign(PINF,x);
468         return PINF;
469     }
470     if(dispinf(x)) {
471         if(disneg(y)) return 0;
472         else          return PINF;
473     }
474     if(disminf(x)) {
475         if(!disneg(y)) {
476             if(disoddint(y)) return MINF;
477             else             return PINF;
478         }
479         if(disoddint(y)) return MZERO;
480         else             return PZERO;
481     }
482     if(dispinf(y)) {
483         if(dgetexp(x)<0x3ff) return PZERO;
484         else                 return PINF;
485     }
486     if(disminf(y)) {
487         if(dgetexp(x)<0x3ff) return PINF;
488         else                 return PZERO;
489     }
490     if(disint(y)) return dpow_0(x,y);
491     if(disneg(x)) return PINF;
492     return dpow_1(x,y);
493 }
494 
495 double WRAPPER_FUNC(hypot)(double x,double y) {
496     check_nan_d2(x, y);
497     int ex,ey;
498     ex=dgetexp(x); ey=dgetexp(y);
499     if(ex>=0x3ff+400||ey>=0x3ff+400) { // overflow, or nearly so
500         x=dldexp(x,-600),y=dldexp(y,-600);
501         return dldexp(sqrt(x*x+y*y), 600);
502     }
503     else if(ex<=0x3ff-400&&ey<=0x3ff-400) { // underflow, or nearly so
504         x=dldexp(x, 600),y=dldexp(y, 600);
505         return dldexp(sqrt(x*x+y*y),-600);
506     }
507     return sqrt(x*x+y*y);
508 }
509 
510 double WRAPPER_FUNC(cbrt)(double x) {
511     check_nan_d1(x);
512     int e;
513     if(disneg(x)) return dneg(cbrt(dneg(x)));
514     if(diszero(x)) return dcopysign(PZERO,x);
515     e=dgetexp(x)-0x3ff;
516     e=(e*0x5555+0x8000)>>16;  // ~e/3, rounded
517     x=dldexp(x,-e*3);
518     x=exp(log(x)*ONETHIRD);
519     return dldexp(x,e);
520 }
521 
522 // reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
523 // 2^52<=|mx|,my<2^53, e>=0; 0<=result<my
524 static i64 drem_0(i64 mx,i64 my,int e,int*pquo) {
525     int quo=0,q,r=0,s;
526     if(e>0) {
527         r=0xffffffffU/(ui32)(my>>36);  // reciprocal estimate Q16
528     }
529     while(e>0) {
530         s=e; if(s>12) s=12;    // gain up to 12 bits on each iteration
531         q=(mx>>38)*r;          // Q30
532         q=((q>>(29-s))+1)>>1;  // Q(s), rounded
533         mx=(mx<<s)-my*q;
534         quo=(quo<<s)+q;
535         e-=s;
536     }
537     if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
538     if(mx>=my) mx-=my,quo++;
539     if(mx<0) mx+=my,quo--;
540     if(mx<0) mx+=my,quo--;
541     if(pquo) *pquo=quo;
542     return mx;
543 }
544 
545 double WRAPPER_FUNC(fmod)(double x,double y) {
546     check_nan_d2(x, y);
547     ui64 ix=*(ui64*)&x,iy=*(ui64*)&y;
548     int sx,ex,ey;
549     i64 mx,my;
550     DUNPACKS(ix,sx,ex,mx);
551     DUNPACK(iy,ey,my);
552     if(ex==0x7ff) return dnan_or(PINF);
553     if(ey==0) return PINF;
554     if(ex==0) {
555         if(!disneg(x)) return PZERO;
556         return MZERO;
557     }
558     if(ex<ey) return x;  // |x|<|y|, including case x=±0
559     mx=drem_0(mx,my,ex-ey,0);
560     if(sx) mx=-mx;
561     return fix642double(mx,0x3ff-ey+52);
562 }
563 
564 double WRAPPER_FUNC(remquo)(double x,double y,int*quo) {
565     check_nan_d2(x, y);
566     ui64 ix=*(ui64*)&x,iy=*(ui64*)&y;
567     int sx,sy,ex,ey,q;
568     i64 mx,my;
569     DUNPACKS(ix,sx,ex,mx);
570     DUNPACKS(iy,sy,ey,my);
571     if(quo) *quo=0;
572     if(ex==0x7ff) return PINF;
573     if(ey==0)     return PINF;
574     if(ex==0)     return PZERO;
575     if(ey==0x7ff) return x;
576     if(ex<ey-1)   return x;  // |x|<|y|/2
577     if(ex==ey-1) {
578         if(mx<=my) return x;   // |x|<=|y|/2, even quotient
579         // here |y|/2<|x|<|y|
580         if(!sx) { // x>|y|/2
581             mx-=my+my;
582             ey--;
583             q=1;
584         } else { // x<-|y|/2
585             mx=my+my-mx;
586             ey--;
587             q=-1;
588         }
589     }
590     else {
591         if(sx) mx=-mx;
592         mx=drem_0(mx,my,ex-ey,&q);
593         if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
594             mx-=my;
595             q++;
596         }
597     }
598     if(sy) q=-q;
599     if(quo) *quo=q;
600     return fix642double(mx,0x3ff-ey+52);
601 }
602 
603 double WRAPPER_FUNC(drem)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
604 
605 double WRAPPER_FUNC(remainder)(double x,double y) { check_nan_d2(x, y); return remquo(x,y,0); }
606 
607 _Pragma("GCC diagnostic pop") // strict-aliasing