/*************************************************************************
**  STK500 Protocol v2 Bootloader/monitor for ATmega128,AT90CAN128
**  require AVR Studio 4.11 build 401 or later
**  (c) Milosz Klosowicz, MikloBit
**
**  Released under GNU GENERAL PUBLIC LICENSE
**  See gpl.txt
**
**  contact: 	support@miklobit.com
**  homepage:	www.miklobit.com
**
** 
**  
**  based partly on oryginal code: 
**  (c) Michael Wolf, webmaster@mictronics.de 
**  (c) Erik Lins, chip45.com      
**	(c) Jason P. Kyle, avr1.org
**
**	12/21/2006 Modified by BiPOM Electronics to support ATMEGA2560
**
**************************************************************************/
#include <ctype.h>

#ifndef F_CPU
#define F_CPU 14745600UL
#endif
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

#include <avr/io.h>
#include <avr/iom2560.h>
#include <avr/boot.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <stdlib.h>

#include "command_v2.h"

/* monitor functions will only be compiled when using ATmega64/128/CAN128, 
 * due to bootblock size constraints 
*/
#if defined(__AVR_ATmega128__) || defined(__AVR_AT90CAN128__)
//#define MONITOR
#include "rtc72423.c"
#endif

#if defined(__AVR_ATmega2560__) 
#define EEMWE   2
#define EEWE    1
#endif




#define app_start()	 asm volatile("jmp 0x0000");	      

#define	STATE_READY		      0x00
#define STATE_START           0x01
#define STATE_GET_SEQ_NUM     0x02
#define STATE_GET_MSG_SIZE1   0x03
#define STATE_GET_MSG_SIZE2   0x04
#define STATE_GET_TOKEN       0x05
#define STATE_GET_DATA        0x06
#define STATE_GET_CS          0x07

/* Set values as required supress popup messages in AVR-Studio */
#define HARDWARE_VERSION      0x02
#define SOFTWARE_MAJOR        0x02
//#define SOFTWARE_MINOR        0x01
#define SOFTWARE_MINOR        0x0A
/* values for possible topcard */
#define STK501		0xAA
#define STK502		0x55
#define STK503		0xFA
#define STK504		0xEE
#define STK505		0xE4
#define STK520		0xDD		

/* value for vtarget: always return 5.0V */
#define VTARGET		0x32
/* value for vadjust: always return 5.0V */
#define VADJUST		0x32
/* prescalled clock frequesncy equal to system clock */
#define PSCALE_FSYS 0x01
#define CMATCH_DEF	0x01
#define SCK_DURATION_DEF 0x01

#define BAUD_RATE		115200

#define MAX_BUF_SIZE	300

/* Adjust to suit whatever pin your hardware uses to enter the bootloader */

/* ATmega128 has two UARTS so two pins are used to enter bootloader and select UART */
/* BL0... means UART0, BL1... means UART1 */
#ifdef __AVR_ATmega128__
#define BL_DDR  DDRF
#define BL_PORT PORTF
#define BL_PIN  PINF
#define BL0     PINF7
#define BL1     PINF6
#elif defined __AVR_AT90CAN128__
#define BL_DDR  DDRF
#define BL_PORT PORTF
#define BL_PIN  PINF
#define BL0     PINF7
#define BL1     PINF6
#else
/* other ATmegas have only one UART, so only one pin is defined to enter bootloader */
#define BL_DDR  DDRG
#define BL_PORT PORTG
#define BL_PIN  PING
#define BL      PING4
#endif



