/**************************************************************************/
/*              Lab11c - Assembly example -Square root extraction         */
/*                                                                        */
/*                                                                        */
/* MSP430 Teaching ROM                                                    */
/* Produced by: MSP430@UBI Group - www.msp430.ubi.pt                      */
/*                                                                        */
/*                                                                        */
/* The assembly routine to perform the calculation of a square root       */
/* should provide the result in a speedy manner and without needing a     */
/* starting point, as in Newton's iterative method.                       */
/*                                                                        */
/* The first step in the manual method of calculating the square root of  */
/* a number is to partition the number into bit pairs, starting with the  */
/* least significant bit;                                                 */
/*                                                                        */
/* The second step consists of adding to the subtraction result the       */
/* following bit pair of the number                                       */
/*                                                                        */
/*                                                                        */
/*                                                                        */
/*                                                                        */
/*                                                                        */
/*                                                                        */
/*                                                                        */
/*                                        Copyright by Texas Instruments  */
/**************************************************************************/
			.cdecls C,LIST,  "msp430x41x.h"		; load C file
;------------------------------------------------------------------------------
            .sect ".sysmem"                     ; Data Space
;------------------------------------------------------------------------------
number		.word	50						; reserve space for one word 

sqrt		.word	0							; reserve space for one word 

count		.char	0x0F						; reserve space for one byte 
;------------------------------------------------------------------------------
            .text                               ; Program Space
;------------------------------------------------------------------------------
			.global BEGIN						; entry point
			
BEGIN:       
			 mov.w   #0A00h,SP               	; initialize stackpointer
		     mov.w   #WDTPW+WDTHOLD,&WDTCTL  	; stop WDT
		     
		     push.w	#number				 		; push data into stak
		     push.w	#sqrt
		     push.w	count	  
		   	
		   	 call	#SQRT_FUNCTION				; call function
		   	 
		   	 ADD	#0x6,SP						; restore SP
		   	
		   	 ret								; exit
;------------------------------------------------------------------------------
; Square root function 
;------------------------------------------------------------------------------
SQRT_FUNCTION:
			push.w	SR						; save  context
			push.w	R4
			push.w	R5
			push.w	R6
			
			push.w	#00						; define temp1
			push.w	#01						; define temp2
			
			mov.w	18(SP),R4				; R4 point to number
			mov.w	16(SP),R5				; R5 point to sqrt
			mov.w #0x0000,0(R5)				; reset sqrt
			
SQRT_LOOP1:
			mov.w	@R4,R6					; take 1º MSB from number
			rla.w	R6
			mov.w	R6,0(R4)
			
			mov.w   2(SP),R6				; put it in temp1
			rlc.w	R6
			mov.w	R6,2(SP)
			
			mov.w	@R4,R6					; take 2º MSB from number
			rla.w	R6
			mov.w	R6,0(R4)
			
			mov.w   2(SP),R6				; put it in temp1
			rlc.w	R6
			
			cmp		#00,R6
			jn		SQRT_ADD				; if temp1 < 0 go SQRT_ADD 
			
SQRT_SUB:									; temp1 = temp1 - temp2
			sub.w	0(SP),R6
			jmp		SQRT_S1
						
SQRT_ADD:									; temp1 = temp1 + temp2
			add.w	0(SP),R6
			mov.w	R6,2(SP)

SQRT_S1:	
			cmp		#00,R6
			jn		SQRT_PRE_ADD
			
SQRT_PRE_SUB:
			mov.w   R6,2(SP)				; save temp1 in stack
			
			mov.w	@R5,R6					; insert 1 in sqrt
			setc
			rlc		R6
			mov		R6,0(R5)				; save sqrt
			rla.w	R6			
			rla.w	R6
			add		#1,R6					; append 01b
			mov.w	R6,0(SP)				; save temp2
			
			mov		14(SP),R6				; test count
			dec		R6
			mov		R6,14(SP)
			cmp 	#00,R6
			jge	 	SQRT_LOOP1
			jmp		SQRT_END
			
SQRT_PRE_ADD:
			mov.w   R6,2(SP)				; save temp1 in stack
			
			mov.w	@R5,R6					; insert 0 in sqrt
			clrc
			rlc		R6
			mov		R6,0(R5)				; save sqrt
			rla.w	R6			
			rla.w	R6
			add		#3,R6					; append 11b
			mov.w	R6,0(SP)				; save temp2
			
			mov		14(SP),R6				; test count
			dec		R6
			mov		R6,14(SP)
			cmp 	#00,R6
			jge	 	SQRT_LOOP1
			
SQRT_END:		
			pop		R6						; restore  context  
			pop		R6	
			pop		R6
			pop		R5
			pop		R4
			pop		SR
			ret								; exit
;------------------------------------------------------------------------------
;           Interrupt Vectors
;------------------------------------------------------------------------------
            .sect   ".reset"                ; MSP430 RESET Vector
            .short  BEGIN
;------------------------------------------------------------------------------
;           
;------------------------------------------------------------------------------            
			.end