; =============================================================================
;
;                         Litos8 floating-point arithmetics
;
; =============================================================================

; -----------------------------------------------------------------------------
; Exported user functions (23):
;	FloatZero - floating-point zero constant
;	FloatOne - floating-point one constant
;	FloatInf - floating-point infinity constant
;	FloatPi - floating-point PI constant
;	FloatNeg - floating-point negative number
;	FloatAbs - floating-point absolute number
;	FloatCmp - floating-point comparison
;	FloatCmpZ - floating-point comparison with zero
;	FloatFact - floating-point factorial
;	FloatInvFact - floating-point invert factorial
;	FloatFromWord - import floating-point from unsigned word
;	FloatFromDWord - import floating-point from unsigned dword
;	FloatFromSWord - import floating-point from signed word
;	FloatFromSDWord - import floating-point from signed dword
;	FloatToWord - export floating-point to unsigned word
;	FloatToDWord - export floating-point to unsigned dword
;	FloatToSWord - export floating-point to signed word
;	FloatToSDWord - export floating-point to signed dword
;	FloatAdd - floating-point addition
;	FloatSub - floating-point subtraction
;	FloatMul - floating-point multiplication
;	FloatDiv - floating-point division
;	FloatInv - floating-point inverse (reciprocal) value
; -----------------------------------------------------------------------------

; Float numbers are in single-precision floating-point format IEEE 754-1985:
;  size 4 bytes:
;	bit 31: sign bit, 1=negative, 0=positive
;	bit 23..30: exponent (8 bits) biased 127 (zero exponent -> value 127)
;	bit 0..22: fraction (23 bits) (without most significant bit "1")
; Exponent is in range -126..+127 (biased value: 1..254)
; Exponent with biased value 0: zero (subnormal numbers are not supported)
; Exponent with biased value 255: infinity (NaN, + and - are not distinguished)
; Zero value: numH = 0000h, infinity value: numH = 7F80h
;
; Numbers are in range 1.1754944e-38 (00800000h) .. 1.7014118e+38 (7F000000h).

; TODO:
; ----
; FactFrac - fraction part
;
; D = x-1
; SQRT x = 1 + 1/2*D - 1/(2*4)*D^2 + (1*3)/(2*4*6)*D^3 - (1*3*5)/(2*4*6*8)*D^4 +
; Before operation normalize to 0.4 .. 1.6
;
; SIN x = x/1! - x^3/3! + x^5/5! - x^7/7! + x^9/9! - ...
; COS x = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! - ...
; Before operation normalize to 0 .. PI
;
; D = (x-1)/(x+1)
; LN x = 2*D + 2*D/3*D^2 + 2*D/5*D^4 + 2*D/7*D^6 + 2*D/9*D^8 + ...
; Before operation normalize to 1 .. 3
;
; LOG10 x = LN x / LN 10
;
; e^x = 1 + x/1! + x^2/2! + x^3/3! + x^4/4! + x^5/5! + ...
;
; ARCTG x (0..1.0) = x - x^3/3 + x^5/5 - x^7/7 + ....
;
; ARCSIN x (0..0.5) = x + x^3/2/3 + 3*x^5/2/4/5 + 3*5*x^7/2/4/6/7 +

		CODE_SECTION

; -----------------------------------------------------------------------------
;                         Floating-point zero constant
; -----------------------------------------------------------------------------
; OUTPUT:	DX:AX = 0.0
; -----------------------------------------------------------------------------

FloatZero:	xor	ax,ax
		xor	dx,dx
		ret

; -----------------------------------------------------------------------------
;                         Floating-point one constant
; -----------------------------------------------------------------------------
; OUTPUT:	DX:AX = 1.0
; -----------------------------------------------------------------------------

FloatOne:	xor	ax,ax
		mov	dx,3f80h
		ret

; -----------------------------------------------------------------------------
;                        Floating-point infinity constant
; -----------------------------------------------------------------------------
; OUTPUT:	DX:AX = 1.#INF000
; -----------------------------------------------------------------------------

FloatInf:	xor	ax,ax
		mov	dx,7f80h
		ret

; -----------------------------------------------------------------------------
;                         Floating-point PI constant
; -----------------------------------------------------------------------------
; OUTPUT:	DX:AX = PI constant (3.14159265)
; -----------------------------------------------------------------------------
;FloatPiConst:	dd	40490fdbh	; 3.14159265

FloatPi:	mov	ax,0fdbh
		mov	dx,4049h
		ret

; -----------------------------------------------------------------------------
;                         Floating-point negate number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = number
; OUTPUT:	DX:AX = result
; NOTES:	Only highest byte of number (DH) is needed.
; -----------------------------------------------------------------------------

FloatNeg:	xor	dh,B7		; change sign
		ret

; -----------------------------------------------------------------------------
;                         Floating-point absolute number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = number
; OUTPUT:	DX:AX = result
; NOTES:	Only highest byte of number (DH) is needed.
; -----------------------------------------------------------------------------

FloatAbs:	and	dh,~B7		; clear sign
		ret

; -----------------------------------------------------------------------------
;                         Floating-point comparison
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = first operand
;		CX:BX = second operand
; OUTPUT:	AL = 1 if first>second, 0 if first=second, -1 if first<second
;		SF, ZF = as for "signed CMP first,second", use JL, JG, JLE,...
; -----------------------------------------------------------------------------

FloatCmp:	cmp	dx,cx		; compare HIGH
		jg	short FloatCmp4	; first > second
		jl	short FloatCmp2	; first < second
		cmp	ax,bx		; compare LOW
		ja	short FloatCmp4	; first > second
		jb	short FloatCmp2	; first < second
		xor	al,al		; AL <- 0
		ret

FloatCmp2:	mov	al,-1		; AL < 0
		or	al,al		; set flags
		ret

FloatCmp4:	mov	al,1		; AL > 0
		or	al,al		; set flags
		ret

; -----------------------------------------------------------------------------
;                    Floating-point comparison with zero
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = operand
; OUTPUT:	AL = 1 if operand > 0, 0 if operand = 0, -1 if operand < 0
;		SF, ZF = as for "signed CMP operand,0", use JL, JG, JLE,...
; -----------------------------------------------------------------------------

FloatCmpZ:	push	bx		; push BX
		push	cx		; push CX

		xor	bx,bx		; BX <- 0
		xor	cx,cx		; CX <- 0
		call	FloatCmp	; compare

		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                         Floating-point factorial
; -----------------------------------------------------------------------------
; INPUT:	AL = integer number 0..34
; OUTPUT:	DX:AX = floating-point number n!
; -----------------------------------------------------------------------------

; ------------- push registers

FloatFact:	push	bx		; push BX

; ------------- overflow

		cmp	al,FACT_MAX	; check max. value
		jbe	short FloatFact2 ; value is OK
		mov	dx,7f80h	; DX <- overflow HIGH
		xor	ax,ax		; AX <- overflow LOW
		jmp	short FloatFact9

; ------------- get factorial value from table

FloatFact2:	cbw			; AX <- input number
		shl	ax,1		; AX <- offset in table
		xchg	ax,bx		; BX <- offset in table
		mov	ax,[FactTab+bx]	; AX <- number LOW
		mov	dx,[FactTab+bx+2] ; DX <- number HIGH

; ------------- pop registers

FloatFact9:	pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                         Floating-point invert factorial
; -----------------------------------------------------------------------------
; INPUT:	AL = integer number 0..34
; OUTPUT:	DX:AX = floating-point number 1/n!
; -----------------------------------------------------------------------------

; ------------- push registers

FloatInvFact:	push	bx		; push BX

; ------------- overflow

		cmp	al,FACT_MAX	; check max. value
		jbe	short FloatInvFact2 ; value is OK
		xor	dx,dx		; DX <- zero HIGH
		xor	ax,ax		; AX <- zero LOW
		jmp	short FloatFact9

