Subversion Repositories FlightCtrl

   1                    # 1 "isqrt.S"
   1                    ;-----------------------------------------------------------------------------;
   0                    
   0                    
   2                    ;-----------------------------------------------------------------------------;
   3                    ; Fast integer squareroot routines for avr-gcc project          (C)ChaN, 2008
   4                    ; http://elm-chan.org/docs/avrlib/sqrt32.S
   5                    ;-----------------------------------------------------------------------------;
   6                    ; uint16_t isqrt32 (uint32_t n);
   7                    ; uint8_t isqrt16 (uint16_t n);
   8                    ; uint16_t ihypot (int16_t x, int16_t y);
   9                    
  10                    ;-----------------------------------------------------------------------------:
  11                    ; 32bit integer squareroot
  12                    ;-----------------------------------------------------------------------------;
  13                    ;   uint16_t isqrt32 (
  14                    ;     uint32_t n
  15                    ;   );
  16                    ;
  17                    ; Return Value:
  18                    ;   Squareroot of n.
  19                    ;
  20                    ; Size  = 53 words
  21                    ; Clock = 532..548 cycles
  22                    ; Stack = 0 byte
  23                    
  24                    .global isqrt32
  25                    .func isqrt32
  26                    
  27:isqrt.S       **** isqrt32:
  28:isqrt.S       ****         clr     r0
  29:isqrt.S       ****         clr     r18
  30:isqrt.S       ****         clr     r19
  31:isqrt.S       ****         clr     r20
  32:isqrt.S       ****         ldi     r21, 1
  33:isqrt.S       ****         clr     r27
  34:isqrt.S       ****         clr     r30
  35:isqrt.S       ****         clr     r31
  36:isqrt.S       ****         ldi     r26, 16
  37:isqrt.S       **** 1:      lsl     r22
  38:isqrt.S       ****         rol     r23
  39:isqrt.S       ****         rol     r24
  40:isqrt.S       ****         rol     r25
  41:isqrt.S       ****         rol     r0
  42:isqrt.S       ****         rol     r18
  43:isqrt.S       ****         rol     r19
  44:isqrt.S       ****         rol     r20
  45:isqrt.S       ****         lsl     r22
  46:isqrt.S       ****         rol     r23
  47:isqrt.S       ****         rol     r24
  48:isqrt.S       ****         rol     r25
  49:isqrt.S       ****         rol     r0
  50:isqrt.S       ****         rol     r18
  51:isqrt.S       ****         rol     r19
  52:isqrt.S       ****         rol     r20
  53:isqrt.S       ****         brpl    2f
  54:isqrt.S       ****         add     r0, r21
  55:isqrt.S       ****         adc     r18, r27
  56:isqrt.S       ****         adc     r19, r30
  57:isqrt.S       ****         adc     r20, r31
  58:isqrt.S       ****         rjmp    3f
  59:isqrt.S       **** 2:      sub     r0, r21
  60:isqrt.S       ****         sbc     r18, r27
  61:isqrt.S       ****         sbc     r19, r30
  62:isqrt.S       ****         sbc     r20, r31
  63:isqrt.S       **** 3:      lsl     r21
  64:isqrt.S       ****         rol     r27
  65:isqrt.S       ****         rol     r30
  66:isqrt.S       ****         andi    r21, 0b11111000
  67:isqrt.S       ****         ori     r21, 0b00000101
  68:isqrt.S       ****         sbrc    r20, 7
  69:isqrt.S       ****         subi    r21, 2
  70:isqrt.S       ****         dec     r26
  71:isqrt.S       ****         brne    1b
  72:isqrt.S       ****         lsr     r30
  73:isqrt.S       ****         ror     r27
  74:isqrt.S       ****         ror     r21
  75:isqrt.S       ****         lsr     r30
  76:isqrt.S       ****         ror     r27
  77:isqrt.S       ****         ror     r21
  78:isqrt.S       ****         mov     r24, r21
  79:isqrt.S       ****         mov     r25, r27
  80                            ret
  81                    .endfunc
  82                    
  83                    
  84                    
  85                    ;-----------------------------------------------------------------------------:
  86                    ; 16bit integer squareroot
  87                    ;-----------------------------------------------------------------------------;
  88                    ;   uint8_t isqrt16 (
  89                    ;     uint16_t n
  90                    ;   );
  91                    ;
  92                    ; Return Value:
  93                    ;   Squareroot of n.
  94                    ;
  95                    ; Size  = 33 words
  96                    ; Clock = 181..189 cycles
  97                    ; Stack = 0 byte
  98                    
  99                    .global isqrt16
 100                    .func isqrt16
 101                    
 102:isqrt.S       **** isqrt16:
 103:isqrt.S       ****         clr     r18
 104:isqrt.S       ****         clr     r19
 105:isqrt.S       ****         ldi     r20, 1
 106:isqrt.S       ****         clr     r21
 107:isqrt.S       ****         ldi     r22, 8
 108:isqrt.S       **** 1:      lsl     r24
 109:isqrt.S       ****         rol     r25
 110:isqrt.S       ****         rol     r18
 111:isqrt.S       ****         rol     r19
 112:isqrt.S       ****         lsl     r24
 113:isqrt.S       ****         rol     r25
 114:isqrt.S       ****         rol     r18
 115:isqrt.S       ****         rol     r19
 116:isqrt.S       ****         brpl    2f
 117:isqrt.S       ****         add     r18, r20
 118:isqrt.S       ****         adc     r19, r21
 119:isqrt.S       ****         rjmp    3f
 120:isqrt.S       **** 2:      sub     r18, r20
 121:isqrt.S       ****         sbc     r19, r21
 122:isqrt.S       **** 3:      lsl     r20
 123:isqrt.S       ****         rol     r21
 124:isqrt.S       ****         andi    r20, 0b11111000
 125:isqrt.S       ****         ori     r20, 0b00000101
 126:isqrt.S       ****         sbrc    r19, 7
 127:isqrt.S       ****         subi    r20, 2
 128:isqrt.S       ****         dec     r22
 129:isqrt.S       ****         brne    1b
 130:isqrt.S       ****         lsr     r21
 131:isqrt.S       ****         ror     r20
 132:isqrt.S       ****         lsr     r21
 133:isqrt.S       ****         ror     r20
 134:isqrt.S       ****         mov     r24, r20
 135                            ret
 136                    .endfunc
 137                    
 138                    
 139                    
 140                    ;-----------------------------------------------------------------------------:
 141                    ; 16bit integer hypot (megaAVR is required)
 142                    ;-----------------------------------------------------------------------------;
 143                    ;   uint16_t ihypot (
 144                    ;     int16_t x,
 145                    ;     int16_t y
 146                    ;   );
 147                    ;
 148                    ; Return Value:
 149                    ;   Squareroot of (x*x + y*y)
 150                    ;
 151                    ; Size  = 42 words
 152                    ; Clock = 581..597 cycles
 153                    ; Stack = 0 byte
 154                    
 155                    .global ihypot
 156                    .func ihypot
 157                    
 158:isqrt.S       **** ihypot:
 159:isqrt.S       ****         clr     r26
 160:isqrt.S       ****         sbrs    r25, 7
 161:isqrt.S       ****         rjmp    1f
 162:isqrt.S       ****         com     r24
 163:isqrt.S       ****         com     r25
 164:isqrt.S       ****         adc     r24, r26
 165:isqrt.S       ****         adc     r25, r26
 166:isqrt.S       **** 1:      sbrs    r23, 7
 167:isqrt.S       ****         rjmp    2f
 168:isqrt.S       ****         com     r22
 169:isqrt.S       ****         com     r23
 170:isqrt.S       ****         adc     r22, r26
 171:isqrt.S       ****         adc     r23, r26
 172:isqrt.S       **** 2:      mul     r22, r22
 173:isqrt.S       ****         movw    r18, r0
 174:isqrt.S       ****         mul     r23, r23
 175:isqrt.S       ****         movw    r20, r0
 176:isqrt.S       ****         mul     r22, r23
 177:isqrt.S       ****         add     r19, r0
 178:isqrt.S       ****         adc     r20, r1
 179:isqrt.S       ****         adc     r21, r26
 180:isqrt.S       ****         add     r19, r0
 181:isqrt.S       ****         adc     r20, r1
 182:isqrt.S       ****         adc     r21, r26
 183:isqrt.S       ****         mul     r24, r24
 184:isqrt.S       ****         movw    r30, r0
 185:isqrt.S       ****         mul     r25, r25
 186:isqrt.S       ****         add     r18, r30
 187:isqrt.S       ****         adc     r19, r31
 188:isqrt.S       ****         adc     r20, r0
 189:isqrt.S       ****         adc     r21, r1
 190:isqrt.S       ****         mul     r24, r25
 191:isqrt.S       ****         add     r19, r0
 192:isqrt.S       ****         adc     r20, r1
 193:isqrt.S       ****         adc     r21, r26
 194:isqrt.S       ****         add     r19, r0
 195:isqrt.S       ****         adc     r20, r1
 196:isqrt.S       ****         adc     r21, r26
 197:isqrt.S       ****         movw    r24, r20
 198:isqrt.S       ****         movw    r22, r18
 199:isqrt.S       ****         clr     r1
 200                            rjmp    isqrt32
DEFINED SYMBOLS
             isqrt.S:26     .text:00000000 isqrt32
             isqrt.S:101    .text:0000006a isqrt16
             isqrt.S:157    .text:000000ac ihypot

NO UNDEFINED SYMBOLS