WebSVN - FlightCtrl - Blame - Rev 1388 - /branches/V0.76g-Arthur P/isqrt.S

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

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-Arthur P/isqrt.S - Rev 1388