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