;***************************************************************************
;
sqrt32 sqrt(r16:r17:r18:r19)=r0:r1
; work r2-r8,r20-r22
;***************************************************************************
sqrt32:
clr r0 ;BH
clr r1 ;BL
;clr r20 ;B1H
;clr r21 ;B2L
clr r3 ;C=r3:r4:r5:r6
clr r4
clr r5
clr r6
clr r7 ;W=r7:r8
clr r8
inc r8
sqrt32_1:
tst r16
breq sqrt32_2
ldi r20,0x80
clr r21
rjmp sqrt32_5
sqrt32_2:
tst r17
breq sqrt32_3
ldi r20,0x08
clr r21
mov r16,r17
mov r17,r18
mov r18,r19
clr r19
rjmp sqrt32_5
sqrt32_3:
tst r18
breq sqrt32_4
ldi r21,0x80
clr r20
mov r16,r18
mov r17,r19
clr r18
clr r19
rjmp sqrt32_5
sqrt32_4:
tst r19
breq sqrt32_e
ldi r21,0x08
clr r20
mov r16,r19
clr r17
clr r18
clr r19
sqrt32_5:
ldi r22,2
sqrt32_51:
lsl r19
rol r18
rol r17
rol r16
rol r6
rol r5
rol r4
rol r3
dec r22
brne sqrt32_51
clr r22
cp r6,r8 ;C:W
cpc r5,r7
cpc r4,r22
cpc r3,r22
brcs sqrt32_6 ;C<W
sub r6,r8 ;C>=W
sbc r5,r7
sbc r4,r22
sbc r3,r22
add r1,r21
add r0,r20
lsl r8
rol r7
ldi r22,3
add r8,r22
ldi r22,0
adc r7,r22
rjmp sqrt32_7
sqrt32_6:
lsl r8
rol r7
ldi r22,0
dec r8
sbc r7,r22
sqrt32_7:
lsr r20
ror r21
tst r20
brne sqrt32_5
tst r21
brne sqrt32_5
sqrt32_e:
ret