; ---------------------------------------------------------------------------

;   Project   : A Fast High Precision Fixed Point Divide
;   Author    : Henry Thomas <http://henri.net>
;   Creation  : 12/7/2005
;   Copyright 2005 Henry Thomas

; ---------------------------------------------------------------------------


	AREA |.text|, CODE
	EXPORT HAL_fpdiv
	GBLA loopCount


num RN 0						; Map register numbers
den RN 1
mod RN 2
cnt RN 3
quo RN 4
sign RN 12

fpdiv

	cmp den, #0					; exception if den == zero
	beq div0

	eor sign, num, den			; sign = num ^ den
	rsbmi den, den, #0			; den = -den if den < 0

	subs mod, den, #1			; mod = den - 1
	beq div1					; return if den == 1

	movs cnt, num				; num = -num if num < 0
	rsbmi num, num, #0

	movs cnt, num, lsr #16		; return if den >= num << 16
	bne cont
	cmp den, num, lsl #16
	bhs numLeDen
cont

	tst den, mod				; if(den & (den - 1) == 0)
	beq powerOf2				; den is power of 2

	stmfd sp!, {r4}				; push r4 (quo) onto the stack

	mov cnt, #28				; count difference in leading zeros
	mov mod, num, lsr #4		; between num and den

	cmp den, mod, lsr #12
	subls cnt, cnt, #16
	movls mod, mod, lsr #16

	cmp den, mod, lsr #4
	subls cnt, cnt, #8
	movls mod, mod, lsr #8

	cmp den, mod
	subls cnt, cnt, #4
	movls mod, mod, lsr #4

	mov num, num, lsl cnt		; mod:num = num << cnt

	mov quo, #0
	rsb den, den, #0			; negate den for divide loop

	adds num, num, num			; start: num = mod:num / den
	add pc, pc, cnt, lsl #4		; skip cnt x 4 x 4 iterations
	nop							; nop instruction to take care of pipelining

loopCount SETA 47

	WHILE loopCount>0			; inner loop x 48

	adcs mod, den, mod, lsl #1
	subcc mod, mod, den
	adc quo, quo, quo
	adds num, num, num

loopCount SETA loopCount-1

	WEND

	adcs mod, den, mod, lsl #1
	subcc mod, mod, den
	adc quo, quo, quo

	cmp sign, #0				; negate quotient if sign < 0
	mov num, quo
	rsbmi num, num, #0
	ldmfd sp!, {r4}				; pop r4 (quo) off the stack
	mov pc, lr

div0
	mov num, #0
	mov pc, lr

div1
	cmp sign, #0
	mov num, num, asl #16
	rsbmi num, num, #0
	mov pc, lr

numLeDen
	mov num, #0					; quotient = 0 if num < den
	moveq num, sign, asr #31	; negate quotient if sign < 0
	orreq num, num, #1			; quotient = 1 if num == den
	mov pc, lr

powerOf2
	mov cnt, #0

	cmp den, #65536
	movhs cnt, #16
	movhs den, den, lsr #16

	cmp den, #256
	addhs cnt, cnt, #8
	movhs den, den, lsr #8

	cmp den, #16
	addhs cnt, cnt, #4
	movhs den, den, lsr #4

	cmp den, #4
	addhi cnt, cnt, #3
	addls cnt, cnt, den, lsr #1

	rsb mod, cnt, #32

	mov den, num, lsr #16		; den:num = num << 16
	mov num, num, lsl #16

	mov num, num, lsr cnt		; num = num >> cnt | den << mod
	orr num, num, den, lsl mod

	cmp sign, #0
	rsbmi num, num, #0			; negate quotient if sign < 0
	mov pc, lr

	END