//1 = movw wird unterstüzt, 0 = konvetionelles mov verwenden .set useMOVW = 1 //1 = Geschwindigkeitsoptimierte Version (~600clk, ~1300byte) //0 = Platzoptimierte Version (~1300 clk, ~150byte) .set useSpeedOptimized = 1 //1 = sichert die verwendeten Register auf dem Stack .set saveRegs = 0 wurzel32fast: /* ####################################################################### Based on an Algorithm by Jim Ulery http://www.azillionmonkeys.com/qed/ulerysqroot.pdf Ported to ASM by Läubi, 2008 Input r5:r4:r3:r2 Output r1:r0 ! Achtung! Die Eingabe wird hierbei zerstört! ! Wenn diese noch benötigt wird vorher sichern! C-Version: static unsigned julery_isqrt(unsigned long val) { unsigned long temp, g=0, b = 0x8000, bshft = 15; do { if ( val >= ( temp = (((g << 1) + b)<>= 1); return g; } ####################################################################### */ //Macros .macro WURZEL_SHIFTLEFT //@0 @1 @2 @3 << 1 lsl @3 rol @2 rol @1 rol @0 .endmacro .macro WURZEL_SHIFTRIGHT //@0 @1 @2 @3 >> 1 lsr @0 ror @1 ror @2 ror @3 .endmacro .macro WURZEL_RORRIGHT ror @0 ror @1 ror @2 ror @3 .endmacro //shift um 0 ... es wird also nur gerechnet .macro WURZEL_lshift0 //--> 6/7 clocks //Highbytes clearen clr r19 clr r18 //temp = g .if useMOVW == 1 movw r17:r16, YH:YL .else mov r17, YH mov r16, YL .endif //temp = temp + (b>>1) subi r16, BYTE1(~@0+1) sbci r17, BYTE2(~@0+1) sbci r18, BYTE3(~@0+1) //sbci r19, BYTE3(~@0+1) //es gibt hier kein weiteres carry! //Wir sind also fertig .endmacro //es wird gerechnet und um 8 geschoben .macro WURZEL_lshift8 //--> 7 clocks //r16 wird 0 sein wegen schieben um 8 nach links am ende clr r16 //Highbyte clearen clr r19 //temp = g (allerdings schon um 8 geschoben) //movw können wir hier leider nicht einsetzen mov r18, YH mov r17, YL //temp = temp + (b>>1) (allerdings schon um 8 geschoben) subi r17, BYTE1(~@0+1) sbci r18, BYTE2(~@0+1) sbci r19, BYTE3(~@0+1) //temp ist schon automatisch geschoben! //Wir sind also fertig .endmacro //es wird gerechnet und um 16 geschoben .macro WURZEL_lshift16 //--> 5/6 clocks //r17:r16 wird 0 sein wegen schieben um 16 nach links am ende clr r16 clr r17 //temp = g (allerdings schon um 16 geschoben) .if useMOVW == 1 movw r19:r18, YH:YL .else mov r19, YH mov r18, YL .endif //temp = temp + (b>>1) (allerdings schon um 16 geschoben) subi r18, LOW(~@0+1) sbci r19, HIGH(~@0+1) //temp ist schon automatisch um 16 geschoben! //Wir sind also fertig .endmacro /* Outerlopp umgeschrieben zu temp = g; temp = temp + (b >> 1); if (b == 1) { temp = temp << 1; temp = temp|1; //oder temp+1 temp = temp << bshft; } else { temp = temp <<(bshft+1); } if ( val >= temp) { g += b; val -= temp; } Auf diese Weise können wir zuweisung addition und shift zusammenfassen */ // @0 = bshft .macro WURZEL32_OUTERLOOP //X = b >> 1 //wir schieben leztendlich um @0+1 = 8 .if @0 == 0 //sonderfall, hier schieben wir eine 1 von b raus //die müssen wir später wieder reinschieben //dafür müssen wir b auch nicht auf temp addieren //Highbytes clearen clr r19 clr r18 //temp = g .if useMOVW == 1 movw r17:r16, YH:YL .else mov r17, YH mov r16, YL .endif //temp = temp + b entfällt (b >> 1 = 0) WURZEL_SHIFTLEFT r19, r18, r17, r16 //"rausgeschobene 1" addieren //es gibt keinen übertrag da wir gerade um einen nach links //geschoben haben ori r16, 1 .elif @0 == 1 WURZEL_lshift0 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 2 WURZEL_lshift0 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 3 WURZEL_lshift0 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 4 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 5 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 6 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 7 WURZEL_lshift8 (@1>>1) .elif @0 == 8 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 9 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 10 WURZEL_lshift8 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 .elif @0 == 11 /*WURZEL_lshift8 (@1>>1) WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16 WURZEL_SHIFTLEFT r19, r18, r17, r16*/ WURZEL_lshift16 (@1>>1) WURZEL_RORRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 12 WURZEL_lshift16 (@1>>1) WURZEL_RORRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 13 WURZEL_lshift16 (@1>>1) WURZEL_RORRIGHT r19, r18, r17, r16 WURZEL_SHIFTRIGHT r19, r18, r17, r16 .elif @0 == 14 WURZEL_lshift16 (@1>>1) WURZEL_RORRIGHT r19, r18, r17, r16 .elif @0 == 15 WURZEL_lshift16 (@1>>1) .else //das sollte nicht passieren, alle Fälle sollten oben abgehandelt sein! .error "ooops!!!" .endif //val >= temp ? cp r2, r16 cpc r3, r17 cpc r4, r18 cpc r5, r19 brlo wurzel32_2f_wd //g + = b //2-complement = invers+1 subi YL, LOW(~@1+1) sbci YH, HIGH(~@1+1) //val -=temp sub r2, r16 sbc r3, r17 sbc r4, r18 sbc r5, r19 wurzel32_2f_wd: .endmacro //function .if useSpeedOptimized == 1 //Register sichern .if saveRegs == 1 push XL push XH push YL push YH push r16 push r17 push r18 push r19 .endif //YH:YL = g = 0 clr YL clr YH //unrooling outer loop @0 = shiftweite, @1 = b WURZEL32_OUTERLOOP 15, (0x8000 >> 0) WURZEL32_OUTERLOOP 14, (0x8000 >> 1) WURZEL32_OUTERLOOP 13, (0x8000 >> 2) WURZEL32_OUTERLOOP 12, (0x8000 >> 3) WURZEL32_OUTERLOOP 11, (0x8000 >> 4) WURZEL32_OUTERLOOP 10, (0x8000 >> 5) WURZEL32_OUTERLOOP 9, (0x8000 >> 6) WURZEL32_OUTERLOOP 8, (0x8000 >> 7) WURZEL32_OUTERLOOP 7, (0x8000 >> 8) WURZEL32_OUTERLOOP 6, (0x8000 >> 9) WURZEL32_OUTERLOOP 5, (0x8000 >> 10) WURZEL32_OUTERLOOP 4, (0x8000 >> 11) WURZEL32_OUTERLOOP 3, (0x8000 >> 12) WURZEL32_OUTERLOOP 2, (0x8000 >> 13) WURZEL32_OUTERLOOP 1, (0x8000 >> 14) WURZEL32_OUTERLOOP 0, (0x8000 >> 15) //Register wieder herstellen mov r0, YL mov r1, YH .if saveRegs == 1 pop r19 pop r18 pop r17 pop r16 pop YH pop YL pop XH pop XL .endif ret .else //Register sichern .if saveRegs == 1 push XL push XH push r17 push r16 push r6 push r7 push r8 push r9 push r10 .endif //X = b ldi XL, LOW(0x8000) ldi XH, HIGH(0x8000) //r1:r0 = g = 0 clr r0 clr r1 //temp = bshft ldi r17, 15 clr r16 wurzel32_2_wloop: //temp = g clr r9 clr r8 mov r7, r1 mov r6, r0 //g << 1 lsl r6 rol r7 rol r8 //leztes rol kann ausgelassen werden da g maximal 16 bit breit //und r9 = 0 //+b add r6, XL adc r7, XH adc r8, r16 adc r9, r16 // << bshft mov r10, r17 wurzel32_2_wl: lsl r6 rol r7 rol r8 rol r9 dec r17 brne wurzel32_2_wl //bshft-- mov r17, r10 dec r17 //val >= temp cp r2, r6 cpc r3, r7 cpc r4, r8 cpc r5, r9 brlo wurzel32_2_wd //g + = b add r0, XL adc r1, XH //val -=temp sub r2, r6 sbc r3, r7 sbc r4, r8 sbc r5, r9 //b >> 1 != 0? wurzel32_2_wd: lsr XH ror XL cpi XL, 1 cpc XH, r16 brne wurzel32_2_wloop //Register wieder herstellen .if saveRegs == 1 pop r10 pop r9 pop r8 pop r7 pop r6 pop r16 pop r17 pop XH pop XL .endif ret .endif