#define SIG1	0x1E	// Always Atmel
#if defined __AVR_ATmega128__
#define SIG2	0x97
#define SIG3	0x02
#define UART0
#elif defined __AVR_AT90CAN128__
#define SIG2	0x97
#define SIG3	0x81
#define UART0
#elif defined __AVR_ATmega64__
#define SIG2	0x96
#define SIG3	0x02
#define UART0
#elif defined __AVR_ATmega32__
#define SIG2	0x95
#define SIG3	0x02
#elif defined __AVR_ATmega16__
#define SIG2	0x94
#define SIG3	0x03
#elif defined __AVR_ATmega8__
#define SIG2	0x93
#define SIG3	0x07
#elif defined __AVR_ATmega162__
#define SIG2	0x94
#define SIG3	0x04
#define UART0
#elif defined __AVR_ATmega163__
#define SIG2	0x94
#define SIG3	0x02
#elif defined __AVR_ATmega169__
#define SIG2	0x94
#define SIG3	0x05
#elif defined __AVR_ATmega8515__
#define SIG2	0x93
#define SIG3	0x06
#elif defined __AVR_ATmega8535__
#define SIG2	0x93
#define SIG3	0x08
#elif defined __AVR_ATmega2560__
#define SIG2	0x98
#define SIG3	0x01
#endif



void putch(char);
char getch(void);
void initPorts(void); // init port pins for bootloader start
void initUart(void);  // check uart selected and init
void bootCheck(void); // check bootloader/application start
void handleMessage(void) ; 
void sendResponse(void);
void cmdSignOn(void);
void cmdReadSignatureIsp(void);
void cmdSetParameter(void);
void cmdEnterProgmodeIsp(void);
void cmdLeaveProgmodeIsp(void);
void cmdChipEraseIsp(void);
void cmdLoadAddress(void);
void cmdGetParameter(void);
void cmdProgramFlashIsp(void);
void cmdReadFlashIsp(void);
void cmdProgramEepromIsp(void);
void cmdReadEepromIsp(void);
void cmdReadFuseLockIsp(void);
void eeprom_wb(unsigned int uiAddress, unsigned char ucData);
unsigned char eeprom_rb(unsigned int uiAddress);
unsigned char readBits( unsigned int address ); // read lock/fuse bits

#ifdef MONITOR	
#define MONITOR_FLAG '!' 
#define MONITOR_PROMPT ':'
#define MONITOR_BUFFER_SIZE 256 
#define MONITOR_DISPLAY_LINES 8

#define MONITOR_CMD_TIME  'T'
#define MONITOR_CMD_DATE  'D'
#define MONITOR_CMD_FLASH 'F'
#define MONITOR_CMD_RAM   'X'
#define MONITOR_CMD_EEPROM  'E'
#define MONITOR_CMD_QUIT 'Q'
#define MONITOR_CMD_HELP '?'

#define MONITOR_MEM_FLASH  0
#define MONITOR_MEM_RAM    1
#define MONITOR_MEM_EEPROM 2

	void monitorMain(void); 
	void monitorInit(void);
	uint32_t monitorDump( uint32_t address, uint8_t lineNum, uint8_t memType );
	void monitorChange( uint32_t address, uint8_t value, uint8_t memType );
	void print( char *s );
	void print_P( uint32_t address );
	char  *monitorReadLine( void );
	char *getValue( char *src,  uint32_t *value, uint8_t len );
	uint8_t htoi( uint8_t val );

static uint8_t monitorWelcome[] PROGMEM = "\nMonitor\n" ;	
static uint8_t monitorQuit[] PROGMEM = "Quit...\n" ;	
static uint8_t monitorError[] PROGMEM = "Error\n" ;	
char monitorBuf[MONITOR_BUFFER_SIZE];
unsigned char monitor_cnt = 0; // check if we enter monitor
#endif



