;Incs and Defs---------------------------------------------------------

.include "atm328p.inc"
.include "global.inc"

;++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;Main Programm

.global main
main:
	ldi		main_r, lo8(RAMEND)						;Stackpointer
	out		SPL, main_r
	ldi		main_r, hi8(RAMEND)
	out		SPH, main_r

	clr		false									;set Registers
	ser		true

	call	usart_init;								;init USART

	ldi		main_r, (1<<PC0)|(1<<PC1)|(1<<PC2)|(1<<PC3)|(1<<PC4)|(1<<PC5)	
	out		DDRC, main_r							;init I/O
	out 	PORTC, false

	sei												;enable Interrupts

	rjmp	mode_select								;select Mode
	rjmp	.-2										;Endlosschleife
	cli												;disable Interrupts

;++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;Mode Selection

mode_select:
	ldi		main_r, 1<<PC0 							;Signal State
	out		PORTC, main_r						

;Receive Frame----------------------------------------------------------

	ldi		timer2_r, (1<<CS22)|(1<<CS21)|(1<<CS20)	;set Prescaler
	sts		TCCR2B, timer2_r						;x1024 (60Hz)
	sts		timer2_bool, false						;reset Timer
	ldi		timer2_r, 6 							;init Time 100ms
	sts		timer2_MAX, timer2_r
	ldi		timer2_r, 1<<TOIE2						;Timer2-OVF enable
	sts		TIMSK2, timer2_r

	ldi		X_l, lo8(cmd_ptr)						;set Command-Ptr
	ldi		X_h, hi8(cmd_ptr)
	lds		usart_r, UCSR0B							;activate Receiver
	sbr 	usart_r, (1<<RXEN0)|(1<<RXCIE0)
	sts		UCSR0B, usart_r

timeout_1:
	lds		main_r, timer2_bool						;check for Timeout
	cpse    main_r, true 
	rjmp	.+8
	ldi		X_l, lo8(cmd_ptr)						;reset Command-Ptr
	ldi		X_h, hi8(cmd_ptr)		
	sts		frame_cnt, false						;reset Frame cnt

	lds		usart_r, frame_cnt
	cpi		usart_r, 0x03							;check if Frame rec
	brne	timeout_1

	sts		TIMSK2, false							;Timer2-OVF disable
	lds		usart_r, UCSR0B							;disable Receiver
	cbr 	usart_r, (1<<RXEN0)|(1<<RXCIE0)
	sts		UCSR0B, usart_r
	sts		UDR0, false								;clear UDR
	sts		frame_cnt, false						;reset Frame cnt

	call	cmd_rebuild								;read Cmd

;Select Mode------------------------------------------------------------

	sbrc  	mode, idle
	rjmp	mode_select
	sbrc  	mode, prog_enable
	call	mode_prog_enable
	sbrc  	mode, page_read
	jmp		mode_page_read
	sbrc	mode, page_write
	rjmp	mode_select
	sbrc  	mode, prog_check
	rjmp	mode_select

;Send Frame-------------------------------------------------------------

send_fin:
	ldi		X_l, lo8(cmd_ptr)						;set Command-Ptr
	ldi		X_h, hi8(cmd_ptr)
	lds		usart_r, UCSR0B							;enable UDRI
	sbr 	usart_r, 1<<UDRIE0
	sts		UCSR0B, usart_r

	lds		usart_r, frame_cnt
	cpi		usart_r, 0x02							;check if Frame send
	brne	.-8

	lds		usart_r, UCSR0B							;disable TX, TXCI
	cbr 	usart_r, (1<<TXEN0)|(1<<TXCIE0)|(1<<UDRIE0)		
	sts		UCSR0B, usart_r
	sts		UDR0, false								;clear UDR
	sts		frame_cnt, false						;reset Frame cnt

	rjmp 	mode_select

;++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;Cmd rebuild

cmd_rebuild:
	lds 	main_r, cmd_mode						;set Mode
	andi	main_r, nibble_lo
	ldi		mode, 0x01
	tst		main_r									
	breq	.+6
	lsl		mode
	dec 	main_r
	rjmp	.-10
	clr 	option									;set Option
	lds		main_r, cmd_opt_lo
	andi	main_r, nibble_lo
	or		option, main_r
	lds		main_r, cmd_opt_hi
	andi	main_r, nibble_lo
	swap	main_r
	or		option, main_r
	brne	.+4										;check valid Mode
	jmp		mode_select								

	ret

;++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;Enable Programming

