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