; ------------- get factorial value from table

FloatInvFact2:	cbw			; AX <- input number
		shl	ax,1		; AX <- offset in table
		xchg	ax,bx		; BX <- offset in table
		mov	ax,[InvFactTab+bx] ; AX <- number LOW
		mov	dx,[InvFactTab+bx+2] ; DX <- number HIGH

; ------------- pop registers

FloatInvFact9:	pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;       Import floating-point number from unsigned word integer number
; -----------------------------------------------------------------------------
; INPUT:	AX = integer unsigned number
; OUTPUT:	DX:AX = floating-point number
; -----------------------------------------------------------------------------

FloatFromWord:	xor	dx,dx		; DX <- 0

; === FloatFromDWord must follow !

; -----------------------------------------------------------------------------
;     Import floating-point number from unsigned dword integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = integer unsigned number
; OUTPUT:	DX:AX = floating-point number
; -----------------------------------------------------------------------------

; ------------- push registers

FloatFromDWord:	push	bx		; push BX
		push	cx		; push CX

; ------------- import using FPU

		IS_FPU			; is FPU numeric coprocessor
		jz	short FloatFromDWord0 ; no FPU numeric coprocessor

		push	bp		; push BP
		mov	bp,sp		; BP <- stack
		sub	sp,8		; local variables
		mov	[bp-8],ax	; integer LOW
		mov	[bp-8+2],dx	; integer HIGH
		mov	word [bp-8+4],0
		mov	word [bp-8+6],0
		fwait			; FPU wait
		fild	qword [bp-8]	; load number
		fwait			; FPU wait
		fstp	dword [bp-8]	; store result into stack
		fwait			; FPU wait
		mov	ax,[bp-8]	; AX <- result LOW
		mov	dx,[bp-8+2]	; DX <- result HIGH
		mov	sp,bp		; return SP
		pop	bp		; pop BP
		jmp	short FloatFromDWord9

; ------------- zero

FloatFromDWord0:or	ax,ax		; number LOW = 0 ?
		jnz	short FloatFromDWord1 ; no 0
		or	dx,dx		; number HIGH = 0 ?
		jz	short FloatFromDWord9 ; result is 0

; ------------- limit number range

FloatFromDWord1:mov	bx,23+127	; BX <- initial exponent for 24 bits
		jmp	short FloatFromDWord3
FloatFromDWord2:shr	dx,1		; limit number HIGH
		rcr	ax,1		; limit number LOW
		rcl	bh,1		; BH <- round carry
		inc	bx		; exponent correction
FloatFromDWord3:or	dh,dh		; is number in valid 24-bit range?
		jnz	short FloatFromDWord2 ; need to lower

; ------------- rounding

		cmp	bh,1		; round 0.50 ?
		jne	short FloatFromDWord6 ; no 0.50, use normal round
		mov	bh,al		; BH <- number LOW
FloatFromDWord6:shr	bh,1		; CF <- round carry
		adc	ax,byte 0	; add round carry
		adc	dx,byte 0	; carry
		or	dh,dh		; overflow?
		jz	short FloatFromDWord8 ; no overflow
		shr	dx,1		; limit number HIGH
		rcr	ax,1		; limit number LOW
		inc	bx		; exponent correction
		jmp	short FloatFromDWord8

; ------------- normalize mantissa

FloatFromDWord7:shl	ax,1		; increase number LOW
		rcl	dx,1		; increase number HIGH
		dec	bx		; exponent correction
FloatFromDWord8:test	dl,80h		; is mantissa in valid range?
		jz	short FloatFromDWord7 ; normalize mantissa

; ------------- composite mantissa with exponent

		shl	dl,1		; DL <- shift mantissa HIGH
		mov	dh,bl		; DH <- exponent
		shr	dh,1		; DH <- shift exponent to position
		rcr	dl,1		; DL <- return mantissa with exponent

; ------------- pop registers

FloatFromDWord9:pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;       Import floating-point number from signed word integer number
; -----------------------------------------------------------------------------
; INPUT:	AX = integer signed number
; OUTPUT:	DX:AX = floating-point number
; -----------------------------------------------------------------------------

FloatFromSWord:	cwd			; DX <- sign

; === FloatFromSDWord must follow !

; -----------------------------------------------------------------------------
;       Import floating-point number from signed dword integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = integer signed number
; OUTPUT:	DX:AX = floating-point number
; -----------------------------------------------------------------------------

FloatFromSDWord:or	dx,dx		; negative number?
		jns	short FloatFromDWord ; not negative number
		neg	ax		; AX <- negate number LOW
		adc	dx,byte 0	; DX <- carry
		neg	dx		; DX <- negate number HIGH
		call	FloatFromDWord	; import positive number
		xor	dh,B7		; change number sig
		ret

; -----------------------------------------------------------------------------
;      Export floating-point number to unsigned dword integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = floating-point number
; OUTPUT:	DX:AX = integer unsigned number
; -----------------------------------------------------------------------------

; ------------- push registers

FloatToDWord:	push	bx		; push BX
		push	cx		; push CX
		push	si		; push SI

; ------------- zero or negative number

		or	dh,dh		; negative number ?
		js	short FloatToDWord0 ; negative number
		test	dx,7f80h	; zero?	
		jnz	short FloatToDWord1 ; not zero
FloatToDWord0:	xor	ax,ax		; AX <- 0
		xor	dx,dx		; DX <- 0
		jmp	FloatToDWord9

; ------------- export using FPU

FloatToDWord1:	IS_FPU			; is FPU numeric coprocessor
		jz	short FloatToDWord2 ; no FPU numeric coprocessor
                                      
		push	bp		; push BP
		mov	bp,sp		; BP <- stack
		sub	sp,8		; local variables
		mov	[bp-8],ax	; number LOW
		mov	[bp-8+2],dx	; number HIGH
		fwait			; FPU wait
		fld	dword [bp-8]	; load number
		fwait			; FPU wait
		fistp	qword [bp-8]	; set number
		fwait			; FPU wait
		mov	ax,[bp-8]	; AX <- result LOW
		mov	dx,[bp-8+2]	; DX <- result HIGH

		cmp	word [bp-8+4],0
		jne	short FloatToDWord12 ; overflow
		cmp	word [bp-8+6],0
		je	short FloatToDWord14 ; no overflow
FloatToDWord12:	xor	ax,ax		; AX <- 0
		dec	ax		; AX <- -1
		cwd			; DX:AX <- -1, max. DWord value

FloatToDWord14:	mov	sp,bp		; return SP
		pop	bp		; pop BP
		jmp	short FloatToDWord9

; ------------- extract exponent -> BX, and mantissa -> DX:AX

FloatToDWord2:	shl	ax,1		; extract exponent B0
		rcl	dx,1
		mov	bh,0
		mov	bl,dh		; BL <- exponent
		mov	dh,0		; DH <- 0, clear exponent
		stc			; most significant bit
		rcr	dl,1		; DX <- back to old position
		rcr	ax,1

; ------------- too small number becomes zero

		sub	bx,126		; BX <- exponent base >= 1
		jl	short FloatToDWord0 ; too small number -> 0

; ------------- too big number becomes -1

		xor	cx,cx		; CX <- 0
		xor	si,si		; SI <- 0
		cmp	bl,32		; check max. exponent
		jbe	short FloatToDWord3 ; exponent is OK
		xor	ax,ax		; AX <- 0
		dec	ax		; AX <- -1
		cwd			; DX:AX <- -1, max. DWord value
		jmp	short FloatToDWord9

; ------------- increase exponent

FloatToDWord3:	cmp	bl,24		; maximal exponent
		jbe	short FloatToDWord5
		dec	bx		; decrease exponent
		shl	ax,1		; shift mantissa left
		rcl	dx,1
		jmp	short FloatToDWord3

