A2osX/SYS/KERNEL.S.MATH.txt

NEW
  AUTO 3,1
*--------------------------------------
MATH32			jmp (.1,x)
.1				.DA MATH32.ADD32
				.DA MATH32.SUB32
				.DA MATH32.UMUL32
				.DA MATH32.IMUL32
				.DA MATH32.UDIV32
				.DA MATH32.IDIV32
				.DA MATH32.UMOD32
				.DA MATH32.IMOD32
				.DA MATH32.UCMP32
				.DA MATH32.ICMP32
*--------------------------------------
MATH32.ADD32	clc
				.HS B0					BCS
MATH32.SUB32	sec
				php
				jsr MATH32.PopACC32

				ldy #0
				ldx #3
				plp
				bcs .1

				lda #$79				ADC Absolute,Y
				.HS 2C					BIT ABS
.1				lda #$F9				SBC Absolute,Y
				sta .3

.2				lda (pStack),y
.3				adc ACC32,y				SELF MODIFIED
				sta (pStack),y
				iny
				dex
				bpl .2

MATH32.SUB32.RTS
				rts
*--------------------------------------
* Returns:
*  ff X < Y
*   0 X = Y
*   1 X > Y
*--------------------------------------
MATH32.UCMP32
MATH32.ICMP32	jsr MATH32.SUB32
				jsr MATH32.PopACC32		A = ACC32+3 (sign)
				tay
				bmi .2

.1				ora ACC32+2
				ora ACC32+1
				ora ACC32
				beq .8
				lda #1
				rts

.2				lda #$ff
.8				rts			
*--------------------------------------
MATH32.UMUL32	clc
				.HS B0					BCS
MATH32.IMUL32	sec

				php
				jsr MATH32.MULDIVMOD.COMMON
				jsr MATH.MUL32

				plp
				bcc MATH32.SUB32.RTS
MATH32.RETURN.ITMP
				lda ACC32.Sign
				eor ARG32.Sign
				bpl MATH32.PutTMP32
				bra MATH32.PutNotTMP32
*--------------------------------------
MATH32.UDIV32	clc
				.HS B0					BCS
MATH32.IDIV32	sec
				clv
				bra MATH32.DIVMOD
MATH32.UMOD32	clc
				.HS B0					BCS
MATH32.IMOD32	sec
				bit MATH32.SUB32.RTS	$60

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

				plp
				bcc .3					unsigned
				bvs MATH32.RETURN.ITMP	IMOD
				lda ACC32.Sign			IDIV
				eor ARG32.Sign
				bpl MATH32.PutARG32
				bra MATH32.PutNotARG32

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

				rts
*--------------------------------------
MATH32.PutNotTMP32
				lda #TMP32
				.HS 2C					BIT ABS
*--------------------------------------
MATH32.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
*--------------------------------------
MATH32.PopACC32	ldy #0

.1				lda (pStack)
				inc pStack
				sta ACC32,y
				iny
				cpy #4
				bne .1
				sta ACC32.Sign
				rts
*--------------------------------------
MATH32.GetARG32	ldy #0

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

				sta ARG32.Sign

MATH32.GetARG32.RTS
				rts
*--------------------------------------
MATH32.MULDIVMOD.COMMON
				php
				jsr MATH32.PopACC32
				jsr MATH32.GetARG32
				plp
				bcc MATH32.GetARG32.RTS
				jsr MATH.ACC32ABS
				jmp MATH.ARG32ABS
*--------------------------------------
* http://6502.org/source/integers/32muldiv.htm
* http://nparker.llx.com/a2/mult.html
*--------------------------------------
* ARG32*ACC32->TMP32
*--------------------------------------
MATH.MUL32		jsr MATH.TMP32ZERO
				ldx #32
.1				lsr ARG32+3
				ror ARG32+2
				ror ARG32+1
				ror ARG32
				bcc .3
				clc

				ldy #0
.2				lda TMP32,y
				adc ACC32,y
				sta TMP32,y

				iny
				tya
				eor #4
				bne .2
.3				jsr MATH.ACC32.T2
				dex
				bne  .1
				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 SHARED.IsHexDigit
				bcs .9
				jsr MATH.A2ACC32
				ldy #0
.1				iny
				lda (ZPPtr2),y
				beq .8
				jsr SHARED.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 SHARED.NextCharPtr2	skip '-'
				sec
.10				ror .80+1				set pos/neg flag

				ldy #$ff

.1				iny
				lda (ZPPtr2),y
				beq .8
				jsr ZP.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

				phy
				jsr MATH.ACC32NEG
				ply

.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