  | 
                              
                                 
                                 
                                  
                                    ;***************************************************************************
                                    ; 
                                    ; File Name		:'Main.asm"
                                    ; Title			:Matmatikai rutinok, test verzio 
                                    ; Date			:2003.07.16.
                                    ; Version		:1.0.0
                                    ; Support telephone	:+36-70-333-4034
                                    ; Support fax		:
                                    ; Support Email		:info@vfx.hu
                                    ; Target MCU		:ATmega128
                                    ; 
                                    ;***************************************************************************
                                    ;	D E S C R I P T I O N
                                    ;
                                    ;
                                    ;***************************************************************************
                                    ;	M O D I F I C A T I O N   H I S T O R Y 
                                    ;
                                    ;
                                    ;       rev.      date      who 	why
                                    ;	----	----------  ---		-----------------------------------
                                    ;	0.01	2003.07.16  VFX		Creation, mult
                                    ;	0.02	2004.03.30  VFX		Add float add & sub 
                                    ;
                                    ;
                                    ;***************************************************************************
                                    ;Hardware
                                    ;***************************************************************************
                                    ;*
                                    ;*	f=external 16MHz (t=-.--ns)
                                    ;*
                                    ;***************************************************************************
                                    ;
                                    ;
                                    ;
                                    .include "m128def.inc"
                                    
                                    ;***************************************************************************
                                    ;* Const Def
                                    
                                    
                                    ;**************************************************************************
                                    ;* Hardware Def.
                                    ;
                                    
                                    ;***************************************************************************
                                    ;** VARIABLES
                                    .DSEG
                                    
                                    ;***************************************************************************
                                    .ESEG
                                    
                                    ;***************************************************************************
                                    ;****		I N T E R R U P T S
                                    ;****
                                    ;***************************************************************************
                                    .CSEG
                                    
                                    RESET:		CLI			;GLOBAL INTERRUP DIS.	
                                    
                                    		ldi R16, high(RAMEND)	;setup stack
                                    		out SPH,R16
                                    		ldi R16, low(RAMEND)
                                    		out SPL,R16
                                    
                                    
                                    ;PI/2 = 81 49 0F DA A2		Sign bit!!!
                                    ;1,570796326734125614166259765625
                                    
                                     		ldi	R24,0x81	;exponens
                                    		ldi	R23,0xC9	;mantissa
                                    		ldi	R22,0x0F
                                    		ldi	R21,0xDA
                                    		ldi	R20,0xA2
                                    		mov	R6,R24
                                    		mov	R5,R23
                                    		mov	R4,R22
                                    		mov	R3,R21
                                    		mov	R2,R20
                                    
                                    ; const 
                                    ;0,79411764699034392833709716796875
                                    
                                    		ldi	R24,0x80			;exponens
                                    		ldi	R23,0x4b			;mantissa
                                    		ldi	R22,0x4b
                                    		ldi	R21,0x4b
                                    		ldi	R20,0x4b
                                    
                                    
                                    ;		rcall	multiply
                                    
                                    		rcall	addition
                                    
                                    veges:
                                    		rjmp veges
                                    
                                    
                                    ; -----------------------------
                                    ; Prepare to multiply or divide
                                    ; -----------------------------
                                    ; This routine is called in succession from multiply and divide to prepare
                                    ; two mantissas by setting the leftmost bit that is used for the sign.
                                    ;  m1: R25, R24:R21
                                    ;  m2:  R6, R5:R2
                                    ;
                                    ;  T flag = Sign of result
                                    ;  R6,R24 corrected exponent
                                    ;  R5,R23 restored sign bit of mantissa
                                    ;
                                    ;   cycles +  (RET) =  Cycles
                                    
                                    PREP_Mul_Div:
                                    		mov		R16,R5
                                    		eor		R16,R23
                                    		bst		R16,7		;T flag = result sign bit
                                    
                                    		ldi		R16,0xA0	;Binary point offset is 0x20 to integer number
                                    		ldi		R17,0x80	;MSB sign bit
                                    
                                    		tst		R24
                                    		brne	PREP_MD01	;First number is floating point format
                                    		add		R24,R16
                                    		rjmp	PREP_MD02
                                    
                                    PREP_MD01:
                                    		or		R23,R17		;restore msb bit
                                    PREP_MD02:		
                                    		tst		R6
                                    		brne	PREP_MD03	;Second number is floating point format
                                    		add		R6,R16
                                    		rjmp	PREP_MD04
                                    
                                    PREP_MD03:
                                    		or		R5,R17		;restore msb bit
                                    PREP_MD04:		
                                    		sub		R24,R17
                                    		sub		R6,R17		;shift zero point by 0x80 (back to 2's complement)
                                    		ret
                                    
                                    ; ------------------------------
                                    ; THE 'MULTIPLICATION' OPERATION
                                    ; ------------------------------
                                    ; (offset: $04 'multiply')
                                    ;
                                    ;  m1:  R6, R5:R2
                                    ;  m2: R24, R23:R20
                                    ;  -------------------
                                    ; Res:  R6, R5:R2
                                    ;
                                    ;   cycles +  (RET) =  Cycles
                                    ;
                                    multiply:
                                    		rcall	PREP_Mul_Div
                                    
                                    		mov		R0,R2
                                    		or		R0,R3
                                    		or		R0,R4
                                    		or		R0,R5
                                    		breq	Res_Zero
                                    		mov		R0,R20
                                    		or		R0,R21
                                    		or		R0,R22
                                    		or		R0,R23
                                    		breq	Res_Zero
                                    
                                    		rcall	mult32
                                    		clr		R7
                                    		rcall	Normalize
                                    		sub		R6,R16			;shift exponent by normalized value
                                    		brcs	NumberTooBig
                                    		add		R6,R24			;2^x *2^y = 2^(x+y)
                                    		brvs	NumberTooBig
                                    		ldi		R16,0x80
                                    		sub		R6,R16			;set offset by 0x80
                                    
                                    		rcall	Round
                                    		brcs	NumberTooBig
                                    
                                    		ldi		R17,0xFF
                                    		bld		R17,7			;get result sign bit
                                    		and		R5,R17			;set result sign bit
                                    
                                    		clc
                                    		ret
                                    NumberTooBig:
                                    		sec
                                    		ret
                                    
                                    Res_Zero:
                                    		clr		R2
                                    		clr		R3
                                    		movw	R4,R2
                                    		clr		R6
                                    		clc
                                    		ret
                                    	
                                    
                                    
                                    
                                    ;***************************************************
                                    ;* Mutiply 32x32 -> 64 bit
                                    ;* 
                                    ;*  R23:R20 x R5:R2 -> R15:R8
                                    ;*
                                    ;*  86 cycles + 4 (RET) = 90 Cycles
                                    ;*
                                    mult32:
                                    		clr	R16
                                    		mul	R20,R2
                                    		movw	R8,R0
                                    		clr	R10
                                    		clr	R11
                                    		movw R12,R10
                                    		movw R14,R10
                                    		mul	R21,R2
                                    		add	R9,R0
                                    		adc	R10,R1
                                    		mul	R22,R2
                                    		add	R10,R0
                                    		adc	R11,R1
                                    		mul	R23,R2
                                    		add	R11,R0
                                    		adc	R12,R1
                                    
                                    		mul	R20,R3
                                    		add	R9,R0
                                    		adc	R10,R1
                                    		adc	R11,R16
                                    		adc	R12,R16
                                    		adc	R13,R16
                                    		mul	R21,R3
                                    		add	R10,R0
                                    		adc	R11,R1
                                    		adc	R12,R16
                                    		adc	R13,R16
                                    		mul	R22,R3
                                    		add	R11,R0
                                    		adc	R12,R1
                                    		adc	R13,R16
                                    		mul	R23,R3
                                    		add	R12,R0
                                    		adc	R13,R1
                                    
                                    		mul	R20,R4
                                    		add	R10,R0
                                    		adc	R11,R1
                                    		adc	R12,R16
                                    		adc	R13,R16
                                    		adc	R14,R16
                                    		mul	R21,R4
                                    		add	R11,R0
                                    		adc	R12,R1
                                    		adc	R13,R16
                                    		adc	R14,R16
                                    		mul	R22,R4
                                    		add	R12,R0
                                    		adc	R13,R1
                                    		adc	R14,R16
                                    		mul	R23,R4
                                    		add	R13,R0
                                    		adc	R14,R1
                                    
                                    		mul	R20,R5
                                    		add	R11,R0
                                    		adc	R12,R1
                                    		adc	R13,R16
                                    		adc	R14,R16
                                    		adc	R15,R16
                                    		mul	R21,R5
                                    		add	R12,R0
                                    		adc	R13,R1
                                    		adc	R14,R16
                                    		adc	R15,R16
                                    		mul	R22,R5
                                    		add	R13,R0
                                    		adc	R14,R1
                                    		adc	R15,R16
                                    		mul	R23,R5
                                    		add	R14,R0
                                    		adc	R15,R1
                                    		ret
                                    
                                    
                                    
                                    ;********************************************************** 
                                    ; R15:R8:R7 -> R15:R12
                                    ;           R16 - 64-exponent
                                    Normalize:
                                    		ldi		R17,64		;max 64 bitet kell vegigrotalni
                                    Norm0:	tst		R15
                                    		brne	Norm1
                                    		mov		R15,R14
                                    		mov		R14,R13
                                    		mov		R13,R12
                                    		mov		R12,R11
                                    		mov		R11,R10
                                    		mov		R10,R9
                                    		mov		R9,R8
                                    		mov		R8,R7
                                    		clr		R7
                                    		subi	R17,8
                                    		cpi		R17,8
                                    		brne	Norm0
                                    Norm1:
                                    								;legnagyobb helyiertek felrotalasa bit7-ig
                                    		sbrc	R15,7
                                    		 rjmp	Norm2
                                    		lsl		R7
                                    		rol		R8
                                    		rol		R9
                                    		rol		R10
                                    		rol		R11
                                    		rol		R12
                                    		rol		R13
                                    		rol		R14
                                    		rol		R15
                                    		dec		R17
                                    		brne	Norm1
                                    Norm2:	
                                    		ldi		R16,64
                                    		sub		R16,R17
                                    								;R16-ban az exponens
                                    		ret
                                    
                                    
                                    
                                    
                                    ; -------------------------------
                                    ; THE 'PREPARE TO ADD' SUBROUTINE
                                    ; -------------------------------
                                    ; This routine is called twice by addition to prepare the two numbers.
                                    ; The sign bit is tested before being set to the implied state.
                                    ; Negative numbers are twos complemented.
                                    ;
                                    ;  m1: R6, R5:R2 -> R6, R7:R5:R2
                                    ;  m2: R24, R23:R20 -> R24, R25:R23:R20
                                    ;					 R25,R7 -> sign
                                    ;
                                    PREP_ADD:
                                    			clr		R25
                                    			clr		R7
                                    			ldi		R16,0x80
                                    
                                    			tst		R24
                                    			brne	Prep_Add1
                                    			mov		R24,R16
                                    
                                    			;ide jon majd a normalas R23:R20
                                    
                                    			rjmp	M1Pos
                                    
                                    Prep_Add1:
                                    			bst		R23,7		;test the sign bit
                                    			or		R23,R16		;set it to implied state
                                    			brtc	M1Pos		;jump if positive number
                                    NEG_M1:
                                    			clr		R8
                                    			clr		R9
                                    			movw	R10,R8
                                    			movw	R12,R8		;clear result regs
                                    
                                    			sub		R8,R20		;complement
                                    			sbc		R9,R21		;add in initial carry or from prev operation
                                    			sbc		R10,R22
                                    			sbc		R11,R23
                                    			sbc		R12,R13
                                    			movw	R20,R8
                                    			movw	R22,R10
                                    			mov		R25,R12			
                                    M1Pos:
                                    
                                    			tst		R6
                                    			brne	Prep_Add2
                                    			mov		R6,R16
                                    
                                    			;ide jon majd a normalas R5:R2
                                    			rjmp	M2Pos
                                    
                                    Prep_Add2:
                                    			bst		R5,7		;test the sign bit
                                    			or		R5,R16		;set it to implied state
                                    			brtc	M2Pos		;jump if positive number
                                    NEG_M2:
                                    			clr		R8
                                    			clr		R9
                                    			movw	R10,R8
                                    			movw	R12,R8		;clear result regs
                                    
                                    			sub		R8,R2		;complement
                                    			sbc		R9,R3		;add in initial carry or from prev operation
                                    			sbc		R10,R4
                                    			sbc		R11,R5
                                    			sbc		R12,R13
                                    			movw	R2,R8
                                    			movw	R4,R10
                                    			mov		R7,R12			
                                    M2Pos:
                                    			ret					;return
                                    
                                    
                                    ; -------------------------
                                    ; THE 'SHIFT FP Right' SUBROUTINE
                                    ; -------------------------
                                    ;	In: mantissa R7:R5:R2
                                    ;	    Exponent : R6
                                    ;	    Shift right by R16
                                    ;	    
                                    ;   Out: R17,R15,R14,R13,R12,R11,R10,R9,R8,R7
                                    ;		 s   s   s   s   s   m   m   m  m  g
                                    SHIFT_FP:
                                    			movw	R12,R2
                                    			movw	R14,R4
                                    			mov		R17,R7
                                    			clr		R7
                                    			clr		R8
                                    			clr		R9
                                    			movw	R10,R8
                                    
                                    			tst		R16
                                    			brne	Shift_fp1
                                    			ret						;diff == 0 no rota
                                    Shift_fp1:
                                    			cpi		R16,0x21		;diff>33 bit
                                    			brcc	ADDEND_0		;we add 0 to 1st param
                                    
                                    EIGHT_SHIFT:
                                    			cpi		R16,8
                                    			brcs	ONE_SHIFT
                                    			mov		R7,R8
                                    			mov		R8,R9
                                    			mov		R9,R10
                                    			mov		R10,R11
                                    			mov		R11,R12
                                    			mov		R12,R13
                                    			mov		R13,R14
                                    			mov		R14,R15
                                    			mov		R15,R17
                                    			bst		R17,7			;negative number?
                                    			clr		R17
                                    			brtc	SHIFT_FP2
                                    			ser		R17
                                    SHIFT_FP2:	subi	R16,8
                                    			rjmp	EIGHT_SHIFT
                                    
                                    ONE_SHIFT:	bst		R17,7			;store sign bit
                                    			lsr		R17
                                    			ror		R15
                                    			ror		R14
                                    			ror		R13
                                    			ror		R12
                                    			ror		R11
                                    			ror		R10
                                    			ror		R9
                                    			ror		R8
                                    			ror		R7
                                    			bld		R17,7			;set back to sign bit
                                    			dec		R16
                                    			brne	ONE_SHIFT
                                    			ret
                                    ADDEND_0:
                                    			clr		R7
                                    			clr		R8
                                    			clr		R9
                                    			movw	R10,R8
                                    			movw	R12,R8
                                    			movw	R14,R8
                                    			movw	R16,R8
                                    			ret
                                    
                                    
                                    
                                    ; ---------------------------
                                    ; THE 'SUBTRACTION' OPERATION
                                    ; ---------------------------
                                    ; (offset: $03 'subtract')
                                    
                                    ;  m1:  R6, R5:R2
                                    ;  m2: R24, R23:R20
                                    ;  -------------------
                                    ; Res:  R6, R5:R2
                                    ;
                                    ;  Subtraction is done by switching the sign byte/bit of the second
                                    ; number and continuing into addition.
                                    
                                    subtract:
                                    		tst		R24
                                    		brne	Sub_NoZero
                                    		mov		R0,R20
                                    		or		R0,R21
                                    		or		R0,R22
                                    		or		R0,R23
                                    		brne	addition
                                    		ret						;x - 0 = x
                                    Sub_NoZero:
                                    		ori		R23,0x80		;make to negative
                                    
                                    ; ------------------------
                                    ; THE 'ADDITION' OPERATION
                                    ; ------------------------
                                    ; (offset: $0F 'addition')
                                    ;
                                    ;  m1:  R6, R5:R2
                                    ;  m2: R24, R23:R20
                                    ;  -------------------
                                    ; Res:  R6, R5:R2
                                    ;
                                    ;   cycles +  (RET) =  Cycles
                                    ;
                                    addition:
                                    			rcall	PREP_ADD
                                    			cp		R24,R6				;exp. M1 < M2?
                                    			brcc	SHIFT_LEN			;to SHIFT_LEN
                                    
                                    			movw	R8,R2				; exchange M1 & M2
                                    			movw	R2,R20
                                    			movw	R20,R8
                                    
                                    			movw	R8,R4
                                    			movw	R4,R22
                                    			movw	R22,R8
                                    
                                    			movw	R8,R6
                                    			movw	R6,R24
                                    			movw	R24,R8
                                    SHIFT_LEN:
                                    			mov		R16,R24
                                    			sub		R16,R6					;length of shift
                                    			mov		R6,R24
                                    			rcall	SHIFT_FP
                                    
                                    					;    R17,R15,R14,R13,R12,R11,R10,R9,R8,R7
                                    					;    s   m   m   m   m   m   m   m  m  g
                                    					; +  R25,R23,R22,R21,R20 0   0   0  0  0 
                                    					;---------------------------------------
                                    					;    R17,R15,R14,R13,R12,R11,R10,R9,R8,R7
                                    
                                    			add		R12,R20
                                    			adc		R13,R21
                                    			adc		R14,R22
                                    			adc		R15,R23
                                    			adc		R17,R25
                                    			brlt	NoAdd_Overf1
                                    
                                    			inc		R6
                                    			brne	Add_Rota
                                    			rjmp	Add_Rep_6
                                    Add_Rota:
                                    			ldi		R16,1
                                    			rcall	ONE_SHIFT
                                    
                                    NoAdd_Overf1:
                                    			mov		R20,R17			;store sign
                                    			bst		R17,7			;sign = positive?
                                    			brtc	Add_Pos
                                    
                                    			clr		R0				;negate
                                    			clr		R1
                                    			movw	R2,R0
                                    			sub		R0,R7
                                    			mov		R7,R0
                                    			movw	R0,R2
                                    
                                    			sbc		R0,R8
                                    			sbc		R1,R9
                                    			movw	R8,R0
                                    
                                    			movw	R0,R2
                                    			sbc		R0,R10
                                    			sbc		R1,R11
                                    			movw	R10,R0
                                    
                                    			movw	R0,R2
                                    			sbc		R0,R12
                                    			sbc		R1,R13
                                    			movw	R12,R0
                                    
                                    			movw	R0,R2
                                    			sbc		R0,R14
                                    			sbc		R1,R15
                                    			movw	R14,R0
                                    
                                    			mov		R0,R2
                                    			sbc		R0,R17
                                    			mov		R17,R0
                                    
                                    Add_Pos:
                                    			tst		R17
                                    			breq	Add_Norm
                                    			inc		R6
                                    			brne	Add_Rota1
                                    			rjmp	Add_Rep_6
                                    Add_Rota1:
                                    			ldi		R16,1
                                    			rcall	ONE_SHIFT
                                    			rjmp	Add_Pos
                                    
                                    Add_Norm:
                                    			rcall	Normalize
                                    	
                                    			bst		R15,7
                                    			brts	Add_Norm1	;ha 7. =0 ,akkor nincs mit tenni
                                    
                                    			rjmp	Res_Zero			
                                    
                                    Add_Norm1:
                                    			sub		R6,R16
                                    			breq	Add_Rep_6
                                    			brcs	Add_Rep_6
                                    
                                    			rcall	Round
                                    			brcs	Add_Rep_6
                                    
                                    
                                    			bst		R20,7
                                    			brts	Add_NegResult
                                    			ldi		R16,0x7F
                                    			and		R5,R16			;make pos sign
                                    
                                    Add_NegResult:
                                    			clc
                                    			ret
                                    
                                    Add_Rep_6:
                                    			sec						;overflow error
                                    			ret
                                    
                                    
                                    Round:
                                    			movw	R4,R14
                                    			movw	R2,R12
                                    
                                    ;ide jon majd a kerekites
                                    
                                    			mov		R0,R2
                                    			or		R0,R3
                                    			or		R0,R4
                                    			or		R0,R5
                                    			brne	Round1
                                    			clr		R6
                                    Round1:
                                    			clc
                                    			ret
                                    
                                    
                                    
                                    
                                  
                                 
                                 
                                 
                                 
                                    
                                    
                                     
                                 
                               | 
                                |