; ------------- decrease exponent

FloatToDWord5:	cmp	bl,24		; minimal exponent
		je	short FloatToDWord6
		inc	bx		; increase exponent
		shr	dx,1
		rcr	ax,1		; shift mantssa right
		rcl	cx,1
		rcl	si,1		; carry from low bits
		jmp	short FloatToDWord5

; ------------- round

FloatToDWord6:	or	si,si		; is carry exact 0.5 ?
		jnz	short FloatToDWord7
		cmp	cx,1
		jne	short FloatToDWord7
		mov	cl,al		; round to even number on 0.5
FloatToDWord7:	rcr	cx,1
		adc	ax,byte 0	; round carry
		adc	dx,byte 0

; ------------- pop registers

FloatToDWord9:  pop	si		; pop SI
		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;        Export floating-point number to unsigned word integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = floating-point number
; OUTPUT:	AX = integer unsigned number
; -----------------------------------------------------------------------------

; ------------- push registers

FloatToWord:	push	dx		; push DX

; ------------- export to unsigned DWord

		call	FloatToDWord	; export to unsigned DWord

; ------------- limit number

		or	dx,dx		; overflow?
		jz	short FloatToWord9 ; number is OK
		xor	ax,ax		; AX <- 0
		dec	ax		; AX <- -1, max. value

; ------------- pop registers

FloatToWord9:	pop	dx		; pop DX
		ret

; -----------------------------------------------------------------------------
;        Export floating-point number to signed dword integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = floating-point number
; OUTPUT:	DX:AX = integer signed number
; -----------------------------------------------------------------------------

; ------------- check if input number is negative

FloatToSDWord:	or	dh,dh		; negative number?
		js	short FloatToSDWord4 ; negative number

; ------------- export positive number

FloatToSDWord1:	call	FloatToDWord	; export to unsigned number
		or	dx,dx		; is number in valid range?
		jns	short FloatToSDWord2 ; number is in valid range
		xor	ax,ax		; AX <- 0
		dec	ax		; AX <- -1
		mov	dx,7fffh	; DX:AX <- max. positive number
FloatToSDWord2:	ret

; ------------- export negative number

FloatToSDWord4:	xor	dh,B7		; change to positive number
		call	FloatToSDWord	; export to unsigned number
		neg	ax		; AX <- negate number LOW
		adc	dx,byte 0	; DX <- carry
		neg	dx		; DX <- negate number HIGH
		js	short FloatToSDWord6 ; number is in valid range
		xor	ax,ax		; AX <- 0
		mov	dx,8000h	; DX:AX <- max. negative number
FloatToSDWord6:	ret

; -----------------------------------------------------------------------------
;        Export floating-point number to signed word integer number
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = floating-point number
; OUTPUT:	AX = integer signed number
; -----------------------------------------------------------------------------

; ------------- push registers

FloatToSWord:	push	dx		; push DX

; ------------- export to signed DWord

		call	FloatToSDWord	; export to signed DWord

; ------------- limit positive number

		or	dx,dx		; negative number?
		js	short FloatToSWord4 ; negative number
		jnz	short FloatToSWord2 ; > 65K
		or	ax,ax		; overflow?
		jns	short FloatToSWord9 ; number is 0..32K
FloatToSWord2:	mov	ax,7fffh	; AX <- limit number
		jmp	short FloatToSWord9

; ------------- limit negative number

FloatToSWord4:	inc	dx		; DX = -1 ?
		jnz	short FloatToSWord6 ; > 65K
		or	ax,ax		; overflow?
		js	short FloatToSWord9 ; number is 0..-32K
FloatToSWord6:	mov	ax,8000h	; AX <- limit number

; ------------- pop registers

FloatToSWord9:	pop	dx		; pop DX
		ret

; -----------------------------------------------------------------------------
;                         Floating-point addition
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = first operand
;		CX:BX = second operand
; OUTPUT:	DX:AX = result
; -----------------------------------------------------------------------------

; ------------- push registers

FloatAdd:	push	bx		; push BX
		push	cx		; push CX
		push	si		; push SI
		push	di		; push DI
		push	bp		; push BP

; ------------- denormalized first operand (biased exponent = 0)

		test	dx,7f80h	; is first operand denormalized or 0?
		jnz	short FloatAdd01;first operand is not denormalized or 0
		test	cx,7f80h	; is second operand denormalized or 0?
		jz	short FloatAdd08 ; both operands are denormalized or 0
		xchg	ax,bx		; AX <- second operand LOW
		mov	dx,cx		; DX <- second operand HIGH
		jmp	short FloatAdd09; second operand becomes to be a result

; ------------- denormalized second operand (biased exponent = 0)

FloatAdd01:	test	cx,7f80h	; is second operand denormalized?
		jnz	short FloatAdd02;second operand is not denormal. or 0
		test	dx,7f80h	; is first operand denormalized or 0?
		jz	FloatAdd08	; both operands are denormalized or 0
		jmp	short FloatAdd09; first operand becomes to be a result

; ------------- infinite value of first operand

FloatAdd02:	mov	si,dx		; SI <- first operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatAdd06 ; infinite value

; ------------- infinite value of second operand

		mov	si,cx		; SI <- second operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatAdd06 ; infinite value

; ------------- check HW support

		IS_FPU			; is FPU numeric coprocessor
		jz	short FloatAdd1	; no FPU numeric coprocessor

; === HW calculation
; ------------- push registers FPU

		mov	bp,sp		; BP <- stack
		sub	sp,4		; local variables

; ------------- load first operand

		mov	[bp-4],ax	; first operand LOW
		mov	[bp-4+2],dx	; first operand HIGH
		fwait			; FPU wait
		fld	dword [bp-4]	; load first operand

; ------------- load second operand

		mov	[bp-4],bx	; second operand LOW
		mov	[bp-4+2],cx	; second operand HIGH
		
; ------------- perform operation

		fwait			; FPU wait
		fadd	dword [bp-4]	; multiplication

; ------------- get result

		fwait			; FPU wait
		fstp	dword [bp-4]	; store result into stack
		fwait			; FPU wait
		mov	ax,[bp-4]	; AX <- result LOW
		mov	dx,[bp-4+2]	; DX <- result HIGH

; ------------- pop registers FPU

		mov	sp,bp		; return SP

; ------------- infinite result

		mov	si,dx		; SI <- result HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		jne	short FloatAdd07 ; no infinite value
FloatAdd06:	mov	dx,7f80h	; infinite value
		xor	ax,ax		; AX <- 0

; ------------- denormalized result or 0 (biased exponent = 0)

FloatAdd07:	test	dx,7f80h	; is result denormalized?
		jnz	short FloatAdd09 ; result is not denormalized
FloatAdd08:	xor	dx,dx		; DX <- 0
		xor	ax,ax		; AX <- 0
FloatAdd09:	jmp	FloatAdd9

; === SW calculation

; num1: DX:AX, num2: CX:BX
; ------------- num1 -> BX:SI, num2 -> CX:DI

FloatAdd1:	xchg	ax,si		; SI <- num1L
		mov	di,bx		; DI <- num2L
		mov	bx,dx		; BX <- num1H

; num1: BX:SI, num2: CX:DI
; ------------- extract sign1 -> DL, sign2 -> DH

		mov	dl,bh		; DL <- num1H
		mov	dh,ch		; DH <- num2H
		and	dx,B7+B15	; DL <- sign1, DH <- sign2

; num1: BX:SI, num2: CX:DI, sign1: DL, sign2: DH
; ------------- split num1 to exp1 -> BH, mant1 -> BL:SI

		shl	bx,1		; BX <- shift left
		stc			; prepare high bit = 1
		rcr	bl,1		; BL <- return mant1H, without exp1

