; Define name of registers:
.alias flag_warmup 2 
.alias flag1 3
.alias flag2 4
.alias flag3 5
.include "HDL50001_pcf.inc"

.text

start:
	jmp systimer
complete

;;;;; Processing systimer ;;;;;
systimer:
	jmp initSysTimer
	setl #IHOOKADDR,P1
	setl #flag_warmup, 3   ; flag_warmup=3 - зажигаем TIM3
                                ; flag_warmup=2 - зажигаем TIM2
                                ; flag_warmup=1 - зажигаем TIM1
                                ; flag_warmup=0 - обрабатываем прерывания TIM1-TIM3  
	setl #flag1, 0
	setl #flag2, 0
	setl #flag3, 1
complete

;Interrupt handler
P1:
	getl #STCR
	and @1, 0xFFFFFFFB
	setl #STCR, @1 
        getl 0x00000010 ; если есть прерывание TIM0, то 4й бит будет равен 1
        rdl TIM0_CR     ;
        and @1, @2      ; read only bit 4
	je @1, CheckTIM1 ; проверяем остальные прерывания
	jne @2, CheckWarmUp
complete 

CheckWarmUp:
        rdl TIM0_CR ; сбросим TIM0 прерывание, небольшой трюк: 
        wrl @1, TIM0_CR ; при записи "1" в 4й бит, этот бит сбросится 
	getl #flag_warmup
	je @1, CheckTIM1 ; обрабатываем прерывания TIM1-TIM3
	jne @2, BeginSetupTimers; #flag_warmup не нулевой, установим периоды T1-T3, обработка прерывания TIM0
complete

BeginSetupTimers:
	jmp Port3Out
	getl #flag_warmup
	subl @1, 1
	setl #flag_warmup, @1
complete

CheckTIM1: ; TIM0 прерывание уже очищено
	getl 0x00000010
        rdl TIM1_CR     ;
        and @1, @2      ; read only bit 4
        je @1, CheckTIM2
        jne @2, Port11Out ;есть прерывание TIM1, остальные прерывания обработаем при следующем вызове systimer?  
complete

CheckTIM2: ; TIM0 прерывание уже очищено
        getl 0x00000010
        rdl TIM2_CR     ;
        and @1, @2      ; read only bit 4
        je @1, CheckTIM3
        jne @2, Port21Out ;есть прерывание TIM2
complete

CheckTIM3: ; TIM0 прерывание уже очищено
        getl 0x00000010
        rdl TIM3_CR     ;
        and @1, @2      ; read only bit 4
        je @1, FinalCleanIRQ
        jne @2, Port31Out ;есть прерывание TIM3
complete

FinalCleanIRQ:
	jmp MainLoop
	setl #PSW, 1
complete 

;; Set counter period and allow interrupt from systimer
initSysTimer:
	jmp StartTimer0 
	getl 0x0000FFFF
	setl #STVALR, @1 
	getl 0x00000002
	setl #MSKR, @1
	getl 0x00000003
	setl #STCR, @1

        getl 0x0000FFFF
        wrl @1, TIM0_CNT  ;warmup 
        getl 0x00000053 ; =2/3*0x000007C
        wrl @1, TIM0_PSC  ;(10Hz) FFFF*7A*Tsys=0.1 sec. =>   65535*122*Tsys = 0.1 => Tsys= 1.5*10^{-7} sec.  
; timers for motor
        getl 0x0000FFFF
        wrl @1, TIM1_CNT 
        getl 0x000007C
        wrl @1, TIM1_PSC ;(10Hz) FFFF*7A*Tsys=0.1 sec. =>   65535*122*Tsys = 0.1 => Tsys= 1.5*10^{-7} sec.
        getl 0x0000FFFF
        wrl @1, TIM2_CNT 
        getl 0x000007C
        wrl @1, TIM2_PSC ;(10Hz) FFFF*7A*Tsys=0.1 sec. =>   65535*122*Tsys = 0.1 => Tsys= 1.5*10^{-7} sec.
        getl 0x0000FFFF
        wrl @1, TIM3_CNT 
        getl 0x000007C
        wrl @1, TIM3_PSC ;(10Hz) FFFF*7A*Tsys=0.1 sec. =>   65535*122*Tsys = 0.1 => Tsys= 1.5*10^{-7} sec.
complete

StartTimer0:
        jmp MainLoop
	setl #PSW, 1
        getl 0x0000001F ;clear bit 4 by write "1"
        wrl @1, TIM0_CR 
complete

;;Start counter, use periodic mode
MainLoop:
	jmp MainLoop 
complete

