/*
 * Bootloader.c
 *
 * Created: 03.03.2019 09:55:52
 * Author : Jens
 */ 

#include "machine.h"
#include <ctype.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/boot.h>
#include <util/delay.h>

#include "uart.h"
#include "srec.h"

typedef void(fp_t)(void);
fp_t *startApplication = (fp_t*)0x0000;        /* Funktionspointer auf 0x0000 */
fp_t *startBootloader = (fp_t*)0x7000;        /* Funktionspointer auf 0x7000 */

#define SPM_PAGESIZE_MASK	0x7FUL

inline void bootloader_enter()
{
	/* Interrupt Vektoren verbiegen */
	
	char sregtemp = SREG;
	char temp = MCUCR;
	MCUCR = temp | (1<<IVCE);
	MCUCR = temp | (1<<IVSEL);
	SREG = sregtemp;
}

inline void bootloader_exit()
{
	/* Interrupt Vektoren wieder gerade biegen */
	char temp = MCUCR;
	MCUCR = temp | (1<<IVCE);
	MCUCR = temp & ~(1<<IVSEL);	
}

typedef enum _eState
{
	StateIdle = 0,
	StateHex,	
} State;

void erase_page(uint16_t page_addr);
void write_page(uint16_t page_addr, uint8_t *buf);
void program_page (uint32_t page, uint8_t *buf);

void init_page(uint8_t *pPageBuffer, size_t size)
{
	for (int i=0; i < size; i++)
	{
		pPageBuffer[i] = 0xFF;
	}
}

void port_init()
{
	DDRB = 0x00;
	DDRD = 0x40;
	PORTD = 0x40;
}

int is_bsel()
{
	return  ((PINB & 1) == 0);
}

void led_bsel(int enable)
{
	if (enable)
	{
		PORTD = 0x00;
	}
	else
	{
		PORTD = 0x40;
	}
}

int main(void)
{
	uint8_t page_buffer[SPM_PAGESIZE];
	uint8_t hex_buffer[64];
	State state = StateIdle;
	State state_next;
	srec_t srec;	
	port_init();

	cli();
	if (is_bsel())
	{
	bootloader_enter();
		led_bsel(1);
		uart_init(115200);
		uart_puts("Hendi-Control Bootloader v1.2\n");
		size_t image_size = 0;
		uint16_t page_addr = 0xFFFF;
		size_t hex_buffer_size = 0;
		int leave_loop = 0;
		int page_is_dirty = 0;
		int force_page_erase = 1;
		do 
		{
			char c;
			if ((c = uart_getc()))
			{
				char C = toupper(c);
			
				state_next = state;
				switch(state)
				{
					case StateIdle:
					if (C == 'S')
					{
						hex_buffer_size = 0;
						state_next = StateHex;
						hex_buffer[hex_buffer_size++] = C;
					
					}
					if (C == 'R')
					{
						startBootloader();
					}
					if (C == 'Q')
					{
						leave_loop = 1;
					}
					break;
				
					case StateHex:
					if(((C != 0x0D) && (C != 0x0A)))
					{
						hex_buffer[hex_buffer_size++] = C;
					}
					else
					{
						int srec_state = srec_decode(&srec, hex_buffer, hex_buffer_size);
						hex_buffer_size = 0;
						switch(srec_state)
						{
							case srec_sob:
							image_size = 0;
							init_page(page_buffer, SPM_PAGESIZE);
							page_is_dirty = 0;
							break;

							case srec_data:
							{
								uint16_t new_page_addr = srec.addr & ~SPM_PAGESIZE_MASK;
								if ((new_page_addr != page_addr) || force_page_erase)
								{
									page_addr = new_page_addr;
									force_page_erase = 0;
									erase_page(page_addr);
								}

								uint16_t start = srec.addr & SPM_PAGESIZE_MASK;
								uint16_t i = 0;
								for (; i < srec.size; i++)
								{
									if (start == SPM_PAGESIZE)
									{
										break;
									}
									page_is_dirty = 1;
									page_buffer[start++] = srec.data[i];
								}
								// Page buffer is complete -> write to flash
								if (start == SPM_PAGESIZE)
								{
									page_is_dirty = 0;
									write_page(page_addr, page_buffer);
									init_page(page_buffer, SPM_PAGESIZE);
								}
							
								if (i < srec.size)
								{
									page_addr += SPM_PAGESIZE;
									page_is_dirty = 1;
									force_page_erase = 1;
									start=0;
								}

								// Copy remaining data to start of page buffer
								for (; i < srec.size; i++)
								{
									page_buffer[start++] = srec.data[i];
								}
								image_size += srec.size;
								uart_putc('.');
							}
							break;
							case srec_rec:
							break;
							case srec_eob:
							state_next = StateIdle;
							if (page_is_dirty)
							{
								if (force_page_erase)
								{
									erase_page(page_addr);
								}
								write_page(page_addr, page_buffer);
								page_is_dirty = 0;
							}
							uart_puts("\nImage size: 0x"); print_word(image_size); uart_puts("\n");
							/* Reenable RWW-section again. We need this if we want to jump back */
							/* to the application after bootloading. */
							boot_rww_enable ();
							break;
							case srec_err:
							startBootloader();
							break;
						}
					}
					break;
				
					default:
					break;

				}
				state = state_next;
			}
		} while (!leave_loop);
		
		// After flash update: Without RWW section enable, jump to app will fail
		boot_rww_enable();
	}
	bootloader_exit();

	/* Return to address 0x0000 */
	startApplication();
}

void erase_page(uint16_t page_addr)
{
	eeprom_busy_wait ();
	boot_spm_busy_wait ();
	boot_page_erase (page_addr);
	
}
void write_page(uint16_t page_addr, uint8_t *pPageBuffer)
{
	uint16_t i;

	boot_spm_busy_wait ();      /* Wait until the memory is erased. */
	
	for (i=0; i<SPM_PAGESIZE; i+=2)
	{
		/* Set up little-endian word. */
		uint16_t w = *(pPageBuffer++);
		uint16_t v = *(pPageBuffer++);
		w += v << 8;
		
		boot_page_fill (page_addr + i, w);
	}
	boot_page_write (page_addr);     /* Store buffer in flash page.		*/
}