; exp1: BH, mant1: BL:SI, num2: CX:DI, sign1: DL, sign2: DH
; ------------- split num2 to exp2 -> CH, mant2 -> CL:DI

		shl	cx,1		; CX <- shift left
		stc			; prepare high bit = 1
		rcr	cl,1		; CL <- return mant2H, without exp2

; exp1: BH, mant1: BL:SI, exp2: CH, mant2: CL:DI, sign1: DL, sign2: DH
; ------------- ensure num1 >= num2, exp2 -> AH, mant2H -> AL

		cmp	bx,cx		; compare number HIGH
		jne	short FloatAdd2	; not equ
		cmp	si,di		; compare number LOW
FloatAdd2:	jae	short FloatAdd22 ; num1 >= num2, OK
		xchg	bx,cx		; exchange number HIGH
		xchg	si,di		; exchange number LOW
		xchg	dl,dh		; exchange signs
FloatAdd22:	xchg	ax,cx		; AH <- exp2, AL <- mant2H

; exp1: BH, mant1: BL:SI, exp2: AH, mant2: AL:DI, sign1: DL, sign2: DH
; ------------- difference of exponents -> CL

		mov	cl,bh		; CL <- exp1
		sub	cl,ah		; CL <- exp. difference (>= 0)
		cmp	cl,24		; check maximal significant difference
		ja	short FloatAdd8	; num2 will not be significant

; exp1: BH, mant1: BL:SI, mant2: AL:DI, sign1: DL, sign2: DH, exp diff.: CL
; ------------- normalize num2 to the same exponent as num1

		mov	ah,0		; AH <- round carry
		mov	ch,0		; CX <- exponent difference
		jcxz	FloatAdd34	; no shift
FloatAdd3:	shr	al,1		; shift AL
		rcr	di,1		; shift DI
		loop	FloatAdd3	; next shift
		rcl	ah,1		; AH <- round carry

; exp1: BH, mant1: BL:SI, mant2: AL:DI, sign1: DL, sign2: DH, round carry: AH
; ------------- add or sub?

FloatAdd34:	xor	dh,dl		; DH <- sign difference
		jnz	short FloatAdd4	; different sign, subtract

; exp1: BH, mant1: BL:SI, mant2: AL:DI, sign1: DL, round carry: AH
; ------------- equal sign, add num2 to num1

		shr	ah,1		; CY <- round carry
		adc	si,di		; SI <- add DI (with carry)
		adc	bl,al		; BL <- add AL
		jnc	short FloatAdd8	; no overflow
		rcr	bl,1		; BL <- rotate right
		rcr	si,1		; SI <- rotate right
		inc	bh		; increase exp1
		cmp	bh,255		; exponent overflow?
		jne	short FloatAdd8	; no overflow

; ------------- overflow

FloatAdd36:	xor	ax,ax		; AX <- 0
		mov	dx,7f80h	; DX <- 7f80h
		jmp	short FloatAdd9

; exp1: BH, mant1: BL:SI, mant2: AL:DI, sign1: DL
; ------------- subtract num2 from num1
		
FloatAdd4:	shr	ah,1		; CY <- round carry
		sbb	si,di		; SI <- sub DI (with carry)
		sbb	bl,al		; BL <- sub AL

; exp: BH, mant: BL:SI, sign: DL
; ------------- check zero result

		mov	al,bl		; AL <- mantH
		mov	ah,0		; AH <- 0
		or	ax,si		; is result 0?
		jnz	short FloatAdd7	; result is not 0

; ------------- underflow to 0

FloatAdd5:	xor	ax,ax		; AX <- 0
		xor	dx,dx		; DX <- 0
		jmp	short FloatAdd9

; exp1: BH, mant1: BL:SI, sign1: DL
; ------------- normalize mantissa

FloatAdd6:	shl	si,1		; shift mantL left
		rcl	bl,1		; shift mantH left
		dec	bh		; decrease exponent
		jz	short FloatAdd5	; underflow
FloatAdd7:	or	bl,bl		; most significant bit reached?
		jns	short FloatAdd6	; continue shifting

; exp1: BH, mant1: BL:SI, sign1: DL
; ------------- combine exponent with mantisa BH:BL:SI (DL) -> DX:AX

FloatAdd8:	xchg	ax,si		; AX <- result LOW
		shl	bl,1		; shift out unused bit B7
		shr	bx,1		; rotate BX to position
		or	bh,dl		; set sign
		mov	dx,bx		; DX <- result HIGH

; ------------- pop registers

FloatAdd9:	pop	bp		; pop BP
		pop	di		; pop di
		pop	si		; pop SI
		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                         Floating-point subtraction
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = first operand
;		CX:BX = second operand
; OUTPUT:	DX:AX = result (= first - second)
; -----------------------------------------------------------------------------

FloatSub:	push	cx		; push CX
		xor	ch,B7		; change sign of second operand
		call	FloatAdd	; add negative number
		pop	cx		; pop CX
		ret

; -----------------------------------------------------------------------------
;                       Floating-point multiplication
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = first operand
;		CX:BX = second operand
; OUTPUT:	DX:AX = result
; -----------------------------------------------------------------------------

; ------------- push registers

FloatMul:	push	bx		; push BX
		push	cx		; push CX
		push	si		; push SI
		push	di		; push DI
		push	bp		; push BP

; ------------- denormalized first operand (biased exponent = 0)

		test	dx,7f80h	; is first operand denormalized or 0?
		jz	short FloatMul08 ; first operand is denormalized or 0

; ------------- denormalized second operand (biased exponent = 0)

		test	cx,7f80h	; is second operand denormalized or 0?
		jz	short FloatMul08 ; second operand is denormalized or 0

; ------------- infinite value of first operand

		mov	si,dx		; SI <- first operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatMul06 ; infinite value

; ------------- infinite value of second operand

		mov	si,cx		; SI <- second operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatMul06 ; infinite value

; ------------- check HW support

		IS_FPU			; is FPU numeric coprocessor
		jz	short FloatMul1	; no FPU numeric coprocessor

; === HW calculation
; ------------- push registers FPU

		mov	bp,sp		; BP <- stack
		sub	sp,4		; local variables

; ------------- load first operand

		mov	[bp-4],ax	; first operand LOW
		mov	[bp-4+2],dx	; first operand HIGH
		fwait			; FPU wait
		fld	dword [bp-4]	; load first operand

; ------------- load second operand

		mov	[bp-4],bx	; second operand LOW
		mov	[bp-4+2],cx	; second operand HIGH
		
; ------------- perform operation

		fwait			; FPU wait
		fmul	dword [bp-4]	; multiplication

; ------------- get result

		fwait			; FPU wait
		fstp	dword [bp-4]	; store result into stack
		fwait			; FPU wait
		mov	ax,[bp-4]	; AX <- result LOW
		mov	dx,[bp-4+2]	; DX <- result HIGH

; ------------- pop registers FPU

		mov	sp,bp		; return SP

; ------------- infinite result

		mov	si,dx		; SI <- result HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		jne	short FloatMul07 ; no infinite value
FloatMul06:	mov	dx,7f80h	; infinite value
		xor	ax,ax		; AX <- 0

; ------------- denormalized result (biased exponent = 0)

FloatMul07:	test	dx,7f80h	; is result denormalized or 0?
		jnz	short FloatMul09 ; result is not denormalized
FloatMul08:	xor	dx,dx		; DX <- 0
		xor	ax,ax		; AX <- 0
FloatMul09:	jmp	FloatMul9

; === SW calculation

; num1: DX:AX, num2: CX:BX
; ------------- prepare result sign -> stack (0 or 8000h), num1L -> SI

FloatMul1:	xchg	ax,si		; SI <- num1L
		mov	ax,dx		; AX <- num1H
		xor	ax,cx		; AX <- xor with num2H
		and	ax,B15		; AX <- result sign
		push	ax		; push AX

