Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

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