1/* 2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7#if PICO_RP2040 8#include "pico/asm_helper.S" 9#include "pico/runtime_init.h" 10#include "pico/bootrom/sf_table.h" 11#include "hardware/divider_helper.S" 12 13pico_default_asm_setup 14 15PICO_RUNTIME_INIT_FUNC_RUNTIME(__aeabi_double_init, PICO_RUNTIME_INIT_AEABI_DOUBLE) 16 17.macro double_section name 18#if PICO_DOUBLE_IN_RAM 19.section RAM_SECTION_NAME(\name), "ax" 20#else 21.section SECTION_NAME(\name), "ax" 22#endif 23.endm 24 25.macro _double_wrapper_func x 26 wrapper_func \x 27.endm 28 29.macro wrapper_func_d1 x 30 _double_wrapper_func \x 31#if PICO_DOUBLE_PROPAGATE_NANS 32 mov ip, lr 33 bl __check_nan_d1 34 mov lr, ip 35#endif 36.endm 37 38.macro wrapper_func_d2 x 39 _double_wrapper_func \x 40#if PICO_DOUBLE_PROPAGATE_NANS 41 mov ip, lr 42 bl __check_nan_d2 43 mov lr, ip 44#endif 45.endm 46 47.section .text 48 49#if PICO_DOUBLE_PROPAGATE_NANS 50.thumb_func 51__check_nan_d1: 52 movs r3, #1 53 lsls r3, #21 54 lsls r2, r1, #1 55 adds r2, r3 56 bhi 1f 57 bx lr 581: 59 bx ip 60 61.thumb_func 62__check_nan_d2: 63 push {r0, r2} 64 movs r2, #1 65 lsls r2, #21 66 lsls r0, r1, #1 67 adds r0, r2 68 bhi 1f 69 lsls r0, r3, #1 70 adds r0, r2 71 bhi 2f 72 pop {r0, r2} 73 bx lr 742: 75 pop {r0, r2} 76 mov r0, r2 77 mov r1, r3 78 bx ip 791: 80 pop {r0, r2} 81 bx ip 82#endif 83 84.macro table_tail_call SF_TABLE_OFFSET 85 push {r3, r4} 86#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED 87#ifndef NDEBUG 88 movs r3, #0 89 mov ip, r3 90#endif 91#endif 92 ldr r3, =sd_table 93 ldr r3, [r3, #\SF_TABLE_OFFSET] 94 str r3, [sp, #4] 95 pop {r3, pc} 96.endm 97 98.macro shimmable_table_tail_call SF_TABLE_OFFSET shim 99 push {r3, r4} 100 ldr r3, =sd_table 101 ldr r3, [r3, #\SF_TABLE_OFFSET] 102#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED 103 mov ip, pc 104#endif 105 str r3, [sp, #4] 106 pop {r3, pc} 107#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED 108.byte \SF_TABLE_OFFSET, 0xdf 109.word \shim 110#endif 111.endm 112 113.macro double_wrapper_section func 114double_section WRAPPER_FUNC_NAME(\func) 115.endm 116 117double_section push_r8_r11 118regular_func push_r8_r11 119 mov r4,r8 120 mov r5,r9 121 mov r6,r10 122 mov r7,r11 123 push {r4-r7} 124 bx r14 125 126double_section pop_r8_r11 127regular_func pop_r8_r11 128 pop {r4-r7} 129 mov r8,r4 130 mov r9,r5 131 mov r10,r6 132 mov r11,r7 133 bx r14 134 135// note generally each function is in a separate section unless there is fall thru or branching between them 136// note fadd, fsub, fmul, fdiv are so tiny and just defer to rom so are lumped together so they can share constant pool 137 138// note functions are word aligned except where they are an odd number of linear instructions 139 140// double FUNC_NAME(__aeabi_dadd)(double, double) double-precision addition 141double_wrapper_section __aeabi_darithmetic 142// double FUNC_NAME(__aeabi_drsub)(double x, double y) double-precision reverse subtraction, y - x 143 144// frsub first because it is the only one that needs alignment 145.align 2 146wrapper_func __aeabi_drsub 147 eors r0, r1 148 eors r1, r0 149 eors r0, r1 150 // fall thru 151 152// double FUNC_NAME(__aeabi_dsub)(double x, double y) double-precision subtraction, x - y 153wrapper_func_d2 __aeabi_dsub 154#if PICO_DOUBLE_PROPAGATE_NANS 155 // we want to return nan for inf-inf or -inf - -inf, but without too much upfront cost 156 mov ip, r0 157 mov r0, r1 158 eors r0, r3 159 bmi 1f // different signs 160 mov r0, ip 161 push {r0-r3, lr} 162 bl 2f 163 b ddiv_dsub_nan_helper 1641: 165 mov r0, ip 1662: 167#endif 168 shimmable_table_tail_call SF_TABLE_FSUB dsub_shim 169 170wrapper_func_d2 __aeabi_dadd 171 shimmable_table_tail_call SF_TABLE_FADD dadd_shim 172 173// double FUNC_NAME(__aeabi_ddiv)(double n, double d) double-precision division, n / d 174wrapper_func_d2 __aeabi_ddiv 175#if PICO_DOUBLE_PROPAGATE_NANS 176 push {r0-r3, lr} 177 bl 1f 178 b ddiv_dsub_nan_helper 1791: 180#endif 181#if !PICO_DIVIDER_DISABLE_INTERRUPTS 182 // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty 183 mov ip, r2 184 ldr r2, =(SIO_BASE) 185 ldr r2, [r2, #SIO_DIV_CSR_OFFSET] 186 lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY 187 bcs ddiv_save_state 188 mov r2, ip 189#else 190 // to avoid worrying about IRQs (or context switches), simply disable interrupts around call 191 push {r4, lr} 192 mrs r4, PRIMASK 193 cpsid i 194 bl ddiv_shim_call 195 msr PRIMASK, r4 196 pop {r4, pc} 197#endif 198ddiv_shim_call: 199 shimmable_table_tail_call SF_TABLE_FDIV ddiv_shim 200 201#if !PICO_DIVIDER_DISABLE_INTERRUPTS 202ddiv_save_state: 203 ldr r2, =(SIO_BASE) 204 save_div_state_and_lr 205 mov r2, ip 206 bl ddiv_shim_call 207 ldr r2, =(SIO_BASE) 208 restore_div_state_and_return 209#endif 210 211ddiv_dsub_nan_helper: 212#if PICO_DOUBLE_PROPAGATE_NANS 213 // check for infinite op infinite (or rather check for infinite result with both 214 // operands being infinite) 215 lsls r2, r1, #1 216 asrs r2, r2, #21 217 adds r2, #1 218 beq 2f 219 add sp, #16 220 pop {pc} 2212: 222 ldr r2, [sp, #4] 223 ldr r3, [sp, #12] 224 lsls r2, #1 225 asrs r2, r2, #21 226 lsls r3, #1 227 asrs r3, r3, #24 228 ands r2, r3 229 adds r2, #1 230 bne 3f 231 // infinite to nan 232 movs r2, #1 233 lsls r2, #19 234 orrs r1, r2 2353: 236 add sp, #16 237 pop {pc} 238#endif 239 240// double FUNC_NAME(__aeabi_dmul)(double, double) double-precision multiplication 241wrapper_func_d2 __aeabi_dmul 242#if PICO_DOUBLE_PROPAGATE_NANS 243 push {r0-r3, lr} 244 bl 1f 245 246 // check for multiplication of infinite by zero (or rather check for infinite result with either 247 // operand 0) 248 lsls r3, r1, #1 249 asrs r3, r3, #21 250 adds r3, #1 251 beq 2f 252 add sp, #16 253 pop {pc} 2542: 255 ldr r2, [sp, #4] 256 ldr r3, [sp, #12] 257 ands r2, r3 258 bne 3f 259 // infinite to nan 260 movs r2, #1 261 lsls r2, #19 262 orrs r1, r2 2633: 264 add sp, #16 265 pop {pc} 2661: 267#endif 268 shimmable_table_tail_call SF_TABLE_FMUL dmul_shim 269 270// void FUNC_NAME(__aeabi_cdrcmple)(double, double) reversed 3-way (<, =, ?>) compare [1], result in PSR ZC flags 271double_wrapper_section __aeabi_cdcmple 272 273wrapper_func __aeabi_cdrcmple 274 push {r0-r7,r14} 275 eors r0, r2 276 eors r2, r0 277 eors r0, r2 278 eors r1, r3 279 eors r3, r1 280 eors r1, r3 281 b __aeabi_dfcmple_guts 282 283// NOTE these share an implementation as we have no excepting NaNs. 284// void FUNC_NAME(__aeabi_cdcmple)(double, double) 3-way (<, =, ?>) compare [1], result in PSR ZC flags 285// void FUNC_NAME(__aeabi_cdcmpeq)(double, double) non-excepting equality comparison [1], result in PSR ZC flags 286@ compare r0:r1 against r2:r3, returning -1/0/1 for <, =, > 287@ also set flags accordingly 288.align 2 289wrapper_func __aeabi_cdcmple 290wrapper_func __aeabi_cdcmpeq 291 push {r0-r7,r14} 292__aeabi_dfcmple_guts: 293 ldr r7,=0x7ff @ flush NaNs and denormals 294 lsls r4,r1,#1 295 lsrs r4,#21 296 beq 1f 297 cmp r4,r7 298 bne 2f 299 lsls r4, r1, #12 300 bhi 7f 3011: 302 movs r0,#0 303 lsrs r1,#20 304 lsls r1,#20 3052: 306 lsls r4,r3,#1 307 lsrs r4,#21 308 beq 1f 309 cmp r4,r7 310 bne 2f 311 lsls r4, r3, #12 312 bhi 7f 3131: 314 movs r2,#0 315 lsrs r3,#20 316 lsls r3,#20 3172: 318 movs r6,#1 319 eors r3,r1 320 bmi 4f @ opposite signs? then can proceed on basis of sign of x 321 eors r3,r1 @ restore r3 322 bpl 2f 323 cmp r3,r1 324 bne 7f 3251: 326 cmp r2,r0 3277: 328 pop {r0-r7,r15} 3292: 330 cmp r1,r3 331 bne 7b 3321: 333 cmp r0,r2 334 pop {r0-r7,r15} 3354: 336 orrs r3,r1 @ make -0==+0 337 adds r3,r3 338 orrs r3,r0 339 orrs r3,r2 340 beq 7b 341 mvns r1, r1 @ carry inverse of r1 sign 342 adds r1, r1 343 pop {r0-r7,r15} 344 345 346// int FUNC_NAME(__aeabi_dcmpeq)(double, double) result (1, 0) denotes (=, ?<>) [2], use for C == and != 347double_wrapper_section __aeabi_dcmpeq 348.align 2 349wrapper_func __aeabi_dcmpeq 350 push {lr} 351 bl __aeabi_cdcmpeq 352 beq 1f 353 movs r0, #0 354 pop {pc} 3551: 356 movs r0, #1 357 pop {pc} 358 359// int FUNC_NAME(__aeabi_dcmplt)(double, double) result (1, 0) denotes (<, ?>=) [2], use for C < 360double_wrapper_section __aeabi_dcmplt 361.align 2 362wrapper_func __aeabi_dcmplt 363 push {lr} 364 bl __aeabi_cdcmple 365 sbcs r0, r0 366 pop {pc} 367 368// int FUNC_NAME(__aeabi_dcmple)(double, double) result (1, 0) denotes (<=, ?>) [2], use for C <= 369double_wrapper_section __aeabi_dcmple 370.align 2 371wrapper_func __aeabi_dcmple 372 push {lr} 373 bl __aeabi_cdcmple 374 bls 1f 375 movs r0, #0 376 pop {pc} 3771: 378 movs r0, #1 379 pop {pc} 380 381// int FUNC_NAME(__aeabi_dcmpge)(double, double) result (1, 0) denotes (>=, ?<) [2], use for C >= 382double_wrapper_section __aeabi_dcmpge 383.align 2 384wrapper_func __aeabi_dcmpge 385 push {lr} 386 // because of NaNs it is better to reverse the args than the result 387 bl __aeabi_cdrcmple 388 bls 1f 389 movs r0, #0 390 pop {pc} 3911: 392 movs r0, #1 393 pop {pc} 394 395// int FUNC_NAME(__aeabi_dcmpgt)(double, double) result (1, 0) denotes (>, ?<=) [2], use for C > 396double_wrapper_section __aeabi_dcmpgt 397wrapper_func __aeabi_dcmpgt 398 push {lr} 399 // because of NaNs it is better to reverse the args than the result 400 bl __aeabi_cdrcmple 401 sbcs r0, r0 402 pop {pc} 403 404// int FUNC_NAME(__aeabi_dcmpun)(double, double) result (1, 0) denotes (?, <=>) [2], use for C99 isunordered() 405double_wrapper_section __aeabi_dcmpun 406wrapper_func __aeabi_dcmpun 407 movs r0, #1 408 lsls r0, #21 409 lsls r2, r1, #1 410 adds r2, r0 411 bhi 1f 412 lsls r2, r3, #1 413 adds r2, r0 414 bhi 1f 415 movs r0, #0 416 bx lr 4171: 418 movs r0, #1 419 bx lr 420 421// double FUNC_NAME(__aeabi_ui2d)(unsigned) unsigned to double (double precision) conversion 422double_wrapper_section __aeabi_ui2d 423 shimmable_table_tail_call SF_TABLE_UINT2FLOAT uint2double_shim 424 425double_wrapper_section __aeabi_i2d 426 427wrapper_func __aeabi_ui2d 428 movs r1, #0 429 cmp r0, #0 430 bne 2f 4311: 432 bx lr 433// double FUNC_NAME(__aeabi_i2d)(int) integer to double (double precision) conversion 434wrapper_func __aeabi_i2d 435 asrs r1, r0, #31 436 eors r0, r1 437 subs r0, r1 438 beq 1b 439 lsls r1, #31 4402: 441 push {r0, r1, r4, lr} 442 ldr r3, =sf_clz_func 443 ldr r3, [r3] 444 blx r3 445 pop {r2, r3} 446 adds r4, r0, #1 447 lsls r2, r4 448 lsls r0, r2, #20 449 lsrs r2, #12 450 ldr r1,=1055 451 subs r1, r4 452 lsls r1, #20 453 orrs r1, r3 454 orrs r1, r2 455 pop {r4, pc} 456 457// int FUNC_NAME(__aeabi_d2iz)(double) double (double precision) to integer C-style conversion [3] 458double_wrapper_section __aeabi_d2iz 459wrapper_func __aeabi_d2iz 460regular_func double2int_z 461 push {r4, lr} 462 lsls r4, r1, #1 463 lsrs r2, r4, #21 464 movs r3, #0x80 465 adds r2, r3 466 lsls r3, #3 467 subs r2, r3 468 lsls r3, #21 469 cmp r2, #126 470 ble 1f 471 subs r2, #158 472 bge 2f 473 asrs r4, r1, #31 474 lsls r1, #12 475 lsrs r1, #1 476 orrs r1, r3 477 negs r2, r2 478 lsrs r1, r2 479 lsls r4, #1 480 adds r4, #1 481 adds r2, #21 482 cmp r2, #32 483 bge 3f 484 lsrs r0, r2 485 orrs r0, r1 486 muls r0, r4 487 pop {r4, pc} 4881: 489 movs r0, #0 490 pop {r4, pc} 4913: 492 mov r0, r1 493 muls r0, r4 494 pop {r4, pc} 4952: 496 // overflow 497 lsrs r0, r1, #31 498 adds r0, r3 499 subs r0, #1 500 pop {r4, pc} 501 502double_section double2int 503regular_func double2int 504 shimmable_table_tail_call SF_TABLE_FLOAT2INT double2int_shim 505 506// unsigned FUNC_NAME(__aeabi_d2uiz)(double) double (double precision) to unsigned C-style conversion [3] 507double_wrapper_section __aeabi_d2uiz 508wrapper_func __aeabi_d2uiz 509regular_func double2uint 510 shimmable_table_tail_call SF_TABLE_FLOAT2UINT double2uint_shim 511 512double_section fix2double 513regular_func fix2double 514 shimmable_table_tail_call SF_TABLE_FIX2FLOAT fix2double_shim 515 516double_section ufix2double 517regular_func ufix2double 518 shimmable_table_tail_call SF_TABLE_UFIX2FLOAT ufix2double_shim 519 520double_section fix642double 521regular_func fix642double 522 shimmable_table_tail_call SF_TABLE_FIX642FLOAT fix642double_shim 523 524double_section ufix2double 525regular_func ufix642double 526 shimmable_table_tail_call SF_TABLE_UFIX642FLOAT ufix642double_shim 527 528// double FUNC_NAME(__aeabi_l2d)(long long) long long to double (double precision) conversion 529double_wrapper_section __aeabi_l2d 530wrapper_func __aeabi_l2d 531 shimmable_table_tail_call SF_TABLE_INT642FLOAT int642double_shim 532 533// double FUNC_NAME(__aeabi_l2f)(long long) long long to double (double precision) conversion 534double_wrapper_section __aeabi_ul2d 535wrapper_func __aeabi_ul2d 536 shimmable_table_tail_call SF_TABLE_UINT642FLOAT uint642double_shim 537 538// long long FUNC_NAME(__aeabi_d2lz)(double) double (double precision) to long long C-style conversion [3] 539double_wrapper_section __aeabi_d2lz 540wrapper_func __aeabi_d2lz 541regular_func double2int64_z 542 cmn r1, r1 543 bcc double2int64 544 push {lr} 545 lsls r1, #1 546 lsrs r1, #1 547 movs r2, #0 548 bl double2ufix64 549 cmp r1, #0 550 bmi 1f 551 movs r2, #0 552 negs r0, r0 553 sbcs r2, r1 554 mov r1, r2 555 pop {pc} 5561: 557 movs r1, #128 558 lsls r1, #24 559 movs r0, #0 560 pop {pc} 561 562double_section double2int64 563regular_func double2int64 564 shimmable_table_tail_call SF_TABLE_FLOAT2INT64 double2int64_shim 565 566// unsigned long long FUNC_NAME(__aeabi_d2ulz)(double) double to unsigned long long C-style conversion [3] 567double_wrapper_section __aeabi_d2ulz 568wrapper_func __aeabi_d2ulz 569 shimmable_table_tail_call SF_TABLE_FLOAT2UINT64 double2uint64_shim 570 571double_section double2fix64 572regular_func double2fix64 573 shimmable_table_tail_call SF_TABLE_FLOAT2FIX64 double2fix64_shim 574 575double_section double2ufix64 576regular_func double2ufix64 577 shimmable_table_tail_call SF_TABLE_FLOAT2UFIX64 double2ufix64_shim 578 579double_section double2fix 580regular_func double2fix 581 shimmable_table_tail_call SF_TABLE_FLOAT2FIX double2fix_shim 582 583double_section double2ufix 584regular_func double2ufix 585 shimmable_table_tail_call SF_TABLE_FLOAT2UFIX double2ufix_shim 586 587double_wrapper_section __aeabi_d2f 5881: 589#if PICO_DOUBLE_PROPAGATE_NANS 590 // copy sign bit and 23 NAN id bits into sign bit and significant id bits, also set high id bit 591 592 lsrs r0, #30 593 lsls r2, r1, #12 594 lsrs r2, #9 595 asrs r1, #22 596 lsls r1, #22 597 orrs r0, r1 598 orrs r0, r2 599 bx lr 600#endif 601wrapper_func __aeabi_d2f 602#if PICO_DOUBLE_PROPAGATE_NANS 603 movs r3, #1 604 lsls r3, #21 605 lsls r2, r1, #1 606 adds r2, r3 607 bhi 1b 608#endif 609 // note double->float in double table at same index as float->double in double table 610 shimmable_table_tail_call SF_TABLE_FLOAT2DOUBLE double2float_shim 611 612double_wrapper_section srqt 613wrapper_func_d1 sqrt 614 shimmable_table_tail_call SF_TABLE_FSQRT dsqrt_shim 615 616double_wrapper_section sincostan_remainder 617regular_func sincostan_remainder 618 ldr r2, =0x54442D18 // 2 * M_PI 619 ldr r3, =0x401921FB 620 push {lr} 621 // note remainder only uses the divider thru integer divider functions 622 // which save and restore themselves 623 bl remainder 624 pop {pc} 625 626double_wrapper_section cos 627#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024 628wrapper_func cos 629 // rom version only works for -1024 < angle < 1024 630 lsls r2, r1, #2 631 bcc 1f 632 lsrs r2, #22 633 cmp r2, #9 634 bge 2f 6351: 636 shimmable_table_tail_call SF_TABLE_FCOS dcos_shim 6372: 638#if PICO_DOUBLE_PROPAGATE_NANS 639 lsls r2, r1, #1 640 asrs r2, #21 641 adds r2, #1 642 bne 3f 643 // infinite to nan 644 movs r2, #1 645 lsls r2, #19 646 orrs r1, r2 647 bx lr 6483: 649#endif 650 push {lr} 651 bl sincostan_remainder 652 pop {r2} 653 mov lr, r2 654 b 1b 655 656double_wrapper_section sin 657#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024 658wrapper_func sin 659 // rom version only works for -1024 < angle < 1024 660 lsls r2, r1, #2 661 bcc 1f 662 lsrs r2, #22 663 cmp r2, #9 664 bge 2f 6651: 666 shimmable_table_tail_call SF_TABLE_FSIN dsin_shim 6672: 668#if PICO_DOUBLE_PROPAGATE_NANS 669 lsls r2, r1, #1 670 asrs r2, #21 671 adds r2, #1 672 bne 3f 673 // infinite to nan 674 movs r2, #1 675 lsls r2, #19 676 orrs r1, r2 677 bx lr 6783: 679#endif 680 push {lr} 681 bl sincostan_remainder 682 pop {r2} 683 mov lr, r2 684 b 1b 685 686double_wrapper_section sincos 687 // out of line remainder code for abs(angle)>=1024 6882: 689#if PICO_DOUBLE_PROPAGATE_NANS 690 lsls r2, r1, #1 691 asrs r2, #21 692 adds r2, #1 693 bne 3f 694 // infinite to nan 695 movs r2, #1 696 lsls r2, #19 697 orrs r1, r2 698 pop {r4-r5} 699 stmia r4!, {r0, r1} 700 stmia r5!, {r0, r1} 701 pop {r4, r5, pc} 7023: 703#endif 704 push {lr} 705 bl sincostan_remainder 706 pop {r2} 707 mov lr, r2 708 b 1f // continue with sincos 709 710wrapper_func sincos 711 push {r2-r5, lr} 712 // rom version only works for -1024 < angle < 1024 713 lsls r2, r1, #2 714 bcc 1f 715 lsrs r2, #22 716 cmp r2, #9 717 bge 2b 7181: 719 bl 2f // call the shim 720 pop {r4-r5} 721 stmia r4!, {r0, r1} 722 stmia r5!, {r2, r3} 723 pop {r4, r5, pc} 724 7252: 726 shimmable_table_tail_call SF_TABLE_V3_FSINCOS sincos_shim_bootstrap 727 728.thumb_func 729sincos_shim_bootstrap: 730 push {r2, r3, r4} 731 movs r3, #0x13 732 ldrb r3, [r3] 733#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED 734 cmp r3, #1 735 bne 1f 736 ldr r3, =dsincos_shim 737 b 2f 738#endif 7391: 740 ldr r3, =dsincos_shim_v2 7412: 742 ldr r2, =sd_table 743 str r3, [r2, #SF_TABLE_V3_FSINCOS] 744 str r3, [sp, #8] 745 pop {r2, r3, pc} 746.thumb_func 747dsincos_shim_v2: 748 push {r4-r7,r14} 749 bl push_r8_r11 750 bl v2_rom_dsincos_internal 751 mov r12,r0 @ save ε 752 bl v2_rom_dcos_finish 753 push {r0,r1} 754 mov r0,r12 755 bl v2_rom_dsin_finish 756 pop {r2,r3} 757 bl pop_r8_r11 758 pop {r4-r7,r15} 759.thumb_func 760v2_rom_dsincos_internal: 761 push {r0, lr} 762 ldr r0, =0x3855 763 str r0, [sp, #4] 764 pop {r0, pc} 765.thumb_func 766v2_rom_dcos_finish: 767 push {r0, r1} 768 ldr r0, =0x389d 769 str r0, [sp, #4] 770 pop {r0, pc} 771.thumb_func 772v2_rom_dsin_finish: 773 push {r0, r1} 774 ldr r0, =0x38d9 775 str r0, [sp, #4] 776 pop {r0, pc} 777 778double_wrapper_section tan 779#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024 780wrapper_func tan 781 // rom version only works for -1024 < angle < 1024 782 lsls r2, r1, #2 783 bcc dtan_in_range 784 lsrs r2, #22 785 cmp r2, #9 786 bge dtan_angle_out_of_range 787dtan_in_range: 788#if !PICO_DIVIDER_DISABLE_INTERRUPTS 789 // to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty 790 mov ip, r2 791 ldr r2, =(SIO_BASE) 792 ldr r2, [r2, #SIO_DIV_CSR_OFFSET] 793 lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY 794 bcs dtan_save_state 795 mov r2, ip 796#else 797 // to avoid worrying about IRQs (or context switches), simply disable interrupts around call 798 push {r4, lr} 799 mrs r4, PRIMASK 800 cpsid i 801 bl dtan_shim_call 802 msr PRIMASK, r4 803 pop {r4, pc} 804#endif 805dtan_shim_call: 806 shimmable_table_tail_call SF_TABLE_FTAN dtan_shim 807#if !PICO_DIVIDER_DISABLE_INTERRUPTS 808dtan_save_state: 809 ldr r2, =(SIO_BASE) 810 save_div_state_and_lr 811 mov r2, ip 812 bl dtan_shim_call 813 ldr r2, =(SIO_BASE) 814 restore_div_state_and_return 815#endif 816dtan_angle_out_of_range: 817#if PICO_DOUBLE_PROPAGATE_NANS 818 lsls r2, r1, #1 819 asrs r2, #21 820 adds r2, #1 821 bne 3f 822 // infinite to nan 823 movs r2, #1 824 lsls r2, #19 825 orrs r1, r2 826 bx lr 8273: 828#endif 829 push {lr} 830 bl sincostan_remainder 831 pop {r2} 832 mov lr, r2 833 b dtan_in_range 834 835double_wrapper_section atan2 836wrapper_func_d2 atan2 837 shimmable_table_tail_call SF_TABLE_FATAN2 datan2_shim 838 839double_wrapper_section exp 840wrapper_func_d1 exp 841 shimmable_table_tail_call SF_TABLE_FEXP dexp_shim 842 843double_wrapper_section log 844wrapper_func_d1 log 845 shimmable_table_tail_call SF_TABLE_FLN dln_shim 846 847#endif