; num1: DX:SI, num2: CX:BX, sign 8000h in stack
; ------------- split num1 to exp1 -> DH, mant1 -> DL:SI

		shl	dx,1		; DX <- shift left
		stc			; prepare high bit = 1
		rcr	dl,1		; DL <- return mant1H, without exp1

; exp1: DH, mant1: DL:SI, num2: CX:BX, sign 8000h in stack
; ------------- split num2 to exp2 -> CH, mant2 -> CL:BX

		shl	cx,1		; CX <- shift left
		stc			; prepare high bit = 1
		rcr	cl,1		; CL <- return mant2H, without exp2

; exp1: DH, mant1: DL:SI, exp2: CH, mant2: CL:BX, sign 8000h in stack
; ------------- exp1 -> CL, mant1H -> BL, mant2H -> BH, mant2L -> DI

		mov	di,bx		; DI <- mant2L
		mov	bl,dl		; BL <- mant1H
		mov	bh,cl		; BH <- mant2H
		mov	cl,dh		; CL <- exp1

; exp1: CL, mant1: BL:SI, exp2: CH, mant2: BH:DI, sign 8000h in stack
; ------------- mant1H * mant2H -> BP (resH)

		mov	al,bl		; AL <- mant1H
		mul	bh		; AX <- mant1H * mant2H
		xchg	ax,bp		; BP <- result HIGH

; exp1: CL, mant1: BL:SI, exp2: CH, mant2: BH:DI, resH: BP, sign 8000h in stack
; ------------- mant2H * mant1L -> BP:BX, mant1H -> AL

		mov	ah,0		; AH <- 0
		mov	al,bh		; AX <- mant2H
		mul	si		; DX:AX <- mant2H * mant1L
		add	bp,dx		; BP <- add to result HIGH
		xchg	ax,bx		; BX <- result LOW, AL <- mant1H

; exp1: CL, mant1: AL:SI, exp2: CH, mant2L: DI, res: BP:BX, sign 8000h in stack
; ------------- mant1H * mant2L -> BP:BX

		mov	ah,0		; AX <- mant1H
		mul	di		; DX:AX <- mant1H * mant2L
		add	bx,ax		; BX <- add to result LOW
		adc	bp,dx		; BP <- add to result HIGH

; exp1: CL, mant1L: SI, exp2: CH, mant2L: DI, res: BP:BX, sign 8000h in stack
; ------------- mant1L * mant2L -> BP:BX

		xchg	ax,si		; AX <- mant1L
		mul	di		; DX:AX <- mant1L * mant2L
		add	bx,dx		; BX <- add to result LOW
		adc	bp,0		; BP <- add carry

; exp1: CL, exp2: CH, res: BP:BX, sign 8000h in stack
; ------------- sum exponents

		mov	ah,0		; AH <- 0
		mov	al,ch		; AL <- exp2
		mov	ch,0		; CX = exp1
		add	ax,cx		; AX <- exp1 + exp2
		sub	ax,127		; AX -= base

; exp: AX, res: BP:BX, sign 8000h in stack
; ------------- carry from mantisa

		shl	bx,1		; BX <- shift mantisa
		rcl	bp,1		; BP <- shift mantisa HIGH
		jc	short FloatMul2	; carry from mantisa
		shl	bx,1		; BX <- shift mantisa if no carry
		rcl	bp,1		; BP <- shift mantisa HIGH
		clc			; NC, no carry
FloatMul2:	adc	ax,0		; AX <- add carry from mantisa

; ------------- exponent underflow, cut out denormalized result

		cmp	ax,1		; minimal exponent value
		jge	short FloatMul4	; no underflow
		xor	ax,ax		; AX <- result LOW = 0, underflow flag
		xor	dx,dx		; DX <- result HIGH = 0, underflow flag
		pop	bx		; pop BX
		jmp	short FloatMul9

; ------------- exponent overflow

FloatMul4:	cmp	ax,255		; exponent overflow?
		jb	short FloatMul6	; exponent is OK
FloatMul5:	xor	ax,ax		; AX <- 0
		mov	dx,7f80h
		pop	bx		; pop BX
		jmp	short FloatMul9

; ------------- combine exponent with mantisa -> DX:AX

FloatMul6:	mov	cx,bp		; CX <- result HIGH
		mov	dh,al		; DH <- exponent
		mov	dl,ch		; DL <- mantisa high
		mov	ah,cl		; AH <- mantisa middle
		mov	al,bh		; AL <- mantisa low
		add	ax,byte 1	; AX <- round to nearest
		adc	dx,byte 0	; DX <- carry
		shr	dx,1		; DX <- rotate mantisa to position
		rcr	ax,1		; AX <- rotate mantisa to position
		cmp	dx,7f80h	; round overflow?
		je	short FloatMul5	; overflow while rounding

; ------------- add sign to result

FloatMul8:	pop	bx		; pop BX (bit 15: sign)
		or	dx,bx		; DX <- add sign

; ------------- pop registers

FloatMul9:	pop	bp		; pop BP
		pop	di		; pop di
		pop	si		; pop SI
		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                         Floating-point division
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = first operand (dividend)
;		CX:BX = second operand (divisor)
; OUTPUT:	DX:AX = result (quotient)
; -----------------------------------------------------------------------------

; ------------- push registers

FloatDiv:	push	bx		; push BX
		push	cx		; push CX
		push	si		; push SI
		push	di		; push DI
		push	bp		; push BP

; ------------- denormalized dividend (biased exponent = 0)

		test	dx,7f80h	; is first operand denormalized or 0?
		jz	short FloatDiv08 ; first operand is denormalized or 0

; ------------- denormalized divisor (biased exponent = 0)

		test	cx,7f80h	; is second operand denormalized or 0?
		jz	short FloatDiv06 ; second operand is denormalized or 0

; ------------- infinite value of dividend

		mov	si,dx		; SI <- first operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatDiv06 ; infinite value

; ------------- infinite value of divisor

		mov	si,cx		; SI <- second operand HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		je	short FloatDiv08 ; infinite value

; ------------- check HW support

		IS_FPU			; is FPU numeric coprocessor
		jz	short FloatDiv1	; no FPU numeric coprocessor

; === HW calculation
; ------------- push registers FPU

		mov	bp,sp		; BP <- stack
		sub	sp,4		; local variables

; ------------- load first operand

		mov	[bp-4],ax	; first operand LOW
		mov	[bp-4+2],dx	; first operand HIGH
		fwait			; FPU wait
		fld	dword [bp-4]	; load first operand

; ------------- load second operand

		mov	[bp-4],bx	; second operand LOW
		mov	[bp-4+2],cx	; second operand HIGH
		
; ------------- perform operation

		fwait			; FPU wait
		fdiv	dword [bp-4]	; division

; ------------- get result

		fwait			; FPU wait
		fstp	dword [bp-4]	; store result into stack
		fwait			; FPU wait
		mov	ax,[bp-4]	; AX <- result LOW
		mov	dx,[bp-4+2]	; DX <- result HIGH

; ------------- pop registers FPU

		mov	sp,bp		; return SP

; ------------- infinite result

		mov	si,dx		; SI <- result HIGH
		and	si,7f80h	; SI <- biased exponent
		cmp	si,7f80h	; check infinite value
		jne	short FloatDiv07 ; no infinite value
FloatDiv06:	mov	dx,7f80h	; infinite value
		xor	ax,ax		; AX <- 0

; ------------- denormalized result (biased exponent = 0)

FloatDiv07:	test	dx,7f80h	; is result denormalized or 0?
		jnz	short FloatDiv09 ; result is not denormalized
FloatDiv08:	xor	dx,dx		; DX <- 0
		xor	ax,ax		; AX <- 0