mode_prog_enable:
	ldi		main_r, 1<<PC1							;Signal State		
	out		PORTC, main_r							

;Chip Erase------------------------------------------------------------


;Programming enable Command--------------------------------------------

	ldi		timer2_r, 1<<CS22						;set Prescaler
	sts		TCCR2B, timer2_r						;x64 (1kHz)
	sts		timer2_bool, false						;reset Timer
	ldi		timer2_r, 250 							;init Time 250ms
	sts		timer2_MAX, timer2_r
	ldi		timer2_r, 1<<TOIE2						;Timer2-OVF enable
	sts		TIMSK2, timer2_r

	lds		main_r, timer2_bool						;wait for 250ms
	cpse    main_r, true
	rjmp	.-8

	sts		TIMSK2, false							;Timer2-OVF disable

	sts		cmd_mode, option 						;signal Prog enable

	jmp		send_fin

;++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;Read Page

mode_page_read:
	ldi		main_r, 1<<PC2							;Signal State
	out		PORTC, main_r

;Send Acknoledgement---------------------------------------------------

	sts		cmd_mode, option 						;verify Mode

	ldi		X_l, lo8(cmd_ptr)						;set Command-Ptr
	ldi		X_h, hi8(cmd_ptr)
	lds		usart_r, UCSR0B							;enable UDRI
	sbr 	usart_r, 1<<UDRIE0
	sts		UCSR0B, usart_r

	lds		usart_r, frame_cnt
	cpi		usart_r, 0x02							;check if Frame send
	brne	.-8

	lds		usart_r, UCSR0B							;disable TX, TXCI
	cbr 	usart_r, (1<<TXEN0)|(1<<TXCIE0)|(1<<UDRIE0)		
	sts		UCSR0B, usart_r
	sts		UDR0, false								;clear UDR
	sts		frame_cnt, false						;reset Frame cnt

;Read Page-------------------------------------------------------------

	ldi		timer2_r, (1<<CS22)|(1<<CS21)|(1<<CS20)	;set Prescaler
	sts		TCCR2B, timer2_r						;x1024 (60Hz)
	sts		timer2_bool, false						;reset Timer
	ldi		timer2_r, 60 							;init Time 1s
	sts		timer2_MAX, timer2_r
	ldi		timer2_r, 1<<TOIE2						;Timer2-OVF enable
	sts		TIMSK2, timer2_r

	ldi		X_l, lo8(page_ptr)						;set Page-Ptr
	ldi		X_h, hi8(page_ptr)
	lds		usart_r, UCSR0B							;activate Receiver
	sbr 	usart_r, (1<<RXEN0)|(1<<RXCIE0)
	sts		UCSR0B, usart_r

timeout_2:
	lds		main_r, timer2_bool						;check for Timeout
	cpse    main_r, true
	rjmp	.+12
	lds		usart_r, UCSR0B							;disable Receiver
	cbr 	usart_r, (1<<RXEN0)|(1<<RXCIE0)
	sts		UCSR0B, usart_r 
	rjmp	mode_page_read

	lds		usart_r, frame_cnt
	mov		main_r, option
	lsl		main_r
	cp		usart_r, main_r							;check if Page rec
	brne	timeout_2

	sts		TIMSK2, false							;Timer2-OVF disable
	lds		usart_r, UCSR0B							;disable Receiver
	cbr 	usart_r, (1<<RXEN0)|(1<<RXCIE0)
	sts		UCSR0B, usart_r
	sts		UDR0, false								;clear UDR
	sts		frame_cnt, false						;reset Frame cnt

;Rebuild Page-----------------------------------------------------------

	ldi		X_l, lo8(page_ptr)						;set Pointer
	ldi		X_h, hi8(page_ptr)						;for Rebuild
	ldi		Y_l, lo8(page_ptr)
	inc 	Y_l
	ldi		Y_h, hi8(page_ptr)
	ldi		Z_l, lo8(page_ptr)
	ldi		Z_h, hi8(page_ptr)

page_rebuild:
	ld 		main_r, X 								;Remove Mask and
	adiw	X_l, 2									;reconstruct Bytes
	ld 		usart_r, Y
	adiw 	Y_l, 2
	andi	main_r, nibble_lo
	andi	usart_r, nibble_lo
	swap	usart_r
	or 		main_r, usart_r
	st 		Z+, main_r	
	dec		option
	brne 	page_rebuild

	ldi		main_r, frame_mask						;signal Page rec
	sts		cmd_mode, main_r 						;by sending mask

	jmp		send_fin