PIC18f4620 Bootloader

//A bootloader designed  for the microchips pic18f4620. 
//Compile using xc8 --chip=18F4620 --ROM=0-bff bootloader.c. Download bootloader.hex to the pic using a pickit2. (see http://robosumo.wordpress.com/2013/02/28/recommended-software-to-install-on-your-own-laptop/ if you're not familiar with this)
//
//To enter programming mode hold RB7 low during reset. RB6 will be set high indicating the bootloader is ready to program. 
//The most strightforward way to program the device is to:
//   1. Build the circuit shown at .
//   2. Download build.bat and serialprog.exe from https://dadorran.wordpress.com.
//   3. Make sure that you have the xc8 compiler installed on your PC (download at http://www.microchip.com/mplabxc8windows)
//   5. Write your code in a file called test.c. (see http://robosumo.wordpress.com/2013/02/28/recommended-software-to-install-on-your-own-laptop/ for example code and download instructions)
//   6. At the DOS prompt type "build test.c"
//
//See https://dadorran.wordpress.com (search for pic18f4620 bootloader) for more details
//By david.dorran@dit.ie

#include <xc.h>
#include <flash.h>
#include <usart.h>
#include <stdio.h>

#pragma config OSC=INTIO67,MCLRE=ON,WDT=OFF,LVP=OFF,BOREN=OFF

//where the application program will reside in memory
#define START_PROG_ADDR 0x0c00


#asm
PSECT intcode 
	GOTO START_PROG_ADDR + 0x8
PSECT intcodelo 
	GOTO START_PROG_ADDR + 0x18
#endasm
void restart(void);
void jump_to_application(void);
int main(void)
{
	int BOOT_serial_data_recieved;
	BOOT_serial_data_recieved = EEPROM_READ(0x00); 
	EEPROM_WRITE(0x00, 0); 
	 
	TRISB = 0b10111111;
	LATD = 0;
	LATBbits.LATB6= 0; //just used for debugging
    if(PORTBbits.RB7 == 0 || BOOT_serial_data_recieved == 21)
    {     
		LATBbits.LATB6= 1; //just used for debugging
        int  k=0, i=0, address_and_checksum_recieved=0, first_address_recieved=0;
        int  checksum, sum=0;
        unsigned long int  address=0;
        unsigned char uart_recieve_buffer[64]; 
        unsigned char flash_contents[64]; 
        unsigned char Orig_OSCCON; 

		
		LATDbits.LATD0 = 1; //just used for debugging
        Orig_OSCCON = OSCCON;
        OSCCON = 0b01110000; //TO DO reset OSCCON back to default after programming
		OSCTUNEbits.PLLEN = 1;
		//open usart - baud rate 115200
		OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, 16);
		INTCONbits.GIE = 0; //disable global interrupts 
		INTCONbits.PEIE = 0; //disable peripheral interrupts 
		
        while(PIR1bits.RCIF == 0 || BOOT_serial_data_recieved == 21);
        if(RCREG == 21  || BOOT_serial_data_recieved == 21) 
        {
            TXREG = 22;
            while (!TXSTAbits.TRMT);     
        }
        else
        {
           restart();
        }
        //Expect to recieve data via serial line in the following way:
        // 1. First two bytes (lower address followd by upper address) indicating the start address for a block of 64 bytes.
        // 2. Third byte is checksum (modulo 256 sum of all 64 data bytes)
        // 3. 64 bytes of data
        // NOTE: if an adress of 0x000 is recieved there is no more data and the code jumps to the new program code 
        k=0;
        while(1)
        {
            
            //wait until data is recieved on serial rx
            while(PIR1bits.RCIF == 0);
            if(!address_and_checksum_recieved)
            {
                k = k +1;
                if(k<3 && !address_and_checksum_recieved)
                {
                    address = address+(unsigned int) (RCREG<<(8*(k-1)));
                    TXREG = address>>(8*(k-1));
                    while (!TXSTAbits.TRMT);  

                    if(address == 0 && k == 2)
                    {
						//done receiving data and programming the PIC since the address value is set to 0. 
                        OSCCON = Orig_OSCCON;
						OSCTUNEbits.PLLEN = 0;
						
						jump_to_application(); 
                    }
                    if(k==2 && address != START_PROG_ADDR && !first_address_recieved)
                    {
                        first_address_recieved = 1;
                    }
                } 
                if(k==3 && !address_and_checksum_recieved)
                {
                    checksum = RCREG;
                    TXREG = RCREG;
                    while (!TXSTAbits.TRMT);  
                    address_and_checksum_recieved = 1;
                    k = 0;
                }
            }
            else
            {
                uart_recieve_buffer[k] = RCREG;
                sum = sum + uart_recieve_buffer[k];
                k = k + 1;
                if(k == 64) //received 64 bytes - ready to write to flash memory
                {   
                    EraseFlash(address,address+63);
                    WriteBlockFlash(address, 1, uart_recieve_buffer ); 
                    ReadFlash(address, 64, flash_contents);
                    for(i=0;i<64;i++)
                    {
                        if(flash_contents[i] != uart_recieve_buffer[i])
                        {
                            TXREG = 2;
                            while (!TXSTAbits.TRMT);   
                            restart();
                        }
                    }                    
                    
                    if((sum & 255) != checksum)
                    {
                        //checksum doesn't match
                        TXREG = 1;
                        while (!TXSTAbits.TRMT);   
                        restart();
                    }
                    else
                    {
                        TXREG = 0;
                        while (!TXSTAbits.TRMT);   
                    }
                    k = 0;
                    sum =0;
                    address_and_checksum_recieved = 0;
                    address = 0;
                }
            }
                
        }
    }
    else
	{						
		jump_to_application();
    }
    return(0);
    
}

void jump_to_application(void)
{
		//redirect the interrupt vectors to allow application program use vectors
		LATBbits.LATB6= 0;
        #asm
            goto START_PROG_ADDR 
        #endasm	
}


void restart(void)
{
	#asm 
		goto start
	#endasm
}