FloatDiv09:	jmp	FloatDiv9

; === SW calculation

; num1: DX:AX, num2: CX:BX
; ------------- prepare result sign -> stack (0 or 8000h), num1L -> SI

FloatDiv1:	xchg	ax,si		; SI <- num1L
		mov	ax,dx		; AX <- num1H
		xor	ax,cx		; AX <- xor with num2H
		and	ax,B15		; AX <- result sign
		push	ax		; push AX

; num1: DX:SI, num2: CX:BX, sign 8000h in stack
; ------------- split num1 to exp1 -> DH, mant1 -> DL:SI

		shl	dx,1		; DX <- shift left
		stc			; prepare high bit = 1
		rcr	dl,1		; DL <- return mant1H, without exp1

; exp1: DH, mant1: DL:SI, num2: CX:BX, sign 8000h in stack
; ------------- split num2 to exp2 -> CH, mant2 -> CL:BX

		shl	cx,1		; CX <- shift left
		stc			; prepare high bit = 1
		rcr	cl,1		; CL <- return mant2H, without exp2

; exp1: DH, mant1: DL:SI, exp2: CH, mant2: CL:BX, sign 8000h in stack
; ------------- exp1 -> CL, mant1H -> BL, mant2H -> BH, mant2L -> DI

		mov	di,bx		; DI <- mant2L
		mov	bl,dl		; BL <- mant1H
		mov	bh,cl		; BH <- mant2H
		mov	cl,dh		; CL <- exp1

; exp1: CL, mant1: BL:SI, exp2: CH, mant2: BH:DI, sign 8000h in stack
; ------------- push exponents, mant1-> BX:SI, mant2 -> CX:DI

		push	cx		; push exponents

		xchg	ax,di		; AX <- mant2L
		mov	ch,bh		; CH <- mant2H
		mov	cl,ah		; CL <- mant2L H
		mov	ah,al		; AH <- mant2L L
		mov	al,0		; AL <- 0
		xchg	ax,di		; DI <- mant2L

		xchg	ax,si		; AX <- mant1L
		mov	bh,bl		; BH <- mant1H
		mov	bl,ah		; BL <- mant1L H
		mov	ah,al		; AH <- mant1L L
		mov	al,0		; AL <- 0
		xchg	ax,si		; SI <- mant1L

; dividend: BX:SI, divisor: CX:DI, stack: exponents and sign
; ------------- divide number -> DX:AX

		xor	dx,dx		; DX <- 0, result HIGH
		xor	ax,ax		; AX <- 0, result LOW
		inc	ax		; AX <- end mark

FloatDiv2:	cmp	bx,cx		; compare operands HIGH
		jne	short FloatDiv22
		cmp	si,di		; compare operands LOW
FloatDiv22:	jb	short FloatDiv24

		sub	si,di		; subtract divisor from dividend
		sbb	bx,cx

FloatDiv24:	cmc			; CF <- 1 if dividend >= divisor
		rcl	ax,1		; AX <- result bit
		rcl	dx,1		; DX <- carry
		jc	short FloatDiv26 ; end

		shr	cx,1		; CX <- shift divisor HIGH
		rcr	di,1		; DI <- shift divisor LOW
		jmp	short FloatDiv2

FloatDiv26:	pop	cx		; CL <- exp1, CH <- exp2

; exp1: CL, exp2: CH, res: DX:AX, sign 8000h in stack
; ------------- subtract exponents

		mov	bh,0		; BH <- 0
		mov	bl,cl		; BL <- exp1
		mov	cl,ch		; CL <- exp2
		mov	ch,0		; CX = exp2
		sub	bx,cx		; BX <- exp1 - exp2
		add	bx,127-1	; BX += base

; exp: BX, res: DX:AX, sign 8000h in stack
; ------------- carry from mantisa

		shl	ax,1		; AX <- shift mantisa
		rcl	dx,1		; DX <- shift mantisa HIGH
		jc	short FloatDiv3	; carry from mantisa
		shl	ax,1		; AX <- shift mantisa if no carry
		rcl	dx,1		; DX <- shift mantisa HIGH
		clc			; NC, no carry
FloatDiv3:	adc	bx,0		; BX <- add carry from mantisa

; ------------- exponent underflow, cut out denormalized result

		cmp	bx,1		; minimal exponent value
		jge	short FloatDiv4	; no underflow
		xor	ax,ax		; AX <- result LOW = 0, underflow flag
		xor	dx,dx		; DX <- result HIGH = 0, underflow flag
		pop	bx		; pop BX
		jmp	short FloatDiv9

; ------------- exponent overflow

FloatDiv4:	cmp	bx,255		; exponent overflow?
		jb	short FloatDiv6	; exponent is OK
FloatDiv5:	xor	ax,ax		; BX <- 0
		mov	dx,7f80h
		pop	bx		; pop BX
		jmp	short FloatDiv9

; ------------- combine exponent with mantisa -> DX:AX

FloatDiv6:	add	ah,1		; AH <- round
		adc	dx,byte 0
		jnc	short FloatDiv7
		shr	dx,1
		rcr	ax,1
		inc	bx		; BX exponent correction on round

FloatDiv7:	mov	al,ah
		mov	ah,dl
		mov	dl,dh
		mov	dh,bl
		shr	dx,1		; DX <- rotate mantisa to position
		rcr	ax,1		; AX <- rotate mantisa to position
		cmp	dx,7f80h	; round overflow?
		je	short FloatDiv5	; overflow while rounding

; ------------- add sign to result

FloatDiv8:	pop	bx		; pop BX (bit 15: sign)
		or	dx,bx		; DX <- add sign

; ------------- pop registers

FloatDiv9:	pop	bp		; pop BP
		pop	di		; pop di
		pop	si		; pop SI
		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                     Floating-point inverse (reciprocal) value
; -----------------------------------------------------------------------------
; INPUT:	DX:AX = operand
; OUTPUT:	DX:AX = result (1/operand)
; -----------------------------------------------------------------------------

FloatInv:	push	bx		; push BX
		push	cx		; push CX

		xchg	ax,bx		; BX <- operand LOW
		mov	cx,dx		; CX <- operand HIGH
		mov	dx,3f80h	; DX <- 1.0 HIGH
		xor	ax,ax		; AX <- 1.0 LOW

		call	FloatDiv	; 1/operand

		pop	cx		; pop CX
		pop	bx		; pop BX
		ret

; -----------------------------------------------------------------------------
;                           Check float arithmetic
; -----------------------------------------------------------------------------

%ifdef DEB_CHECKFPU

; ------------- message

CheckFPU:	mov	bx,FPUMsg1	; BX <- message
		call	DispConTextStr	; display text

		IS_FPU			; is FPU numeric coprocessor
		jnz	short CheckFPU4	; FPU numeric coprocessor OK
		mov	bx,FPUMsgNoFPU	; BX <- message
		call	DispConTextStr	; display text
CheckFPU0:	jmp	short CheckFPU0	; halt system

; ------------- random numbers

CheckFPU4:	call	RandDWord	; generate random DWord
		mov	[FpuM1],ax	; random number M1 LOW
		mov	[FpuM1+2],dx	; random number M1 HIGH

		call	RandDWord	; generate random DWord
		mov	[FpuM2],ax	; random number M2 LOW
		mov	[FpuM2+2],dx	; random number M2 HIGH

; ------------- FloatAdd (not check subtraction with similar exponent)

		mov	ax,[FpuM1+2]	; AX <- number M1 HIGH
		xor	ax,[FpuM2+2]	; check sign
		jns	short CheckFPU41 ; sign is OK
		mov	ax,[FpuM1+2]	; AX <- number M1 HIGH
		mov	dx,[FpuM2+2]	; DX <- number M2 HIGH
		and	ax,7f80h	; AX <- M1 exponent
		and	dx,7f80h	; DX <- M2 exponent
		sub	ax,dx		; compare exponents
		cmp	ax,80h*3
		jg	short CheckFPU41
		cmp	ax,-80h*3
		jge	short CheckFPU42 ; similar exponents