unsigned char bootuart0=0,bootuart1=0;
unsigned char rx_buffer[MAX_BUF_SIZE];  // RX buffer
unsigned char tx_buffer[MAX_BUF_SIZE];  // TX buffer
unsigned char *rx_pntr = &rx_buffer[0];  // pointer to rx buffer
unsigned char *tx_pntr = &tx_buffer[0];  // pointer to rx buffer
unsigned char *size_pntr = &tx_buffer[2];    // pointer to msg size buffer field
unsigned char *answer_id_pntr = &tx_buffer[5];    // pointer to answer id buffer field
unsigned int msg_size=0x00;  // Rx message size
unsigned char msg_cs=0x00;  // calculated message checksum
unsigned long address_flash = 0L; // actual address in flash memory
unsigned int address_eeprom = 0x00; // actual address in eeprom memory
unsigned int n_bytes = 0x00;  // number of databytes in message
unsigned char n_pages = 0x00;  // number of page to program
unsigned char sequence_number=0x00;  // sequence number from host
unsigned char answer_id=0x00;  // answer cmd id
unsigned int i=0, j=0;  // for loop variables
unsigned char echo = 0;  // rs232 terminal echo


#if 0
void printmes( char *s )  
{
   while( *s ) {
   		putch( *s );
   		s++ ;
   }	
}

void tiprintf( const char *format, ... )
{
    char msgBuf[64] = {0};
    va_list arglist;
    va_start( arglist, format );
    vsprintf(msgBuf,format,arglist );
    va_end( arglist );
    printmes(msgBuf);
}

/********************************************************************************************
;	Function:		PrintDataBlock
;
;	Description:	Prints data buffer  
;
;	Inputs: 	 	buffer - pointer to data buffer
;					length - length of data buffer 
;
;	Returns:		Nothing	 	 
**********************************************************************************************/
void PrintDataBlock (uint8_t* buffer, uint16_t length)
{
	uint16_t ndx;
	for( ndx = 0; ndx < length ; ndx++)	
	{
		if (!(ndx%16)) tiprintf ("\n\r%04x",ndx);
		tiprintf (" %02x",*buffer++);
	}
}
void testCode(void)
{
	int i=0;
	printmes ("\n\rTestCode");
	for(i = 0; i < MAX_BUF_SIZE; i++) 
	{ 
		rx_buffer[i] = 0x00;  // clear data buffer
	}

	// n_Bytes = 32;
	rx_pntr = &rx_buffer[0];
	rx_buffer[1] = SPM_PAGESIZE / 256;
	rx_buffer[2] = SPM_PAGESIZE % 256;
	// address
	address_flash = 32768;
	//
	printmes ("\n\rREAD DATA BLOCK");
	cmdReadFlashIsp();
	PrintDataBlock (&tx_buffer[2], SPM_PAGESIZE);
	//
	for(i = 0; i < MAX_BUF_SIZE; i++) 
	{ 
		rx_buffer[i] = i;  // clear data buffer
	}
	rx_buffer[1] = SPM_PAGESIZE / 256;
	rx_buffer[2] = SPM_PAGESIZE % 256;
	rx_pntr = &rx_buffer[0];
	address_flash = 32768;
	tx_pntr = &tx_buffer[0];

	cmdProgramFlashIsp();
	printmes ("\n\rEXIT");
	for ( ;; );
}
#endif
/*----------------------------------------------------------------------------*/ 
/* execute command  CMD_READ_FUSE_ISP,CMD_READ_LOCK_ISP                       */
/*----------------------------------------------------------------------------*/ 
 
void cmdReadFuseLockIsp(void)  {
	unsigned int command,address;

	msg_size = 4;  		// set message length
	*(tx_pntr++) = *rx_pntr;
	*(tx_pntr++) = STATUS_CMD_OK;
	
	command = *(rx_pntr+2) * 256 + *(rx_pntr+3);
	switch( command ) {
		case 0x5800: address = 0x0001; break; // lock bits		
		case 0x5000: address = 0x0000; break; // fuse low	
		case 0x5808: address = 0x0003; break; // fuse hi	
		case 0x5008: address = 0x0002; break; // fuse ext
		default: address = 0x0000;			
	}
	
	*(tx_pntr++) = readBits(address);	 

    *(tx_pntr++) = STATUS_CMD_OK;	
	
}   
 /* read lock/fuse bits */
