/*
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "pico/asm_helper.S"
#include "pico/bootrom/sf_table.h"

__pre_init __aeabi_double_init, 00020

.syntax unified
.cpu cortex-m0plus
.thumb

.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
#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
    mov ip, pc
#endif
    str r3, [sp, #4]
    pop {r3, pc}
#if PICO_DOUBLE_SUPPORT_ROM_V1
.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
   shimmable_table_tail_call SF_TABLE_FDIV ddiv_shim

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
    rsbs r0, #0
    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}
    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

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
    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
#if PICO_DOUBLE_PROPAGATE_NANS
.align 2
1:
    pop {r2, r3}
    stmia r2!, {r0, r1}
    mov lr, r3
    pop {r3}
    stmia r3!, {r0, r1}
    bx lr
#endif
.thumb_func
sincos_shim_bootstrap:
    push {r2, r3, r4}
    movs r3, #0x13
    ldrb r3, [r3]
#if PICO_DOUBLE_SUPPORT_ROM_V1
    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 1f
    lsrs r2, #22
    cmp r2, #9
    bge 2f
1:
    shimmable_table_tail_call SF_TABLE_FTAN dtan_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 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