CheckFPU41:	mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		mov	bx,[FpuM2]	; BX <- number M2 LOW
		mov	cx,[FpuM2+2]	; CX <- number M2 HIGH
		call	FloatAdd	; addition HW
		mov	[FpuHW],ax	; store result HW LOW
		mov	[FpuHW+2],dx	; store result HW HIGH

		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> SW
		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatAdd	; addition SW
		mov	[FpuSW],ax	; store result SW LOW
		mov	[FpuSW+2],dx	; store result SW HIGH
		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> HW

		mov	si,FpuDifAdd
		call	CheckFPURes	; check result
		mov	bx,FPUMsgFloatAdd ; BX <- error message
		jc	short CheckFPUErr1 ; error

; ------------- FloatMul

CheckFPU42:	mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		mov	bx,[FpuM2]	; BX <- number M2 LOW
		mov	cx,[FpuM2+2]	; CX <- number M2 HIGH
		call	FloatMul	; multiplication HW
		mov	[FpuHW],ax	; store result HW LOW
		mov	[FpuHW+2],dx	; store result HW HIGH

		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> SW
		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatMul	; multiplication SW
		mov	[FpuSW],ax	; store result SW LOW
		mov	[FpuSW+2],dx	; store result SW HIGH
		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> HW

		mov	si,FpuDifMul
		call	CheckFPURes	; check result
		mov	bx,FPUMsgFloatMul ; BX <- error message
CheckFPUErr1:	jc	short CheckFPUErr2 ; error

; ------------- FloatDiv

		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		mov	bx,[FpuM2]	; BX <- number M2 LOW
		mov	cx,[FpuM2+2]	; CX <- number M2 HIGH
		call	FloatDiv	; division HW
		mov	[FpuHW],ax	; store result HW LOW
		mov	[FpuHW+2],dx	; store result HW HIGH

		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> SW
		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatDiv	; division SW
		mov	[FpuSW],ax	; store result SW LOW
		mov	[FpuSW+2],dx	; store result SW HIGH
		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> HW

		mov	si,FpuDifDiv
		call	CheckFPURes	; check result
		mov	bx,FPUMsgFloatDiv ; BX <- error message
CheckFPUErr2:	jc	short CheckFPUErr3 ; error

; ------------- FloatFromDWord

		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatFromDWord	; import from DWord HW
		mov	[FpuHW],ax	; store result HW LOW
		mov	[FpuHW+2],dx	; store result HW HIGH

		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> SW
		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatFromDWord	; import from DWord SW
		mov	[FpuSW],ax	; store result SW LOW
		mov	[FpuSW+2],dx	; store result SW HIGH
		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> HW

		mov	si,FpuDifFrom
		call	CheckFPURes	; check result
		mov	bx,FPUMsgFloatFrom ; BX <- error message
CheckFPUErr3:	jc	short CheckFPUErr4 ; error

; ------------- FloatToDWord

		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatToDWord	; export to DWord HW
		mov	[FpuHW],ax	; store result HW LOW
		mov	[FpuHW+2],dx	; store result HW HIGH

		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> SW
		mov	ax,[FpuM1]	; AX <- number M1 LOW
		mov	dx,[FpuM1+2]	; DX <- number M1 HIGH
		call	FloatToDWord	; export to DWord SW
		mov	[FpuSW],ax	; store result SW LOW
		mov	[FpuSW+2],dx	; store result SW HIGH
		xor	byte [CPUFlags],CPU_FPU ; FPU flag -> HW

		mov	si,FpuDifTo
		call	CheckFPURes	; check result
		mov	bx,FPUMsgFloatTo ; BX <- error message
CheckFPUErr4:	jc	short CheckFPUErr ; error

; ------------- display statistic

CheckCPU42:	add	word [FpuTrials],1
		adc	word [FpuTrials+2],0

		call	GetDateTimeUnix	; get Unix time
		cmp	ax,[FpuTime]	; compare seconds
		je	short CheckFPU9	; 1 second not elapsed yet
		mov	[FpuTime],ax	; sav new current time

		mov	bx,FPUMsgTrials	; BX <- message Trials
		call	DispConTextStr	; display text
		mov	ax,[FpuTrials]	; AX <- trials LOW
		mov	dx,[FpuTrials+2] ; DX <- trials HIGH
		call	DispConDWord	; display counter

		mov	bx,FPUMsgTrials2 ; BX <- message Trials2
		call	DispConTextStr	; display text
		mov	ax,[FpuDifAdd]	; AX <- trials 2 LOW
		mov	dx,[FpuDifAdd+2] ; DX <- trials 2 HIGH
		call	DispConDWord	; display counter
		call	DispConSpc

		mov	ax,[FpuDifMul]	; AX <- trials 2 LOW
		mov	dx,[FpuDifMul+2] ; DX <- trials 2 HIGH
		call	DispConDWord	; display counter
		call	DispConSpc

		mov	ax,[FpuDifDiv]	; AX <- trials 2 LOW
		mov	dx,[FpuDifDiv+2] ; DX <- trials 2 HIGH
		call	DispConDWord	; display counter
		call	DispConSpc

		mov	ax,[FpuDifFrom]	; AX <- trials 2 LOW
		mov	dx,[FpuDifFrom+2] ; DX <- trials 2 HIGH
		call	DispConDWord	; display counter
		call	DispConSpc

		mov	ax,[FpuDifTo]	; AX <- trials 2 LOW
		mov	dx,[FpuDifTo+2] ; DX <- trials 2 HIGH
		call	DispConDWord	; display counter
		call	DispConSpc

CheckFPU9:	jmp	CheckFPU4	; next step

; ------------- ERROR

CheckFPUErr:	push	bx		; push BX
		mov	bx,FPUMsgErr	; BX <- message ERROR
		call	DispConTextStr	; display text
		pop	bx		; pop BX
		call	DispConTextStr	; display function name

; ------------- display register M1

		mov	bx,FPUMsgM1	; BX <- message M1
		call	DispConTextStr	; display text
		mov	ax,[FpuM1]	; AX <- M1 LOW
		mov	dx,[FpuM1+2]	; DX <- M1 HIGH
		call	DispConHDWord	; display HEX DWord

; ------------- display register M2

		mov	bx,FPUMsgM2	; BX <- message M2
		call	DispConTextStr	; display text
		mov	ax,[FpuM2]	; AX <- M2 LOW
		mov	dx,[FpuM2+2]	; DX <- M2 HIGH
		call	DispConHDWord	; display HEX DWord

; ------------- display register HW

		mov	bx,FPUMsgHW	; BX <- message HW
		call	DispConTextStr	; display text
		mov	ax,[FpuHW]	; AX <- HW LOW
		mov	dx,[FpuHW+2]	; DX <- HW HIGH
		call	DispConHDWord	; display HEX DWord
		
; ------------- display register SW

		mov	bx,FPUMsgSW	; BX <- message SW
		call	DispConTextStr	; display text
		mov	ax,[FpuSW]	; AX <- SW LOW
		mov	dx,[FpuSW+2]	; DX <- SW HIGH
		call	DispConHDWord	; display HEX DWord

		call	DispConNewLine	; new line
CheckFPUHalt:	jmp	short CheckFPUHalt ; halt system

; ------------- check result (CY = error)

CheckFPURes:	mov	ax,[FpuSW]
		mov	dx,[FpuSW+2]
		sub	ax,[FpuHW]
		sbb	dx,[FpuHW+2]

; ------------- check some difference

		jnz	short CheckFPURes0
		or	ax,ax
		jnz	short CheckFPURes0
		ret

