Debian Codegraph @

libgcc/config/avr/libf7/libf7.c

raw
1 /* Copyright (C) 2019-2025 Free Software Foundation, Inc.
2
3 This file is part of LIBF7, which is part of GCC.
4
5 GCC is free software; you can redistribute it and/or modify it under
6 the terms of the GNU General Public License as published by the Free
7 Software Foundation; either version 3, or (at your option) any later
8 version.
9
10 GCC is distributed in the hope that it will be useful, but WITHOUT ANY
11 WARRANTY; without even the implied warranty of MERCHANTABILITY or
12 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
13 for more details.
14
15 Under Section 7 of GPL version 3, you are granted additional
16 permissions described in the GCC Runtime Library Exception, version
17 3.1, as published by the Free Software Foundation.
18
19 You should have received a copy of the GNU General Public License and
20 a copy of the GCC Runtime Library Exception along with this program;
21 see the files COPYING3 and COPYING.RUNTIME respectively. If not, see
22 <http://www.gnu.org/licenses/>. */
23
24 #include "libf7.h"
25
26 #ifndef __AVR_TINY__
27
28 #define ALIAS(X, Y) \
29 F7_WEAK __attribute__((__alias__(F7_STRINGY(X)))) __typeof__(X) Y;
30
31 #define DALIAS(...) // empty
32 #define LALIAS(...) // empty
33
34 #ifndef IN_LIBGCC2
35
36 #include <stdio.h>
37 #include <assert.h>
38
39 #define in_libgcc false
40
41 _Static_assert (sizeof (f7_t) == 10 && F7_MANT_BYTES == 7,
42 "libf7 will only work with 7-byte mantissa.");
43 #else
44
45 #define in_libgcc true
46
47 #if __SIZEOF_DOUBLE__ == 8
48 #undef DALIAS
49 #define DALIAS(X,Y) \
50 F7_WEAK __attribute__((__alias__(F7_STRINGY(X)))) __typeof__(X) Y;
51 #endif
52
53 #if __SIZEOF_LONG_DOUBLE__ == 8
54 #undef LALIAS
55 #define LALIAS(X,Y) \
56 F7_WEAK __attribute__((__alias__(F7_STRINGY(X)))) __typeof__(X) Y;
57 #endif
58
59 #endif // in libgcc
60
61 static F7_INLINE
62 void f7_assert (bool x)
63 {
64 if (!in_libgcc && !x)
65 __builtin_abort();
66 }
67
68 static F7_INLINE
69 int16_t abs_ssat16 (int16_t a)
70 {
71 _Sat _Fract sa = __builtin_avr_rbits (a);
72 return __builtin_avr_bitsr (__builtin_avr_absr (sa));
73 }
74
75 static F7_INLINE
76 int16_t add_ssat16 (int16_t a, int16_t b)
77 {
78 _Sat _Fract sa = __builtin_avr_rbits (a);
79 _Sat _Fract sb = __builtin_avr_rbits (b);
80 return __builtin_avr_bitsr (sa + sb);
81 }
82
83 static F7_INLINE
84 int16_t sub_ssat16 (int16_t a, int16_t b)
85 {
86 _Sat _Fract sa = __builtin_avr_rbits (a);
87 _Sat _Fract sb = __builtin_avr_rbits (b);
88 return __builtin_avr_bitsr (sa - sb);
89 }
90
91 static F7_INLINE
92 int8_t ssat8_range (int16_t a, int8_t range)
93 {
94 if (a >= range)
95 return range;
96 if (a <= -range)
97 return -range;
98 return a;
99 }
100
101
102 #define IN_LIBF7_H
103 #define F7_CONST_DEF(NAME, FLAGS, M6, M5, M4, M3, M2, M1, M0, EXPO) \
104 F7_UNUSED static const uint8_t F7_(const_##NAME##_msb) = M6; \
105 F7_UNUSED static const int16_t F7_(const_##NAME##_expo) = EXPO;
106 #include "libf7-const.def"
107 #undef F7_CONST_DEF
108 #undef IN_LIBF7_H
109
110
111 /*
112 libgcc naming converntions for conversions:
113
114 __float<fmode><fmode> : Convert float modes.
115 __floatun<imode><fmode>: Convert unsigned integral to float.
116 __fix<fmode><imode> : Convert float to signed integral.
117 __fixuns<fmode><imode> : Convert float to unsigned integral.
118 */
119
120
121 #ifdef F7MOD_floatundidf_
122 F7_WEAK
123 f7_double_t __floatundidf (uint64_t x)
124 {
125 f7_t xx;
126 f7_set_u64 (&xx, x);
127 return f7_get_double (&xx);
128 }
129 #endif // F7MOD_floatundidf_
130
131
132 #ifdef F7MOD_floatdidf_
133 F7_WEAK
134 f7_double_t __floatdidf (int64_t x)
135 {
136 f7_t xx;
137 f7_set_s64 (&xx, x);
138 return f7_get_double (&xx);
139 }
140 #endif // F7MOD_floatdidf_
141
142
143 extern uint64_t d_to_ufx (f7_double_t, int8_t) F7ASM(f7_d_to_fx64_asm);
144 extern int64_t d_to_sfx (f7_double_t, int8_t) F7ASM(f7_d_to_fx64_asm);
145
146 #ifdef F7MOD_fractdfda_
147 F7_WEAK
148 int64_t __fractdfda (f7_double_t d)
149 {
150 return d_to_sfx (d, -__DA_FBIT__);
151 }
152 #endif // F7MOD_fractdfda_
153
154 #ifdef F7MOD_fractdfuda_
155 F7_WEAK
156 uint64_t __fractdfuda (f7_double_t d)
157 {
158 return d_to_ufx (d, __UDA_FBIT__);
159 }
160 #endif // F7MOD_fractdfuda_
161
162 #ifdef F7MOD_fractdfta_
163 F7_WEAK
164 int64_t __fractdfta (f7_double_t d)
165 {
166 return d_to_sfx (d, -__TA_FBIT__);
167 }
168 #endif // F7MOD_fractdfta_
169
170 #ifdef F7MOD_fractdfuta_
171 F7_WEAK
172 uint64_t __fractdfuta (f7_double_t d)
173 {
174 return d_to_ufx (d, __UTA_FBIT__);
175 }
176 #endif // F7MOD_fractdfuta_
177
178 #ifdef F7MOD_fractdfdq_
179 F7_WEAK
180 int64_t __fractdfdq (f7_double_t d)
181 {
182 return d_to_sfx (d, -__DQ_FBIT__);
183 }
184 #endif // F7MOD_fractdfdq_
185
186 #ifdef F7MOD_fractdfudq_
187 F7_WEAK
188 uint64_t __fractdfudq (f7_double_t d)
189 {
190 return d_to_ufx (d, __UDQ_FBIT__);
191 }
192 #endif // F7MOD_fractdfudq_
193
194
195 #ifdef F7MOD_init_
196 f7_t* f7_init_impl (uint64_t mant, uint8_t flags, f7_t *cc, int16_t expo)
197 {
198 flags &= F7_FLAGS;
199 if (f7_class_number (flags))
200 {
201 uint8_t msb;
202 while ((__builtin_memcpy (&msb, (uint8_t*) &mant + 7, 1), msb))
203 {
204 mant >>= 1;
205 expo = add_ssat16 (expo, 1);
206 }
207 *(uint64_t*) cc->mant = mant;
208 cc->expo = add_ssat16 (expo, F7_MANT_BITS-1);
209
210 cc = f7_normalize_asm (cc);
211 }
212
213 cc->flags = flags;
214
215 return cc;
216 }
217 #endif // F7MOD_init_
218
219
220 #ifdef F7MOD_set_s16_
221 f7_t* f7_set_s16_impl (f7_t *cc, int16_t i16)
222 {
223 uint16_t u16 = (uint16_t) i16;
224 uint8_t flags = 0;
225 if (i16 < 0)
226 {
227 u16 = -u16;
228 flags = F7_FLAG_sign;
229 }
230 f7_set_u16_impl (cc, u16);
231 cc->flags = flags;
232 return cc;
233 }
234 #endif // F7MOD_set_s16_
235
236
237 #ifdef F7MOD_set_u16_
238 f7_t* f7_set_u16_impl (f7_t *cc, uint16_t u16)
239 {
240 f7_clr (cc);
241 F7_MANT_HI2 (cc) = u16;
242 cc->expo = 15;
243 return f7_normalize_asm (cc);
244 }
245 #endif // F7MOD_set_u16_
246
247
248 #ifdef F7MOD_set_s32_
249 f7_t* f7_set_s32 (f7_t *cc, int32_t i32)
250 {
251 uint32_t u32 = (uint32_t) i32;
252 uint8_t flags = 0;
253 if (i32 < 0)
254 {
255 u32 = -u32;
256 flags = F7_FLAG_sign;
257 }
258 cc = f7_set_u32 (cc, u32);
259 cc->flags = flags;
260 return cc;
261 }
262 #endif // F7MOD_set_s32_
263
264
265 #ifdef F7MOD_set_u32_
266 f7_t* f7_set_u32 (f7_t *cc, uint32_t u32)
267 {
268 f7_clr (cc);
269 F7_MANT_HI4 (cc) = u32;
270 cc->expo = 31;
271 return f7_normalize_asm (cc);
272 }
273 #endif // F7MOD_set_u32_
274
275
276 // IEEE 754 single
277 // float = s bbbbbbbb mmmmmmmmmmmmmmmmmmmmmmm
278 // 31
279 // s = sign
280 // b = biased exponent, bias = 127
281 // m = mantissa
282
283 // +0 = 0 0 0
284 // -0 = 1 0 0
285 // Inf = S B 0 = S * Inf, B = 0xff
286 // NaN = S B M, B = 0xff, M != 0
287 // Sub-normal = S 0 M = S * 0.M * 2^{1 - bias}, M != 0
288 // Normal = S B M = S * 1.M * 2^{B - bias}, B = 1 ... 0xfe
289
290 #define FLT_DIG_EXP 8
291 #define FLT_DIG_MANT (31 - FLT_DIG_EXP)
292 #define FLT_MAX_EXP ((1 << FLT_DIG_EXP) - 1)
293 #define FLT_EXP_BIAS (FLT_MAX_EXP >> 1)
294
295 #ifdef F7MOD_set_float_
296 F7_WEAK
297 void f7_set_float (f7_t *cc, float f)
298 {
299 uint32_t val32;
300
301 _Static_assert (__SIZEOF_FLOAT__ == 4, "");
302 _Static_assert (__FLT_MANT_DIG__ == 24, "");
303 __builtin_memcpy (&val32, &f, __SIZEOF_FLOAT__);
304
305 uint16_t val16 = val32 >> 16;
306 val16 >>= FLT_DIG_MANT - 16;
307
308 uint8_t expo_biased = val16 & FLT_MAX_EXP;
309 bool sign = val16 & (1u << FLT_DIG_EXP);
310
311 f7_clr (cc);
312
313 uint32_t mant = val32 & ((1ul << FLT_DIG_MANT) -1);
314
315 if (mant == 0)
316 {
317 if (expo_biased == 0)
318 return;
319 if (expo_biased >= FLT_MAX_EXP)
320 return f7_set_inf (cc, sign);
321 }
322
323 if (expo_biased == 0)
324 expo_biased = 1; // Sub-normal: biased expo of 1 was encoded as 0.
325 else if (expo_biased < FLT_MAX_EXP)
326 mant |= (1ul << FLT_DIG_MANT);
327 else
328 return f7_set_nan (cc);
329
330 __builtin_memcpy (& F7_MANT_HI4 (cc), &mant, 4);
331
332 cc->expo = expo_biased - FLT_EXP_BIAS + 31 - FLT_DIG_MANT;
333 f7_normalize_asm (cc);
334 f7_set_sign (cc, sign);
335 }
336 ALIAS (f7_set_float, f7_extendsfdf2)
337 #endif // F7MOD_set_float_
338
339
340 #ifdef F7MOD_get_float_
341 static F7_INLINE
342 float make_float (uint32_t x)
343 {
344 float ff;
345 __builtin_memcpy (&ff, &x, 4);
346 return ff;
347
348 }
349
350 F7_WEAK
351 float f7_get_float (const f7_t *aa)
352 {
353 uint8_t a_class = f7_classify (aa);
354
355 if (f7_class_nan (a_class))
356 return make_float (0xffc00000 /* NaN: Biased expo = 0xff, mant != 0 */);
357
358 uint32_t mant;
359 __builtin_memcpy (&mant, &F7_MANT_CONST_HI4 (aa), 4);
360
361 uint8_t expo8 = 0;
362 uint8_t mant_offset = FLT_DIG_EXP;
363 int16_t c_expo = add_ssat16 (aa->expo, FLT_EXP_BIAS);
364
365 if (f7_class_zero (a_class) || c_expo <= -FLT_DIG_MANT)
366 {
367 // Zero or tiny.
368 return 0.0f;
369 }
370 else if (c_expo >= FLT_MAX_EXP || f7_class_inf (a_class))
371 {
372 // Inf or overflow.
373 expo8 = FLT_MAX_EXP;
374 mant = 0;
375 }
376 else if (c_expo > 0)
377 {
378 // Normal.
379 expo8 = c_expo;
380 }
381 else
382 {
383 // Sub-normal: -DIG_MANT < c_expo <= 0.
384 // Encoding of 0 represents a biased exponent of 1.
385 // mant_offset in 9...31.
386 expo8 = 0;
387 mant_offset += 1 - c_expo;
388 }
389
390 uint16_t expo16 = expo8 << (FLT_DIG_MANT - 16);
391
392 if (f7_class_sign (a_class))
393 expo16 |= 1u << (FLT_DIG_EXP + FLT_DIG_MANT - 16);
394
395 mant >>= mant_offset;
396
397 __asm ("cbr %T0%t2, 1 << (7 & %2)" "\n\t"
398 "or %C0, %A1" "\n\t"
399 "or %D0, %B1"
400 : "+d" (mant)
401 : "r" (expo16), "n" (FLT_DIG_MANT));
402
403 return make_float (mant);
404 }
405 ALIAS (f7_get_float, f7_truncdfsf2)
406 #endif // F7MOD_get_float_
407
408 #define DBL_DIG_EXP 11
409 #define DBL_DIG_MANT (63 - DBL_DIG_EXP)
410 #define DBL_MAX_EXP ((1 << DBL_DIG_EXP) - 1)
411 #define DBL_EXP_BIAS (DBL_MAX_EXP >> 1)
412
413
414 #ifdef F7MOD_set_double_
415 void f7_set_double_impl (f7_double_t val64, f7_t *cc)
416 {
417 f7_clr (cc);
418 register uint64_t mant __asm ("r18") = val64 & ((1ull << DBL_DIG_MANT) -1);
419
420 uint16_t val16 = 3[(uint16_t*) & val64];
421 val16 >>= DBL_DIG_MANT - 48;
422
423 uint16_t expo_biased = val16 & DBL_MAX_EXP;
424 bool sign = val16 & (1u << DBL_DIG_EXP);
425
426 if (mant == 0)
427 {
428 if (expo_biased == 0)
429 return;
430 if (expo_biased >= DBL_MAX_EXP)
431 return f7_set_inf (cc, sign);
432 }
433 __asm ("" : "+r" (mant));
434
435 if (expo_biased == 0)
436 expo_biased = 1; // Sub-normal: biased expo of 1 was encoded as 0.
437 else if (expo_biased < DBL_MAX_EXP)
438 mant |= (1ull << DBL_DIG_MANT);
439 else
440 return f7_set_nan (cc);
441
442 *(uint64_t*) & cc->mant = mant;
443
444 cc->expo = expo_biased - DBL_EXP_BIAS + 63 - DBL_DIG_MANT - 8;
445 f7_normalize_asm (cc);
446 f7_set_sign (cc, sign);
447 }
448 #endif // F7MOD_set_double_
449
450
451 #ifdef F7MOD_set_pdouble_
452 void f7_set_pdouble (f7_t *cc, const f7_double_t *val64)
453 {
454 f7_set_double (cc, *val64);
455 }
456 #endif // F7MOD_set_pdouble_
457
458
459 #ifdef F7MOD_get_double_
460 static F7_INLINE
461 uint64_t clr_r18 (void)
462 {
463 extern void __clr_8 (void);
464 register uint64_t r18 __asm ("r18");
465 __asm ("%~call %x[f]" : "=r" (r18) : [f] "i" (__clr_8));
466 return r18;
467 }
468
469 static F7_INLINE
470 f7_double_t make_double (uint64_t x)
471 {
472 register f7_double_t r18 __asm ("r18") = x;
473 __asm ("" : "+r" (r18));
474 return r18;
475 }
476
477 F7_WEAK
478 f7_double_t f7_get_double (const f7_t *aa)
479 {
480 uint8_t a_class = f7_classify (aa);
481
482 if (f7_class_nan (a_class))
483 {
484 uint64_t nan = clr_r18() | (0x7fffull << 48);
485 return make_double (nan);
486 }
487
488 uint64_t mant;
489 __builtin_memcpy (&mant, & aa->mant, 8);
490
491 mant &= 0x00ffffffffffffff;
492
493 // PR115419: The least significant nibble tells how to round:
494 // Tie breaks are rounded to even (Banker's rounding).
495 uint8_t lsn = mant & 0xff;
496 lsn &= 0xf;
497 // The LSB of the outgoing double is at bit 3.
498 if (lsn & (1 << 3))
499 ++lsn;
500 if (lsn > (1 << 2))
501 {
502 // FIXME: For subnormals, rounding is premature and should be
503 // done *after* the mantissa has been shifted into place
504 // (or the round value be shifted left accordingly).
505 // Round.
506 mant += 1u << (F7_MANT_BITS - (1 + DBL_DIG_MANT) - 1);
507 }
508
509 uint8_t dex;
510 register uint64_t r18 __asm ("r18") = mant;
511 // dex = Overflow ? 1 : 0.
512 __asm ("bst %T[mant]%T[bitno]" "\n\t"
513 "clr %0" "\n\t"
514 "bld %0,0"
515 : "=r" (dex), [mant] "+r" (r18)
516 : [bitno] "n" (64 - 8));
517
518 mant = r18 >> dex;
519
520 uint16_t expo16 = 0;
521 uint16_t mant_offset = DBL_DIG_EXP - 8;
522 int16_t c_expo = add_ssat16 (aa->expo, DBL_EXP_BIAS + dex);
523
524 if (f7_class_zero (a_class) || c_expo <= -DBL_DIG_MANT)
525 {
526 // Zero or tiny.
527 return make_double (clr_r18());
528 }
529 else if (c_expo >= DBL_MAX_EXP || f7_class_inf (a_class))
530 {
531 // Inf or overflow.
532 expo16 = DBL_MAX_EXP;
533 mant = clr_r18();
534 }
535 else if (c_expo > 0)
536 {
537 // Normal.
538 expo16 = c_expo;
539 }
540 else
541 {
542 // Sub-normal: -DIG_MANT < c_expo <= 0.
543 // Encoding expo of 0 represents a biased exponent of 1.
544 // mant_offset in 5...55 = 63-8.
545 mant_offset += 1 - c_expo;
546 }
547
548 expo16 <<= (DBL_DIG_MANT - 48);
549
550 if (f7_class_sign (a_class))
551 expo16 |= 1u << (DBL_DIG_EXP + DBL_DIG_MANT - 48);
552
553 // mant >>= mant_offset;
554 mant = f7_lshrdi3 (mant, mant_offset);
555
556 r18 = mant;
557 __asm ("cbr %T0%t2, 1 << (7 & %2)" "\n\t"
558 "or %r0+6, %A1" "\n\t"
559 "or %r0+7, %B1"
560 : "+r" (r18)
561 : "r" (expo16), "n" (DBL_DIG_MANT));
562
563 return make_double (r18);
564 }
565 #endif // F7MOD_get_double_
566
567
568 #ifdef F7MOD_fabs_
569 F7_WEAK
570 void f7_fabs (f7_t *cc, const f7_t *aa)
571 {
572 f7_abs (cc, aa);
573 }
574 #endif // F7MOD_fabs_
575
576
577 #ifdef F7MOD_neg_
578 F7_WEAK
579 f7_t* f7_neg (f7_t *cc, const f7_t *aa)
580 {
581 f7_copy (cc, aa);
582
583 uint8_t c_class = f7_classify (cc);
584
585 if (! f7_class_zero (c_class))
586 cc->sign = ! f7_class_sign (c_class);
587
588 return cc;
589 }
590 #endif // F7MOD_neg_
591
592
593 #ifdef F7MOD_frexp_
594 F7_WEAK
595 void f7_frexp (f7_t *cc, const f7_t *aa, int *expo)
596 {
597 uint8_t a_class = f7_classify (aa);
598
599 if (f7_class_nan (a_class))
600 return f7_set_nan (cc);
601
602 if (f7_class_inf (a_class) || aa->expo == INT16_MAX)
603 return f7_set_inf (cc, f7_class_sign (a_class));
604
605 if (! f7_msbit (aa))
606 {
607 *expo = 0;
608 return f7_clr (cc);
609 }
610
611 *expo = 1 + aa->expo;
612 cc->flags = a_class & F7_FLAG_sign;
613 cc->expo = -1;
614 f7_copy_mant (cc, aa);
615 }
616 #endif // F7MOD_frexp_
617
618 #ifdef F7MOD_get_s16_
619 F7_WEAK
620 int16_t f7_get_s16 (const f7_t *aa)
621 {
622 extern int16_t to_s16 (const f7_t*, uint8_t) F7ASM(f7_to_integer_asm);
623 return to_s16 (aa, 0xf);
624 }
625 #endif // F7MOD_get_s16_
626
627
628 #ifdef F7MOD_get_s32_
629 F7_WEAK
630 int32_t f7_get_s32 (const f7_t *aa)
631 {
632 extern int32_t to_s32 (const f7_t*, uint8_t) F7ASM(f7_to_integer_asm);
633 return to_s32 (aa, 0x1f);
634 }
635 ALIAS (f7_get_s32, f7_fixdfsi)
636 #endif // F7MOD_get_s32_
637
638
639 #ifdef F7MOD_get_s64_
640 F7_WEAK
641 int64_t f7_get_s64 (const f7_t *aa)
642 {
643 extern int64_t to_s64 (const f7_t*, uint8_t) F7ASM(f7_to_integer_asm);
644 return to_s64 (aa, 0x3f);
645 }
646 ALIAS (f7_get_s64, f7_fixdfdi)
647 #endif // F7MOD_get_s64_
648
649 #ifdef F7MOD_get_u16_
650 F7_WEAK
651 uint16_t f7_get_u16 (const f7_t *aa)
652 {
653 extern uint16_t to_u16 (const f7_t*, uint8_t) F7ASM(f7_to_unsigned_asm);
654 return to_u16 (aa, 0xf);
655 }
656 #endif // F7MOD_get_u16_
657
658
659 #ifdef F7MOD_get_u32_
660 F7_WEAK
661 uint32_t f7_get_u32 (const f7_t *aa)
662 {
663 extern uint32_t to_u32 (const f7_t*, uint8_t) F7ASM(f7_to_unsigned_asm);
664 return to_u32 (aa, 0x1f);
665 }
666 ALIAS (f7_get_u32, f7_fixunsdfsi)
667 #endif // F7MOD_get_u32_
668
669
670 #ifdef F7MOD_get_u64_
671 F7_WEAK
672 uint64_t f7_get_u64 (const f7_t *aa)
673 {
674 extern int64_t to_u64 (const f7_t*, uint8_t) F7ASM(f7_to_unsigned_asm);
675 return to_u64 (aa, 0x3f);
676 }
677 ALIAS (f7_get_u64, f7_fixunsdfdi)
678 #endif // F7MOD_get_u64_
679
680
681 #ifdef F7MOD_cmp_unordered_
682 F7_NOINLINE
683 static int8_t cmp_u8 (uint8_t a_class, uint8_t b_class, bool sign_a);
684
685 F7_WEAK
686 int8_t f7_cmp_unordered (const f7_t *aa, const f7_t *bb, bool with_sign)
687 {
688 uint8_t a_class = f7_classify (aa);
689 uint8_t b_class = f7_classify (bb);
690
691 uint8_t a_sign = f7_class_sign (a_class) & with_sign;
692 uint8_t b_sign = f7_class_sign (b_class) & with_sign;
693 uint8_t ab_class = a_class | b_class;
694 ab_class &= with_sign - 2;
695
696 if (f7_class_nan (ab_class))
697 return INT8_MIN;
698
699 if (a_sign != b_sign)
700 return b_sign - a_sign;
701
702 if (f7_class_inf (ab_class))
703 return cmp_u8 (a_class, b_class, a_sign);
704
705 if (f7_class_zero (ab_class))
706 return cmp_u8 (b_class, a_class, a_sign);
707
708 if (aa->expo < bb->expo)
709 return a_sign ? 1 : -1;
710
711 if (aa->expo > bb->expo)
712 return a_sign ? -1 : 1;
713
714 return cmp_u8 (1 + f7_cmp_mant (aa, bb), 1, a_sign);
715 }
716
717
718 int8_t cmp_u8 (uint8_t a_class, uint8_t b_class, bool sign_a)
719 {
720 int8_t c;
721 __asm ("sub %[a], %[b]" "\n\t"
722 "breq 1f" "\n\t"
723 "sbc %[c], %[c]" "\n\t"
724 "sbci %[c], -1" "\n\t"
725 "sbrc %[s], 0" "\n\t"
726 "neg %[c]" "\n\t"
727 "1:"
728 : [c] "=d" (c)
729 : [a] "0" (a_class), [b] "r" (b_class), [s] "r" (sign_a));
730 return c;
731 }
732 #endif // F7MOD_cmp_unordered_
733
734
735 #ifdef F7MOD_cmp_abs_
736 F7_WEAK
737 int8_t f7_cmp_abs (const f7_t *aa, const f7_t *bb)
738 {
739 return f7_cmp_unordered (aa, bb, false /* no signs */);
740 }
741 #endif // F7MOD_cmp_abs_
742
743
744 #ifdef F7MOD_cmp_
745 F7_WEAK
746 int8_t f7_cmp (const f7_t *aa, const f7_t *bb)
747 {
748 return f7_cmp_unordered (aa, bb, true /* with signs */);
749 }
750 #endif // F7MOD_cmp_
751
752
753 #ifdef F7MOD_abscmp_msb_ge_
754 // Compare absolute value of Number aa against a f7_t represented
755 // by msb and expo.
756 F7_WEAK
757 bool f7_abscmp_msb_ge (const f7_t *aa, uint8_t msb, int16_t expo)
758 {
759 uint8_t a_msb = aa->mant[F7_MANT_BYTES - 1];
760
761 if (0 == (0x80 & a_msb))
762 // 0 or subnormal.
763 return false;
764
765 return aa->expo == expo
766 ? a_msb >= msb
767 : aa->expo > expo;
768 }
769 #endif // F7MOD_abscmp_msb_ge_
770
771 #ifdef F7MOD_lt_
772 F7_WEAK
773 bool f7_lt_impl (const f7_t *aa, const f7_t *bb)
774 {
775 return f7_lt (aa, bb);
776 }
777 #endif // F7MOD_lt_
778
779 #ifdef F7MOD_le_
780 F7_WEAK
781 bool f7_le_impl (const f7_t *aa, const f7_t *bb)
782 {
783 return f7_le (aa, bb);
784 }
785 #endif // F7MOD_le_
786
787 #ifdef F7MOD_gt_
788 F7_WEAK
789 bool f7_gt_impl (const f7_t *aa, const f7_t *bb)
790 {
791 return f7_gt (aa, bb);
792 }
793 #endif // F7MOD_gt_
794
795 #ifdef F7MOD_ge_
796 F7_WEAK
797 bool f7_ge_impl (const f7_t *aa, const f7_t *bb)
798 {
799 return f7_ge (aa, bb);
800 }
801 #endif // F7MOD_ge_
802
803 #ifdef F7MOD_ne_
804 F7_WEAK
805 bool f7_ne_impl (const f7_t *aa, const f7_t *bb)
806 {
807 return f7_ne (aa, bb);
808 }
809 #endif // F7MOD_ne_
810
811 #ifdef F7MOD_eq_
812 F7_WEAK
813 bool f7_eq_impl (const f7_t *aa, const f7_t *bb)
814 {
815 return f7_eq (aa, bb);
816 }
817 #endif // F7MOD_eq_
818
819
820 #ifdef F7MOD_unord_
821 F7_WEAK
822 bool f7_unord_impl (const f7_t *aa, const f7_t *bb)
823 {
824 return f7_unordered (aa, bb);
825 }
826 #endif // F7MOD_unord_
827
828
829 #ifdef F7MOD_minmax_
830 F7_WEAK
831 f7_t* f7_minmax (f7_t *cc, const f7_t *aa, const f7_t *bb, bool do_min)
832 {
833 int8_t cmp = f7_cmp_unordered (aa, bb, true /* with signs */);
834
835 if (cmp == INT8_MIN)
836 return (f7_set_nan (cc), cc);
837
838 if (do_min)
839 cmp = -cmp;
840
841 return f7_copy (cc, cmp >= 0 ? aa : bb);
842 }
843 #endif // F7MOD_minmax_
844
845
846 #ifdef F7MOD_fmax_
847 F7_WEAK
848 f7_t* f7_fmax (f7_t *cc, const f7_t *aa, const f7_t *bb)
849 {
850 return f7_minmax (cc, aa, bb, false);
851 }
852 ALIAS (f7_fmax, f7_max)
853 #endif // F7MOD_fmax_
854
855
856 #ifdef F7MOD_fmin_
857 F7_WEAK
858 f7_t* f7_fmin (f7_t *cc, const f7_t *aa, const f7_t *bb)
859 {
860 return f7_minmax (cc, aa, bb, true);
861 }
862 ALIAS (f7_fmin, f7_min)
863 #endif // F7MOD_fmin_
864
865
866 #ifdef F7MOD_mulx_
867 F7_WEAK
868 uint8_t f7_mulx (f7_t *cc, const f7_t *aa, const f7_t *bb, bool no_rounding)
869 {
870 uint8_t a_class = f7_classify (aa);
871 uint8_t b_class = f7_classify (bb);
872 // From this point on, no more access aa->flags or bb->flags
873 // to avoid early-clobber when writing cc->flags.
874
875 uint8_t ab_class = a_class | b_class;
876 // If either value is NaN, return NaN.
877 if (f7_class_nan (ab_class)
878 // Any combination of Inf and 0.
879 || (f7_class_zero (ab_class) && f7_class_inf (ab_class)))
880 {
881 cc->flags = F7_FLAG_nan;
882 return 0;
883 }
884 // If either value is 0.0, return 0.0.
885 if (f7_class_zero (ab_class))
886 {
887 f7_clr (cc);
888 return 0;
889 }
890 // We have 2 non-zero numbers-or-INF.
891
892 uint8_t c_sign = (a_class ^ b_class) & F7_FLAG_sign;
893 uint8_t c_inf = ab_class & F7_FLAG_inf;
894 cc->flags = c_sign | c_inf;
895 if (c_inf)
896 return 0;
897
898 int16_t expo = add_ssat16 (aa->expo, bb->expo);
899 // Store expo and handle expo = INT16_MIN and INT16_MAX.
900 if (f7_store_expo (cc, expo))
901 return 0;
902
903 return f7_mul_mant_asm (cc, aa, bb, no_rounding);
904 }
905 #endif // F7MOD_mulx_
906
907
908 #ifdef F7MOD_square_
909 F7_WEAK
910 void f7_square (f7_t *cc, const f7_t *aa)
911 {
912 f7_mul (cc, aa, aa);
913 }
914 #endif // F7MOD_square_
915
916
917 #ifdef F7MOD_mul_
918 F7_WEAK
919 void f7_mul (f7_t *cc, const f7_t *aa, const f7_t *bb)
920 {
921 f7_mulx (cc, aa, bb, false);
922 }
923 #endif // F7MOD_mul_
924
925
926 #ifdef F7MOD_Iadd_
927 F7_WEAK void f7_Iadd (f7_t *cc, const f7_t *aa) { f7_add (cc, cc, aa); }
928 #endif
929
930 #ifdef F7MOD_Isub_
931 F7_WEAK void f7_Isub (f7_t *cc, const f7_t *aa) { f7_sub (cc, cc, aa); }
932 #endif
933
934 #ifdef F7MOD_Imul_
935 F7_WEAK void f7_Imul (f7_t *cc, const f7_t *aa) { f7_mul (cc, cc, aa); }
936 #endif
937
938 #ifdef F7MOD_Idiv_
939 F7_WEAK void f7_Idiv (f7_t *cc, const f7_t *aa) { f7_div (cc, cc, aa); }
940 #endif
941
942 #ifdef F7MOD_IRsub_
943 F7_WEAK void f7_IRsub (f7_t *cc, const f7_t *aa) { f7_sub (cc, aa, cc); }
944 #endif
945
946 #ifdef F7MOD_Ineg_
947 F7_WEAK void f7_Ineg (f7_t *cc) { f7_neg (cc, cc); }
948 #endif
949
950 #ifdef F7MOD_Isqrt_
951 F7_WEAK void f7_Isqrt (f7_t *cc) { f7_sqrt (cc, cc); }
952 #endif
953
954 #ifdef F7MOD_Isquare_
955 F7_WEAK void f7_Isquare (f7_t *cc) { f7_square (cc, cc); }
956 #endif
957
958 #ifdef F7MOD_Ildexp_
959 F7_WEAK f7_t* f7_Ildexp (f7_t *cc, int ex) { return f7_ldexp (cc, cc, ex); }
960 #endif
961
962
963 #ifdef F7MOD_add_
964 F7_WEAK
965 void f7_add (f7_t *cc, const f7_t *aa, const f7_t *bb)
966 {
967 f7_addsub (cc, aa, bb, false);
968 }
969 #endif // F7MOD_add_
970
971
972 #ifdef F7MOD_sub_
973 F7_WEAK
974 void f7_sub (f7_t *cc, const f7_t *aa, const f7_t *bb)
975 {
976 f7_addsub (cc, aa, bb, true);
977 }
978 #endif // F7MOD_sub_
979
980
981 #ifdef F7MOD_fdim_
982 F7_WEAK
983 void f7_fdim (f7_t *cc, const f7_t *aa, const f7_t *bb)
984 {
985 int8_t cmp = f7_cmp_unordered (aa, bb, true /*with_sign*/);
986 if (cmp == INT8_MIN)
987 return f7_set_nan (cc);
988 if (cmp < 0)
989 return f7_clr (cc);
990
991 f7_sub (cc, aa, bb);
992 }
993 #endif // F7MOD_fdim_
994
995
996 #ifdef F7MOD_addsub_
997 static void return_with_sign (f7_t *cc, const f7_t *aa, int8_t c_sign)
998 {
999 __asm (";;; return with sign");
1000 f7_copy (cc, aa);
1001 if (c_sign != -1)
1002 f7_set_sign (cc, c_sign);
1003 }
1004
1005 F7_WEAK
1006 void f7_addsub (f7_t *cc, const f7_t *aa, const f7_t *bb, bool neg_b)
1007 {
1008 uint8_t a_class = f7_classify (aa);
1009 uint8_t b_class = f7_classify (bb);
1010 // From this point on, no more access aa->flags or bb->flags
1011 // to avoid early-clobber when writing cc->flags.
1012
1013 // Hande NaNs.
1014 if (f7_class_nan (a_class | b_class))
1015 return f7_set_nan (cc);
1016
1017 bool a_sign = f7_class_sign (a_class);
1018 bool b_sign = f7_class_sign (b_class) ^ neg_b;
1019
1020 // Add the mantissae?
1021 bool do_add = a_sign == b_sign;
1022
1023 // Handle +Infs and -Infs.
1024 bool a_inf = f7_class_inf (a_class);
1025 bool b_inf = f7_class_inf (b_class);
1026
1027 if (a_inf && b_inf)
1028 {
1029 if (do_add)
1030 return f7_set_inf (cc, a_sign);
1031 else
1032 return f7_set_nan (cc);
1033 }
1034 else if (a_inf)
1035 return f7_set_inf (cc, a_sign);
1036 else if (b_inf)
1037 return f7_set_inf (cc, b_sign);
1038
1039 int16_t shift16 = sub_ssat16 (aa->expo, bb->expo);
1040
1041 // aa + 0 = aa.
1042 // Also check MSBit to get rid of Subnormals and 0.
1043 if (shift16 > F7_MANT_BITS || f7_is0 (bb))
1044 return return_with_sign (cc, aa, -1);
1045
1046 // 0 + bb = bb.
1047 // 0 - bb = -bb.
1048 // Also check MSBit to get rid of Subnormals and 0.
1049 if (shift16 < -F7_MANT_BITS || f7_is0 (aa))
1050 return return_with_sign (cc, bb, b_sign);
1051
1052 // Now aa and bb are non-zero, non-NaN, non-Inf.
1053 // shift > 0 ==> |a| > |b|
1054 // shift < 0 ==> |a| < |b|
1055 int8_t shift = (int8_t) shift16;
1056 bool c_sign = a_sign;
1057 if (shift < 0
1058 || (shift == 0 && f7_cmp_mant (aa, bb) < 0))
1059 {
1060 const f7_t *p = aa; aa = bb; bb = p;
1061 c_sign = b_sign;
1062 shift = -shift;
1063 }
1064 uint8_t shift2 = (uint8_t) (shift << 1);
1065
1066 cc->expo = aa->expo;
1067 // From this point on, no more access aa->expo or bb->expo
1068 // to avoid early-clobber when writing cc->expo.
1069
1070 cc->flags = c_sign; _Static_assert (F7_FLAGNO_sign == 0, "");
1071
1072 // This function uses neither .expo nor .flags from either aa or bb,
1073 // hence there is early-clobber for cc->expo and cc->flags.
1074 f7_addsub_mant_scaled_asm (cc, aa, bb, shift2 | do_add);
1075 }
1076 #endif // F7MOD_addsub_
1077
1078
1079 #ifdef F7MOD_madd_msub_
1080 F7_WEAK
1081 void f7_madd_msub (f7_t *cc, const f7_t *aa, const f7_t *bb, const f7_t *dd,
1082 bool neg_d)
1083 {
1084 f7_t xx7, *xx = &xx7;
1085 uint8_t x_lsb = f7_mulx (xx, aa, bb, true /* no rounding */);
1086 uint8_t x_sign = f7_signbit (xx);
1087 int16_t x_expo = xx->expo;
1088 f7_addsub (xx, xx, dd, neg_d);
1089 // Now add LSB. If cancellation occured in the add / sub, then we have the
1090 // chance of extra 8 bits of precision. Turn LSByte into f7_t.
1091 f7_clr (cc);
1092 cc->expo = sub_ssat16 (x_expo, F7_MANT_BITS);
1093 cc->mant[F7_MANT_BYTES - 1] = x_lsb;
1094 cc = f7_normalize_asm (cc);
1095 cc->flags = x_sign;
1096 f7_Iadd (cc, xx);
1097 }
1098 #endif // F7MOD_madd_msub_
1099
1100 #ifdef F7MOD_madd_
1101 F7_WEAK
1102 void f7_madd (f7_t *cc, const f7_t *aa, const f7_t *bb, const f7_t *dd)
1103 {
1104 f7_madd_msub (cc, aa, bb, dd, false);
1105 }
1106 #endif // F7MOD_madd_
1107
1108 #ifdef F7MOD_msub_
1109 F7_WEAK
1110 void f7_msub (f7_t *cc, const f7_t *aa, const f7_t *bb, const f7_t *dd)
1111 {
1112 f7_madd_msub (cc, aa, bb, dd, true);
1113 }
1114 #endif // F7MOD_msub_
1115
1116
1117 #ifdef F7MOD_ldexp_
1118 F7_WEAK
1119 f7_t* f7_ldexp (f7_t *cc, const f7_t *aa, int delta)
1120 {
1121 uint8_t a_class = f7_classify (aa);
1122
1123 cc->flags = a_class;
1124
1125 // Inf and NaN.
1126 if (! f7_class_number (a_class))
1127 return cc;
1128
1129 if (f7_msbit (aa) == 0)
1130 return (f7_clr (cc), cc);
1131
1132 int16_t expo = add_ssat16 (delta, aa->expo);
1133 // Store expo and handle expo = INT16_MIN and INT16_MAX.
1134 if (! f7_store_expo (cc, expo))
1135 f7_copy_mant (cc, aa);
1136
1137 return cc;
1138 }
1139 #endif // F7MOD_ldexp_
1140
1141
1142 #if USE_LPM
1143 #elif USE_LD
1144 #else
1145 #error need include "asm-defs.h"
1146 #endif // USE_LPM
1147
1148 /*
1149 Handling constants:
1150
1151 F7_PCONST (PVAR, X)
1152
1153 Set f7_t [const] *PVAR to an LD address for one
1154 of the f7_const_X[_P] constants.
1155 PVAR might be set to point to a local auto that serves
1156 as temporary storage for f7_const_X_P. PVAR is only
1157 valid in the current block.
1158
1159 const f7_t* F7_PCONST_U16 (PVAR, <ident> X) // USE_LD
1160 f7_t* F7_PCONST_U16 (PVAR, uint16_t X) // USE_LPM
1161
1162 Set f7_t [const] *PVAR to an LD address for one of the
1163 f7_const_X[_P] constants. PVAR might be set to point to a
1164 local auto that serves as temporary storage for X. PVAR is
1165 only valid in the current block.
1166
1167 F7_PCONST_VAR (PVAR, VAR)
1168
1169 VAR is a pointer variable holding the address of some f7_const_X[_P]
1170 constant. Set [const] f7_t *PVAR to a respective LD address.
1171 PVAR might be set to point to a local auto that serves
1172 as temporary storage for f7_const_X_P. PVAR is only
1173 valid in the current block.
1174
1175 F7_CONST_ADDR (<ident> CST, f7_t* PTMP)
1176
1177 Return an LD address to some f7_const_X[_P] constant.
1178 *PTMP might be needed to hold a copy of f7_const_X_P in RAM.
1179
1180 f7_t* F7_U16_ADDR (uint16_t X, f7_t* PTMP) // USE_LPM
1181 const f7_t* F7_U16_ADDR (<cst-ident> X, <unused>) // USE_LD
1182
1183 Return an LD address to compile-time constant uint16_t X which is
1184 also known as f7_const_X[_P]. *PTMP might be set to (f7_t) X.
1185
1186 f7_t* f7_const (f7_t *PVAR, <cst-ident> X)
1187
1188 Copy f7_const_X[_P] to *PVAR.
1189
1190 f7_t* f7_copy_flash (f7_t *DST, const f7_t *SRC)
1191
1192 Copy to *DST with LD (from .rodata in flash) if the address
1193 space is linear, or with LPM (from .progmem.data) if the
1194 address space is not linear.
1195
1196 f7_t* f7_copy (f7_t *DST, const f7_t* SRC)
1197
1198 Copy to RAM using LD.
1199
1200 f7_t* f7_copy_P (f7_t *DST, const f7_t *SRC)
1201
1202 Copy to RAM using LPM.
1203 */
1204
1205 #if USE_LPM
1206 #define F7_RAW_CONST_ADDR(CST) \
1207 & F7_(const_##CST##_P)
1208
1209 #define F7_PCONST(PVAR, CST) \
1210 f7_t _var_for_##CST; \
1211 f7_copy_P (& _var_for_##CST, & F7_(const_##CST##_P)); \
1212 PVAR = & _var_for_##CST
1213
1214 #define F7_PCONST_U16(PVAR, CST) \
1215 f7_t _var_for_##CST; \
1216 PVAR = f7_set_u16 (& _var_for_##CST, CST)
1217
1218 #define F7_PCONST_VAR(PVAR, VAR) \
1219 f7_t _var_for_##VAR; \
1220 f7_copy_P (& _var_for_##VAR, VAR); \
1221 PVAR = & _var_for_##VAR
1222
1223 #define MAYBE_const // empty
1224
1225 #define F7_CONST_ADDR(CST, PTMP) \
1226 f7_copy_P ((PTMP), & F7_(const_##CST##_P))
1227
1228 #define F7_U16_ADDR(CST, PTMP) \
1229 f7_set_u16 ((PTMP), CST)
1230
1231 #elif USE_LD
1232 #define F7_RAW_CONST_ADDR(CST) \
1233 & F7_(const_##CST)
1234
1235 #define F7_PCONST(PVAR, CST) \
1236 PVAR = & F7_(const_##CST)
1237
1238 #define F7_PCONST_U16(PVAR, CST) \
1239 PVAR = & F7_(const_##CST)
1240
1241 #define F7_PCONST_VAR(PVAR, VAR) \
1242 PVAR = (VAR)
1243
1244 #define F7_CONST_ADDR(CST, PTMP) \
1245 (& F7_(const_##CST))
1246
1247 #define F7_U16_ADDR(CST, PTMP) \
1248 (& F7_(const_##CST))
1249
1250 #define MAYBE_const const
1251 #endif
1252
1253
1254
1255 #define DD(str, X) \
1256 do { \
1257 LOG_PSTR (PSTR (str)); \
1258 f7_dump (X); \
1259 } while (0)
1260
1261 #undef DD
1262 #define DD(...) (void) 0
1263
1264
1265 #ifdef F7MOD_sqrt_
1266 F7_WEAK
1267 void f7_sqrt (f7_t *cc, const f7_t *aa)
1268 {
1269 uint8_t a_class = f7_classify (aa);
1270
1271 if (f7_class_nan (a_class) || f7_class_sign (a_class))
1272 return f7_set_nan (cc);
1273
1274 if (f7_class_inf (a_class))
1275 return f7_set_inf (cc, 0);
1276
1277 if (f7_class_zero (a_class))
1278 return f7_clr (cc);
1279
1280 f7_sqrt_approx_asm (cc, aa);
1281 }
1282 #endif // F7MOD_sqrt_
1283
1284
1285 #ifdef F7MOD_hypot_
1286 F7_WEAK
1287 void f7_hypot (f7_t *cc, const f7_t *aa, const f7_t *bb)
1288 {
1289 f7_t xx7, *xx = &xx7;
1290
1291 f7_square (xx, aa);
1292 f7_square (cc, bb);
1293 f7_Iadd (cc, xx);
1294 f7_Isqrt (cc);
1295 }
1296 #endif // F7MOD_hypot_
1297
1298
1299 #ifdef F7MOD_const_m1_
1300 #include "libf7-constdef.h"
1301 #endif // -1
1302
1303 #ifdef F7MOD_const_1_2_
1304 #include "libf7-constdef.h"
1305 #endif // 1/2
1306
1307 #ifdef F7MOD_const_1_3_
1308 #include "libf7-constdef.h"
1309 #endif // 1/3
1310
1311 #ifdef F7MOD_const_ln2_
1312 #include "libf7-constdef.h"
1313 #endif // ln2
1314
1315 #ifdef F7MOD_const_1_ln2_
1316 #include "libf7-constdef.h"
1317 #endif // 1_ln2
1318
1319 #ifdef F7MOD_const_ln10_
1320 #include "libf7-constdef.h"
1321 #endif // ln10
1322
1323 #ifdef F7MOD_const_1_ln10_
1324 #include "libf7-constdef.h"
1325 #endif // 1_ln10
1326
1327 #ifdef F7MOD_const_1_
1328 #include "libf7-constdef.h"
1329 #endif // 1
1330
1331 #ifdef F7MOD_const_sqrt2_
1332 #include "libf7-constdef.h"
1333 #endif // sqrt2
1334
1335 #ifdef F7MOD_const_2_
1336 #include "libf7-constdef.h"
1337 #endif // 2
1338
1339 #ifdef F7MOD_const_pi_
1340 #include "libf7-constdef.h"
1341 #endif // pi
1342
1343
1344 #ifdef F7MOD_divx_
1345
1346 // C /= A
1347 extern void f7_div_asm (f7_t*, const f7_t*, uint8_t);
1348
1349 F7_WEAK
1350 void f7_divx (f7_t *cc, const f7_t *aa, const f7_t *bb, uint8_t quot_bits)
1351 {
1352 uint8_t a_class = f7_classify (aa);
1353 uint8_t b_class = f7_classify (bb);
1354 // From this point on, no more access aa->flags or bb->flags
1355 // to avoid early-clobber when writing cc->flags.
1356
1357 // If either value is NaN, return NaN.
1358 if (f7_class_nan (a_class | b_class)
1359 // If both values are Inf or both are 0, return NaN.
1360 || f7_class_zero (a_class & b_class)
1361 || f7_class_inf (a_class & b_class)
1362 // Inf / 0 = NaN.
1363 || (f7_class_inf (a_class) && f7_class_zero (b_class)))
1364 {
1365 return f7_set_nan (cc);
1366 }
1367
1368 // 0 / B = 0 for non-zero, non-NaN B.
1369 // A / Inf = 0 for non-zero numbers A.
1370 if (f7_class_zero (a_class) || f7_class_inf (b_class))
1371 return f7_clr (cc);
1372
1373 uint8_t c_sign = (a_class ^ b_class) & F7_FLAG_sign;
1374
1375 if (f7_class_inf (a_class) || f7_class_zero (b_class))
1376 return f7_set_inf (cc, c_sign);
1377
1378 cc->flags = c_sign; _Static_assert (F7_FLAGNO_sign == 0, "");
1379 int16_t expo = sub_ssat16 (aa->expo, bb->expo);
1380
1381 // Store expo and handle expo = INT16_MIN and INT16_MAX.
1382 if (f7_store_expo (cc, expo))
1383 return;
1384
1385 f7_t ss7, *ss = &ss7;
1386 ss->flags = cc->flags;
1387 ss->expo = cc->expo;
1388
1389 f7_copy_mant (ss, aa);
1390 f7_div_asm (ss, bb, quot_bits);
1391 f7_copy (cc, ss);
1392 }
1393 #endif // F7MOD_divx_
1394
1395
1396 #ifdef F7MOD_div_
1397 F7_WEAK
1398 void f7_div (f7_t *cc, const f7_t *aa, const f7_t *bb)
1399 {
1400 /* When f7_divx calls f7_div_asm, dividend and divisor are valid
1401 mantissae, i.e. their MSBit is set. Therefore, the quotient will
1402 be in [0x0.ff..., 0x0.40...] and to adjust it, at most 1 left-shift
1403 is needed. Compute F7_MANT_BITS + 2 bits of the quotient:
1404 One bit is used for rounding, and one bit might be consumed by the
1405 mentioned left-shift. */
1406
1407 f7_divx (cc, aa, bb, 2 + F7_MANT_BITS);
1408 }
1409 #endif // F7MOD_div_
1410
1411
1412 #ifdef F7MOD_div1_
1413 F7_WEAK
1414 void f7_div1 (f7_t *cc, const f7_t *aa)
1415 {
1416 F7_PCONST_U16 (const f7_t *one, 1);
1417 f7_div (cc, one, aa);
1418 }
1419 #endif // F7MOD_div_
1420
1421
1422 #ifdef F7MOD_fmod_
1423 F7_WEAK
1424 void f7_fmod (f7_t *cc, const f7_t *aa, const f7_t *bb)
1425 {
1426 uint8_t a_class = f7_classify (aa);
1427 uint8_t b_class = f7_classify (bb);
1428
1429 if (! f7_class_number (a_class)
1430 || f7_class_nan (b_class)
1431 || f7_class_zero (b_class))
1432 {
1433 return f7_set_nan (cc);
1434 }
1435
1436 // A == 0 and B != 0 => 0.
1437 if (f7_class_zero (a_class))
1438 return f7_clr (cc);
1439
1440 f7_t zz7, *zz = & zz7;
1441
1442 f7_div (zz, aa, bb);
1443
1444 // Z in Z, |Z| <= |A/B|.
1445 f7_trunc (zz, zz);
1446
1447 // C = A - Z * B.
1448 f7_msub (cc, zz, bb, aa);
1449 cc->flags ^= F7_FLAG_sign;
1450 }
1451 #endif // F7MOD_fmod_
1452
1453
1454 #ifdef F7MOD_truncx_
1455 F7_WEAK
1456 f7_t* f7_truncx (f7_t *cc, const f7_t *aa, bool do_floor)
1457 {
1458 uint8_t a_class = f7_classify (aa);
1459
1460 if (! f7_class_nonzero (a_class))
1461 return f7_copy (cc, aa);
1462
1463 bool sign = f7_class_sign (a_class);
1464
1465 int16_t a_expo = aa->expo;
1466
1467 if (a_expo < 0)
1468 {
1469 // |A| < 1.
1470 if (sign & do_floor)
1471 return f7_set_s16 (cc, -1);
1472
1473 f7_clr (cc);
1474 return cc;
1475 }
1476 else if (a_expo >= F7_MANT_BITS - 1)
1477 // A == floor (A).
1478 return f7_copy (cc, aa);
1479
1480 f7_t tmp7, *tmp = &tmp7;
1481
1482 // Needed if aa === cc.
1483 f7_copy (tmp, aa);
1484
1485 cc->flags = sign;
1486 cc->expo = a_expo;
1487 f7_clr_mant_lsbs (cc, aa, F7_MANT_BITS - 1 - a_expo);
1488
1489 if (do_floor && cc->sign && f7_cmp_mant (cc, tmp) != 0)
1490 {
1491 F7_PCONST_U16 (const f7_t *one, 1);
1492 f7_Isub (cc, one);
1493 }
1494
1495 return cc;
1496 }
1497 #endif // F7MOD_truncx_
1498
1499
1500 #ifdef F7MOD_floor_
1501 F7_WEAK
1502 f7_t* f7_floor (f7_t *cc, const f7_t *aa)
1503 {
1504 return f7_truncx (cc, aa, true);
1505 }
1506 #endif // F7MOD_floor_
1507
1508
1509 #ifdef F7MOD_trunc_
1510 F7_WEAK
1511 f7_t* f7_trunc (f7_t *cc, const f7_t *aa)
1512 {
1513 return f7_truncx (cc, aa, false);
1514 }
1515 #endif // F7MOD_trunc_
1516
1517
1518 #ifdef F7MOD_ceil_
1519 F7_WEAK
1520 void f7_ceil (f7_t *cc, const f7_t *aa)
1521 {
1522 cc = f7_copy (cc, aa);
1523 cc->flags ^= F7_FLAG_sign;
1524 cc = f7_floor (cc, cc);
1525 f7_Ineg (cc);
1526 }
1527 #endif // F7MOD_ceil_
1528
1529
1530 #ifdef F7MOD_round_
1531 F7_WEAK
1532 void f7_round (f7_t *cc, const f7_t *aa)
1533 {
1534 f7_t tmp;
1535 (void) tmp;
1536 const f7_t *half = F7_CONST_ADDR (1_2, &tmp);
1537
1538 f7_addsub (cc, aa, half, f7_signbit (aa));
1539 f7_trunc (cc, cc);
1540 }
1541 #endif // F7MOD_round_
1542
1543
1544 #ifdef F7MOD_horner_
1545
1546 // Assertion when using this function is that either cc != xx,
1547 // or if cc == xx, then tmp1 must be non-NULL and tmp1 != xx.
1548 // In General, the calling functions have a spare f7_t object available
1549 // and can pass it down to save some stack.
1550 // Moreover, the power series must have degree 1 at least.
1551
1552 F7_WEAK
1553 void f7_horner (f7_t *cc, const f7_t *xx, uint8_t n_coeff, const f7_t *coeff,
1554 f7_t *tmp1)
1555 {
1556 f7_assert (n_coeff > 1);
1557
1558 if (cc != xx)
1559 tmp1 = cc;
1560 else
1561 f7_assert (tmp1 != NULL && tmp1 != xx);
1562
1563 f7_t *yy = tmp1;
1564 f7_t tmp27, *tmp2 = &tmp27;
1565
1566 n_coeff--;
1567 const f7_t *pcoeff = coeff + n_coeff;
1568
1569 f7_copy_flash (yy, pcoeff);
1570
1571 if (yy->flags & F7_FLAG_plusx)
1572 f7_Iadd (yy, xx);
1573
1574 while (1)
1575 {
1576 --pcoeff;
1577 #if 1
1578 f7_Imul (yy, xx);
1579 const f7_t *cst = USE_LD ? pcoeff : f7_copy_P (tmp2, pcoeff);
1580 if (coeff == pcoeff)
1581 return f7_add (cc, yy, cst);
1582 f7_Iadd (yy, cst);
1583 #else
1584 const f7_t *cst = USE_LD ? pcoeff : f7_copy_P (tmp2, pcoeff);
1585 f7_madd (yy, yy, xx, cst);
1586 if (coeff == pcoeff)
1587 {
1588 f7_copy (cc, yy);
1589 return;
1590 }
1591 #endif
1592 }
1593
1594 __builtin_unreachable();
1595 }
1596 #endif // F7MOD_horner_
1597
1598
1599 #ifdef F7MOD_log_
1600 F7_WEAK
1601 void f7_log (f7_t *cc, const f7_t *aa)
1602 {
1603 f7_logx (cc, aa, NULL);
1604 }
1605 #endif // F7MOD_log_
1606
1607
1608 #ifdef F7MOD_log2_
1609 F7_WEAK
1610 void f7_log2 (f7_t *cc, const f7_t *aa)
1611 {
1612 f7_logx (cc, aa, F7_RAW_CONST_ADDR (1_ln2));
1613 }
1614 #endif // F7MOD_log2_
1615
1616
1617 #ifdef F7MOD_log10_
1618 F7_WEAK
1619 void f7_log10 (f7_t *cc, const f7_t *aa)
1620 {
1621 f7_logx (cc, aa, F7_RAW_CONST_ADDR (1_ln10));
1622 }
1623 #endif // F7MOD_log10_
1624
1625
1626 #ifdef F7MOD_logx_
1627
1628 #define ARRAY_NAME coeff_artanh
1629 #include "libf7-array.def"
1630 #undef ARRAY_NAME
1631
1632 // Compute P * ln(A) if P != NULL and ln(A), otherwise.
1633 // P is a LD-address if USE_LD and a LPM-address if USE_LPM.
1634 // Assumption is that P > 0.
1635
1636 F7_WEAK
1637 void f7_logx (f7_t *cc, const f7_t *aa, const f7_t *p)
1638 {
1639 uint8_t a_class = f7_classify (aa);
1640
1641 if (f7_class_nan (a_class) || f7_class_sign (a_class))
1642 return f7_set_nan (cc);
1643
1644 if (f7_class_inf (a_class))
1645 return f7_set_inf (cc, 0);
1646
1647 if (f7_class_zero (a_class))
1648 return f7_set_inf (cc, 1);
1649
1650 f7_t *yy = cc;
1651 f7_t xx7, *xx = &xx7;
1652 f7_t tmp7, *tmp = &tmp7;
1653
1654 // Y in [1, 2] = A * 2 ^ (-a_expo).
1655 int16_t a_expo = aa->expo;
1656 f7_copy (yy, aa);
1657 yy->expo = 0;
1658
1659 // Y in [1 / sqrt2, sqrt2].
1660
1661 if (f7_abscmp_msb_ge (yy, F7_(const_sqrt2_msb), F7_(const_sqrt2_expo)))
1662 {
1663 yy->expo = -1;
1664 a_expo = add_ssat16 (a_expo, 1);
1665 }
1666
1667 const f7_t *one = F7_U16_ADDR (1, & tmp7);
1668
1669 // X := (Y - 1) / (Y + 1), |X| <= (sqrt2 - 1) / (sqrt2 + 1) ~ 0.172.
1670 f7_sub (xx, yy, one);
1671 f7_Iadd (yy, one);
1672 f7_Idiv (xx, yy);
1673
1674 // Y := X^2, |Y| < 0.03.
1675 f7_square (yy, xx);
1676
1677 // Y := artanh (X^2) / X
1678 f7_horner (yy, yy, n_coeff_artanh, coeff_artanh, tmp);
1679
1680 // C = X * Y = ln A - a_expo * ln2.
1681 f7_mul (cc, xx, yy);
1682
1683 // X := a_expo * ln2.
1684 f7_set_s16 (xx, a_expo);
1685 f7_Imul (xx, F7_CONST_ADDR (ln2, & tmp7));
1686
1687 // C = ln A.
1688 f7_Iadd (cc, xx);
1689
1690 if (p && USE_LPM)
1691 f7_Imul (cc, f7_copy_P (tmp, p));
1692 if (p && USE_LD)
1693 f7_Imul (cc, p);
1694 }
1695 #endif // F7MOD_logx_
1696
1697
1698 #ifdef F7MOD_exp_
1699
1700 #define ARRAY_NAME coeff_exp
1701 #include "libf7-array.def"
1702 #undef ARRAY_NAME
1703
1704 #define STATIC static
1705 #include "libf7-constdef.h" // ln2_low
1706 #undef STATIC
1707
1708 F7_WEAK
1709 void f7_exp (f7_t *cc, const f7_t *aa)
1710 {
1711 uint8_t a_class = f7_classify (aa);
1712
1713 if (f7_class_nan (a_class))
1714 return f7_set_nan (cc);
1715
1716 /* The maximal exponent of 2 for a double is 1023, hence we may limit
1717 to |A| < 1023 * ln2 ~ 709. We limit to 1024 = 2^10 */
1718
1719 if (f7_class_inf (a_class)
1720 || (f7_class_nonzero (a_class) && aa->expo >= 10))
1721 {
1722 if (f7_class_sign (a_class))
1723 return f7_clr (cc);
1724 else
1725 return f7_set_inf (cc, 0);
1726 }
1727
1728 f7_t const *cst;
1729 f7_t qq7, *qq = &qq7;
1730
1731 F7_PCONST (cst, ln2);
1732
1733 // We limited |A| to 1024 and are now dividing by ln2, hence Q will
1734 // be at most 1024 / ln2 ~ 1477 and fit into 11 bits. We will
1735 // round Q anyway, hence only request 11 bits from the division and
1736 // one additional bit that might be needed to normalize the quotient.
1737 f7_divx (qq, aa, cst, 1 + 11);
1738
1739 // Use the smallest (by absolute value) remainder system.
1740 f7_round (qq, qq);
1741 int16_t q = f7_get_s16 (qq);
1742
1743 // Reducing A mod ln2 gives |C| <= ln2 / 2, C = -A mod ln2.
1744 f7_msub (cc, qq, cst, aa);
1745
1746 // Corrigendum: We added Q * ln2; now add Q times the low part of ln2
1747 // for better precision. Due to |C| < |A| this is not a no-op in general.
1748 const f7_t *yy = F7_CONST_ADDR (ln2_low, &_var_for_ln2);
1749 f7_madd (cc, qq, yy, cc);
1750
1751 // Because we computed C = -A mod ...
1752 cc->flags ^= F7_FLAG_sign;
1753
1754 // Reduce further to |C| < ln2 / 8 which is the range of our MiniMax poly.
1755 const uint8_t MAX_LN2_RED = 3;
1756 int8_t scal2 = 0;
1757
1758 while (f7_abscmp_msb_ge (cc, F7_(const_ln2_msb),
1759 F7_(const_ln2_expo) - MAX_LN2_RED))
1760 {
1761 scal2++;
1762 cc->expo--;
1763 }
1764
1765 f7_horner (cc, cc, n_coeff_exp, coeff_exp, qq);
1766
1767 while (--scal2 >= 0)
1768 f7_Isquare (cc);
1769
1770 f7_Ildexp (cc, q);
1771 }
1772 #endif // F7MOD_exp_
1773
1774
1775 #ifdef F7MOD_pow10_
1776 F7_WEAK
1777 void f7_pow10 (f7_t *cc, const f7_t *aa)
1778 {
1779 const f7_t *p_ln10;
1780 F7_PCONST (p_ln10, ln10);
1781 f7_mul (cc, aa, p_ln10);
1782 f7_exp (cc, cc);
1783 }
1784 ALIAS (f7_pow10, f7_exp10)
1785 #endif // F7MOD_pow10_
1786
1787
1788 #ifdef F7MOD_cbrt_
1789 F7_WEAK
1790 void f7_cbrt (f7_t *cc, const f7_t *aa)
1791 {
1792 f7_copy (cc, aa);
1793 const f7_t *p_1_3;
1794 uint8_t c_flags = cc->flags;
1795 cc->flags &= ~F7_FLAG_sign;
1796 f7_log (cc, cc);
1797 F7_PCONST (p_1_3, 1_3);
1798 f7_Imul (cc, p_1_3);
1799 f7_exp (cc, cc);
1800
1801 if (c_flags & F7_FLAG_sign)
1802 cc->flags |= F7_FLAG_sign;
1803 }
1804 #endif // F7MOD_cbrt_
1805
1806
1807 #ifdef F7MOD_pow_
1808 F7_WEAK
1809 void f7_pow (f7_t *cc, const f7_t *aa, const f7_t *bb)
1810 {
1811 #if 0
1812 f7_t slots[cc == bb];
1813 f7_t *yy = cc == bb ? slots : cc;
1814 #else
1815 f7_t yy7, *yy = &yy7;
1816 #endif
1817 f7_log (yy, aa);
1818 f7_Imul (yy, bb);
1819 f7_exp (cc, yy);
1820 }
1821 #endif // F7MOD_pow_
1822
1823
1824 #ifdef F7MOD_powi_
1825 F7_WEAK
1826 void f7_powi (f7_t *cc, const f7_t *aa, int ii)
1827 {
1828 uint16_t u16 = ii;
1829 f7_t xx27, *xx2 = &xx27;
1830 bool cc_is_one = true;
1831 bool expo_is_neg = false;
1832
1833 if (ii < 0)
1834 {
1835 u16 = -u16;
1836 expo_is_neg = true;
1837 }
1838
1839 f7_copy (xx2, aa);
1840
1841 while (1)
1842 {
1843 if (u16 & 1)
1844 {
1845 if (cc_is_one)
1846 {
1847 // C *= X2 simplifies to C = X2.
1848 f7_copy (cc, xx2);
1849 cc_is_one = false;
1850 }
1851 else
1852 f7_Imul (cc, xx2);
1853 }
1854
1855 if (! cc_is_one
1856 && ! f7_is_nonzero (cc))
1857 break;
1858
1859 u16 >>= 1;
1860 if (u16 == 0)
1861 break;
1862 f7_Isquare (xx2);
1863 }
1864
1865 if (cc_is_one)
1866 f7_set_u16 (cc, 1);
1867 else if (expo_is_neg)
1868 f7_div1 (cc, cc);
1869 }
1870 #endif // F7MOD_powi_
1871
1872
1873 #ifdef F7MOD_sincos_
1874
1875 #define ARRAY_NAME coeff_sin
1876 #define FOR_SIN
1877 #include "libf7-array.def"
1878 #undef FOR_SIN
1879 #undef ARRAY_NAME
1880
1881 #define ARRAY_NAME coeff_cos
1882 #define FOR_COS
1883 #include "libf7-array.def"
1884 #undef FOR_COS
1885 #undef ARRAY_NAME
1886
1887 #define STATIC static
1888 #include "libf7-constdef.h" // pi_low
1889 #undef STATIC
1890
1891 typedef union
1892 {
1893 struct
1894 {
1895 bool neg_sin : 1; // Must be bit F7_FLAGNO_sign.
1896 bool neg_cos : 1;
1897 bool do_sin: 1;
1898 bool do_cos: 1;
1899 bool swap_sincos : 1;
1900 uint8_t res : 3;
1901 };
1902 uint8_t bits;
1903 } sincos_t;
1904
1905
1906 F7_WEAK
1907 void f7_sincos (f7_t *ss, f7_t *cc, const f7_t *aa)
1908 {
1909 uint8_t a_class = f7_classify (aa);
1910
1911 sincos_t sc = { .bits = a_class & F7_FLAG_sign };
1912 if (ss != NULL) sc.do_sin = 1;
1913 if (cc != NULL) sc.do_cos = 1;
1914
1915 if (f7_class_nan (a_class) || f7_class_inf (a_class))
1916 {
1917 if (sc.do_sin) f7_set_nan (ss);
1918 if (sc.do_cos) f7_set_nan (cc);
1919 return;
1920 }
1921
1922 f7_t pi7, *pi = &pi7;
1923 f7_t xx7, *xx = &xx7;
1924 f7_t yy7, *yy = &yy7;
1925 f7_t *hh = sc.do_sin ? ss : cc;
1926
1927 // X = |A|
1928 f7_copy (xx, aa);
1929 xx->flags = 0;
1930
1931 // Y is how often we subtract PI from X.
1932 f7_clr (yy);
1933 f7_const (pi, pi);
1934
1935 if (f7_abscmp_msb_ge (xx, F7_(const_pi_msb), F7_(const_pi_expo) + 1))
1936 {
1937 pi->expo = 1 + F7_(const_pi_expo); // 2*pi
1938
1939 // Y = X / 2pi.
1940 f7_div (yy, xx, pi);
1941
1942 // The integral part of |A| / pi mod 2 is bit 55 - x_expo.
1943 if (yy->expo >= F7_MANT_BITS && !f7_is_zero (yy))
1944 {
1945 // Too big for sensible calculation: Should this be NaN instead?
1946 if (sc.do_sin) f7_clr (ss);
1947 if (sc.do_cos) f7_clr (cc);
1948 return;
1949 }
1950
1951 // X -= 2pi * [ X / 2pi ]
1952 f7_floor (yy, yy);
1953
1954 f7_msub (xx, yy, pi, xx);
1955 xx->flags ^= F7_FLAG_sign;
1956
1957 // We divided by 2pi, but Y should count times we subtracted pi.
1958 yy->expo++;
1959 }
1960
1961 pi->expo = F7_(const_pi_expo); // pi
1962 f7_sub (hh, xx, pi);
1963 if (!f7_signbit (hh))
1964 {
1965 // H = X - pi >= 0 => X >= pi
1966 // sin(x) = -sin(x - pi)
1967 // cos(x) = -cos(x - pi)
1968 f7_copy (xx, hh);
1969 // Y: We subtracted pi one more time.
1970 f7_Iadd (yy, f7_set_u16 (hh, 1));
1971 sc.neg_sin ^= 1;
1972 sc.neg_cos ^= 1;
1973 }
1974
1975 pi->expo = F7_(const_pi_expo) - 1; // pi/2
1976 if (f7_gt (xx, pi))
1977 {
1978 // x > pi/2
1979 // sin(x) = sin(pi - x)
1980 // cos(x) = -cos(pi - x)
1981 pi->expo = F7_(const_pi_expo); // pi
1982 f7_IRsub (xx, pi);
1983 // Y: We subtracted pi one more time (and then negated).
1984 f7_Iadd (yy, f7_set_u16 (hh, 1));
1985 yy->flags ^= F7_FLAG_sign;
1986 sc.neg_cos ^= 1;
1987 }
1988
1989 pi->expo = F7_(const_pi_expo) - 2; // pi/4
1990 if (f7_gt (xx, pi))
1991 {
1992 // x > pi/4
1993 // sin(x) = cos(pi/2 - x)
1994 // cos(x) = sin(pi/2 - x)
1995 pi->expo = F7_(const_pi_expo) - 1; // pi/2
1996 f7_IRsub (xx, pi);
1997 // Y: We subtracted pi/2 one more time (and then negated).
1998 f7_Iadd (yy, f7_set_1pow2 (hh, -1, 0));
1999 yy->flags ^= F7_FLAG_sign;
2000 sc.swap_sincos = 1;
2001 }
2002
2003 if (!f7_is0 (yy))
2004 {
2005 // Y counts how often we subtracted pi from X in order to
2006 // get 0 <= X < pi/4 as small as possible (Y is 0 mod 0.5).
2007 // Now also subtract the low part of pi:
2008 // f7_const_pi_low = pi - f7_const_pi in order to get more precise
2009 // results in the cases where the final result is close to 0.
2010 const f7_t *pi_low = F7_CONST_ADDR (pi_low, pi);
2011 //f7_const (pi, pi_low);
2012 f7_Imul (yy, pi_low);
2013 f7_Isub (xx, yy);
2014 }
2015
2016 // X in [0, pi/4].
2017 // X^2 in [0, pi^2/16] ~ [0, 0.6169]
2018
2019 f7_square (yy, xx);
2020
2021 f7_t *x_sin = xx;
2022 f7_t *x_cos = yy;
2023
2024 if ((sc.do_sin && !sc.swap_sincos)
2025 || (sc.do_cos && sc.swap_sincos))
2026 {
2027 f7_horner (hh, yy, n_coeff_sin, coeff_sin, NULL);
2028 f7_mul (x_sin, hh, xx);
2029 }
2030
2031 if ((sc.do_cos && !sc.swap_sincos)
2032 || (sc.do_sin && sc.swap_sincos))
2033 {
2034 f7_horner (x_cos, yy, n_coeff_cos, coeff_cos, hh);
2035 }
2036
2037 if (sc.swap_sincos)
2038 {
2039 x_sin = yy;
2040 x_cos = xx;
2041 }
2042
2043 if (sc.do_sin)
2044 {
2045 x_sin->flags ^= sc.bits;
2046 x_sin->flags &= F7_FLAG_sign;
2047 f7_copy (ss, x_sin);
2048 }
2049
2050 if (sc.do_cos)
2051 {
2052 x_cos->flags = sc.neg_cos;
2053 f7_copy (cc, x_cos);
2054 }
2055 }
2056 #endif // F7MOD_sincos_
2057
2058 #ifdef F7MOD_sin_
2059 F7_WEAK
2060 void f7_sin (f7_t *ss, const f7_t *aa)
2061 {
2062 f7_sincos (ss, NULL, aa);
2063 }
2064 #endif // F7MOD_sin_
2065
2066 #ifdef F7MOD_cos_
2067 F7_WEAK
2068 void f7_cos (f7_t *cc, const f7_t *aa)
2069 {
2070 f7_sincos (NULL, cc, aa);
2071 }
2072 #endif // F7MOD_cos_
2073
2074
2075 #ifdef F7MOD_tan_
2076 F7_WEAK
2077 void f7_tan (f7_t *tt, const f7_t *aa)
2078 {
2079 f7_t xcos;
2080 f7_sincos (tt, & xcos, aa);
2081 f7_Idiv (tt, & xcos);
2082 }
2083 #endif // F7MOD_tan_
2084
2085
2086 #ifdef F7MOD_cotan_
2087 F7_WEAK
2088 void f7_cotan (f7_t *ct, const f7_t *aa)
2089 {
2090 f7_t xcos;
2091 f7_sincos (ct, & xcos, aa);
2092 f7_div (ct, & xcos, ct);
2093 }
2094 #endif // F7MOD_cotan_
2095
2096
2097 #ifdef F7MOD_sinhcosh_
2098 F7_WEAK
2099 void f7_sinhcosh (f7_t *cc, const f7_t *aa, bool do_sinh)
2100 {
2101 f7_t xx7, *xx = &xx7;
2102 // C = exp(A)
2103 f7_exp (cc, aa);
2104 // X = exp(-A)
2105 f7_div (xx, f7_set_u16 (xx, 1), cc);
2106 // sinh(A) = (exp(A) - exp(-A)) / 2
2107 // cosh(A) = (exp(A) + exp(-A)) / 2
2108 f7_addsub (cc, cc, xx, do_sinh);
2109 cc->expo--;
2110 }
2111 #endif // F7MOD_sinhcosh_
2112
2113
2114 #ifdef F7MOD_sinh_
2115
2116 #define ARRAY_NAME coeff_sinh
2117 #include "libf7-array.def"
2118 #undef ARRAY_NAME
2119
2120 F7_WEAK
2121 void f7_sinh (f7_t *cc, const f7_t *aa)
2122 {
2123 if (aa->expo <= -2)
2124 {
2125 // For small values, exp(A) - exp(-A) suffers from cancellation, hence
2126 // use a MiniMax polynomial for |A| < 0.5.
2127 f7_t xx7, *xx = &xx7;
2128 f7_t hh7, *hh = &hh7;
2129 f7_square (xx, aa);
2130 f7_horner (hh, xx, n_coeff_sinh, coeff_sinh, NULL);
2131 f7_mul (cc, aa, hh);
2132 return;
2133 }
2134
2135 f7_sinhcosh (cc, aa, true);
2136 }
2137 #endif // F7MOD_sinh_
2138
2139
2140 #ifdef F7MOD_cosh_
2141 F7_WEAK
2142 void f7_cosh (f7_t *cc, const f7_t *aa)
2143 {
2144 f7_sinhcosh (cc, aa, false);
2145 }
2146 #endif // F7MOD_cosh_
2147
2148
2149 #ifdef F7MOD_tanh_
2150 F7_WEAK
2151 void f7_tanh (f7_t *cc, const f7_t *aa)
2152 {
2153 // tanh(A) = (exp(2A) - 1) / (exp(2A) + 1)
2154 f7_t xx7, *xx = &xx7;
2155 F7_PCONST_U16 (const f7_t *one, 1);
2156 // C = 2A
2157 f7_copy (cc, aa);
2158 cc->expo++;
2159 // C = exp(2A)
2160 f7_exp (cc, cc);
2161 // X = exp(2A) + 1
2162 f7_add (xx, cc, one);
2163 // C = exp(2A) - 1
2164 f7_Isub (cc, one);
2165 // C = tanh(A)
2166 f7_Idiv (cc, xx);
2167 }
2168 #endif // F7MOD_tanh_
2169
2170
2171 #ifdef F7MOD_atan_
2172
2173 #define MINIMAX_6_6_IN_0_1
2174
2175 #define ARRAY_NAME coeff_atan_zahler
2176 #define FOR_NUMERATOR
2177 #include "libf7-array.def"
2178 #undef FOR_NUMERATOR
2179 #undef ARRAY_NAME
2180
2181 #define ARRAY_NAME coeff_atan_nenner
2182 #define FOR_DENOMINATOR
2183 #include "libf7-array.def"
2184 #undef FOR_DENOMINATOR
2185 #undef ARRAY_NAME
2186
2187 #include "libf7-constdef.h"
2188
2189 F7_WEAK
2190 void f7_atan (f7_t *cc, const f7_t *aa)
2191 {
2192 uint8_t a_class = f7_classify (aa);
2193 uint8_t flags = a_class & F7_FLAG_sign;
2194
2195 if (f7_class_nan (a_class))
2196 return f7_set_nan (cc);
2197
2198 f7_t yy7, *yy = &yy7;
2199 f7_t zz7, *zz = &zz7;
2200
2201 if (f7_class_inf (a_class))
2202 {
2203 f7_set_u16 (cc, 0);
2204 goto do_Inf;
2205 }
2206
2207 // C = |A|
2208 f7_copy (cc, aa);
2209 cc->flags = 0;
2210
2211 if (!f7_class_zero (a_class) && cc->expo >= 0)
2212 {
2213 // C >= 1: use atan (x) + atan (1/x) = pi / 2 to reduce to [0, 1].
2214 flags |= 1 << 1;
2215 f7_div (cc, f7_set_u16 (yy, 1), cc);
2216 }
2217 #if !defined (MINIMAX_6_6_IN_0_1)
2218 const uint8_t const_a_msb = 0x89;
2219 const int16_t const_a_expo = -2;
2220 if (f7_abscmp_msb_ge (cc, const_a_msb, const_a_expo))
2221 {
2222 // We have C in [0, 1] and we want to use argument reduction by means
2223 // of addition theorem atan(x) - atan(y) = atan((x - y) / (1 + xy)).
2224 // We want to split [0, 1] into [0, a] u [a, 1] in such a way that
2225 // the upper interval will be mapped to [-a, a]. The system is easy
2226 // to solve and yiels
2227 // y = 1 / sqrt (3) ~ 0.57735 atan(y) = pi / 6
2228 // a = (1 - y) / (1 + y) ~ 0.26795 ~ 0x0.8930A2F * 2^-1.
2229 flags |= 1 << 2;
2230 // C <- (C - Y) / (1 + C * Y) in [-a, a]
2231 const f7_t *cst = F7_CONST_ADDR (1_sqrt3, zz);
2232 f7_mul (yy, cc, cst);
2233 f7_Isub (cc, cst);
2234 f7_Iadd (yy, F7_U16_ADDR (1, zz));
2235 f7_Idiv (cc, yy);
2236 }
2237 #endif
2238 // C <- C * p(C^2) / q(C^2)
2239 f7_square (yy, cc);
2240 f7_horner (zz, yy, n_coeff_atan_zahler, coeff_atan_zahler, NULL);
2241 f7_Imul (zz, cc);
2242 f7_horner (cc, yy, n_coeff_atan_nenner, coeff_atan_nenner, NULL);
2243 f7_div (cc, zz, cc);
2244
2245 #if !defined (MINIMAX_6_6_IN_0_1)
2246 if (flags & (1 << 2))
2247 f7_Iadd (cc, F7_CONST_ADDR (pi_6, yy));
2248 #endif
2249
2250 if (flags & (1 << 1))
2251 {
2252 do_Inf:;
2253 // Y = pi / 2
2254 f7_const (yy, pi);
2255 yy->expo = F7_(const_pi_expo) - 1;
2256 f7_IRsub (cc, yy);
2257 }
2258
2259 cc->flags = a_class & F7_FLAG_sign;
2260 }
2261 #undef MINIMAX_6_6_IN_0_1
2262 #endif // F7MOD_atan_
2263
2264
2265 #ifdef F7MOD_atan2_
2266 F7_WEAK
2267 void f7_atan2 (f7_t *cc, const f7_t *yy, const f7_t *xx)
2268 {
2269 uint8_t y_class = f7_classify (yy);
2270 uint8_t x_class = f7_classify (xx);
2271
2272 // (NaN, *) -> NaN
2273 // (*, NaN) -> NaN
2274 if (f7_class_nan (y_class | x_class))
2275 return f7_set_nan (cc);
2276
2277 // (0, 0) -> 0
2278 if (f7_class_zero (y_class & x_class))
2279 return f7_clr (cc);
2280
2281 f7_t pi7, *pi = &pi7;
2282 f7_const (pi, pi);
2283
2284 // (Inf, +Inf) -> +pi/4; (-Inf, +Inf) -> +3pi/4
2285 // (Inf, -Inf) -> -pi/4; (-Inf, -Inf) -> -3pi/4
2286 if (f7_class_inf (y_class & x_class))
2287 {
2288 f7_copy (cc, pi);
2289 if (! f7_class_sign (x_class))
2290 cc->expo = F7_(const_pi_expo) - 1; // pi / 2
2291 pi->expo = F7_(const_pi_expo) - 2; // pi / 4
2292 f7_Isub (cc, pi);
2293 cc->flags = y_class & F7_FLAG_sign;
2294 return;
2295 }
2296
2297 // sign(pi) := sign(y)
2298 pi->flags = y_class & F7_FLAG_sign;
2299
2300 // Only use atan(*) with |*| <= 1.
2301
2302 if (f7_cmp_abs (yy, xx) > 0)
2303 {
2304 // |y| > |x|: atan2 = sgn(y) * pi/2 - atan (x / y);
2305 pi->expo = F7_(const_pi_expo) - 1; // +- pi / 2
2306 f7_div (cc, xx, yy);
2307 f7_atan (cc, cc);
2308 f7_IRsub (cc, pi);
2309 }
2310 else
2311 {
2312 // x > |y|: atan2 = atan (y / x)
2313 // x < -|y|: atan2 = atan (y / x) +- pi
2314 f7_div (cc, yy, xx);
2315 f7_atan (cc, cc);
2316 if (f7_class_sign (x_class))
2317 f7_Iadd (cc, pi);
2318 }
2319 }
2320 #endif // F7MOD_atan2_
2321
2322
2323 #ifdef F7MOD_asinacos_
2324
2325 #define ARRAY_NAME coeff_func_a_zahler
2326 #define FOR_NUMERATOR
2327 #include "libf7-array.def"
2328 #undef FOR_NUMERATOR
2329 #undef ARRAY_NAME
2330
2331 #define ARRAY_NAME coeff_func_a_nenner
2332 #define FOR_DENOMINATOR
2333 #include "libf7-array.def"
2334 #undef FOR_DENOMINATOR
2335 #undef ARRAY_NAME
2336
2337 typedef union
2338 {
2339 struct
2340 {
2341 bool sign : 1; // Must be bit F7_FLAGNO_sign.
2342 bool do_acos : 1; // From caller.
2343 bool have_acos : 1; // What we compute from rational approx p/q.
2344 uint8_t res : 5;
2345 };
2346 uint8_t bits;
2347 } asinacos_t;
2348
2349 F7_WEAK
2350 void f7_asinacos (f7_t *cc, const f7_t *aa, uint8_t what)
2351 {
2352 f7_t xx7, *xx = &xx7;
2353 f7_t xx27, *xx2 = &xx27;
2354
2355 asinacos_t flags = { .bits = what | f7_signbit (aa) };
2356
2357 f7_abs (xx, aa);
2358
2359 int8_t cmp = f7_cmp (xx, f7_set_u16 (cc, 1));
2360
2361 if (cmp == INT8_MIN
2362 || cmp > 0)
2363 {
2364 return f7_set_nan (cc);
2365 }
2366
2367 if (xx->expo <= -2 || f7_is_zero (xx))
2368 {
2369 // |A| < 1/2: asin(x) = x * a(2*x^2)
2370 f7_square (xx2, xx);
2371 xx2->expo ++;
2372 }
2373 else
2374 {
2375 // |A| > 1/2: acos (1-x) = sqrt(2*x) * a(x)
2376 // C is 1 from above.
2377 f7_IRsub (xx, cc);
2378 f7_copy (xx2, xx);
2379 flags.have_acos = 1;
2380 }
2381
2382 // MiniMax [5/4] numerator.
2383 f7_horner (cc, xx2, n_coeff_func_a_zahler, coeff_func_a_zahler, NULL);
2384
2385 if (flags.have_acos)
2386 {
2387 xx->expo ++;
2388 f7_Isqrt (xx);
2389 }
2390 f7_Imul (cc, xx);
2391
2392 // MiniMax [5/4] denominator.
2393 f7_horner (xx, xx2, n_coeff_func_a_nenner, coeff_func_a_nenner, NULL);
2394
2395 f7_Idiv (cc, xx);
2396
2397 /*
2398 With the current value of C, we have:
2399
2400 | | do_asin | do_acos
2401 | C | A <= 0 | A >= 0 | A <= 0 | A >= 0
2402 ----------+------------+-----------+----------+----------+----------
2403 have_asin | asin (|A|) | -C | C | pi/2 + C | pi/2 - C
2404 have_acos | acos (|A|) | -pi/2 + C | pi/2 - C | pi - C | C
2405
2406 Result = n_pi2 * pi/2 + C * (c_sign ? -1 : 1)
2407 Result (A, do_asin) = asin (A)
2408 Result (A, do_acos) = acos (A)
2409
2410 with
2411 c_sign = do_acos ^ have_acos ^ a_sign
2412 n_pi2 = do_acos + have_acos * (a_sign ^ do_acos) ? (-1 : 1)
2413 n_pi2 in { -1, 0, 1, 2 }
2414 */
2415
2416 // All what matters for c_sign is bit 0.
2417 uint8_t c_sign = flags.bits;
2418 int8_t n_pi2 = flags.do_acos;
2419 c_sign ^= flags.do_acos;
2420 if (flags.have_acos)
2421 {
2422 n_pi2++;
2423 __asm ("" : "+r" (n_pi2));
2424 if (c_sign & 1) // c_sign & 1 = a_sign ^ do_acos
2425 n_pi2 -= 2;
2426 c_sign++;
2427 }
2428
2429 cc->flags = c_sign & F7_FLAG_sign;
2430
2431 if (n_pi2)
2432 {
2433 f7_const (xx, pi);
2434 if (n_pi2 < 0)
2435 xx->sign = 1;
2436 if (n_pi2 != 2)
2437 xx->expo = F7_(const_pi_expo) - 1;
2438
2439 f7_Iadd (cc, xx);
2440 }
2441 }
2442 #endif // F7MOD_asinacos_
2443
2444
2445 #ifdef F7MOD_asin_
2446 F7_WEAK
2447 void f7_asin (f7_t *cc, const f7_t *aa)
2448 {
2449 f7_asinacos (cc, aa, 0);
2450 }
2451 #endif // F7MOD_asin_
2452
2453
2454 #ifdef F7MOD_acos_
2455 F7_WEAK
2456 void f7_acos (f7_t *cc, const f7_t *aa)
2457 {
2458 f7_asinacos (cc, aa, 1 << 1);
2459 }
2460 #endif // F7MOD_acos_
2461
2462
2463 #ifndef IN_LIBGCC2
2464
2465 #ifdef F7MOD_put_C_
2466
2467 #include <stdio.h>
2468 #include <avr/pgmspace.h>
2469
2470 static F7_INLINE
2471 uint8_t f7_hex_digit (uint8_t nibble)
2472 {
2473 nibble = (uint8_t) (nibble + '0');
2474 if (nibble > '9')
2475 nibble = (uint8_t) (nibble + ('a' - '0' - 10));
2476 return nibble;
2477 }
2478
2479 static void f7_put_hex2 (uint8_t x, FILE *stream)
2480 {
2481 putc ('0', stream);
2482 if (x)
2483 {
2484 putc ('x', stream);
2485 putc (f7_hex_digit (x >> 4), stream);
2486 putc (f7_hex_digit (x & 0xf), stream);
2487 }
2488 }
2489
2490 #define XPUT(str) \
2491 fputs_P (PSTR (str), stream)
2492
2493 // Write to STREAM a line that is appropriate for usage in libf7-const.def.
2494
2495 F7_WEAK
2496 void f7_put_CDEF (const char *name, const f7_t *aa, FILE *stream)
2497 {
2498 char buf[7];
2499 XPUT ("F7_CONST_DEF (");
2500 fputs (name, stream);
2501 XPUT (",\t");
2502 uint8_t a_class = f7_classify (aa);
2503 if (! f7_class_nonzero (a_class))
2504 {
2505 f7_put_hex2 (a_class & F7_FLAGS, stream);
2506 XPUT (",\t0,0,0,0,0,0,0,\t0)");
2507 return;
2508 }
2509 putc ('0' + (a_class & F7_FLAGS), stream);
2510 XPUT (",\t");
2511
2512 for (uint8_t i = 0; i < F7_MANT_BYTES; i++)
2513 {
2514 f7_put_hex2 (aa->mant[F7_MANT_BYTES-1 - i], stream);
2515 putc (',', stream);
2516 }
2517 putc ('\t', stream);
2518
2519 itoa (aa->expo, buf, 10);
2520 fputs (buf, stream);
2521 XPUT (")");
2522 }
2523
2524 void f7_put_C (const f7_t *aa, FILE *stream)
2525 {
2526 char buf[7];
2527
2528 uint8_t a_class = f7_classify (aa);
2529 if (f7_class_nan (a_class))
2530 {
2531 XPUT ("{ .is_nan = 1 }");
2532 return;
2533 }
2534 bool sign = a_class & F7_FLAG_sign;
2535
2536 if (f7_class_inf (a_class))
2537 {
2538 XPUT ("{ .is_nan = 1, .sign = ");
2539 putc ('0' + sign, stream);
2540 XPUT (" }");
2541 return;
2542 }
2543
2544 XPUT ("{ .sign = ");
2545 putc ('0' + sign, stream);
2546
2547 XPUT (", .mant = { ");
2548 for (uint8_t i = 0; i < F7_MANT_BYTES; i++)
2549 {
2550 f7_put_hex2 (aa->mant[F7_MANT_BYTES-1 - i], stream);
2551 if (i != F7_MANT_BYTES - 1)
2552 putc (',', stream);
2553 }
2554
2555 XPUT (" }, .expo = ");
2556 itoa (aa->expo, buf, 10);
2557 fputs (buf, stream);
2558 XPUT (" }");
2559 }
2560 #endif //F7MOD_put_C_
2561
2562
2563 #ifdef F7MOD_dump_
2564
2565 #include <avr/pgmspace.h>
2566
2567 #ifndef AVRTEST_H
2568
2569 #include <stdio.h>
2570
2571 static void LOG_PSTR (const char *str)
2572 {
2573 fputs_P (str, stdout);
2574 }
2575
2576 static void LOG_PFMT_U16 (const char *fmt, uint16_t x)
2577 {
2578 printf_P (fmt, x);
2579 }
2580
2581 static void LOG_PFMT_FLOAT (const char *fmt, float x)
2582 {
2583 printf_P (fmt, x);
2584 }
2585
2586 #define LOG_X8(X) LOG_PFMT_U16 (PSTR (" 0x%02x "), (uint8_t)(X))
2587 #define LOG_PFMT_S16(FMT, X) LOG_PFMT_U16 (FMT, (unsigned)(X))
2588 #define LOG_PFMT_ADDR(FMT, X) LOG_PFMT_U16 (FMT, (unsigned)(X))
2589
2590 #endif // AVRTEST_H
2591
2592 static void dump_byte (uint8_t b)
2593 {
2594 LOG_PSTR (PSTR (" "));
2595 for (uint8_t i = 0; i < 8; i++)
2596 {
2597 LOG_PSTR ((b & 0x80) ? PSTR ("1") : PSTR ("0"));
2598 b = (uint8_t) (b << 1);
2599 }
2600 }
2601
2602 void f7_dump_mant (const f7_t *aa)
2603 {
2604 LOG_PSTR (PSTR ("\tmant ="));
2605 for (int i = F7_MANT_BYTES - 1; i >= 0; i--)
2606 LOG_X8 (aa->mant[i]);
2607 LOG_PSTR (PSTR ("\n\t ="));
2608
2609 for (int i = F7_MANT_BYTES - 1; i >= 0; i--)
2610 dump_byte (aa->mant[i]);
2611 LOG_PSTR (PSTR ("\n"));
2612 }
2613
2614 void f7_dump (const f7_t *aa)
2615 {
2616 LOG_PFMT_ADDR (PSTR ("\n0x%04x\tflags = "), aa);
2617 dump_byte (aa->flags);
2618 uint8_t a_class = f7_classify (aa);
2619 LOG_PSTR (PSTR (" = "));
2620 LOG_PSTR (f7_class_sign (a_class) ? PSTR ("-") : PSTR ("+"));
2621 if (f7_class_inf (a_class)) LOG_PSTR (PSTR ("Inf "));
2622 if (f7_class_nan (a_class)) LOG_PSTR (PSTR ("NaN "));
2623 if (f7_class_zero (a_class)) LOG_PSTR (PSTR ("0 "));
2624 if (f7_class_number (a_class)) LOG_PSTR (PSTR ("Number "));
2625
2626 LOG_PFMT_FLOAT (PSTR (" = %.10g\n"), f7_get_float (aa));
2627 LOG_PFMT_S16 (PSTR ("\texpo = %d\n"), aa->expo);
2628
2629 f7_dump_mant (aa);
2630 }
2631 #endif // F7MOD_dump_
2632
2633 #endif // ! libgcc
2634
2635 #endif // !AVR_TINY