/*
 * 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;
	}
}

int main(void)
{
	uint8_t page_buffer[SPM_PAGESIZE];
	uint8_t hex_buffer[64];
	State state = StateIdle;
	State state_next;
	srec_t srec;	
	cli();
	bootloader_enter();
	
	uart_init(9600);
	
	uart_puts("Hendi-Control Bootloader v1.1\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);
	
	uart_puts("Exit bootloader!\n\n");

	bootloader_exit();

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

void erase_page(uint16_t page_addr)
{
//	uart_puts("ErasePage: "); print_word(page_addr); uart_puts("\n");
	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;

//	uart_puts("Write page 0x"); print_word(page_addr); uart_puts("\n");
	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.		*/
}