unsigned char readBits( unsigned int address ) {
	asm volatile(
				"mov	r31,r25 \n\t"	
		       	"mov	r30,r24 \n\t"	
				"lds	r24,%0 \n\t"		       	
				"ori	r24,0x09 \n\t"
				"sts	%0,r24 \n\t"									
		       	"lpm	\n\t" 
		       	"mov	r24,r0  \n\t" 
		       	: "=m" (SPMCSR)
				);     
	return address&0xFF;
}
/*----------------------------------------------------------------------------*/ 
/* execute command  CMD_READ_FLASH_ISP                                        */
/*----------------------------------------------------------------------------*/ 
void cmdReadFlashIsp(void) {

	*(tx_pntr++) = CMD_READ_FLASH_ISP;
	*(tx_pntr++) = STATUS_CMD_OK;
	
	n_bytes=((*(rx_pntr+1)*256)+*(rx_pntr+2));  // number of databytes to read from flash 
	for(i=0;i < n_bytes; i++) { // fill data buffer with n_bytes
#if defined(__AVR_ATmega128__) || defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2560__)		
    	*(tx_pntr++) = pgm_read_byte_far(address_flash++);
#else
    	*(tx_pntr++) = pgm_read_byte(address_flash++);
#endif    	
	}
	msg_size = n_bytes + 3;  // set message length
	*(tx_pntr++) = STATUS_CMD_OK;	
	
}   // end of readFlashIsp
/*----------------------------------------------------------------------------*/ 
/* execute command  CMD_PROGRAM_FLASH_ISP                                     */
/*----------------------------------------------------------------------------*/ 
void cmdProgramFlashIsp(void) {
unsigned /*char*/int byte1,byte2;
unsigned long address, address_page1,address_page2,offset;
	
	n_bytes = ((*(rx_pntr+1)*256)+*(rx_pntr+2));  	// number of databytes to flash
	rx_pntr += 10;  								// set pointer to flash data
	address_page1 = address_flash - ( address_flash % SPM_PAGESIZE ); // addres of 1st page to program
	address_page2 = ( address_flash + n_bytes ) - 
	                ( ( address_flash + n_bytes ) % SPM_PAGESIZE ); // addres of last page to program
	n_pages = (( address_page2 - address_page1) / SPM_PAGESIZE ) + 1;  	// number of pages to flash	  
	offset = address_flash - address_page1 ; 
	#if 0
	tiprintf ("\n\r page size = %u",SPM_PAGESIZE);
	tiprintf ("\n\r address_page1 = %u",address_page1);
	tiprintf ("\n\r address_page2 = %u",address_page2);
	tiprintf ("\n\r n_pages = %u",n_pages);             		
	tiprintf ("\n\r offset = %u",offset);  
	tiprintf ("\n\r n_bytes = %u",n_bytes);  
	tiprintf ("\n\r*");
	#endif
	for( j = 0; j < n_pages; j++ )  { 					// do for all pages in message
        for(i=0; i < SPM_PAGESIZE; i += 2)  		// load old content of flash page
        {
        	address = address_page1 + i;
        	if( (address < address_flash  ) ||
        	    (address > ( address_flash + n_bytes) ) )  {  // copy to buffer old data
#if defined(__AVR_ATmega128__) || defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2560__)		
	    		byte1 = pgm_read_byte_far(address);
	    		byte2 = pgm_read_byte_far(address + 1);
#else
	    		byte1 = pgm_read_byte(address_page1 + i );
	    		byte2 = pgm_read_byte(address_page1 + i + 1);    		
#endif          
			}
			else  {			// copy to buffer new data
				byte1 = *rx_pntr;
				byte2 = *(rx_pntr+1);
				rx_pntr += 2;								
			}	
			boot_page_fill(i,/*(unsigned int)*/(byte1 |(byte2 << 8)));
        }
		boot_page_erase((unsigned long)address_page1);  	// clear page
        while(boot_spm_busy());	        				

        boot_page_write((unsigned long)address_page1);  	// write page to flash
        while(boot_spm_busy());