Port3Out: ; установим длительность импульса TIM3, обработка прерывания TIM0
	getl #flag_warmup
	subl @1, 2 
	je @1, Switch3On ; #flag_warmup был равен 3 
        jne @2, Port2Out	
complete

Switch3On: ; установим длительность импульса TIM3, обработка прерывания TIM0
	setl #PSW, 1
	jmp MainLoop; #IRETADDR ;? Других прерываний нет?  
        getl 0x0000001F ;clear bit 4 by write "1"
        wrl @1, TIM3_CR
complete

Port2Out: ; установим длительность импульса TIM2, обработка прерывания TIM0
        getl #flag_warmup ;2
        subl @1, 1        
        je @1, Switch2On ; #flag_warmup был равен 2
        jne @2, Port1Out 
complete

Switch2On: ; установим длительность импульса TIM2, обработка прерывания TIM0
	setl #PSW, 1
        jmp MainLoop; #IRETADDR ;? Других прерываний нет?  
        getl 0x0000001F ;clear bit 4 by write "1"
        wrl @1, TIM2_CR
complete

Port1Out: ; установим длительность импульса TIM1, обработка прерывания TIM0
        getl #flag_warmup ;1
;        subl @1, 1        
        je @1, Switch1On ; #flag_warmup был равен 1
        jne @2, CheckTIM1; уходим на проверку TIM1-TIM3 прерываний ; Port11Out ;???  
complete

Switch1On: ; установим длительность импульса TIM1, обработка прерывания TIM0
	setl #PSW, 1
        jmp MainLoop; #IRETADDR ;? Других прерываний нет?  
        getl 0x0000001F ;clear bit 4 by write "1"
        wrl @1, TIM1_CR
complete 

; ===================
Port11Out:
        rdl TIM1_CR
        wrl @1, TIM1_CR ; сбрасываем прерывание TIM1
        getl #flag1
        je @1, disable1
        jne @2, enable1
complete

Port21Out:
        rdl TIM2_CR
        wrl @1, TIM2_CR ; сбрасываем прерывание TIM2
        getl #flag2
        je @1, disable2
        jne @2, enable2
complete

Port31Out:
        rdl TIM3_CR
        wrl @1, TIM3_CR ; сбрасываем прерывание TIM3
        getl #flag3
        je @1, disable3
        jne @2, enable3
complete

; 1st halfbridge down
disable1: 
        jmp CheckTIM2
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
        getl 0x00000076; Hi1 down
        and @1, @2 
        getl 0x00000020; Lo1 up
        or @1, @2
        wrl @1, GPIOC_OUT 
        setl #flag1, 1
complete

;  1st halfbridge up
enable1:
;       Hi1    (X9:5)  PC0 0x00000001
;       Lo1    (X9:11) PC5 0x00000020 
;       Hi2    (X9:10) PC4 0x00000010
;       Lo2    (X9:7)  PC2 0x00000004
;       Hi3    (X9:6)  PC1 0x00000002 
;       Lo3    (X9:12) PC6 0x00000040 
;  GPIOC_DIR  ============ 0x00000077

        jmp CheckTIM2
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
	getl 0x00000057 ; Lo1 down
	and @1, @2 
        getl 0x00000001 ; Hi1 up
        or @1, @2 
        wrl @1, GPIOC_OUT; 
        setl #flag1, 0
complete

; 2nd halfbridge down
disable2: 
        jmp CheckTIM3
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
        getl 0x00000067 ; Hi2 down
        and @1, @2 
	getl 0x00000004 ; Lo2 up
	or @1, @2
        wrl @1, GPIOC_OUT 
        setl #flag2, 1         
complete

; 2nd halfbridge up
enable2: 
        jmp CheckTIM3 
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
        getl 0x00000073 ; Lo2 down
	and @1, @2 
        getl 0x00000010 ; Hi2 up 
        or @1, @2
        wrl @1, GPIOC_OUT ; 
        setl #flag2, 0
complete

; 3rd halfbridge down
disable3:
        setl #PSW, 1
        jmp #IRETADDR
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
        getl 0x00000075 ; Hi3 down
        and @1, @2 
	getl 0x00000040 ; Lo3 up
	or @1, @2        
	wrl @1, GPIOC_OUT
        setl #flag3, 1 
complete

; 3rd halfbridge up
enable3: 
        setl #PSW, 1
        jmp #IRETADDR
        getl 0x00000077
        wrl @1, GPIOC_DIR
        rdl GPIOC_OUT
	getl 0x00000037 ; Lo3 down
	and @1, @2 
        getl 0x00000002 ; Hi3 up
        or @1, @2
        wrl @1, GPIOC_OUT        
        setl #flag3, 0
complete

