16-bit Square Root

This subroutine will calculate a square value of 16-bit value at r3:r2. Result is 16-bit at r17:r16 and will be rounded to nearest integer (0.5 rounds up). The value at R3:R2 will be changed.
Cycles include call & return: 95 - 102.

R17:R16 = sqrt(R3:R2)
sqrt16:

    ldi   R17,0xc0     

    ldi   R16,0x40     

    clc                

sqrt16_1:

    brcs  sqrt16_2     

    cp    R3,R16       

    brcs  sqrt16_3     

sqrt16_2:

    sub   R3,R16       

    or    R16,R17      

sqrt16_3:

    lsr   R17          

    lsl   R2

    rol   R3           

    eor   R16,R17      

    andi  R17,0xfe     

    brne  sqrt16_1    

    brcs  sqrt16_4    

    cp    R16,R3      

    brcc  sqrt16_5    

sqrt16_4:

    sbc   R2,R17          

    sbc   R3,R16          

    inc   R16             

sqrt16_5:

    lsl   R2              

    rol   R3               

    brcs  sqrt16_6         

    cp    R16,R3           

sqrt16_6:

    adc   R16,R17          

    ret