A2osX/SYS/KERNEL.S.MATH.txt

NEW
  AUTO 3,1
*/--------------------------------------
* # FAdd,FSub,FMult,FDiv,FPwr
* Return X+Y, X-Y, X*Y, X/Y, X^Y
* ## ASM
* **In:**
* `>PUSHF X (float)`
* `>PUSHF Y (float)`
* `>FPU fadd`
* `>FPU fsub`
* `>FPU fmult`
* `>FPU fdiv`
* `>FPU fpwr`
* ## RETURN VALUE
*  On stack (float)
*\--------------------------------------
*/--------------------------------------
* # Log,Sqr,Exp,Cos,Sin,Tan,ATan
* Return Log(x), Sqr(x), E^X, Cos(x), Sin(X), Tan(x), ATan(x)
* ## C
* `float log ( float x);`
* `float sqr ( float x);`
* `float exp ( float x);`
* `float cos ( float x);`
* `float sin ( float x);`
* `float tan ( float x);`
* `float atan ( float x);`
* ## ASM
* **In:**
* `>PUSHF x (Float)`
* `>FPU log`
* ## RETURN VALUE
*  On stack (Float)
*\--------------------------------------
*/--------------------------------------
* # float
* Return 'floated' long
* ## C
* `float f = (float)12345678;
* ## ASM
* **In:**
*  `>PUSHL X` (long)
* ## RETURN VALUE
*  On stack (float)
*\--------------------------------------
K.Float			ldy #3

.1				lda (pStack),y
				sta FAC,y			Reverse Order
				dey
				bpl .1
				
				dec pStack				keep 5 bytes on stack
				
				stz	FAC.SIGN
				lda #$80+32
				sta FAC					Positive + Exp=32
				
				ldx #FPU.LTOF
*--------------------------------------
MATH.RomCallPushFAC
				jsr GP.ROMCALL
				jmp GP.GETFAC.ON.STACK
*/--------------------------------------
* # lrintf
* Return float rounded into a long
* ## C
* `long int lrintf (float x);`
* ## ASM
* **In:**
* `>PUSHF x`
* `>SYSCALL lrintf`
* ## RETURN VALUE
*  On stack (long)
*\--------------------------------------
K.lrintf		jsr GP.SetFAC
				
				inc pStack				keep 4 bytes on stack
				
				ldx #FPU.QINT
				jsr GP.ROMCALL	

				ldy #3
				
.1				lda FAC+1,y
				sta (pStack),y
				dey
				bpl .1

				clc
				rts				
*--------------------------------------
* http://6502.org/source/integers/32muldiv.htm
* http://nparker.llx.com/a2/mult.html
*--------------------------------------
* >PUSHL ARG
* >PUSHL ACC
* >FPU ADD32
* >PULLL (ARG+ACC)
*--------------------------------------
MATH.ADD32		clc
				.HS B0					BCS
MATH.SUB32		sec
				
				php
				
				jsr MATH.PopACC32

				ldy #0
				ldx #3
				
				plp
				bcs .2
				
.1				lda (pStack),y
				adc ACC32,y
				sta (pStack),y
				iny
				dex
				bpl .1
				
				rts
				
.2				lda (pStack),y
				sbc ACC32,y
				sta (pStack),y
				iny
				dex
				bpl .2
				
MATH.SUB32.RTS	rts
*--------------------------------------
* >PUSHL ARG
* >PUSHL ACC
* >(I)MUL32
* >PULLL (ARG*ACC)
*--------------------------------------
MATH.UMUL32		clc
				.HS B0					BCS
MATH.IMUL32		sec

				php
				jsr MATH.MULDIVMOD.COMMON
				jsr MATH.MUL32

				plp
				bcc MATH.SUB32.RTS
				
MATH.RETURN.ITMP
				lda ACC32.Sign
				eor ARG32.Sign
				bpl MATH.PutTMP32
				bra MATH.PutNotTMP32
*--------------------------------------
* >PUSHL ARG
* >PUSHL ACC
* >(I)DIV32
* >PULLL (ARG / ACC)
*--------------------------------------
* >PUSHL ARG
* >PUSHL ACC
* >(I)MOD32
* >PULLL (ARG mod ACC)
*--------------------------------------
MATH.UDIV32		clc
				.HS B0					BCS
MATH.IDIV32		sec
				clv
				bra MATH.DIVMOD
				
MATH.UMOD32		clc
				.HS B0					BCS
MATH.IMOD32		sec
				bit MATH.SUB32.RTS		$60

MATH.DIVMOD		php
				jsr MATH.MULDIVMOD.COMMON
				jsr MATH.DIVMOD32

				plp
				bcc .3
				bvs MATH.RETURN.ITMP		IMOD
				
				lda ACC32.Sign			IDIV
				eor ARG32.Sign
				bmi MATH.PutARG32
				bra MATH.PutNotARG32

.3				bvs MATH.PutARG32		DIV
*--------------------------------------
MATH.PutTMP32	lda #TMP32				MOD
				.HS 2C					BIT ABS
*--------------------------------------
MATH.PutARG32	lda #ARG32
				sta .1+1
				
				ldy #3
				
.1				lda $ff,y				SELF MODIFIED
				sta (pStack),y
				dey
				bpl .1

				rts
*--------------------------------------
MATH.PutNotTMP32
				lda #TMP32
				.HS 2C					BIT ABS
*--------------------------------------
MATH.PutNotARG32
				lda #ARG32
				sta .1+1
				
				ldy #0
				ldx #3
				
				sec

.1				lda $ff,y				SELF MODIFIED
				eor #$ff				two's complement of X bytes
				adc #0
				sta (pStack),y
				iny
				dex
				bpl .1
				
				rts
*--------------------------------------
MATH.PopACC32	ldy #0

.1				lda (pStack),y
				sta ACC32,y
				iny
				cpy #4
				bne .1
				
				sta ACC32.Sign
				
				tya
*				sec				
				adc pStack
				sta pStack

				rts
*--------------------------------------
MATH.GetARG32	ldy #0

.1				lda (pStack),y
				sta ARG32,y
				iny
				cpy #4
				bne .1

				sta ARG32.Sign

				rts
*--------------------------------------
MATH.MULDIVMOD.COMMON
				php

				jsr MATH.PopACC32
				jsr MATH.GetARG32
				
				plp
				bcc .1
				
				jsr MATH.ACC32ABS
				jsr MATH.ARG32ABS

.1				rts
*--------------------------------------
* ARG32*ACC32->TMP32
*--------------------------------------
MATH.MUL32		jsr MATH.TMP32ZERO
				
				ldx #32
				
.2				lsr ARG32+3
				ror ARG32+2
				ror ARG32+1
				ror ARG32
				
				bcc .3
				
				clc

				lda TMP32
				adc ACC32
				sta TMP32

				lda TMP32+1
				adc ACC32+1
				sta TMP32+1

				lda TMP32+2
				adc ACC32+2
				sta TMP32+2

				lda TMP32+3
				adc ACC32+3
				sta TMP32+3
				
.3				jsr MATH.ACC32.T2
				
				dex
				bne  .2
				
				clc
				rts
*--------------------------------------
* ARG32 = ARG32 div ACC32
* TMP32 = ARG32 mod ACC32
*--------------------------------------
MATH.DIVMOD32	jsr MATH.TMP32ZERO
				
				ldx #32
				
.1				asl ARG32
				rol ARG32+1
				rol ARG32+2
				rol ARG32+3
				
				rol TMP32
				rol TMP32+1
				rol TMP32+2
				rol TMP32+3
				
				sec 
				
				lda TMP32
				sbc ACC32
				pha
				lda TMP32+1
				sbc ACC32+1
				pha
				lda TMP32+2
				sbc ACC32+2
				pha
				lda TMP32+3
				sbc ACC32+3
				bcs .2
				
				pla
				pla
				pla
				
				dex
				bne .1

				rts
				
.2				sta TMP32+3
				pla
				sta TMP32+2
				pla
				sta TMP32+1
				pla
				sta TMP32
				
				inc ARG32				bit0 always 0 because of .1 asl

				dex
				bne .1

				rts
*--------------------------------------
MATH.A2STR10NP	ldx #3					3 digit max
				ldy #0					Disable Padding
MATH.A2STR10	jsr MATH.A2ACC32
				clc						unsigned
*--------------------------------------
* CS = signed long
* CC = unsigned long
* X  = Pad Len
* Y  = Pad Char
*--------------------------------------
MATH.ACC322STR10
				stx .5+1				Pad Len
				sty .81+1				Pad Char

				stz A2osX.NumStrLen
				bcc .1

				clc
				
				lda ACC32+3
				bpl .1
				 
				jsr MATH.ACC32NEG

				sec
				
.1				ror ACC32.Sign			Save sign

				ldx #4

.2				stz RESULT,x			Clear all 5 bytes
				dex
				bpl .2
				
				sed						switch to BCD mode

				ldx #32					let's roll 32 bits
				
.3				jsr MATH.ACC32.T2

				ldy #4
				
.4				lda RESULT,y
				adc RESULT,y
				sta RESULT,y
				dey
				bpl .4
				
				dex
				bne .3
				
				cld
		
				ldx .5+1				no padding
				beq .6
				
				lda #10					starts at 10-padlen
				sec
.5				sbc #$ff				SELF MODIFIED
				tax
	
.6				txa
				lsr
				tay
				lda RESULT,y
				bcs .7
				
				lsr
				lsr
				lsr
				lsr

.7				and #$0F
				ora #$30

				cmp #'0'
				beq .80
				
				ldy #'0'				next 0s always printed
				sty .81+1
				
				bra .82

.80				cpx #9					last char, print always
				beq .82
								
.81				lda #$ff				SELF MODIFIED	Get Padding char	
				beq .87
	
.82				bit ACC32.Sign			"-" to print ?
				bpl .86

				pha
				lda #'-'
				jsr .88
				stz ACC32.Sign
				pla
				
.86				jsr .88
				
.87				inx
				cpx #10
				bne .6
				
				lda #0
				
.88				ldy A2osX.NumStrLen
				inc A2osX.NumStrLen
				sta A2osX.NumStrBuf,y
				
				rts
*--------------------------------------
* Convert Hex int at ZPPtr2 to ACC32
*--------------------------------------
MATH.Hex2ACC32	lda (ZPPtr2)
				beq .9
				
				jsr MEM.IsHexDigit
				bcs .9
				
				jsr MATH.A2ACC32
				
				ldy #0
				
.1				iny
				lda (ZPPtr2),y
				beq .8
				jsr MEM.IsHexDigit
				bcs .8
				
				pha
				
				ldx #4
				
.2				jsr MATH.ACC32.T2
				dex
				bne .2
				
				pla
				ora ACC32
				sta ACC32
				bra .1
				
.8				clc
				rts				

.9				sec
				rts
*--------------------------------------
* Convert A to 2 hex digits in AX
*--------------------------------------
MATH.AToHexAX	pha
				and #$0F
				jsr .8
				tax

				pla
				lsr
				lsr
				lsr
				lsr
				
.8				ora #$30
				cmp #'9'+1
				bcc .9
				adc #6
				
.9				rts				
*--------------------------------------
* Convert Decimal int at ZPPtr2 to ACC32
*--------------------------------------
MATH.Dec2ACC32	jsr MATH.ACC32ZERO
				
				clc
				lda (ZPPtr2)
				eor #'-'
				bne .10
				
				jsr MEM.NextCharPtr2	skip '-'
				sec
				
.10				ror .80+1				set pos/neg flag

				ldy #$ff

.1				iny
				lda (ZPPtr2),y
				beq .8
				
				jsr IsDigit
				bcs .8

				phy						Save Y, pointing to next char
				jsr MATH.ACC32.T10
				ply
				
				bcs .9

				lda (ZPPtr2),y
				and #$0F
				
*				clc

				adc ACC32
				sta ACC32
				bcc .1
				inc ACC32+1
				bne .1
				inc ACC32+2
				bne .1
				inc ACC32+3
				bne .1					if 0, overflow!!!
				
.9				lda #E.INUM
				sec
				rts
				
.8				tya						no digit parsed...error
				beq .9
				
.80				lda #$FF				SELF MODIFIED
				bpl .88
				
				jsr MATH.ACC32NEG
				
.88				clc
				rts		
*--------------------------------------
MATH.ACC32.T10	ldx #3

.1				lda ACC32,x				save ACC32 for 4+1
				pha
				dex
				bpl .1

				jsr MATH.ACC32.T2		ACC32 * 2 -> ACC32
				
				jsr MATH.ACC32.T2		ACC32 * 4 -> ACC32
				
				ldx #0
				ldy #4
				
*				clc

.2				pla						ACC32 * 4 + ACC32 -> ACC32
				adc ACC32,x
				sta ACC32,x
				inx
				dey
				bne .2
				
MATH.ACC32.T2	asl ACC32
				rol ACC32+1
				rol ACC32+2
				rol ACC32+3
				
				rts
*--------------------------------------
MATH.A2ACC32	sta ACC32
				.HS 2C					BIT ABS
MATH.ACC32ZERO	stz ACC32
				stz ACC32+1
				stz ACC32+2
				stz ACC32+3
				rts
*--------------------------------------
MATH.TMP32ZERO	stz TMP32
				stz TMP32+1
				stz TMP32+2
				stz TMP32+3
				rts
*--------------------------------------
MATH.ARG32ABS	lda ARG32.Sign
				bmi MATH.ARG32NEG
				rts
*--------------------------------------
MATH.ACC32ABS	lda ACC32.Sign
				bpl MATH.ACC32NEG.RTS
*--------------------------------------
MATH.ACC32NEG	ldy #ACC32
				.HS 2C					BIT ABS
				
MATH.ARG32NEG	ldy #ARG32

				ldx #3
				
				sec

.1				lda $0,y				two's complement of X bytes
				eor #$ff
				adc #0
				sta $0,y
				iny
				dex
				bpl .1
				
MATH.ACC32NEG.RTS
				rts
*--------------------------------------
MAN
SAVE USR/SRC/SYS/KERNEL.S.MATH
LOAD USR/SRC/SYS/KERNEL.S
ASM