/*
SLdt 220620   (Disclaimer: None. Sue me.)

!cave! - Copyright: keine Ahnung, kommt urprünglich aus dem Internet

Assembler: avrasm2

32-bit-Quadratwurzel

Standardregister 'null' mit Inhalt $00
*/

sqrt:                   ; tmp1,0 := sqrt(tmp3,2,1,0)    ca. 300 T     (nach Ruud van Gessel)

#define mr1         r18
#define mr0         r19
#define mo3         tmp0
#define mo2         tmp1
#define mo1         tmp2
#define mo0         tmp3
#define rot_mask0   r20
#define rot_mask1   r21
    push    mr1
    push    mr0
    push    rot_mask0
    push    rot_mask1
    ldi     mr0,$40
    sub     mr1,mr1
    ldi     rot_mask0,$C0
    ldi     rot_mask1,$00
sqrt_loop:
    brcs    pc+4
     cp     mo1,mr1
     cpc    mo0,mr0
     brcs   pc+5

      sub   mo1,mr1
      sbc   mo0,mr0
      or    mr1,rot_mask1
      or    mr0,rot_mask0
    lsr     rot_mask0
    ror     rot_mask1
    eor     mr0,rot_mask0
    eor     mr1,rot_mask1
    rol     mo3
    rol     mo2
    rol     mo1
    rol     mo0
    sbrs    mo3,0
  rjmp      sqrt_loop

#ifdef  C_sqrt_rnd      ; round
    brcs    pc+4
     cp     mr1,mo1
     cpc    mr0,mo0
     brcc   pc+5

      sbc   mo2,null
      sbc   mo1,mr1
      sbc   mo0,mr0
      inc   mr1
    lsl     mo2
    rol     mo1
    rol     mo0
    brcs    pc+3
     cp     mr1,mo1
     cpc    mr0,mo0
    adc     mr1,null
    adc     mr0,null
#else                   ; floor
    brcs  pc+4
     lsl  mo2
     cpc  mr1,mo1
     cpc  mr0,mo0
    adc   mr1,null
#endif
    mov     tmp0,mr1
    mov     tmp1,mr0
    pop     rot_mask1
    pop     rot_mask0
    pop     mr0
    pop     mr1
#undef  mr1
#undef  mr0
#undef  mo3
#undef  mo2
#undef  mo1
#undef  mo0
#undef  rot_mask0
#undef  rot_mask1
    ret