blob: 425385e13a25f84591542b75962868bbe701c49c [file] [log] [blame]
/*
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/asm_helper.S"
#include "pico/bootrom/sf_table.h"
#include "hardware/divider_helper.S"
pico_default_asm_setup
__pre_init __aeabi_double_init, 00020
.macro double_section name
#if PICO_DOUBLE_IN_RAM
.section RAM_SECTION_NAME(\name), "ax"
#else
.section SECTION_NAME(\name), "ax"
#endif
.endm
.macro _double_wrapper_func x
wrapper_func \x
.endm
.macro wrapper_func_d1 x
_double_wrapper_func \x
#if PICO_DOUBLE_PROPAGATE_NANS
mov ip, lr
bl __check_nan_d1
mov lr, ip
#endif
.endm
.macro wrapper_func_d2 x
_double_wrapper_func \x
#if PICO_DOUBLE_PROPAGATE_NANS
mov ip, lr
bl __check_nan_d2
mov lr, ip
#endif
.endm
.section .text
#if PICO_DOUBLE_PROPAGATE_NANS
.thumb_func
__check_nan_d1:
movs r3, #1
lsls r3, #21
lsls r2, r1, #1
adds r2, r3
bhi 1f
bx lr
1:
bx ip
.thumb_func
__check_nan_d2:
push {r0, r2}
movs r2, #1
lsls r2, #21
lsls r0, r1, #1
adds r0, r2
bhi 1f
lsls r0, r3, #1
adds r0, r2
bhi 2f
pop {r0, r2}
bx lr
2:
pop {r0, r2}
mov r0, r2
mov r1, r3
bx ip
1:
pop {r0, r2}
bx ip
#endif
.macro table_tail_call SF_TABLE_OFFSET
push {r3, r4}
#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
#ifndef NDEBUG
movs r3, #0
mov ip, r3
#endif
#endif
ldr r3, =sd_table
ldr r3, [r3, #\SF_TABLE_OFFSET]
str r3, [sp, #4]
pop {r3, pc}
.endm
.macro shimmable_table_tail_call SF_TABLE_OFFSET shim
push {r3, r4}
ldr r3, =sd_table
ldr r3, [r3, #\SF_TABLE_OFFSET]
#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
mov ip, pc
#endif
str r3, [sp, #4]
pop {r3, pc}
#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
.byte \SF_TABLE_OFFSET, 0xdf
.word \shim
#endif
.endm
.macro double_wrapper_section func
double_section WRAPPER_FUNC_NAME(\func)
.endm
double_section push_r8_r11
regular_func push_r8_r11
mov r4,r8
mov r5,r9
mov r6,r10
mov r7,r11
push {r4-r7}
bx r14
double_section pop_r8_r11
regular_func pop_r8_r11
pop {r4-r7}
mov r8,r4
mov r9,r5
mov r10,r6
mov r11,r7
bx r14
// note generally each function is in a separate section unless there is fall thru or branching between them
// note fadd, fsub, fmul, fdiv are so tiny and just defer to rom so are lumped together so they can share constant pool
// note functions are word aligned except where they are an odd number of linear instructions
// double FUNC_NAME(__aeabi_dadd)(double, double) double-precision addition
double_wrapper_section __aeabi_darithmetic
// double FUNC_NAME(__aeabi_drsub)(double x, double y) double-precision reverse subtraction, y - x
// frsub first because it is the only one that needs alignment
.align 2
wrapper_func __aeabi_drsub
eors r0, r1
eors r1, r0
eors r0, r1
// fall thru
// double FUNC_NAME(__aeabi_dsub)(double x, double y) double-precision subtraction, x - y
wrapper_func_d2 __aeabi_dsub
#if PICO_DOUBLE_PROPAGATE_NANS
// we want to return nan for inf-inf or -inf - -inf, but without too much upfront cost
mov ip, r0
mov r0, r1
eors r0, r3
bmi 1f // different signs
mov r0, ip
push {r0-r3, lr}
bl 2f
b ddiv_dsub_nan_helper
1:
mov r0, ip
2:
#endif
shimmable_table_tail_call SF_TABLE_FSUB dsub_shim
wrapper_func_d2 __aeabi_dadd
shimmable_table_tail_call SF_TABLE_FADD dadd_shim
// double FUNC_NAME(__aeabi_ddiv)(double n, double d) double-precision division, n / d
wrapper_func_d2 __aeabi_ddiv
#if PICO_DOUBLE_PROPAGATE_NANS
push {r0-r3, lr}
bl 1f
b ddiv_dsub_nan_helper
1:
#endif
#if !PICO_DIVIDER_DISABLE_INTERRUPTS
// to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
mov ip, r2
ldr r2, =(SIO_BASE)
ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
bcs ddiv_save_state
mov r2, ip
#else
// to avoid worrying about IRQs (or context switches), simply disable interrupts around call
push {r4, lr}
mrs r4, PRIMASK
cpsid i
bl ddiv_shim_call
msr PRIMASK, r4
pop {r4, pc}
#endif
ddiv_shim_call:
shimmable_table_tail_call SF_TABLE_FDIV ddiv_shim
#if !PICO_DIVIDER_DISABLE_INTERRUPTS
ddiv_save_state:
ldr r2, =(SIO_BASE)
save_div_state_and_lr
mov r2, ip
bl ddiv_shim_call
ldr r2, =(SIO_BASE)
restore_div_state_and_return
#endif
ddiv_dsub_nan_helper:
#if PICO_DOUBLE_PROPAGATE_NANS
// check for infinite op infinite (or rather check for infinite result with both
// operands being infinite)
lsls r2, r1, #1
asrs r2, r2, #21
adds r2, #1
beq 2f
add sp, #16
pop {pc}
2:
ldr r2, [sp, #4]
ldr r3, [sp, #12]
lsls r2, #1
asrs r2, r2, #21
lsls r3, #1
asrs r3, r3, #24
ands r2, r3
adds r2, #1
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
3:
add sp, #16
pop {pc}
#endif
// double FUNC_NAME(__aeabi_dmul)(double, double) double-precision multiplication
wrapper_func_d2 __aeabi_dmul
#if PICO_DOUBLE_PROPAGATE_NANS
push {r0-r3, lr}
bl 1f
// check for multiplication of infinite by zero (or rather check for infinite result with either
// operand 0)
lsls r3, r1, #1
asrs r3, r3, #21
adds r3, #1
beq 2f
add sp, #16
pop {pc}
2:
ldr r2, [sp, #4]
ldr r3, [sp, #12]
ands r2, r3
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
3:
add sp, #16
pop {pc}
1:
#endif
shimmable_table_tail_call SF_TABLE_FMUL dmul_shim
// void FUNC_NAME(__aeabi_cdrcmple)(double, double) reversed 3-way (<, =, ?>) compare [1], result in PSR ZC flags
double_wrapper_section __aeabi_cdcmple
wrapper_func __aeabi_cdrcmple
push {r0-r7,r14}
eors r0, r2
eors r2, r0
eors r0, r2
eors r1, r3
eors r3, r1
eors r1, r3
b __aeabi_dfcmple_guts
// NOTE these share an implementation as we have no excepting NaNs.
// void FUNC_NAME(__aeabi_cdcmple)(double, double) 3-way (<, =, ?>) compare [1], result in PSR ZC flags
// void FUNC_NAME(__aeabi_cdcmpeq)(double, double) non-excepting equality comparison [1], result in PSR ZC flags
@ compare r0:r1 against r2:r3, returning -1/0/1 for <, =, >
@ also set flags accordingly
.align 2
wrapper_func __aeabi_cdcmple
wrapper_func __aeabi_cdcmpeq
push {r0-r7,r14}
__aeabi_dfcmple_guts:
ldr r7,=0x7ff @ flush NaNs and denormals
lsls r4,r1,#1
lsrs r4,#21
beq 1f
cmp r4,r7
bne 2f
lsls r4, r1, #12
bhi 7f
1:
movs r0,#0
lsrs r1,#20
lsls r1,#20
2:
lsls r4,r3,#1
lsrs r4,#21
beq 1f
cmp r4,r7
bne 2f
lsls r4, r3, #12
bhi 7f
1:
movs r2,#0
lsrs r3,#20
lsls r3,#20
2:
movs r6,#1
eors r3,r1
bmi 4f @ opposite signs? then can proceed on basis of sign of x
eors r3,r1 @ restore r3
bpl 2f
cmp r3,r1
bne 7f
1:
cmp r2,r0
7:
pop {r0-r7,r15}
2:
cmp r1,r3
bne 7b
1:
cmp r0,r2
pop {r0-r7,r15}
4:
orrs r3,r1 @ make -0==+0
adds r3,r3
orrs r3,r0
orrs r3,r2
beq 7b
mvns r1, r1 @ carry inverse of r1 sign
adds r1, r1
pop {r0-r7,r15}
// int FUNC_NAME(__aeabi_dcmpeq)(double, double) result (1, 0) denotes (=, ?<>) [2], use for C == and !=
double_wrapper_section __aeabi_dcmpeq
.align 2
wrapper_func __aeabi_dcmpeq
push {lr}
bl __aeabi_cdcmpeq
beq 1f
movs r0, #0
pop {pc}
1:
movs r0, #1
pop {pc}
// int FUNC_NAME(__aeabi_dcmplt)(double, double) result (1, 0) denotes (<, ?>=) [2], use for C <
double_wrapper_section __aeabi_dcmplt
.align 2
wrapper_func __aeabi_dcmplt
push {lr}
bl __aeabi_cdcmple
sbcs r0, r0
pop {pc}
// int FUNC_NAME(__aeabi_dcmple)(double, double) result (1, 0) denotes (<=, ?>) [2], use for C <=
double_wrapper_section __aeabi_dcmple
.align 2
wrapper_func __aeabi_dcmple
push {lr}
bl __aeabi_cdcmple
bls 1f
movs r0, #0
pop {pc}
1:
movs r0, #1
pop {pc}
// int FUNC_NAME(__aeabi_dcmpge)(double, double) result (1, 0) denotes (>=, ?<) [2], use for C >=
double_wrapper_section __aeabi_dcmpge
.align 2
wrapper_func __aeabi_dcmpge
push {lr}
// because of NaNs it is better to reverse the args than the result
bl __aeabi_cdrcmple
bls 1f
movs r0, #0
pop {pc}
1:
movs r0, #1
pop {pc}
// int FUNC_NAME(__aeabi_dcmpgt)(double, double) result (1, 0) denotes (>, ?<=) [2], use for C >
double_wrapper_section __aeabi_dcmpgt
wrapper_func __aeabi_dcmpgt
push {lr}
// because of NaNs it is better to reverse the args than the result
bl __aeabi_cdrcmple
sbcs r0, r0
pop {pc}
// int FUNC_NAME(__aeabi_dcmpun)(double, double) result (1, 0) denotes (?, <=>) [2], use for C99 isunordered()
double_wrapper_section __aeabi_dcmpun
wrapper_func __aeabi_dcmpun
movs r0, #1
lsls r0, #21
lsls r2, r1, #1
adds r2, r0
bhi 1f
lsls r2, r3, #1
adds r2, r0
bhi 1f
movs r0, #0
bx lr
1:
movs r0, #1
bx lr
movs r0, #0
bx lr
// double FUNC_NAME(__aeabi_ui2d)(unsigned) unsigned to double (double precision) conversion
double_wrapper_section __aeabi_ui2d
shimmable_table_tail_call SF_TABLE_UINT2FLOAT uint2double_shim
double_wrapper_section __aeabi_i2d
wrapper_func __aeabi_ui2d
movs r1, #0
cmp r0, #0
bne 2f
1:
bx lr
// double FUNC_NAME(__aeabi_i2d)(int) integer to double (double precision) conversion
wrapper_func __aeabi_i2d
asrs r1, r0, #31
eors r0, r1
subs r0, r1
beq 1b
lsls r1, #31
2:
push {r0, r1, r4, lr}
ldr r3, =sf_clz_func
ldr r3, [r3]
blx r3
pop {r2, r3}
adds r4, r0, #1
lsls r2, r4
lsls r0, r2, #20
lsrs r2, #12
ldr r1,=1055
subs r1, r4
lsls r1, #20
orrs r1, r3
orrs r1, r2
pop {r4, pc}
// int FUNC_NAME(__aeabi_d2iz)(double) double (double precision) to integer C-style conversion [3]
double_wrapper_section __aeabi_d2iz
wrapper_func __aeabi_d2iz
regular_func double2int_z
push {r4, lr}
lsls r4, r1, #1
lsrs r2, r4, #21
movs r3, #0x80
adds r2, r3
lsls r3, #3
subs r2, r3
lsls r3, #21
cmp r2, #126
ble 1f
subs r2, #158
bge 2f
asrs r4, r1, #31
lsls r1, #12
lsrs r1, #1
orrs r1, r3
negs r2, r2
lsrs r1, r2
lsls r4, #1
adds r4, #1
adds r2, #21
cmp r2, #32
bge 3f
lsrs r0, r2
orrs r0, r1
muls r0, r4
pop {r4, pc}
1:
movs r0, #0
pop {r4, pc}
3:
mov r0, r1
muls r0, r4
pop {r4, pc}
2:
// overflow
lsrs r0, r1, #31
adds r0, r3
subs r0, #1
pop {r4, pc}
double_section double2int
regular_func double2int
shimmable_table_tail_call SF_TABLE_FLOAT2INT double2int_shim
// unsigned FUNC_NAME(__aeabi_d2uiz)(double) double (double precision) to unsigned C-style conversion [3]
double_wrapper_section __aeabi_d2uiz
wrapper_func __aeabi_d2uiz
regular_func double2uint
shimmable_table_tail_call SF_TABLE_FLOAT2UINT double2uint_shim
double_section fix2double
regular_func fix2double
shimmable_table_tail_call SF_TABLE_FIX2FLOAT fix2double_shim
double_section ufix2double
regular_func ufix2double
shimmable_table_tail_call SF_TABLE_UFIX2FLOAT ufix2double_shim
double_section fix642double
regular_func fix642double
shimmable_table_tail_call SF_TABLE_FIX642FLOAT fix642double_shim
double_section ufix2double
regular_func ufix642double
shimmable_table_tail_call SF_TABLE_UFIX642FLOAT ufix642double_shim
// double FUNC_NAME(__aeabi_l2d)(long long) long long to double (double precision) conversion
double_wrapper_section __aeabi_l2d
wrapper_func __aeabi_l2d
shimmable_table_tail_call SF_TABLE_INT642FLOAT int642double_shim
// double FUNC_NAME(__aeabi_l2f)(long long) long long to double (double precision) conversion
double_wrapper_section __aeabi_ul2d
wrapper_func __aeabi_ul2d
shimmable_table_tail_call SF_TABLE_UINT642FLOAT uint642double_shim
// long long FUNC_NAME(__aeabi_d2lz)(double) double (double precision) to long long C-style conversion [3]
double_wrapper_section __aeabi_d2lz
wrapper_func __aeabi_d2lz
regular_func double2int64_z
cmn r1, r1
bcc double2int64
push {lr}
lsls r1, #1
lsrs r1, #1
movs r2, #0
bl double2ufix64
cmp r1, #0
bmi 1f
movs r2, #0
negs r0, r0
sbcs r2, r1
mov r1, r2
pop {pc}
1:
movs r1, #128
lsls r1, #24
movs r0, #0
pop {pc}
double_section double2int64
regular_func double2int64
shimmable_table_tail_call SF_TABLE_FLOAT2INT64 double2int64_shim
// unsigned long long FUNC_NAME(__aeabi_d2ulz)(double) double to unsigned long long C-style conversion [3]
double_wrapper_section __aeabi_d2ulz
wrapper_func __aeabi_d2ulz
shimmable_table_tail_call SF_TABLE_FLOAT2UINT64 double2uint64_shim
double_section double2fix64
regular_func double2fix64
shimmable_table_tail_call SF_TABLE_FLOAT2FIX64 double2fix64_shim
double_section double2ufix64
regular_func double2ufix64
shimmable_table_tail_call SF_TABLE_FLOAT2UFIX64 double2ufix64_shim
double_section double2fix
regular_func double2fix
shimmable_table_tail_call SF_TABLE_FLOAT2FIX double2fix_shim
double_section double2ufix
regular_func double2ufix
shimmable_table_tail_call SF_TABLE_FLOAT2UFIX double2ufix_shim
double_wrapper_section __aeabi_d2f
1:
#if PICO_DOUBLE_PROPAGATE_NANS
// copy sign bit and 23 NAN id bits into sign bit and significant id bits, also set high id bit
lsrs r0, #30
lsls r2, r1, #12
lsrs r2, #9
asrs r1, #22
lsls r1, #22
orrs r0, r1
orrs r0, r2
bx lr
#endif
wrapper_func __aeabi_d2f
#if PICO_DOUBLE_PROPAGATE_NANS
movs r3, #1
lsls r3, #21
lsls r2, r1, #1
adds r2, r3
bhi 1b
#endif
// note double->float in double table at same index as float->double in double table
shimmable_table_tail_call SF_TABLE_FLOAT2DOUBLE double2float_shim
double_wrapper_section srqt
wrapper_func_d1 sqrt
shimmable_table_tail_call SF_TABLE_FSQRT dsqrt_shim
double_wrapper_section sincostan_remainder
regular_func sincostan_remainder
ldr r2, =0x54442D18 // 2 * M_PI
ldr r3, =0x401921FB
push {lr}
// note remainder only uses the divider thru integer divider functions
// which save and restore themselves
bl remainder
pop {pc}
double_wrapper_section cos
#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
wrapper_func cos
// rom version only works for -1024 < angle < 1024
lsls r2, r1, #2
bcc 1f
lsrs r2, #22
cmp r2, #9
bge 2f
1:
shimmable_table_tail_call SF_TABLE_FCOS dcos_shim
2:
#if PICO_DOUBLE_PROPAGATE_NANS
lsls r2, r1, #1
asrs r2, #21
adds r2, #1
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
bx lr
3:
#endif
push {lr}
bl sincostan_remainder
pop {r2}
mov lr, r2
b 1b
double_wrapper_section sin
#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
wrapper_func sin
// rom version only works for -1024 < angle < 1024
lsls r2, r1, #2
bcc 1f
lsrs r2, #22
cmp r2, #9
bge 2f
1:
shimmable_table_tail_call SF_TABLE_FSIN dsin_shim
2:
#if PICO_DOUBLE_PROPAGATE_NANS
lsls r2, r1, #1
asrs r2, #21
adds r2, #1
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
bx lr
3:
#endif
push {lr}
bl sincostan_remainder
pop {r2}
mov lr, r2
b 1b
double_wrapper_section sincos
// out of line remainder code for abs(angle)>=1024
2:
#if PICO_DOUBLE_PROPAGATE_NANS
lsls r2, r1, #1
asrs r2, #21
adds r2, #1
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
pop {r4-r5}
stmia r4!, {r0, r1}
stmia r5!, {r0, r1}
pop {r4, r5, pc}
3:
#endif
push {lr}
bl sincostan_remainder
pop {r2}
mov lr, r2
b 1f // continue with sincos
wrapper_func sincos
push {r2-r5, lr}
// rom version only works for -1024 < angle < 1024
lsls r2, r1, #2
bcc 1f
lsrs r2, #22
cmp r2, #9
bge 2b
1:
bl 2f // call the shim
pop {r4-r5}
stmia r4!, {r0, r1}
stmia r5!, {r2, r3}
pop {r4, r5, pc}
2:
shimmable_table_tail_call SF_TABLE_V3_FSINCOS sincos_shim_bootstrap
.thumb_func
sincos_shim_bootstrap:
push {r2, r3, r4}
movs r3, #0x13
ldrb r3, [r3]
#if PICO_DOUBLE_SUPPORT_ROM_V1 && PICO_RP2040_B0_SUPPORTED
cmp r3, #1
bne 1f
ldr r3, =dsincos_shim
b 2f
#endif
1:
ldr r3, =dsincos_shim_v2
2:
ldr r2, =sd_table
str r3, [r2, #SF_TABLE_V3_FSINCOS]
str r3, [sp, #8]
pop {r2, r3, pc}
.thumb_func
dsincos_shim_v2:
push {r4-r7,r14}
bl push_r8_r11
bl v2_rom_dsincos_internal
mov r12,r0 @ save ε
bl v2_rom_dcos_finish
push {r0,r1}
mov r0,r12
bl v2_rom_dsin_finish
pop {r2,r3}
bl pop_r8_r11
pop {r4-r7,r15}
.thumb_func
v2_rom_dsincos_internal:
push {r0, lr}
ldr r0, =0x3855
str r0, [sp, #4]
pop {r0, pc}
.thumb_func
v2_rom_dcos_finish:
push {r0, r1}
ldr r0, =0x389d
str r0, [sp, #4]
pop {r0, pc}
.thumb_func
v2_rom_dsin_finish:
push {r0, r1}
ldr r0, =0x38d9
str r0, [sp, #4]
pop {r0, pc}
double_wrapper_section tan
#don't use _d1 as we're doing a range check anyway and infinites/nans are bigger than 1024
wrapper_func tan
// rom version only works for -1024 < angle < 1024
lsls r2, r1, #2
bcc dtan_in_range
lsrs r2, #22
cmp r2, #9
bge dtan_angle_out_of_range
dtan_in_range:
#if !PICO_DIVIDER_DISABLE_INTERRUPTS
// to support IRQ usage (or context switch) we must save/restore divider state around call if state is dirty
mov ip, r2
ldr r2, =(SIO_BASE)
ldr r2, [r2, #SIO_DIV_CSR_OFFSET]
lsrs r2, #SIO_DIV_CSR_DIRTY_SHIFT_FOR_CARRY
bcs dtan_save_state
mov r2, ip
#else
// to avoid worrying about IRQs (or context switches), simply disable interrupts around call
push {r4, lr}
mrs r4, PRIMASK
cpsid i
bl dtan_shim_call
msr PRIMASK, r4
pop {r4, pc}
#endif
dtan_shim_call:
shimmable_table_tail_call SF_TABLE_FTAN dtan_shim
#if !PICO_DIVIDER_DISABLE_INTERRUPTS
dtan_save_state:
ldr r2, =(SIO_BASE)
save_div_state_and_lr
mov r2, ip
bl dtan_shim_call
ldr r2, =(SIO_BASE)
restore_div_state_and_return
#endif
dtan_angle_out_of_range:
#if PICO_DOUBLE_PROPAGATE_NANS
lsls r2, r1, #1
asrs r2, #21
adds r2, #1
bne 3f
// infinite to nan
movs r2, #1
lsls r2, #19
orrs r1, r2
bx lr
3:
#endif
push {lr}
bl sincostan_remainder
pop {r2}
mov lr, r2
b dtan_in_range
double_wrapper_section atan2
wrapper_func_d2 atan2
shimmable_table_tail_call SF_TABLE_FATAN2 datan2_shim
double_wrapper_section exp
wrapper_func_d1 exp
shimmable_table_tail_call SF_TABLE_FEXP dexp_shim
double_wrapper_section log
wrapper_func_d1 log
shimmable_table_tail_call SF_TABLE_FLN dln_shim