forked from mrdudz/cc65-floatlib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
float.s
731 lines (634 loc) · 17.8 KB
/
float.s
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
BINARYFORMAT_CBM_UNPACKED = 0
BINARYFORMAT_CBM_PACKED = 1
BINARYFORMAT_IEEE755 = 2 ; TODO
; BEWARE: also change in float.h
;BINARYFORMAT = BINARYFORMAT_CBM_UNPACKED
BINARYFORMAT = BINARYFORMAT_CBM_PACKED
.segment "LOWCODE"
;---------------------------------------------------------------------------------------------
__basicon:
sei
ldx #$37
stx $01
rts
__basicoff:
ldx #$36
stx $01
cli
rts
;---------------------------------------------------------------------------------------------
; first come the actual stubs to floating point routines, these are ment to be
; used from further written ml-math routines aswell. (maybe also from the compiler?!)
;---------------------------------------------------------------------------------------------
.include "float.inc"
.importzp sreg, ptr1
;---------------------------------------------------------------------------------------------
; converter integer types to float
;---------------------------------------------------------------------------------------------
___float_s8_to_fac:
;a: low
__float_s8_to_fac:
jsr __basicon
jsr BASIC_s8_to_FAC
jmp __basicoff
___float_u8_to_fac:
;a: low
tay
;y: low
__float_u8_to_fac:
jsr __basicon
jsr BASIC_u8_to_FAC
jmp __basicoff
; get C-parameter (signed int), convert to FAC
___float_s16_to_fac:
;a: low x: high
tay
txa
;y: low a: high
; convert signed int (YA) to FAC
__float_s16_to_fac:
jsr __basicon ; enable BASIC (trashes X)
jsr BASIC_s16_to_FAC
jmp __basicoff
; get C-parameter (unsigned short), convert to FAC
___float_u16_to_fac:
;a: low x: high
tay
txa
;y: low a: high
__float_u16_to_fac:
sta FAC_MANTISSA0
sty FAC_MANTISSA1
jsr __basicon
ldx #$90
sec
jsr BASIC_u16_to_FAC
jmp __basicoff
; return to C, FAC as unsigned int
__float_fac_to_u16:
jsr __basicon
jsr BASIC_FAC_to_u16
jsr __basicoff
ldx FAC_MANTISSA2
lda FAC_MANTISSA3
rts
;---------------------------------------------------------------------------------------------
; converter float to string and back
;---------------------------------------------------------------------------------------------
; this converts to exponential form, ie what %e in printf would give you
__float_fac_to_str:
jsr __basicon
jsr BASIC_FAC_to_string
jmp __basicoff
___float_str_to_fac:
; jsr popax
__float_str_to_fac:
sta $22
stx $23
ldy #$00
@l: lda ($22),y
beq @s
iny
bne @l
@s: tya
jsr __basicon
jsr BASIC_string_to_FAC
jmp __basicoff
;---------------------------------------------------------------------------------------------
; get C-parameter (float), convert to FAC
___float_float_to_fac:
.if BINARYFORMAT = BINARYFORMAT_CBM_UNPACKED
sta FAC_MANTISSA1 ; 3
stx FAC_MANTISSA0 ; 2
ldy sreg ; 1
sty FAC_EXPONENT
ldy sreg+1 ; 0
sty FAC_SIGN
ldx #$00
stx FAC_MANTISSA2
stx FAC_MANTISSA3
stx FAC_ROUNDING
.endif
.if BINARYFORMAT = BINARYFORMAT_CBM_PACKED
sta FAC_MANTISSA2 ; 3
stx FAC_MANTISSA1 ; 2
lda sreg ; 1
ora #$80
sta FAC_MANTISSA0
; bit7=0 sign=0
; bit7=1 sign=$ff
ldx #0
lda sreg
bpl @pos
dex
@pos:
stx FAC_SIGN
ldy sreg+1 ; 0
sty FAC_EXPONENT
ldx #$00
stx FAC_MANTISSA3
stx FAC_ROUNDING
.endif
rts
; load BASIC float into FAC
; in: pointer (a/x) to BASIC float (not packed)
__float_float_to_fac: ; only used in ATAN2?
sta ptr1
stx ptr1+1
ldy #$00
lda (ptr1),y
sta FAC_EXPONENT
iny
lda (ptr1),y
sta FAC_MANTISSA0
iny
lda (ptr1),y
sta FAC_MANTISSA1
iny
lda (ptr1),y
sta FAC_MANTISSA2
iny
lda (ptr1),y
sta FAC_MANTISSA3
iny
lda (ptr1),y
sta FAC_SIGN
ldx #$00
stx FAC_ROUNDING
; always load arg after fac so these can
; be removed in funcs that only take fac
eor ARG_SIGN
sta FAC_SIGN_COMPARE
rts
; get C-parameter (two floats), to FAC and ARG
___float_float_to_fac_arg:
jsr ___float_float_to_fac
___float_float_to_arg:
ldy #$03
jsr ldeaxysp
.if BINARYFORMAT = BINARYFORMAT_CBM_UNPACKED
sta ARG_MANTISSA1 ; 3
stx ARG_MANTISSA0 ; 2
ldy sreg ; 1
sty ARG_EXPONENT
ldx #$00
stx ARG_MANTISSA2
stx ARG_MANTISSA3
lda sreg+1 ; 0
sta ARG_SIGN
eor FAC_SIGN
sta FAC_SIGN_COMPARE ; sign compare
.endif
.if BINARYFORMAT = BINARYFORMAT_CBM_PACKED
sta ARG_MANTISSA2 ; 3
stx ARG_MANTISSA1 ; 2
lda sreg ; 1
ora #$80
sta ARG_MANTISSA0
; bit7=0 sign=0
; bit7=1 sign=$ff
ldx #0
lda sreg ; 1
bpl @pos
dex
@pos:
stx ARG_SIGN
ldy sreg+1 ; 0
sty ARG_EXPONENT
ldx #$00
stx ARG_MANTISSA3
lda ARG_SIGN
eor FAC_SIGN
sta FAC_SIGN_COMPARE ; sign compare
.endif
jmp incsp4
; load BASIC float into ARG
; in: pointer (a/x) to BASIC float (not packed)
__float_float_to_arg: ; only used in ATAN2?
sta ptr1
stx ptr1+1
ldy #$00
lda (ptr1),y
sta ARG_EXPONENT
iny
lda (ptr1),y
sta ARG_MANTISSA0
iny
lda (ptr1),y
sta ARG_MANTISSA1
iny
lda (ptr1),y
sta ARG_MANTISSA2
iny
lda (ptr1),y
sta ARG_MANTISSA3
iny
lda (ptr1),y
sta ARG_SIGN
; sign compare
eor FAC_SIGN
sta FAC_SIGN_COMPARE
rts
; return to C, float as unsigned long
___float_fac_to_float:
.if BINARYFORMAT = BINARYFORMAT_CBM_UNPACKED
lda FAC_SIGN
sta sreg+1 ; 0
lda FAC_EXPONENT
sta sreg ; 1
ldx FAC_MANTISSA0 ; 2
lda FAC_MANTISSA1 ; 3
.endif
.if BINARYFORMAT = BINARYFORMAT_CBM_PACKED
lda FAC_EXPONENT
sta sreg+1 ; 0
; use the MSB of the mantissa for the sign
lda FAC_SIGN ; either $ff or $00
ora #$7f ; -> $ff or $7f
and FAC_MANTISSA0 ; bit7 of mantissa is always 1
sta sreg ; 1
ldx FAC_MANTISSA1 ; 2
lda FAC_MANTISSA2 ; 3
.endif
rts
;; store float in memory
;; in: dest. pointer (a/x), float in FAC
;__float_fac_to_float: ; UNUSED
; sta ptr1
; stx ptr1+1
; ldy #$00
; lda FAC_EXPONENT
; sta (ptr1),y
; iny
; lda FAC_MANTISSA0
; sta (ptr1),y
; iny
; lda FAC_MANTISSA1
; sta (ptr1),y
; iny
; lda FAC_MANTISSA2
; sta (ptr1),y
; iny
; lda FAC_MANTISSA3
; sta (ptr1),y
; iny
; lda FAC_SIGN
; sta (ptr1),y
; rts
;; store packed float in memory
;; in: dest. pointer (a/x), float in FAC
;__float_fac_to_float_packed: ; UNUSED
; sta ptr1
; stx ptr1+1
; ldy #4
; lda FAC_MANTISSA3
; sta (ptr1),y
; dey
; lda FAC_MANTISSA2
; sta (ptr1),y
; dey
; lda FAC_MANTISSA1
; sta (ptr1),y
; dey
;; use the MSB of the mantissa for the sign
; lda FAC_SIGN
; ora #$7f
; and FAC_MANTISSA0
; sta (ptr1),y
; dey
; lda FAC_EXPONENT
; sta (ptr1),y
; rts
;; store packed float in memory
;; in: dest. pointer (a/x), float in ARG
__float_arg_to_float_packed:
sta ptr1
stx ptr1+1
ldy #4
lda ARG_MANTISSA3
sta (ptr1),y
dey
lda ARG_MANTISSA2
sta (ptr1),y
dey
lda ARG_MANTISSA1
sta (ptr1),y
dey
; use the MSB of the mantissa for the sign
lda ARG_SIGN
ora #$7f
and ARG_MANTISSA0
sta (ptr1),y
dey
lda ARG_EXPONENT
sta (ptr1),y
rts
;---------------------------------------------------------------------------------------------
.export __ftostr
.importzp ptr1
.import popax, ldeaxysp, incsp4
; convert float to string
; char* __fastcall__ _ftostr(char *d, float s);
;-> char* __fastcall__ _ftostr(char *d, unsigned long s);
__ftostr:
jsr ___float_float_to_fac
jsr __float_fac_to_str
___float_strbuf_to_string:
jsr popax ; ptr to string
__float_strbuf_to_string:
sta ptr1
stx ptr1+1
ldy #$00
@l:
lda $0100,y
sta (ptr1),y
beq @s
iny
bne @l
@s:
lda ptr1
ldx ptr1+1
rts
.export __strtof
; convert a string to a float
; float __fastcall__ _strtof(char *d);
;-> unsigned long __fastcall__ _strtof(char *d);
__strtof:
jsr ___float_str_to_fac
jmp ___float_fac_to_float
.export __ctof
; convert char to float
; float __fastcall__ _ctof(char v);
;-> unsigned long __fastcall__ _ctof(char v);
__ctof:
jsr ___float_s8_to_fac
jmp ___float_fac_to_float
.export __utof
; convert unsigned char to float
; float __fastcall__ _utof(unsigned char v);
;-> unsigned long __fastcall__ _utof(unsigned char v);
__utof:
jsr ___float_u8_to_fac
jmp ___float_fac_to_float
.export __stof
; convert short to float
; float __fastcall__ _stof(unsigned short v);
;-> unsigned long __fastcall__ _stof(unsigned short v);
__stof:
jsr ___float_u16_to_fac
jmp ___float_fac_to_float
.export __itof
; convert integer to float
; float __fastcall__ _itof(int v);
;-> unsigned long __fastcall__ _itof(int v);
__itof:
;a: low x: high
jsr ___float_s16_to_fac
jmp ___float_fac_to_float
.export __ftoi
; convert float to integer
; int __fastcall__ _ftoi(float f);
;-> int __fastcall__ _ftoi(unsigned long f);
__ftoi:
jsr ___float_float_to_fac
jmp __float_fac_to_u16
;---------------------------------------------------------------------------------------------
; these functions take one arg (in FAC) and return result (in FAC) aswell
;---------------------------------------------------------------------------------------------
.macro __ffunc1 addr
jsr ___float_float_to_fac
jsr __basicon
jsr addr
jsr __basicoff
jmp ___float_fac_to_float
.endmacro
.export __fabs, __fatn, __fcos, __fexp, __fint, __flog
.export __frnd, __fsgn, __fsin, __fsqr, __ftan, __fnot, __fround
__fabs: __ffunc1 BASIC_FAC_Abs
__fatn: __ffunc1 BASIC_FAC_Atn
__fcos: __ffunc1 BASIC_FAC_Cos
__fexp: __ffunc1 BASIC_FAC_Exp
;__ffre: __ffunc1 BASIC_FAC_Fre
__fint: __ffunc1 BASIC_FAC_Int
__flog: __ffunc1 BASIC_FAC_Log
;__fpos: __ffunc1 BASIC_FAC_Pos
__frnd: __ffunc1 BASIC_FAC_Rnd
__fsgn: __ffunc1 BASIC_FAC_Sgn
__fsin: __ffunc1 BASIC_FAC_Sin
__fsqr: __ffunc1 BASIC_FAC_Sqr
__ftan: __ffunc1 BASIC_FAC_Tan
__fnot: __ffunc1 BASIC_FAC_Not
__fround: __ffunc1 BASIC_FAC_Round
;---------------------------------------------------------------------------------------------
; these functions take two args (in FAC and ARG) and return result (in FAC)
;---------------------------------------------------------------------------------------------
__float_ret2:
;jsr __basicoff
ldx #$36
stx $01
cli
jmp ___float_fac_to_float ; also pops pointer to float
.macro __ffunc2a addr
jsr ___float_float_to_fac_arg
jsr __basicon
lda FAC_EXPONENT
jsr addr
jmp __float_ret2
.endmacro
.macro __ffunc2b addr
jsr ___float_float_to_fac_arg
jsr __basicon
jsr addr
jmp __float_ret2
.endmacro
.export __fadd, __fsub, __fmul, __fdiv, __fpow
; float __fastcall__ _fadd(float f, float a);
;-> unsigned long __fastcall__ _fadd(unsigned long f, unsigned long a);
__fadd: __ffunc2a BASIC_ARG_FAC_Add
__fsub: __ffunc2a BASIC_ARG_FAC_Sub
__fmul: __ffunc2a BASIC_ARG_FAC_Mul
__fdiv: __ffunc2a BASIC_ARG_FAC_Div
__fpow: __ffunc2a BASIC_ARG_FAC_Pow
.export __fand, __for
__fand: __ffunc2b BASIC_ARG_FAC_And
__for: __ffunc2b BASIC_ARG_FAC_Or
__float_ret3:
;jsr __basicoff
ldx #$36
stx $01
cli
ldx #0
rts
.bss
tempfloat:
.res 5
.SEGMENT "LOWCODE"
.export __fcmp
__fcmp:
jsr ___float_float_to_fac_arg
lda #<tempfloat
ldx #>tempfloat
jsr __float_arg_to_float_packed
lda #<tempfloat
ldy #>tempfloat
___float_cmp_fac_arg:
jsr __basicon
; in: FAC=(x1) a/y= ptr lo/hi to x2
jsr BASIC_FAC_cmp
; a=0 (==) / a=1 (>) / a=255 (<)
jmp __float_ret3
.export __ftestsgn
__ftestsgn:
jsr ___float_float_to_fac
;___float_testsgn_fac:
jsr __basicon
; in: FAC(x1)
jsr BASIC_FAC_testsgn
jmp __float_ret3
___float_testsgn_fac:
lda FAC_EXPONENT
beq @s
lda FAC_SIGN
rol a
lda #$ff
bcs @s
lda #$01
@s:
rts
___float_testsgn_arg:
lda ARG_EXPONENT
beq @s
lda ARG_SIGN
rol a
lda #$ff
bcs @s
lda #$01
@s:
rts
;---------------------------------------------------------------------------------------------
; polynom1 f(x)=a1+a2*x^2+a3*x^3+...+an*x^n
;---------------------------------------------------------------------------------------------
.export __fpoly1
__fpoly1:
jsr ___float_float_to_fac
;jsr popya
jsr popax
tay
txa
jsr __basicon
jsr BASIC_FAC_Poly1
jmp __float_ret2
;---------------------------------------------------------------------------------------------
; polynom2 f(x)=a1+a2*x^3+a3*x^5+...+an*x^(2n-1)
;---------------------------------------------------------------------------------------------
.export __fpoly2
__fpoly2:
jsr ___float_float_to_fac
;jsr popya
jsr popax
tay
txa
jsr __basicon
jsr BASIC_FAC_Poly1
jmp __float_ret2
;---------------------------------------------------------------------------------------------
__float_atn_fac:
jsr __basicon
jsr BASIC_FAC_Atn
jmp __basicoff
__float_div_fac_arg:
jsr __basicon
lda FAC_EXPONENT
jsr BASIC_ARG_FAC_Div
jmp __basicoff
__float_add_fac_arg:
jsr __basicon
lda FAC_EXPONENT
jsr BASIC_ARG_FAC_Add
jmp __basicoff
__float_swap_fac_arg: ; only used in ATAN2
lda FAC_EXPONENT
ldx ARG_EXPONENT
stx FAC_EXPONENT
sta ARG_EXPONENT
lda FAC_MANTISSA0
ldx ARG_MANTISSA0
stx FAC_MANTISSA0
sta ARG_MANTISSA0
lda FAC_MANTISSA1
ldx ARG_MANTISSA1
stx FAC_MANTISSA1
sta ARG_MANTISSA1
lda FAC_MANTISSA2
ldx ARG_MANTISSA2
stx FAC_MANTISSA2
sta ARG_MANTISSA2
lda FAC_MANTISSA3
ldx ARG_MANTISSA3
stx FAC_MANTISSA3
sta ARG_MANTISSA3
lda FAC_SIGN
ldx ARG_SIGN
stx FAC_SIGN
sta ARG_SIGN
rts
.export __fneg
__fneg:
jsr ___float_float_to_fac
lda FAC_EXPONENT
beq @sk
lda FAC_SIGN
eor #$FF
sta FAC_SIGN
@sk:
jmp ___float_fac_to_float
__f_pi2: .byte $81,$80+$49,$0f,$da,$a1,$00
__f_pi: .byte $82,$80+$49,$0f,$da,$a1,$00
__f_1pi2: .byte $83,$80+$16,$cb,$e3,$f9,$00
.export __fatan2
; float _fatan2(float x, float y)
;-> unsigned long _fatan2(unsigned long x, unsigned long y)
__fatan2:
jsr ___float_float_to_fac_arg
jsr ___float_testsgn_arg
beq @s11 ; =0
bpl @s12 ; <0
; arg>0
; a=atn(y/x)
jsr __float_swap_fac_arg
jsr __float_div_fac_arg
jsr __float_atn_fac
jmp __float_ret2
@s12: ; arg<0
; a=atn(y/x)+pi
jsr __float_swap_fac_arg
jsr __float_div_fac_arg
jsr __float_atn_fac
lda #<__f_pi
ldx #>__f_pi
jsr __float_float_to_arg
jsr __float_add_fac_arg
jmp __float_ret2
@s11: ; arg=0
jsr ___float_testsgn_fac
beq @s21 ; =0
bpl @s22 ; <0
; fac >0
; a= 0.5*pi
lda #<__f_pi2
ldx #>__f_pi2
jsr __float_float_to_fac
jmp __float_ret2
; fac =0
@s21:
; a= 0
lda #$00
sta FAC_MANTISSA0
jmp __float_ret2
; fac <0
@s22:
; a= 1.5*pi
lda #<__f_1pi2
ldx #>__f_1pi2
jsr __float_float_to_fac
jmp __float_ret2