
;***************************************************************************
;
; File Name :'Main.asm"
; Title :Matmatikai rutinok, test verzio
; Date :2003.07.16.
; Version :1.0.0
; Support telephone :+36703334034
; 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  64exponent
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 bit7ig
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
;R16ban 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