CheckFPURes0:	cmp	si,FpuDifFrom
		je	short CheckFPURes22
		cmp	si,FpuDifTo
		je	short CheckFPURes22
		add	word [si],byte 1
		adc	word [si+2],byte 0

; ------------- check results with some toleration

		add	ax,byte 1
		adc	dx,byte 0
		jnz	short CheckFPURes2
		cmp	ax,2
		ja	short CheckFPURes2
CheckFPURes1:	clc
		ret

; ------------- result is near zero boundary

CheckFPURes2:	cmp	word [FpuSW+2],byte 0
		jne	short CheckFPURes3
		cmp	word [FpuHW+2],0080h
		je	short CheckFPURes1
		cmp	word [FpuHW+2],8080h
		je	short CheckFPURes1
CheckFPURes22:	stc
		ret

CheckFPURes3:	cmp	word [FpuHW+2],byte 0
		jne	short CheckFPURes4
		cmp	word [FpuSW+2],0080h
		je	short CheckFPURes1
		cmp	word [FpuSW+2],8080h
		je	short CheckFPURes1
		stc
		ret

; ------------- result is near infinity boundary

CheckFPURes4:	cmp	word [FpuSW+2],7f80h
		jne	short CheckFPURes5
		cmp	word [FpuHW+2],7f7fh
		je	short CheckFPURes1
		cmp	word [FpuHW+2],0ff7fh
		je	short CheckFPURes1
		stc
		ret

CheckFPURes5:	cmp	word [FpuHW+2],7f80h
		jne	short CheckFPURes6
		cmp	word [FpuSW+2],7f7fh
		je	short CheckFPURes1
		cmp	word [FpuSW+2],0ff7fh
		je	short CheckFPURes1
CheckFPURes6:	stc
		ret

%endif

; -----------------------------------------------------------------------------
;                            Constant data
; -----------------------------------------------------------------------------

		CONST_SECTION

%ifdef DEB_CHECKFPU
FPUMsg1:	CTEXT	13,10,"Testing float: Add,Mul,Div,FromDW,ToDW",13,10
FPUMsgNoFPU:	CTEXT	13,10,"No hw FPU unit!"
FPUMsgTrials:	CTEXT	13,"Trials "
FPUMsgTrials2:	CTEXT	", small diff: "
FPUMsgErr:	CTEXT	13,10,"ERROR in "
FPUMsgFloatAdd:	CTEXT	"FloatAdd"
FPUMsgFloatMul:	CTEXT	"FloatMul"
FPUMsgFloatDiv:	CTEXT	"FloatDiv"
FPUMsgFloatFrom:CTEXT	"FloatFromDWord"
FPUMsgFloatTo:	CTEXT	"FloatToDWord"
FPUMsgM1:	CTEXT	": M1="
FPUMsgM2:	CTEXT	" M2="
FPUMsgHW:	CTEXT	" HW="
FPUMsgSW:	CTEXT	" SW="
%endif

; ------------- factorial table n! (35 numbers, coefficient 0..34)

FactTab:	dd	FLOAT_ZERO	; 0: 0
		dd	FLOAT_ONE	; 1: 1
		dd	40000000h	; 2: 2
		dd	40c00000h	; 3: 6
		dd	41c00000h	; 4: 24
		dd	42f00000h	; 5: 120
		dd	44340000h	; 6: 720
		dd	459d8000h	; 7: 5040
		dd	471d8000h	; 8: 40320
		dd	48b13000h	; 9: 362880
		dd	4a5d7c00h	; 10: 3628800
		dd	4c184540h	; 11: 39916800
		dd	4de467e0h	; 12: 4.790016e+008
		dd	4fb99466h	; 13: 6.2270208e+009
		dd	51a261d9h	; 14: 8.7178289e+010
		dd	53983bbch	; 15: 1.3076744e+012
		dd	55983bbch	; 16: 2.0922791e+013
		dd	57a1bf77h	; 17: 3.5568741e+014
		dd	59b5f766h	; 18: 6.4023735e+015
		dd	5bd815cah	; 19: 1.216451e+017
		dd	5e070d9eh	; 20: 2.432902e+018
		dd	603141dfh	; 21: 5.1090941e+019
		dd	6273ba93h	; 22: 1.1240007e+021
		dd	64af2e1ah	; 23: 2.5852017e+022
		dd	67036293h	; 24: 6.2044838e+023
		dd	694d4a06h	; 25: 1.551121e+025
		dd	6ba6cc25h	; 26: 4.0329146e+026
		dd	6e0cbc3fh	; 27: 1.0888869e+028
		dd	7076496fh	; 28: 3.0488835e+029
		dd	72df328ch	; 29: 8.8417619e+030
		dd	75513f63h	; 30: 2.6525285e+032
		dd	77cab568h	; 31: 8.2228384e+033
		dd	7a4ab568h	; 32: 2.6313083e+035
		dd	7cd10b14h	; 33: 8.6833179e+036
		dd	7f5e1bc5h	; 34: 2.952328e+038

; ------------- invert factorial table 1/n! (35 numbers, coefficient 0..34)

InvFactTab:	dd	FLOAT_INF	; 0: NaN
		dd	FLOAT_ONE	; 1: 1
		dd	3f000000h	; 2: 0.5
		dd	3e2aaaabh	; 3: 0.16666667
		dd	3d2aaaabh	; 4: 0.041666668
		dd	3c088889h	; 5: 0.0083333338
		dd	3ab60b61h	; 6: 0.0013888889
		dd	39500d01h	; 7: 0.0001984127
		dd	37d00d01h	; 8: 2.4801588e-005
		dd	3638ef1dh	; 9: 2.7557319e-006
		dd	3493f27eh	; 10: 2.755732e-007
		dd	32d7322bh	; 11: 2.5052108e-008
		dd	310f76c7h	; 12: 2.0876756e-009
		dd	2f309231h	; 13: 1.6059044e-010
		dd	2d49cba5h	; 14: 1.1470745e-011
		dd	2b573f9fh	; 15: 7.6471636e-013
		dd	29573f9fh	; 16: 4.7794773e-014
		dd	274a963ch	; 17: 2.8114574e-015
		dd	253413c3h	; 18: 1.5619207e-016
		dd	2317a4dah	; 19: 8.2206351e-018
		dd	20f2a15dh	; 20: 4.1103176e-019
		dd	1eb8dc78h	; 21: 1.9572942e-020
		dd	1c8671cbh	; 22: 8.896791e-022
		dd	1a3b0da1h	; 23: 3.8681703e-023
		dd	17f96781h	; 24: 1.6117376e-024
		dd	159f9e67h	; 25: 6.4469503e-026
		dd	13447430h	; 26: 2.4795963e-027
		dd	10e8d58eh	; 27: 9.1836898e-029
		dd	0e850c51h	; 28: 3.2798892e-030
		dd	0c12cfcch	; 29: 1.1309962e-031
		dd	099c9963h	; 30: 3.7699878e-033
		dd	0721a697h	; 31: 1.216125e-034
		dd	04a1a697h	; 32: 3.8003907e-036
		dd	021cc093h	; 33: 1.1516336e-037
		dd	FLOAT_ZERO	; 34: 3.387157e-039

; -----------------------------------------------------------------------------
;                            Uninitialised data
; -----------------------------------------------------------------------------

		DATA_SECTION

%ifdef DEB_CHECKFPU
FpuM1:		resd	1
FpuM2:		resd	1
FpuHW:		resd	1
FpuSW:		resd	1
FpuTrials:	resd	1
FpuDifAdd:	resd	1
FpuDifMul:	resd	1
FpuDifDiv:	resd	1
FpuDifFrom:	resd	1
FpuDifTo:	resd	1
FpuTime:	resw	1
%endif
