1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
|
// Copyright 2015 Mark Owen ([email protected])
// SPDX-License-Identifier: GPL-2.0-or-later
.ifndef include_faster
.equ include_faster,1
.endif
.ifndef include_conversions
.equ include_conversions,1
.endif
.ifndef include_scientific
.equ include_scientific,1
.endif
.if include_scientific
.equ include_conversions,1
.endif
.syntax unified
.cpu cortex-m0
.thumb
.global qfp_fadd
.global qfp_fsub
.global qfp_fmul
.global qfp_fdiv
.global qfp_fcmp
.if include_conversions
.global qfp_float2int
.global qfp_float2fix
.global qfp_float2uint
.global qfp_float2ufix
.global qfp_int2float
.global qfp_fix2float
.global qfp_uint2float
.global qfp_ufix2float
.endif
.if include_scientific
.global qfp_fcos
.global qfp_fsin
.global qfp_ftan
.global qfp_fatan2
.global qfp_fexp
.global qfp_fln
.global qfp_fsqrt
.endif
.if include_faster
.global qfp_fdiv_fast
.global qfp_fsqrt_fast
.endif
// exchange r0<->r1, r2<->r3
xchxy:
push {r0,r2,r14}
mov r0,r1
mov r2,r3
pop {r1,r3,r15}
// IEEE single precision floats in r0,r1-> mantissae in r1,r0 exponents in r3,r2 *respectively*
// trashes r4
unpackxy:
push {r14}
bl unpackx
bl xchxy
pop {r4}
mov r14,r4
// IEEE single in r0-> signed (two's complemennt) mantissa in r0 9Q23 (24 significant bits), signed exponent (bias removed) in r2
// trashes r4; zero, denormal -> mantissa=+/-1, exponent=-380; Inf, NaN -> mantissa=+/-1, exponent=+640
unpackx:
lsrs r2,r0,#23 // save exponent and sign
lsls r0,#9 // extract mantissa
lsrs r0,#9
movs r4,#1
lsls r4,#23
orrs r0,r4 // reinstate implied leading 1
cmp r2,#255 // test sign bit
uxtb r2,r2 // clear it
bls 1f // branch on positive
rsbs r0,#0 // negate mantissa
1:
subs r2,#1
cmp r2,#254 // zero/denormal/Inf/NaN?
bhs 2f
subs r2,#126 // remove exponent bias: can now be -126..+127
bx r14
2: // here with special-case values
cmp r0,#0
mov r0,r4 // set mantissa to +1
bpl 3f
rsbs r0,#0 // zero/denormal/Inf/NaN: mantissa=+/-1
3:
subs r2,#126 // zero/denormal: exponent -> -127; Inf, NaN: exponent -> 128
lsls r2,#2 // zero/denormal: exponent -> -508; Inf, NaN: exponent -> 512
adds r2,#128 // zero/denormal: exponent -> -380; Inf, NaN: exponent -> 640
bx r14
// normalise and pack signed mantissa in r0 nominally 3Q29, signed exponent in r2-> IEEE single in r0
// trashes r4, preserves r1,r3
// r5: "sticky bits", must be zero iff all result bits below r0 are zero for correct rounding
packx:
lsrs r4,r0,#31 // save sign bit
lsls r4,r4,#31 // sign now in b31
bpl 2f // skip if positive
cmp r5,#0
beq 11f
adds r0,#1 // fiddle carry in to following rsb if sticky bits are non-zero
11:
rsbs r0,#0 // can now treat r0 as unsigned
packx0:
bmi 3f // catch r0=0x80000000 case
2:
subs r2,#1 // normalisation loop
adds r0,r0
beq 1f // zero? special case
bpl 2b // normalise so leading "1" in bit 31
3:
adds r2,#129 // (mis-)offset exponent
bne 12f // special case: highest denormal can round to lowest normal
adds r0,#0x80 // in special case, need to add 256 to r0 for rounding
bcs 4f // tripped carry? then have leading 1 in C as required
12:
adds r0,#0x80 // rounding
bcs 4f // tripped carry? then have leading 1 in C as required (and result is even so can ignore sticky bits)
cmp r5,#0
beq 7f // sticky bits zero?
8:
lsls r0,#1 // remove leading 1
9:
subs r2,#1 // compensate exponent on this path
4:
cmp r2,#254
bge 5f // overflow?
adds r2,#1 // correct exponent offset
ble 10f // denormal/underflow?
lsrs r0,#9 // align mantissa
lsls r2,#23 // align exponent
orrs r0,r2 // assemble exponent and mantissa
6:
orrs r0,r4 // apply sign
1:
bx r14
5:
movs r0,#0xff // create infinity
lsls r0,#23
b 6b
10:
movs r0,#0 // create zero
bx r14
7: // sticky bit rounding case
lsls r5,r0,#24 // check bottom 8 bits of r0
bne 8b // in rounding-tie case?
lsrs r0,#9 // ensure even result
lsls r0,#10
b 9b
// unpack two arguments (r0,r1) and shift one down to have common exponent, returned in r2; note that arguments are exchanged
// sticky bits shifted off bottom of smaller argument in r5
// following code is unnecessarily general for fadd, but is shared with atan2
unpackxyalign:
push {r14}
bl unpackxy
lsls r0,r0,#6 // Q29
lsls r1,r1,#6 // Q29
subs r4,r2,r3 // calculate shift
bge 1f // x>=y?
mov r2,r3 // no: take common exponent from y
mov r5,r0 // potential sticky bits from x
rsbs r4,#0 // make shift positive
asrs r0,r4
cmp r4,#32
blo 2f
movs r0,#0 // large shift, so all bits are sticky and result is zero
pop {r15}
1:
mov r5,r1 // common exponent from x; potential sticky bits from y
asrs r1,r4
cmp r4,#32
blo 2f
movs r1,#0 // large shift, so all bits are sticky and result is zero
pop {r15}
2:
rsbs r4,#0
adds r4,#32
lsls r5,r4 // extract sticky bits
pop {r15}
.thumb_func
qfp_fsub:
movs r2,#1 // subtract: flip sign bit of second argument and fall through to fadd
lsls r2,#31
eors r1,r2
.thumb_func
qfp_fadd:
push {r4,r5,r14}
bl unpackxyalign
adds r0,r1 // do addition
bne 2f // not in Inf-Inf case?
cmp r2,#200
blt 2f
movs r0,#1
lsls r0,#29 // for Inf-Inf, set mantissa to +1 to prevent zero result
2:
packret: // common return point: "pack and return"
bl packx
pop {r4,r5,r15}
// signed multiply r0 1Q23 by r1 4Q23, result in r0 7Q25, sticky bits in r5
// trashes r3,r4
mul0:
uxth r3,r0 // Q23
asrs r4,r1,#16 // Q7
muls r3,r4 // L*H, Q30 signed
asrs r4,r0,#16 // Q7
uxth r5,r1 // Q23
muls r4,r5 // H*L, Q30 signed
adds r3,r4 // sum of middle partial products
uxth r4,r0
muls r4,r5 // L*L, Q46 unsigned
lsls r5,r4,#16 // initialise sticky bits from low half of low partial product
lsrs r4,#16 // Q25
adds r3,r4 // add high half of low partial product to sum of middle partial products
// (cannot generate carry by limits on input arguments)
asrs r0,#16 // Q7
asrs r1,#16 // Q7
muls r0,r1 // H*H, Q14 signed
lsls r0,#11 // high partial product Q25
lsls r1,r3,#27 // sticky
orrs r5,r1 // collect further sticky bits
asrs r1,r3,#5 // middle partial products Q25
adds r0,r1 // final result
bx r14
.thumb_func
qfp_fcmp:
movs r2,#1 // initialise result
lsls r3,r2,#31 // r3=0x80000000
tst r0,r3 // check sign of first argument
beq 1f
subs r0,r3,r0 // convert to 2's complement form for direct comparison
1:
tst r1,r3 // repeat for second argument
beq 2f
subs r1,r3,r1
2:
subs r0,r1 // perform comparison
beq 4f // equal? return 0
bgt 3f // r0>r1? return +1
rsbs r2,#0 // r0<r1: return -1
3:
mov r0,r2
4:
bx r14
.thumb_func
qfp_fmul:
push {r4,r5,r14}
bl unpackxy
add r2,r3 // sum exponents
adds r2,#4 // adjust exponent for pack which expects Q29
fmul0:
bl mul0
b packret
.thumb_func
qfp_fdiv:
push {r4,r5,r14}
fdiv_n:
bl unpackxy
movs r5,#1 // result cannot fall exactly half-way between two representable numbers (exercise for reader - note that
// we do not handle denormals) so there will always be sticky bits
cmp r0,#0 // check divisor sign
bpl 1f
rsbs r0,#0 // ensure divisor positive
rsbs r1,#0 // preserve sign of result
1:
movs r4,#0
cmp r1,#0 // check sign of dividend
bpl 2f
rsbs r1,#0 // result will be negative
mvns r4,r4 // save sign as 0x00000000 or 0xffffffff
2: // now do unsigned division on unpacked values {r1,r3}/{r0,r2}
cmp r3,#200 // dividend is an infinity? return it
bge 3f
rsbs r2,#0
cmp r2,#200 // divisor is zero? return infinity
bge 3f
adds r2,r3 // difference of exponents
movs r3,#0x40 // termination marker (calculate enough bits to do rounding correctly)
2: // division loop
subs r1,r0 // trial subtraction
bcs 1f
add r1,r0 // restore if failed
1:
adcs r3,r3 // shift in result bit
add r1,r1 // shift up dividend
bcc 2b // loop until marker appears in carry
lsls r0,r3,#4 // align for packing
4:
eors r0,r4 // apply sign
b packret
infret:
movs r4,#0
3:
mov r0,r1
movs r2,#255 // return infinity
b 4b
.if include_faster
// The fast divide routine uses an initial approximation to the reciprocal of the divisor based on the top four bits of the mantissa
// followed by three Newton-Raphson iterations, resulting in about 27 bits of accuracy. This reciprocal is then multiplied by
// the dividend.
// The (fixed-point) reciprocal calculation is carefully implemented to preserve the necessary accuracy throughout. In places
// the implied binary point is not within the stored value. For example where "Q47" is shown below it means that the least
// significant bit of the value has significance 2^-47 and hence the most significant bit has significance 2^-16. In these
// cases the value is known to be very close to an integer (usually 1) and so the bits of greater significance do not need
// to be stored.
// The reciprocal calculation has been tested for all possible input mantissa values.
.thumb_func
qfp_fdiv_fast:
push {r4,r5,r14}
fdiv_fast_n:
bl unpackxy
cmp r0,#0
bpl 1f
rsbs r0,#0
rsbs r1,#0 // ensure divisor positive
1:
cmp r3,#200
bge infret // dividend is an infinity? return it
rsbs r2,#0
cmp r2,#200 // divisor is zero?
bge infret // return infinity
adds r2,r3 // difference of exponents
adr r4,rcpapp-8 // the first 8 elements of the table are never accessed because of the mantissa's leading 1
lsrs r3,r0,#20 // y Q23; y>>20 Q7
ldrb r4,[r4,r3] // m=rcpapp[(y>>20)&7]; // Q8, .5<m<1
lsls r3,r4,#2 // m<<2 // Q10 first Newton-Raphson iteration
muls r3,r0 // s=y*(m<<2); // Q33
lsls r4,#8 // m<<=8; // Q16
asrs r3,#21 // s>>=21; // Q12
muls r3,r4 // s*=m; // Q28
asrs r3,#12 // s>>=12; // Q16
subs r4,r3 // m=m-s; // Q16
mov r3,r4 // s=y*m // Q39 second Newton-Raphson iteration
muls r4,r0 // ...
asrs r4,#16 // s>>=16; // Q23
muls r4,r3 // s*=m; // Q39
lsls r3,#8 // m<<=8; // Q24
asrs r4,#15 // s>>=15; // Q24
subs r3,r4 // m=m-s; // Q24
lsls r4,r3,#7 // \/ s=y*m; // Q47 third Newton-Raphson iteration
muls r3,r0 // /\ m<<=7; // Q31
asrs r3,#15 // s>>=15; // Q32
lsrs r0,r4,#16 // s*=(m>>16); // Q47
muls r3,r0 // ...
asrs r3,#16 // s>>=16; // Q31
subs r0,r4,r3 // m=m-s; // Q31
div0:
adds r0,#7 // rounding; reduce systematic error
lsrs r0,#4 // Q27
b fmul0 // drop into multiplication code to calculate result
// The fast square root routine uses an initial approximation to the reciprocal of the square root of the argument based
// on the top four bits of the mantissa (possibly shifted one place to make the exponent even). It then performs three
// Newton-Raphson iterations, resulting in about 28-29 bits of accuracy. This reciprocal is then multiplied by
// the original argument to produce the result.
// Again, the fixed-point calculation is carefully implemented to preserve accuracy, and similar comments to those
// made above on the fast division routine apply.
// The reciprocal square root calculation has been tested for all possible (possibly shifted) input mantissa values.
.thumb_func
qfp_fsqrt_fast:
push {r4,r5,r14}
bl unpackx
movs r1,r0
bmi infret // negative? return -Inf
asrs r0,r2,#1 // check LSB of exponent
bcc 1f
lsls r1,#1 // was odd: double mantissa; mantissa y now 1..4 Q23
1:
adds r2,#4 // correction for packing
adr r4,rsqrtapp-4// first four table entries are never accessed because of the mantissa's leading 1
lsrs r3,r1,#21 // y>>21 Q2
ldrb r4,[r4,r3] // initial approximation to reciprocal square root m Q8
lsrs r0,r1,#7 // y>>7 // Q16 first Newton-Raphson iteration
muls r0,r4 // m*y
muls r0,r4 // s=m*y*y // Q32
asrs r0,#12 // s>>12
muls r0,r4 // m*s // Q28
asrs r0,#13 // m*s // Q15
lsls r4,#8 // m // Q16
subs r4,r0 // m=(m<<8)-(s>>13) // Q16-Q15/2 -> Q16
mov r0,r4 // // second Newton-Raphson iteration
muls r0,r0 // u=m*m // Q32
lsrs r0,#16 // u>>16 // Q16
lsrs r3,r1,#7 // y>>7 // Q16
muls r0,r3 // s=u*(y>>7) // Q32
asrs r0,#12 // s>>12 // Q20
muls r0,r4 // s*m // Q36
asrs r0,#21 // s*m // Q15
subs r4,r0 // m=m-s // Q16-Q15/2
mov r0,r4 // // third Newton-Raphson iteration
muls r0,r0 // u=m*m // Q32
lsrs r3,r0,#12 // now multiply u and y in two parts: u>>12
muls r3,r1 // first partial product (u>>12)*y Q43
lsls r0,#20
lsrs r0,#20 // u&0xfff
lsrs r5,r1,#12 // y>>12
muls r0,r5 // second partial product (u&0xfff)*(y>>12) Q43
add r0,r3 // s=u*y // Q43
asrs r0,#15 // s>>15 // Q28
muls r0,r4 // (s>>15)*m // Q44
lsls r4,#13 // m<<13 // Q29
asrs r0,#16 // s>>16 // Q28
subs r0,r4,r0 // // Q29-Q28/2
asrs r2,#1 // halve exponent
bcc div0 // was y shifted?
lsrs r0,#1
lsls r1,#1 // shift y back
b div0 // round and complete with multiplication
.align 2
// round(2^15./[136:16:248])
rcpapp:
.byte 0xf1,0xd8,0xc3,0xb2, 0xa4,0x98,0x8d,0x84
// round(sqrt(2^22./[72:16:248]))
rsqrtapp:
.byte 0xf1,0xda,0xc9,0xbb, 0xb0,0xa6,0x9e,0x97, 0x91,0x8b,0x86,0x82
.endif
.if include_conversions
// convert float to signed int, rounding towards -Inf, clamping
.thumb_func
qfp_float2int:
movs r1,#0 // fall through
// convert float in r0 to signed fixed point in r0, clamping
.thumb_func
qfp_float2fix:
push {r4,r14}
bl unpackx
add r2,r1 // incorporate binary point position into exponent
subs r2,#23 // r2 is now amount of left shift required
blt 1f // requires right shift?
cmp r2,#7 // overflow?
ble 4f
3: // overflow
asrs r1,r0,#31 // +ve:0 -ve:0xffffffff
mvns r1,r1 // +ve:0xffffffff -ve:0
movs r0,#1
lsls r0,#31
5:
eors r0,r1 // +ve:0x7fffffff -ve:0x80000000 (unsigned path: 0xffffffff)
pop {r4,r15}
1:
rsbs r2,#0 // right shift for r0, >0
cmp r2,#32
blt 2f // more than 32 bits of right shift?
movs r2,#32
2:
asrs r0,r0,r2
pop {r4,r15}
// unsigned version
.thumb_func
qfp_float2uint:
movs r1,#0 // fall through
.thumb_func
qfp_float2ufix:
push {r4,r14}
bl unpackx
add r2,r1 // incorporate binary point position into exponent
movs r1,r0
bmi 5b // negative? return zero
subs r2,#23 // r2 is now amount of left shift required
blt 1b // requires right shift?
mvns r1,r0 // ready to return 0xffffffff
cmp r2,#8 // overflow?
bgt 5b
4:
lsls r0,r0,r2 // result fits, left shifted
pop {r4,r15}
// convert signed int to float, rounding
.thumb_func
qfp_int2float:
movs r1,#0 // fall through
// convert signed fix to float, rounding; number of r0 bits after point in r1
.thumb_func
qfp_fix2float:
push {r4,r5,r14}
1:
movs r2,#29
subs r2,r1 // fix exponent
packretns: // pack and return, sticky bits=0
movs r5,#0
b packret
// unsigned version
.thumb_func
qfp_uint2float:
movs r1,#0 // fall through
.thumb_func
qfp_ufix2float:
push {r4,r5,r14}
cmp r0,#0
bge 1b // treat <2^31 as signed
movs r2,#30
subs r2,r1 // fix exponent
lsls r5,r0,#31 // one sticky bit
lsrs r0,#1
b packret
.endif
.if include_scientific
// All the scientific functions are implemented using the CORDIC algorithm. For notation,
// details not explained in the comments below, and a good overall survey see
// "50 Years of CORDIC: Algorithms, Architectures, and Applications" by Meher et al.,
// IEEE Transactions on Circuits and Systems Part I, Volume 56 Issue 9.
// Register use:
// r0: x
// r1: y
// r2: z/omega
// r3: coefficient pointer
// r4,r12: m
// r5: i (shift)
cordic_start: // initialisation
movs r5,#0 // initial shift=0
mov r12,r4
b 5f
cordic_vstep: // one step of algorithm in vector mode
cmp r1,#0 // check sign of y
bgt 4f
b 1f
cordic_rstep: // one step of algorithm in rotation mode
cmp r2,#0 // check sign of angle
bge 1f
4:
subs r1,r6 // negative rotation: y=y-(x>>i)
rsbs r7,#0
adds r2,r4 // accumulate angle
b 2f
1:
adds r1,r6 // positive rotation: y=y+(x>>i)
subs r2,r4 // accumulate angle
2:
mov r4,r12
muls r7,r4 // apply sign from m
subs r0,r7 // finish rotation: x=x{+/-}(y>>i)
5:
ldmia r3!,{r4} // fetch next angle from table and bump pointer
lsrs r4,#1 // repeated angle?
bcs 3f
adds r5,#1 // adjust shift if not
3:
mov r6,r0
asrs r6,r5 // x>>i
mov r7,r1
asrs r7,r5 // y>>i
lsrs r4,#1 // shift end flag into carry
bx r14
// CORDIC rotation mode
cordic_rot:
push {r6,r7,r14}
bl cordic_start // initialise
1:
bl cordic_rstep
bcc 1b // step until table finished
asrs r6,r0,#14 // remaining small rotations can be linearised: see IV.B of paper referenced above
asrs r7,r1,#14
asrs r2,#3
muls r6,r2 // all remaining CORDIC steps in a multiplication
muls r7,r2
mov r4,r12
muls r7,r4
asrs r6,#12
asrs r7,#12
subs r0,r7 // x=x{+/-}(yz>>k)
adds r1,r6 // y=y+(xz>>k)
cordic_exit:
pop {r6,r7,r15}
// CORDIC vector mode
cordic_vec:
push {r6,r7,r14}
bl cordic_start // initialise
1:
bl cordic_vstep
bcc 1b // step until table finished
4:
cmp r1,#0 // continue as in cordic_vstep but without using table; x is not affected as y is small
bgt 2f // check sign of y
adds r1,r6 // positive rotation: y=y+(x>>i)
subs r2,r4 // accumulate angle
b 3f
2:
subs r1,r6 // negative rotation: y=y-(x>>i)
adds r2,r4 // accumulate angle
3:
asrs r6,#1
asrs r4,#1 // next "table entry"
bne 4b
b cordic_exit
.thumb_func
qfp_fsin: // calculate sin and cos using CORDIC rotation method
push {r4,r5,r14}
movs r1,#24
bl qfp_float2fix // range reduction by repeated subtraction/addition in fixed point
ldr r4,pi_q29
lsrs r4,#4 // 2pi Q24
1:
subs r0,r4
bge 1b
1:
adds r0,r4
bmi 1b // now in range 0..2pi
lsls r2,r0,#2 // z Q26
lsls r5,r4,#1 // pi Q26 (r4=pi/2 Q26)
ldr r0,=#0x136e9db4 // initialise CORDIC x,y with scaling
movs r1,#0
1:
cmp r2,r4 // >pi/2?
blt 2f
subs r2,r5 // reduce range to -pi/2..pi/2
rsbs r0,#0 // rotate vector by pi
b 1b
2:
lsls r2,#3 // Q29
adr r3,tab_cc // circular coefficients
movs r4,#1 // m=1
bl cordic_rot
adds r1,#9 // fiddle factor to make sin(0)==0
movs r2,#0 // exponents to zero
movs r3,#0
movs r5,#0 // no sticky bits
bl packx // pack cosine
bl xchxy
b packretns // pack sine
.thumb_func
qfp_fcos:
push {r14}
bl qfp_fsin
mov r0,r1 // extract cosine result
pop {r15}
.thumb_func
qfp_ftan:
push {r4,r5,r14}
bl qfp_fsin // sine in r0/r2, cosine in r1/r3
.if include_faster
b fdiv_fast_n // sin/cos
.else
b fdiv_n
.endif
.thumb_func
qfp_fexp: // calculate cosh and sinh using rotation method; add to obtain exp
push {r4,r5,r14}
movs r1,#24
bl qfp_float2fix // Q24: covers entire valid input range
asrs r1,r0,#16 // Q8
ldr r2,=#5909 // log_2(e) Q12
muls r1,r2 // estimate exponent of result Q20
asrs r1,#19 // Q1
adds r1,#1 // rounding
asrs r1,#1 // rounded estimate of exponent of result
push {r1} // save for later
lsls r2,r0,#5 // Q29
ldr r0,=#0x162e42ff // ln(2) Q29
muls r1,r0 // accurate contribution of estimated exponent
subs r2,r1 // residual to be exponentiated, approximately -.5..+.5 Q29
ldr r0,=#0x2c9e15ca // initialise CORDIC x,y with scaling
movs r1,#0
adr r3,tab_ch // hyperbolic coefficients
mvns r4,r1 // m=-1
bl cordic_rot // calculate cosh and sinh
add r0,r1 // exp=cosh+sinh
pop {r2} // recover exponent
b packretns // pack result
.thumb_func
qfp_fsqrt: // calculate sqrt and ln using vector method
push {r4,r5,r14}
bl unpackx
movs r1,r0 // -ve argument?
bmi 3f // return -Inf, -Inf
ldr r1,=#0x0593C2B9 // scale factor for CORDIC
bl mul0 // Q29
asrs r1,r2,#1 // halve exponent
bcc 1f
adds r1,#1 // was odd: add 1 and shift mantissa
asrs r0,#1
1:
push {r1} // save exponent/2 for later
mov r1,r0
ldr r3,=#0x0593C2B9 // re-use constant
lsls r3,#2
adds r0,r3 // "a+1"
subs r1,r3 // "a-1"
movs r2,#0
adr r3,tab_ch // hyperbolic coefficients
mvns r4,r2 // m=-1
bl cordic_vec
mov r1,r2 // keep ln result
pop {r2} // retrieve exponent/2
2:
movs r3,r2
b packretns // pack sqrt result
3:
movs r2,#255
b 2b
.thumb_func
qfp_fln:
push {r4,r5,r14}
bl qfp_fsqrt // get unpacked ln in r1/r3; exponent has been halved
cmp r3,#70 // ln(Inf)?
bgt 3b // return Inf
rsbs r3,#0
cmp r3,#70
bgt 1f // ln(0)? return -Inf
ldr r0,=#0x0162e430 // ln(4) Q24
muls r0,r3 // contribution from negated, halved exponent
adds r1,#8 // round result of ln
asrs r1,#4 // Q24
subs r0,r1,r0 // add in contribution from (negated) exponent
movs r2,#5 // pack expects Q29
b packretns
1:
mvns r0,r0 // make result -Inf
b 3b
.thumb_func
qfp_fatan2:
push {r4,r5,r14}
bl unpackxyalign // convert to fixed point (ensure common exponent, which is discarded)
asrs r0,#1
asrs r1,#1
movs r2,#0 // initial angle
cmp r0,#0 // x negative
bge 5f
rsbs r0,#0 // rotate to 1st/4th quadrants
rsbs r1,#0
ldr r2,pi_q29 // pi Q29
5:
adr r3,tab_cc // circular coefficients
movs r4,#1 // m=1
bl cordic_vec // also produces magnitude (with scaling factor 1.646760119), which is discarded
mov r0,r2 // result here is -pi/2..3pi/2 Q29
ldr r2,pi_q29 // pi Q29
adds r4,r0,r2 // attempt to fix -3pi/2..-pi case
bcs 6f // -pi/2..0? leave result as is
subs r4,r0,r2 // <pi? leave as is
bmi 6f
subs r0,r4,r2 // >pi: take off 2pi
6:
subs r0,#1 // fiddle factor so atan2(0,1)==0
movs r2,#0 // exponent for pack
b packretns
.align 2
.ltorg
// first entry in following table is pi Q29
pi_q29:
// circular CORDIC coefficients: atan(2^-i), b0=flag for preventing shift, b1=flag for end of table
tab_cc:
.word 0x1921fb54*4+1 // no shift before first iteration
.word 0x0ed63383*4+0
.word 0x07d6dd7e*4+0
.word 0x03fab753*4+0
.word 0x01ff55bb*4+0
.word 0x00ffeaae*4+0
.word 0x007ffd55*4+0
.word 0x003fffab*4+0
.word 0x001ffff5*4+0
.word 0x000fffff*4+0
.word 0x0007ffff*4+0
.word 0x00040000*4+0
.word 0x00020000*4+0+2 // +2 marks end
// hyperbolic CORDIC coefficients: atanh(2^-i), flags as above
tab_ch:
.word 0x1193ea7b*4+0
.word 0x1193ea7b*4+1 // repeat i=1
.word 0x082c577d*4+0
.word 0x04056247*4+0
.word 0x0200ab11*4+0
.word 0x0200ab11*4+1 // repeat i=4
.word 0x01001559*4+0
.word 0x008002ab*4+0
.word 0x00400055*4+0
.word 0x0020000b*4+0
.word 0x00100001*4+0
.word 0x00080001*4+0
.word 0x00040000*4+0
.word 0x00020000*4+0
.word 0x00020000*4+1+2 // repeat i=12
.endif
qfp_lib_end:
|