prog8/compiler/res/prog8lib/c64/floats.asm

742 lines
14 KiB
NASM

; --- low level floating point assembly routines for the C64
FL_ONE_const .byte 129 ; 1.0
FL_ZERO_const .byte 0,0,0,0,0 ; 0.0
FL_LOG2_const .byte $80, $31, $72, $17, $f8 ; log(2)
floats_store_reg .byte 0 ; temp storage
ub2float .proc
; -- convert ubyte in SCRATCH_ZPB1 to float at address A/Y
; clobbers A, Y
stx P8ZP_SCRATCH_REG
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
ldy P8ZP_SCRATCH_B1
lda #0
jsr GIVAYF
_fac_to_mem ldx P8ZP_SCRATCH_W2
ldy P8ZP_SCRATCH_W2+1
jsr MOVMF
ldx P8ZP_SCRATCH_REG
rts
.pend
b2float .proc
; -- convert byte in SCRATCH_ZPB1 to float at address A/Y
; clobbers A, Y
stx P8ZP_SCRATCH_REG
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
lda P8ZP_SCRATCH_B1
jsr FREADSA
jmp ub2float._fac_to_mem
.pend
uw2float .proc
; -- convert uword in SCRATCH_ZPWORD1 to float at address A/Y
stx P8ZP_SCRATCH_REG
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
lda P8ZP_SCRATCH_W1
ldy P8ZP_SCRATCH_W1+1
jsr GIVUAYFAY
jmp ub2float._fac_to_mem
.pend
w2float .proc
; -- convert word in SCRATCH_ZPWORD1 to float at address A/Y
stx P8ZP_SCRATCH_REG
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
ldy P8ZP_SCRATCH_W1
lda P8ZP_SCRATCH_W1+1
jsr GIVAYF
jmp ub2float._fac_to_mem
.pend
cast_from_uw .proc
; -- uword in A/Y into float var at (P8ZP_SCRATCH_W2)
stx P8ZP_SCRATCH_REG
jsr GIVUAYFAY
jmp ub2float._fac_to_mem
.pend
cast_from_w .proc
; -- word in A/Y into float var at (P8ZP_SCRATCH_W2)
stx P8ZP_SCRATCH_REG
jsr GIVAYFAY
jmp ub2float._fac_to_mem
.pend
cast_from_ub .proc
; -- ubyte in Y into float var at (P8ZP_SCRATCH_W2)
stx P8ZP_SCRATCH_REG
jsr FREADUY
jmp ub2float._fac_to_mem
.pend
cast_from_b .proc
; -- byte in A into float var at (P8ZP_SCRATCH_W2)
stx P8ZP_SCRATCH_REG
jsr FREADSA
jmp ub2float._fac_to_mem
.pend
cast_as_uw_into_ya .proc ; also used for float 2 ub
; -- cast float at A/Y to uword into Y/A
jsr MOVFM
jmp cast_FAC1_as_uw_into_ya
.pend
cast_as_w_into_ay .proc ; also used for float 2 b
; -- cast float at A/Y to word into A/Y
jsr MOVFM
jmp cast_FAC1_as_w_into_ay
.pend
cast_FAC1_as_uw_into_ya .proc ; also used for float 2 ub
; -- cast fac1 to uword into Y/A
stx P8ZP_SCRATCH_REG
jsr GETADR ; into Y/A
ldx P8ZP_SCRATCH_REG
rts
.pend
cast_FAC1_as_w_into_ay .proc ; also used for float 2 b
; -- cast fac1 to word into A/Y
stx P8ZP_SCRATCH_REG
jsr AYINT
ldy $64
lda $65
ldx P8ZP_SCRATCH_REG
rts
.pend
stack_b2float .proc
; -- b2float operating on the stack
inx
lda P8ESTACK_LO,x
stx P8ZP_SCRATCH_REG
jsr FREADSA
jmp push_fac1._internal
.pend
stack_w2float .proc
; -- w2float operating on the stack
inx
ldy P8ESTACK_LO,x
lda P8ESTACK_HI,x
stx P8ZP_SCRATCH_REG
jsr GIVAYF
jmp push_fac1._internal
.pend
stack_ub2float .proc
; -- ub2float operating on the stack
inx
lda P8ESTACK_LO,x
stx P8ZP_SCRATCH_REG
tay
lda #0
jsr GIVAYF
jmp push_fac1._internal
.pend
stack_uw2float .proc
; -- uw2float operating on the stack
inx
lda P8ESTACK_LO,x
ldy P8ESTACK_HI,x
stx P8ZP_SCRATCH_REG
jsr GIVUAYFAY
jmp push_fac1._internal
.pend
stack_float2w .proc ; also used for float2b
jsr pop_float_fac1
stx P8ZP_SCRATCH_REG
jsr AYINT
ldx P8ZP_SCRATCH_REG
lda $64
sta P8ESTACK_HI,x
lda $65
sta P8ESTACK_LO,x
dex
rts
.pend
stack_float2uw .proc ; also used for float2ub
jsr pop_float_fac1
stx P8ZP_SCRATCH_REG
jsr GETADR
ldx P8ZP_SCRATCH_REG
sta P8ESTACK_HI,x
tya
sta P8ESTACK_LO,x
dex
rts
.pend
push_float .proc
; ---- push mflpt5 in A/Y onto stack
; (taking 3 stack positions = 6 bytes of which 1 is padding)
sta P8ZP_SCRATCH_W1
sty P8ZP_SCRATCH_W1+1
ldy #0
lda (P8ZP_SCRATCH_W1),y
sta P8ESTACK_LO,x
iny
lda (P8ZP_SCRATCH_W1),y
sta P8ESTACK_HI,x
dex
iny
lda (P8ZP_SCRATCH_W1),y
sta P8ESTACK_LO,x
iny
lda (P8ZP_SCRATCH_W1),y
sta P8ESTACK_HI,x
dex
iny
lda (P8ZP_SCRATCH_W1),y
sta P8ESTACK_LO,x
dex
rts
.pend
pop_float .proc
; ---- pops mflpt5 from stack to memory A/Y
; (frees 3 stack positions = 6 bytes of which 1 is padding)
sta P8ZP_SCRATCH_W1
sty P8ZP_SCRATCH_W1+1
ldy #4
inx
lda P8ESTACK_LO,x
sta (P8ZP_SCRATCH_W1),y
dey
inx
lda P8ESTACK_HI,x
sta (P8ZP_SCRATCH_W1),y
dey
lda P8ESTACK_LO,x
sta (P8ZP_SCRATCH_W1),y
dey
inx
lda P8ESTACK_HI,x
sta (P8ZP_SCRATCH_W1),y
dey
lda P8ESTACK_LO,x
sta (P8ZP_SCRATCH_W1),y
rts
.pend
pop_float_fac1 .proc
; -- pops float from stack into FAC1
lda #<fmath_float1
ldy #>fmath_float1
jsr pop_float
lda #<fmath_float1
ldy #>fmath_float1
jmp MOVFM
.pend
copy_float .proc
; -- copies the 5 bytes of the mflt value pointed to by P8ZP_SCRATCH_W1,
; into the 5 bytes pointed to by A/Y. Clobbers A,Y.
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
ldy #0
lda (P8ZP_SCRATCH_W1),y
sta (P8ZP_SCRATCH_W2),y
iny
lda (P8ZP_SCRATCH_W1),y
sta (P8ZP_SCRATCH_W2),y
iny
lda (P8ZP_SCRATCH_W1),y
sta (P8ZP_SCRATCH_W2),y
iny
lda (P8ZP_SCRATCH_W1),y
sta (P8ZP_SCRATCH_W2),y
iny
lda (P8ZP_SCRATCH_W1),y
sta (P8ZP_SCRATCH_W2),y
rts
.pend
inc_var_f .proc
; -- add 1 to float pointed to by A/Y
sta P8ZP_SCRATCH_W1
sty P8ZP_SCRATCH_W1+1
stx P8ZP_SCRATCH_REG
jsr MOVFM
lda #<FL_ONE_const
ldy #>FL_ONE_const
jsr FADD
ldx P8ZP_SCRATCH_W1
ldy P8ZP_SCRATCH_W1+1
jsr MOVMF
ldx P8ZP_SCRATCH_REG
rts
.pend
dec_var_f .proc
; -- subtract 1 from float pointed to by A/Y
sta P8ZP_SCRATCH_W1
sty P8ZP_SCRATCH_W1+1
stx P8ZP_SCRATCH_REG
lda #<FL_ONE_const
ldy #>FL_ONE_const
jsr MOVFM
lda P8ZP_SCRATCH_W1
ldy P8ZP_SCRATCH_W1+1
jsr FSUB
ldx P8ZP_SCRATCH_W1
ldy P8ZP_SCRATCH_W1+1
jsr MOVMF
ldx P8ZP_SCRATCH_REG
rts
.pend
pop_2_floats_f2_in_fac1 .proc
; -- pop 2 floats from stack, load the second one in FAC1 as well
lda #<fmath_float2
ldy #>fmath_float2
jsr pop_float
lda #<fmath_float1
ldy #>fmath_float1
jsr pop_float
lda #<fmath_float2
ldy #>fmath_float2
jmp MOVFM
.pend
fmath_float1 .byte 0,0,0,0,0 ; storage for a mflpt5 value
fmath_float2 .byte 0,0,0,0,0 ; storage for a mflpt5 value
push_fac1 .proc
; -- push the float in FAC1 onto the stack
stx P8ZP_SCRATCH_REG
_internal ldx #<fmath_float1
ldy #>fmath_float1
jsr MOVMF
lda #<fmath_float1
ldy #>fmath_float1
ldx P8ZP_SCRATCH_REG
jmp push_float
.pend
pow_f .proc
; -- push f1 ** f2 on stack
lda #<fmath_float2
ldy #>fmath_float2
jsr pop_float
lda #<fmath_float1
ldy #>fmath_float1
jsr pop_float
stx P8ZP_SCRATCH_REG
lda #<fmath_float1
ldy #>fmath_float1
jsr CONUPK ; fac2 = float1
lda #<fmath_float2
ldy #>fmath_float2
jsr FPWR
jmp push_fac1._internal
.pend
div_f .proc
; -- push f1/f2 on stack
jsr pop_2_floats_f2_in_fac1
stx P8ZP_SCRATCH_REG
lda #<fmath_float1
ldy #>fmath_float1
jsr FDIV
jmp push_fac1._internal
.pend
add_f .proc
; -- push f1+f2 on stack
jsr pop_2_floats_f2_in_fac1
stx P8ZP_SCRATCH_REG
lda #<fmath_float1
ldy #>fmath_float1
jsr FADD
jmp push_fac1._internal
.pend
sub_f .proc
; -- push f1-f2 on stack
jsr pop_2_floats_f2_in_fac1
stx P8ZP_SCRATCH_REG
lda #<fmath_float1
ldy #>fmath_float1
jsr FSUB
jmp push_fac1._internal
.pend
mul_f .proc
; -- push f1*f2 on stack
jsr pop_2_floats_f2_in_fac1
stx P8ZP_SCRATCH_REG
lda #<fmath_float1
ldy #>fmath_float1
jsr FMULT
jmp push_fac1._internal
.pend
neg_f .proc
; -- toggle the sign bit on the stack
lda P8ESTACK_HI+3,x
eor #$80
sta P8ESTACK_HI+3,x
rts
.pend
var_fac1_less_f .proc
; -- is the float in FAC1 < the variable AY?
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #255
beq +
lda #0
rts
+ lda #1
rts
.pend
var_fac1_lesseq_f .proc
; -- is the float in FAC1 <= the variable AY?
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #0
beq +
cmp #255
beq +
lda #0
rts
+ lda #1
rts
.pend
var_fac1_greater_f .proc
; -- is the float in FAC1 > the variable AY?
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #1
beq +
lda #0
rts
+ lda #1
rts
.pend
var_fac1_greatereq_f .proc
; -- is the float in FAC1 >= the variable AY?
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #0
beq +
cmp #1
beq +
lda #0
rts
+ lda #1
rts
.pend
var_fac1_notequal_f .proc
; -- are the floats numbers in FAC1 and the variable AY *not* identical?
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
and #1
rts
.pend
vars_equal_f .proc
; -- are the mflpt5 numbers in P8ZP_SCRATCH_W1 and AY identical?
sta P8ZP_SCRATCH_W2
sty P8ZP_SCRATCH_W2+1
ldy #0
lda (P8ZP_SCRATCH_W1),y
cmp (P8ZP_SCRATCH_W2),y
bne _false
iny
lda (P8ZP_SCRATCH_W1),y
cmp (P8ZP_SCRATCH_W2),y
bne _false
iny
lda (P8ZP_SCRATCH_W1),y
cmp (P8ZP_SCRATCH_W2),y
bne _false
iny
lda (P8ZP_SCRATCH_W1),y
cmp (P8ZP_SCRATCH_W2),y
bne _false
iny
lda (P8ZP_SCRATCH_W1),y
cmp (P8ZP_SCRATCH_W2),y
bne _false
lda #1
rts
_false lda #0
rts
.pend
equal_f .proc
; -- are the two mflpt5 numbers on the stack identical?
inx
inx
inx
inx
lda P8ESTACK_LO-3,x
cmp P8ESTACK_LO,x
bne _equals_false
lda P8ESTACK_LO-2,x
cmp P8ESTACK_LO+1,x
bne _equals_false
lda P8ESTACK_LO-1,x
cmp P8ESTACK_LO+2,x
bne _equals_false
lda P8ESTACK_HI-2,x
cmp P8ESTACK_HI+1,x
bne _equals_false
lda P8ESTACK_HI-1,x
cmp P8ESTACK_HI+2,x
bne _equals_false
_equals_true lda #1
_equals_store inx
sta P8ESTACK_LO+1,x
rts
_equals_false lda #0
beq _equals_store
.pend
notequal_f .proc
; -- are the two mflpt5 numbers on the stack different?
jsr equal_f
eor #1 ; invert the result
sta P8ESTACK_LO+1,x
rts
.pend
vars_less_f .proc
; -- is float in AY < float in P8ZP_SCRATCH_W2 ?
jsr MOVFM
lda P8ZP_SCRATCH_W2
ldy P8ZP_SCRATCH_W2+1
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #255
bne +
lda #1
rts
+ lda #0
rts
.pend
vars_lesseq_f .proc
; -- is float in AY <= float in P8ZP_SCRATCH_W2 ?
jsr MOVFM
lda P8ZP_SCRATCH_W2
ldy P8ZP_SCRATCH_W2+1
stx P8ZP_SCRATCH_REG
jsr FCOMP
ldx P8ZP_SCRATCH_REG
cmp #255
bne +
- lda #1
rts
+ cmp #0
beq -
lda #0
rts
.pend
less_f .proc
; -- is f1 < f2?
jsr compare_floats
cmp #255
beq compare_floats._return_true
bne compare_floats._return_false
.pend
lesseq_f .proc
; -- is f1 <= f2?
jsr compare_floats
cmp #255
beq compare_floats._return_true
cmp #0
beq compare_floats._return_true
bne compare_floats._return_false
.pend
greater_f .proc
; -- is f1 > f2?
jsr compare_floats
cmp #1
beq compare_floats._return_true
bne compare_floats._return_false
.pend
greatereq_f .proc
; -- is f1 >= f2?
jsr compare_floats
cmp #1
beq compare_floats._return_true
cmp #0
beq compare_floats._return_true
bne compare_floats._return_false
.pend
compare_floats .proc
lda #<fmath_float2
ldy #>fmath_float2
jsr pop_float
lda #<fmath_float1
ldy #>fmath_float1
jsr pop_float
lda #<fmath_float1
ldy #>fmath_float1
jsr MOVFM ; fac1 = flt1
lda #<fmath_float2
ldy #>fmath_float2
stx P8ZP_SCRATCH_REG
jsr FCOMP ; A = flt1 compared with flt2 (0=equal, 1=flt1>flt2, 255=flt1<flt2)
ldx P8ZP_SCRATCH_REG
rts
_return_false lda #0
_return_result sta P8ESTACK_LO,x
dex
rts
_return_true lda #1
bne _return_result
.pend
set_array_float_from_fac1 .proc
; -- set the float in FAC1 in the array (index in A, array in P8ZP_SCRATCH_W1)
sta P8ZP_SCRATCH_B1
asl a
asl a
clc
adc P8ZP_SCRATCH_B1
ldy P8ZP_SCRATCH_W1+1
clc
adc P8ZP_SCRATCH_W1
bcc +
iny
+ stx floats_store_reg
tax
jsr MOVMF
ldx floats_store_reg
rts
.pend
set_0_array_float .proc
; -- set a float in an array to zero (index in A, array in P8ZP_SCRATCH_W1)
sta P8ZP_SCRATCH_B1
asl a
asl a
clc
adc P8ZP_SCRATCH_B1
tay
lda #0
sta (P8ZP_SCRATCH_W1),y
iny
sta (P8ZP_SCRATCH_W1),y
iny
sta (P8ZP_SCRATCH_W1),y
iny
sta (P8ZP_SCRATCH_W1),y
iny
sta (P8ZP_SCRATCH_W1),y
rts
.pend
set_array_float .proc
; -- set a float in an array to a value (index in A, float in P8ZP_SCRATCH_W1, array in P8ZP_SCRATCH_W2)
sta P8ZP_SCRATCH_B1
asl a
asl a
clc
adc P8ZP_SCRATCH_B1
adc P8ZP_SCRATCH_W2
ldy P8ZP_SCRATCH_W2+1
bcc +
iny
+ jmp copy_float
; -- copies the 5 bytes of the mflt value pointed to by SCRATCH_ZPWORD1,
; into the 5 bytes pointed to by A/Y. Clobbers A,Y.
.pend
equal_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
beq _true
bne _false
_true lda #1
sta P8ESTACK_LO,x
dex
rts
_false lda #0
sta P8ESTACK_LO,x
dex
rts
.pend
notequal_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
bne equal_zero._true
beq equal_zero._false
.pend
greater_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
beq equal_zero._false
bpl equal_zero._true
jmp equal_zero._false
.pend
less_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
bmi equal_zero._true
jmp equal_zero._false
.pend
greaterequal_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
bpl equal_zero._true
jmp equal_zero._false
.pend
lessequal_zero .proc
jsr floats.pop_float_fac1
jsr floats.SIGN
beq equal_zero._true
bmi equal_zero._true
jmp equal_zero._false
.pend