1 /*
2  * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #include "pico/types.h"
8 #include "pico/float.h"
9 #include "pico/platform.h"
10 
11 typedef uint32_t ui32;
12 typedef int32_t i32;
13 
14 #define PINF ( HUGE_VAL)
15 #define MINF (-HUGE_VAL)
16 #define NANF ((float)NAN)
17 #define PZERO (+0.0)
18 #define MZERO (-0.0)
19 
20 #define PI       3.14159265358979323846
21 #define LOG2     0.69314718055994530941
22 // Unfortunately in double precision ln(10) is very close to half-way between to representable numbers
23 #define LOG10    2.30258509299404568401
24 #define LOG2E    1.44269504088896340737
25 #define LOG10E   0.43429448190325182765
26 #define ONETHIRD 0.33333333333333333333
27 
28 #define PIf       3.14159265358979323846f
29 #define LOG2f     0.69314718055994530941f
30 #define LOG2Ef    1.44269504088896340737f
31 #define LOG10Ef   0.43429448190325182765f
32 #define ONETHIRDf 0.33333333333333333333f
33 
34 #define FUNPACK(x,e,m) e=((x)>>23)&0xff,m=((x)&0x007fffff)|0x00800000
35 #define FUNPACKS(x,s,e,m) s=((x)>>31),FUNPACK((x),(e),(m))
36 
37 _Pragma("GCC diagnostic push")
38 _Pragma("GCC diagnostic ignored \"-Wstrict-aliasing\"")
39 
fisnan(float x)40 static inline bool fisnan(float x) {
41     ui32 ix=*(i32*)&x;
42     return ix * 2 > 0xff000000u;
43 }
44 
45 #if PICO_FLOAT_PROPAGATE_NANS
46 #define check_nan_f1(x) if (fisnan((x))) return (x)
47 #define check_nan_f2(x,y) if (fisnan((x))) return (x); else if (fisnan((y))) return (y);
48 #else
49 #define check_nan_f1(x) ((void)0)
50 #define check_nan_f2(x,y) ((void)0)
51 #endif
52 
fgetsignexp(float x)53 static inline int fgetsignexp(float x) {
54     ui32 ix=*(ui32*)&x;
55     return (ix>>23)&0x1ff;
56 }
57 
fgetexp(float x)58 static inline int fgetexp(float x) {
59     ui32 ix=*(ui32*)&x;
60     return (ix>>23)&0xff;
61 }
62 
fldexp(float x,int de)63 static inline float fldexp(float x,int de) {
64     ui32 ix=*(ui32*)&x,iy;
65     int e;
66     e=fgetexp(x);
67     if(e==0||e==0xff) return x;
68     e+=de;
69     if(e<=0) iy=ix&0x80000000; // signed zero for underflow
70     else if(e>=0xff) iy=(ix&0x80000000)|0x7f800000ULL; // signed infinity on overflow
71     else iy=ix+((ui32)de<<23);
72     return *(float*)&iy;
73 }
74 
WRAPPER_FUNC(ldexpf)75 float WRAPPER_FUNC(ldexpf)(float x, int de) {
76     check_nan_f1(x);
77     return fldexp(x, de);
78 }
79 
fcopysign(float x,float y)80 static inline float fcopysign(float x,float y) {
81     ui32 ix=*(ui32*)&x,iy=*(ui32*)&y;
82     ix=((ix&0x7fffffff)|(iy&0x80000000));
83     return *(float*)&ix;
84 }
85 
WRAPPER_FUNC(copysignf)86 float WRAPPER_FUNC(copysignf)(float x, float y) {
87     check_nan_f2(x,y);
88     return fcopysign(x, y);
89 }
90 
fiszero(float x)91 static inline int fiszero(float x)  { return fgetexp    (x)==0; }
fispzero(float x)92 static inline int fispzero(float x) { return fgetsignexp(x)==0; }
fismzero(float x)93 static inline int fismzero(float x) { return fgetsignexp(x)==0x100; }
fisinf(float x)94 static inline int fisinf(float x)   { return fgetexp    (x)==0xff; }
fispinf(float x)95 static inline int fispinf(float x)  { return fgetsignexp(x)==0xff; }
fisminf(float x)96 static inline int fisminf(float x)  { return fgetsignexp(x)==0x1ff; }
97 
fisint(float x)98 static inline int fisint(float x) {
99     ui32 ix=*(ui32*)&x,m;
100     int e=fgetexp(x);
101     if(e==0) return 1;       // 0 is an integer
102     e-=0x7f;                 // remove exponent bias
103     if(e<0) return 0;        // |x|<1
104     e=23-e;                  // bit position in mantissa with significance 1
105     if(e<=0) return 1;       // |x| large, so must be an integer
106     m=(1<<e)-1;              // mask for bits of significance <1
107     if(ix&m) return 0;       // not an integer
108     return 1;
109 }
110 
fisoddint(float x)111 static inline int fisoddint(float x) {
112     ui32 ix=*(ui32*)&x,m;
113     int e=fgetexp(x);
114     e-=0x7f;                 // remove exponent bias
115     if(e<0) return 0;        // |x|<1; 0 is not odd
116     e=23-e;                  // bit position in mantissa with significance 1
117     if(e<0) return 0;        // |x| large, so must be even
118     m=(1<<e)-1;              // mask for bits of significance <1 (if any)
119     if(ix&m) return 0;       // not an integer
120     if(e==23) return 1;      // value is exactly 1
121     return (ix>>e)&1;
122 }
123 
fisstrictneg(float x)124 static inline int fisstrictneg(float x) {
125     ui32 ix=*(ui32*)&x;
126     if(fiszero(x)) return 0;
127     return ix>>31;
128 }
129 
fisneg(float x)130 static inline int fisneg(float x) {
131     ui32 ix=*(ui32*)&x;
132     return ix>>31;
133 }
134 
fneg(float x)135 static inline float fneg(float x) {
136     ui32 ix=*(ui32*)&x;
137     ix^=0x80000000;
138     return *(float*)&ix;
139 }
140 
fispo2(float x)141 static inline int fispo2(float x) {
142     ui32 ix=*(ui32*)&x;
143     if(fiszero(x)) return 0;
144     if(fisinf(x)) return 0;
145     ix&=0x007fffff;
146     return ix==0;
147 }
148 
fnan_or(float x)149 static inline float fnan_or(float x) {
150 #if PICO_FLOAT_PROPAGATE_NANS
151     return NANF;
152 #else
153     return x;
154 #endif
155 }
156 
WRAPPER_FUNC(truncf)157 float WRAPPER_FUNC(truncf)(float x) {
158     check_nan_f1(x);
159     ui32 ix=*(ui32*)&x,m;
160     int e=fgetexp(x);
161     e-=0x7f;                 // remove exponent bias
162     if(e<0) {                // |x|<1
163         ix&=0x80000000;
164         return *(float*)&ix;
165     }
166     e=23-e;                  // bit position in mantissa with significance 1
167     if(e<=0) return x;       // |x| large, so must be an integer
168     m=(1<<e)-1;              // mask for bits of significance <1
169     ix&=~m;
170     return *(float*)&ix;
171 }
172 
WRAPPER_FUNC(roundf)173 float WRAPPER_FUNC(roundf)(float x) {
174     check_nan_f1(x);
175     ui32 ix=*(ui32*)&x,m;
176     int e=fgetexp(x);
177     e-=0x7f;                 // remove exponent bias
178     if(e<-1) {               // |x|<0.5
179         ix&=0x80000000;
180         return *(float*)&ix;
181     }
182     if(e==-1) {              // 0.5<=|x|<1
183         ix&=0x80000000;
184         ix|=0x3f800000;        // ±1
185         return *(float*)&ix;
186     }
187     e=23-e;                  // bit position in mantissa with significance 1, <=23
188     if(e<=0) return x;       // |x| large, so must be an integer
189     m=1<<(e-1);              // mask for bit of significance 0.5
190     ix+=m;
191     m=m+m-1;                 // mask for bits of significance <1
192     ix&=~m;
193     return *(float*)&ix;
194 }
195 
WRAPPER_FUNC(floorf)196 float WRAPPER_FUNC(floorf)(float x) {
197     check_nan_f1(x);
198     ui32 ix=*(ui32*)&x,m;
199     int e=fgetexp(x);
200     if(e==0) {       // x==0
201         ix&=0x80000000;
202         return *(float*)&ix;
203     }
204     e-=0x7f;                 // remove exponent bias
205     if(e<0) {                // |x|<1, not zero
206         if(fisneg(x)) return -1;
207         return PZERO;
208     }
209     e=23-e;                  // bit position in mantissa with significance 1
210     if(e<=0) return x;       // |x| large, so must be an integer
211     m=(1<<e)-1;              // mask for bit of significance <1
212     if(fisneg(x)) ix+=m;     // add 1-ε to magnitude if negative
213     ix&=~m;                  // truncate
214     return *(float*)&ix;
215 }
216 
WRAPPER_FUNC(ceilf)217 float WRAPPER_FUNC(ceilf)(float x) {
218     check_nan_f1(x);
219     ui32 ix=*(ui32*)&x,m;
220     int e=fgetexp(x);
221     if(e==0) {       // x==0
222         ix&=0x80000000;
223         return *(float*)&ix;
224     }
225     e-=0x7f;                 // remove exponent bias
226     if(e<0) {                // |x|<1, not zero
227         if(fisneg(x)) return MZERO;
228         return 1;
229     }
230     e=23-e;                  // bit position in mantissa with significance 1
231     if(e<=0) return x;       // |x| large, so must be an integer
232     m=(1<<e)-1;              // mask for bit of significance <1
233     if(!fisneg(x)) ix+=m;    // add 1-ε to magnitude if positive
234     ix&=~m;                  // truncate
235     return *(float*)&ix;
236 }
237 
WRAPPER_FUNC(asinf)238 float WRAPPER_FUNC(asinf)(float x) {
239     check_nan_f1(x);
240     float u;
241     u=(1.0f-x)*(1.0f+x);
242     if(fisstrictneg(u)) return fnan_or(PINF);
243     return atan2f(x,sqrtf(u));
244 }
245 
WRAPPER_FUNC(acosf)246 float WRAPPER_FUNC(acosf)(float x) {
247     check_nan_f1(x);
248     float u;
249     u=(1.0f-x)*(1.0f+x);
250     if(fisstrictneg(u)) return fnan_or(PINF);
251     return atan2f(sqrtf(u),x);
252 }
253 
WRAPPER_FUNC(atanf)254 float WRAPPER_FUNC(atanf)(float x) {
255     check_nan_f1(x);
256     if(fispinf(x)) return (float)( PIf/2);
257     if(fisminf(x)) return (float)(-PIf/2);
258     return atan2f(x,1.0f);
259 }
260 
WRAPPER_FUNC(sinhf)261 float WRAPPER_FUNC(sinhf)(float x) {
262     check_nan_f1(x);
263     return fldexp((expf(x)-expf(fneg(x))),-1);
264 }
265 
WRAPPER_FUNC(coshf)266 float WRAPPER_FUNC(coshf)(float x) {
267     check_nan_f1(x);
268     return fldexp((expf(x)+expf(fneg(x))),-1);
269 }
270 
WRAPPER_FUNC(tanhf)271 float WRAPPER_FUNC(tanhf)(float x) {
272     check_nan_f1(x);
273     float u;
274     int e;
275     e=fgetexp(x);
276     if(e>=4+0x7f) {             // |x|>=16?
277         if(!fisneg(x)) return  1;  // 1 << exp 2x; avoid generating infinities later
278         else           return -1;  // 1 >> exp 2x
279     }
280     u=expf(fldexp(x,1));
281     return (u-1.0f)/(u+1.0f);
282 }
283 
WRAPPER_FUNC(asinhf)284 float WRAPPER_FUNC(asinhf)(float x) {
285     check_nan_f1(x);
286     int e;
287     e=fgetexp(x);
288     if(e>=16+0x7f) {                                   // |x|>=2^16?
289         if(!fisneg(x)) return      logf(     x )+LOG2f;  // 1/x^2 << 1
290         else           return fneg(logf(fneg(x))+LOG2f); // 1/x^2 << 1
291     }
292     if(x>0) return      (float)log(sqrt((double)x*(double)x+1.0)+(double)x);
293     else    return fneg((float)log(sqrt((double)x*(double)x+1.0)-(double)x));
294 }
295 
WRAPPER_FUNC(acoshf)296 float WRAPPER_FUNC(acoshf)(float x) {
297     check_nan_f1(x);
298     int e;
299     if(fisneg(x)) x=fneg(x);
300     e=fgetexp(x);
301     if(e>=16+0x7f) return logf(x)+LOG2f;           // |x|>=2^16?
302     return (float)log(sqrt(((double)x+1.0)*((double)x-1.0))+(double)x);
303 }
304 
WRAPPER_FUNC(atanhf)305 float WRAPPER_FUNC(atanhf)(float x) {
306     check_nan_f1(x);
307     return fldexp(logf((1.0f+x)/(1.0f-x)),-1);
308 }
309 
WRAPPER_FUNC(exp2f)310 float WRAPPER_FUNC(exp2f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG2); }
WRAPPER_FUNC(log2f)311 float WRAPPER_FUNC(log2f)(float x) { check_nan_f1(x); return logf(x)*LOG2Ef;  }
WRAPPER_FUNC(exp10f)312 float WRAPPER_FUNC(exp10f)(float x) { check_nan_f1(x); return (float)exp((double)x*LOG10); }
WRAPPER_FUNC(log10f)313 float WRAPPER_FUNC(log10f)(float x) { check_nan_f1(x); return logf(x)*LOG10Ef; }
314 
WRAPPER_FUNC(expm1f)315 float WRAPPER_FUNC(expm1f)(float x) { check_nan_f1(x); return (float)(exp((double)x)-1); }
WRAPPER_FUNC(log1pf)316 float WRAPPER_FUNC(log1pf)(float x) { check_nan_f1(x); return (float)(log(1+(double)x)); }
WRAPPER_FUNC(fmaf)317 float WRAPPER_FUNC(fmaf)(float x,float y,float z) {
318     check_nan_f2(x,y);
319     check_nan_f1(z);
320     return (float)((double)x*(double)y+(double)z);
321 } // has double rounding so not exact
322 
323 // general power, x>0
fpow_1(float x,float y)324 static inline float fpow_1(float x,float y) {
325     return (float)exp(log((double)x)*(double)y); // using double-precision intermediates for better accuracy
326 }
327 
fpow_int2(float x,int y)328 static float fpow_int2(float x,int y) {
329     float u;
330     if(y==1) return x;
331     u=fpow_int2(x,y/2);
332     u*=u;
333     if(y&1) u*=x;
334     return u;
335 }
336 
337 // for the case where x not zero or infinity, y small and not zero
fpowint_1(float x,int y)338 static inline float fpowint_1(float x,int y) {
339     if(y<0) x=1.0f/x,y=-y;
340     return fpow_int2(x,y);
341 }
342 
343 // for the case where x not zero or infinity
fpowint_0(float x,int y)344 static float fpowint_0(float x,int y) {
345     int e;
346     if(fisneg(x)) {
347         if(fisoddint(y)) return fneg(fpowint_0(fneg(x),y));
348         else             return      fpowint_0(fneg(x),y);
349     }
350     if(fispo2(x)) {
351         e=fgetexp(x)-0x7f;
352         if(y>=256) y= 255;  // avoid overflow
353         if(y<-256) y=-256;
354         y*=e;
355         return fldexp(1,y);
356     }
357     if(y==0) return 1;
358     if(y>=-32&&y<=32) return fpowint_1(x,y);
359     return fpow_1(x,y);
360 }
361 
WRAPPER_FUNC(powintf)362 float WRAPPER_FUNC(powintf)(float x,int y) {
363     _Pragma("GCC diagnostic push")
364     _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
365     if(x==1.0f||y==0) return 1;
366     if(x==0.0f) {
367         if(y>0) {
368             if(y&1) return x;
369             else    return 0;
370         }
371         if((y&1)) return fcopysign(PINF,x);
372         return PINF;
373     }
374     _Pragma("GCC diagnostic pop")
375     check_nan_f1(x);
376     if(fispinf(x)) {
377         if(y<0) return 0;
378         else    return PINF;
379     }
380     if(fisminf(x)) {
381         if(y>0) {
382             if((y&1)) return MINF;
383             else      return PINF;
384         }
385         if((y&1)) return MZERO;
386         else      return PZERO;
387     }
388     return fpowint_0(x,y);
389 }
390 
391 // for the case where y is guaranteed a finite integer, x not zero or infinity
fpow_0(float x,float y)392 static float fpow_0(float x,float y) {
393     int e,p;
394     if(fisneg(x)) {
395         if(fisoddint(y)) return fneg(fpow_0(fneg(x),y));
396         else             return      fpow_0(fneg(x),y);
397     }
398     p=(int)y;
399     if(fispo2(x)) {
400         e=fgetexp(x)-0x7f;
401         if(p>=256) p= 255;  // avoid overflow
402         if(p<-256) p=-256;
403         p*=e;
404         return fldexp(1,p);
405     }
406     if(p==0) return 1;
407     if(p>=-32&&p<=32) return fpowint_1(x,p);
408     return fpow_1(x,y);
409 }
410 
WRAPPER_FUNC(powf)411 float WRAPPER_FUNC(powf)(float x,float y) {
412     _Pragma("GCC diagnostic push")
413     _Pragma("GCC diagnostic ignored \"-Wfloat-equal\"")
414     if(x==1.0f||fiszero(y)) return 1;
415     check_nan_f2(x,y);
416     if(x==-1.0f&&fisinf(y)) return 1;
417     _Pragma("GCC diagnostic pop")
418     if(fiszero(x)) {
419         if(!fisneg(y)) {
420             if(fisoddint(y)) return x;
421             else             return 0;
422         }
423         if(fisoddint(y)) return fcopysign(PINF,x);
424         return PINF;
425     }
426     if(fispinf(x)) {
427         if(fisneg(y)) return 0;
428         else          return PINF;
429     }
430     if(fisminf(x)) {
431         if(!fisneg(y)) {
432             if(fisoddint(y)) return MINF;
433             else             return PINF;
434         }
435         if(fisoddint(y)) return MZERO;
436         else             return PZERO;
437     }
438     if(fispinf(y)) {
439         if(fgetexp(x)<0x7f) return PZERO;
440         else                return PINF;
441     }
442     if(fisminf(y)) {
443         if(fgetexp(x)<0x7f) return PINF;
444         else                return PZERO;
445     }
446     if(fisint(y)) return fpow_0(x,y);
447     if(fisneg(x)) return PINF;
448     return fpow_1(x,y);
449 }
450 
WRAPPER_FUNC(hypotf)451 float WRAPPER_FUNC(hypotf)(float x,float y) {
452     check_nan_f2(x,y);
453     int ex,ey;
454     ex=fgetexp(x); ey=fgetexp(y);
455     if(ex>=0x7f+50||ey>=0x7f+50) { // overflow, or nearly so
456         x=fldexp(x,-70),y=fldexp(y,-70);
457         return fldexp(sqrtf(x*x+y*y), 70);
458     }
459     else if(ex<=0x7f-50&&ey<=0x7f-50) { // underflow, or nearly so
460         x=fldexp(x, 70),y=fldexp(y, 70);
461         return fldexp(sqrtf(x*x+y*y),-70);
462     }
463     return sqrtf(x*x+y*y);
464 }
465 
WRAPPER_FUNC(cbrtf)466 float WRAPPER_FUNC(cbrtf)(float x) {
467     check_nan_f1(x);
468     int e;
469     if(fisneg(x)) return fneg(cbrtf(fneg(x)));
470     if(fiszero(x)) return fcopysign(PZERO,x);
471     e=fgetexp(x)-0x7f;
472     e=(e*0x5555+0x8000)>>16;  // ~e/3, rounded
473     x=fldexp(x,-e*3);
474     x=expf(logf(x)*ONETHIRDf);
475     return fldexp(x,e);
476 }
477 
478 // reduces mx*2^e modulo my, returning bottom bits of quotient at *pquo
479 // 2^23<=|mx|,my<2^24, e>=0; 0<=result<my
frem_0(i32 mx,i32 my,int e,int * pquo)480 static i32 frem_0(i32 mx,i32 my,int e,int*pquo) {
481     int quo=0,q,r=0,s;
482     if(e>0) {
483         r=0xffffffffU/(ui32)(my>>7);  // reciprocal estimate Q16
484     }
485     while(e>0) {
486         s=e; if(s>12) s=12;    // gain up to 12 bits on each iteration
487         q=(mx>>9)*r;           // Q30
488         q=((q>>(29-s))+1)>>1;  // Q(s), rounded
489         mx=(mx<<s)-my*q;
490         quo=(quo<<s)+q;
491         e-=s;
492     }
493     if(mx>=my) mx-=my,quo++; // when e==0 mx can be nearly as big as 2my
494     if(mx>=my) mx-=my,quo++;
495     if(mx<0) mx+=my,quo--;
496     if(mx<0) mx+=my,quo--;
497     if(pquo) *pquo=quo;
498     return mx;
499 }
500 
WRAPPER_FUNC(fmodf)501 float WRAPPER_FUNC(fmodf)(float x,float y) {
502     check_nan_f2(x,y);
503     ui32 ix=*(ui32*)&x,iy=*(ui32*)&y;
504     int sx,ex,ey;
505     i32 mx,my;
506     FUNPACKS(ix,sx,ex,mx);
507     FUNPACK(iy,ey,my);
508     if(ex==0xff) {
509         return fnan_or(PINF);
510     }
511     if(ey==0) return PINF;
512     if(ex==0) {
513         if(!fisneg(x)) return PZERO;
514         return MZERO;
515     }
516     if(ex<ey) return x;  // |x|<|y|, including case x=±0
517     mx=frem_0(mx,my,ex-ey,0);
518     if(sx) mx=-mx;
519     return fix2float(mx,0x7f-ey+23);
520 }
521 
WRAPPER_FUNC(remquof)522 float WRAPPER_FUNC(remquof)(float x,float y,int*quo) {
523     check_nan_f2(x,y);
524     ui32 ix=*(ui32*)&x,iy=*(ui32*)&y;
525     int sx,sy,ex,ey,q;
526     i32 mx,my;
527     FUNPACKS(ix,sx,ex,mx);
528     FUNPACKS(iy,sy,ey,my);
529     if(quo) *quo=0;
530     if(ex==0xff) return PINF;
531     if(ey==0)    return PINF;
532     if(ex==0)    return PZERO;
533     if(ey==0xff) return x;
534     if(ex<ey-1)  return x;  // |x|<|y|/2
535     if(ex==ey-1) {
536         if(mx<=my) return x;  // |x|<=|y|/2, even quotient
537         // here |y|/2<|x|<|y|
538         if(!sx) { // x>|y|/2
539             mx-=my+my;
540             ey--;
541             q=1;
542         } else { // x<-|y|/2
543             mx=my+my-mx;
544             ey--;
545             q=-1;
546         }
547     }
548     else {
549         if(sx) mx=-mx;
550         mx=frem_0(mx,my,ex-ey,&q);
551         if(mx+mx>my || (mx+mx==my&&(q&1)) ) { // |x|>|y|/2, or equality and an odd quotient?
552             mx-=my;
553             q++;
554         }
555     }
556     if(sy) q=-q;
557     if(quo) *quo=q;
558     return fix2float(mx,0x7f-ey+23);
559 }
560 
WRAPPER_FUNC(dremf)561 float WRAPPER_FUNC(dremf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
562 
WRAPPER_FUNC(remainderf)563 float WRAPPER_FUNC(remainderf)(float x,float y) { check_nan_f2(x,y); return remquof(x,y,0); }
564 
565 _Pragma("GCC diagnostic pop") // strict-aliasing
566