/****************************************************************************/
/* Copyright 2009 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* boot.c derived from Microchip AN1157                                     */
/****************************************************************************/
#include <p24fxxxx.h>
#include "GenericTypeDefs.h"
#include "memory.h"
#include "config.h"
#include "dig_io.h"


/* JTAG/Code Protect/Write Protect/Clip-on Emulation mode
Watchdog Timer/ICD pins select */
_CONFIG1(JTAGEN_OFF & GCP_OFF & GWRP_OFF & COE_OFF & FWDTEN_OFF & ICS_PGx2) 
/* Enable CLK switch, Disable CLK monitor, OSCO or Fosc/2, 
Primary Oscillator Mode: Disabled, Internal Fast RC oscillator */
_CONFIG2(FCKSM_CSDCMD & OSCIOFNC_OFF & IOL1WAY_OFF & POSCMOD_XT & FNOSC_PRI)
/* Disable CLK switch and CLK monitor, OSCO or Fosc/2, HS oscillator,
Primary oscillator */

_CONFIG3(WPEND_WPSTARTMEM & WPCFG_WPCFGEN & WPDIS_WPEN & WPFP_WPFP2)


/***************************** global variables *****************************/
DWORD resetVector;
DWORD_VAL sourceAddr;

WORD responseBytes;
BYTE buffer[MAX_PACKET_SIZE + 1];
/*************************** function prototypes ****************************/
void ioMap();
void putChar(BYTE b);
void getChar(BYTE* b);
void readPM(WORD length, DWORD_VAL sourceAddr);
void writePM(WORD length, DWORD_VAL sourceAddr);
void erasePM(WORD length, DWORD_VAL sourceAddr);
void getCommand();
void handleCommand();
void putResponse(WORD responseLen);

void doSetBlinky(int state)
{
    Nop();
    
    if ( state )
        LATFbits.LATF5 = 1;
    else
        LATFbits.LATF5 = 0;
    
    Nop();
}

int doGetBlinky()
{
    Nop();
    return (int)LATFbits.LATF5;
}

int waitForBootRequest()
{
    BYTE b;
    int char_count = 0;
    int loop_count = 0;

    /* wait for the "UUUU" bootloader request */
    while ( ++loop_count < 0x1FFF )
    {    
        /* check for receive errors */
        if ( U1STA & 0x000E )
        {
            /* clear parity and/or framing 
            errors by reading a byte */            
            b = U1RXREG;
            /* clear any framing errors */
            U1STAbits.OERR = 0;
        }

        /* if you got a byte check it */
        if ( U1STAbits.URXDA )
        {
            b = U1RXREG;
            if ( b == 0x55 )
            {
                /* count the valid chars */
                ++char_count;
                /* clear the loop counter 
                if you got a valid byte */
                loop_count = 0;
            }
            else
            {
                return FALSE;
            }
            
            /* you got the char sequence return true */
            if ( char_count > 3 )
                return TRUE;
        }
    }
    
    /* you did not get the char sequence return false */
    return FALSE;
}

int main()
{
    int i;
    int start_app = TRUE;
    DWORD flash_word;

    /* map the I/O and config the UART */
    ioMap();

    /* read the bootloader flag */
    sourceAddr.Val = BOOT_FLAG_ADDR;
    flash_word = ReadLatch(sourceAddr.word.HW, sourceAddr.word.LW);
    
    /* if the bootloader flag is set, stay in the bootloader */
    if (flash_word == 0x0000AAAA)
        start_app = FALSE;

    /* read APP_RESET_VECTOR */
    sourceAddr.Val = APP_RESET_VECTOR;
    flash_word = ReadLatch(sourceAddr.word.HW, sourceAddr.word.LW);

    /* if the app reset vector is undefined, stay in the bootloader */
    if (flash_word == 0xFFFFFF)
    {
        resetVector = BOOT_RESET_VECTOR;
        start_app = FALSE;
    }
    else
    {
        resetVector = APP_RESET_VECTOR;
    }
    
    /* turn on blinky while waiting for boot request sequence */
    doSetBlinky(1);

    /* wait for the boot request char sequence */
    if ( waitForBootRequest() )
    {
        /* wait for the xcvr to power up */
        for (i = 0; i < 100; ++i)
            Nop();
        
        /* acknowledge boot request */
        putChar('U');

        /* if you get a boot request, stat in the boot loader */
        start_app = FALSE;
    }

    if ( start_app )
    {
        /* turn off blinky before entering app */
        doSetBlinky(0);
        /* rest to application */
        ResetDevice(resetVector);
    }

    for(;;)
    {
        getCommand();
        handleCommand();
        putResponse(responseBytes);
    }
}


void getCommand()
{
    BYTE RXByte;
    BYTE checksum;
    
    WORD dataCount;

    for(;;)
    {
        /* get a char */
        getChar(&RXByte);   
        
        /* If it's a STX you gotta packet start */
        if (RXByte == STX)
        { 
            /* get the second byte packet */
            getChar(&RXByte);

            if (RXByte == STX)
            {    
                /* clear data count and checksum */
                checksum = 0;    
                dataCount = 0;

                while ( dataCount <= (MAX_PACKET_SIZE + 1) )
                {
                    /* get a data byte */
                    getChar(&RXByte);

                    switch(RXByte)
                    {   
                        /* if you get an STX start over */
                        case STX:   checksum = 0; 
                                    dataCount = 0;
                                    break;

                        /* if you get an ETX then you're done */
                        case ETX:   checksum = ~checksum + 1;
                                    Nop();
                                    
                                    /* if the checksum is OK, return */
                                    if ( checksum == 0 ) 
                                        return;

                                    /* restart if checksum fails */
                                    dataCount = 0xFFFF;    
                                    break;

                        /* if DLE, treat next as data */
                        case DLE:   getChar(&RXByte);
                        
                        /* get data, put in buffer */
                        default:    checksum += RXByte;
                                    buffer[dataCount++] = RXByte;
                                    break;
                    }
                }
            }
        }
    }
}

void handleCommand()
{
    BYTE command;
    BYTE length;

    WORD_VAL temp;
    WORD bytesRead = 0;

    /* get command and data length from buffer */
    command = buffer[0];
    length = buffer[1];

    /* zero length is a RESET command */
    if (length == 0x00)
    {
        U1MODEbits.UARTEN = 0;
        doSetBlinky(0);
        ResetDevice(resetVector);
    }

    /* get the address from buffer */
    sourceAddr.v[0] = buffer[2];        
    sourceAddr.v[1] = buffer[3];
    sourceAddr.v[2] = buffer[4];
    sourceAddr.v[3] = 0;

    /* process the commnand */
    switch(command)
    {
        /* read the bootloader version */
        case RD_VER:    buffer[2] = MINOR_VERSION;
                        buffer[3] = MAJOR_VERSION;
                        responseBytes = 4;
                        break;
        
        /* read flash memory */
        case RD_FLASH:  readPM(length, sourceAddr); 
                        responseBytes = (length * PM_INSTR_SIZE) + 5;
                        break;
        
        /* write flash memory */
        case WT_FLASH:  writePM(length, sourceAddr);    
                        responseBytes = 1;
                        break;

        /* erase flash memory */
        case ER_FLASH:  erasePM(length, sourceAddr);    
                        responseBytes = 1;
                        break;

        /* read config memory */
        case RD_CONFIG: while (bytesRead < length)
                        {
                            /* read the bytes */
                            temp.Val = ReadLatch(sourceAddr.word.HW, 
                                                 sourceAddr.word.LW);

                            /* store bytes in buffer */
                            buffer[bytesRead + 5] = temp.v[0];
                            ++bytesRead; 
                            
                            /* increment the address pointer */
                            sourceAddr.Val += 2;
                        }

                        /* the number of response bytes */
                        responseBytes = length + 5;
                        break;

        /* verify OK.  note: this is a dummy operation for
        backward compatibility with microchip programmer */
        case VERIFY_OK: responseBytes = 1;
                        break;

        default: break;
    }
}

void putResponse(WORD responseLen)
{
    WORD i;
    BYTE data;
    BYTE checksum;

    /* put start of packet */
    putChar(STX);
    putChar(STX);

    /* output buffer as response packet */
    checksum = 0;
    for (i = 0; i < responseLen; i++)
    {
        /* get data from repsonse buffer and clac checksum */
        data = buffer[i];
        checksum += data;

        /* if control character, stuff DLE */
        if ( (data == STX) || (data == ETX) || (data == DLE) )
            putChar(DLE);

        /* send the data */
        putChar(data);
    }

    /* calc the final checksum */
    checksum = ~checksum + 1;

    /* if the checkum happens to equal a control character, send a DLE */
    if( (checksum == STX) || (checksum == ETX) || (checksum == DLE) )
        putChar(DLE);

    /* send the chacksum */
    putChar(checksum);
    /* send the end of packet */
    putChar(ETX);

    while(!U1STAbits.TRMT)
        ;   /* wait for transmit to finish */
}

void putChar(BYTE b)
{
    while(U1STAbits.UTXBF)
        ; /* wait for FIFO space */
    
    /* put the the char in the xmit FIFO */
    U1TXREG = b;
}

void getChar(BYTE* b)
{
    BYTE dummy;

    for(;;)
    {    
        /* check for receive errors */
        if ((U1STA & 0x000E) != 0x0000)
        {
            dummy = U1RXREG;        /* dummy read to clear FERR/PERR */
            U1STAbits.OERR = 0;     /* clear OERR to keep receiving */
        }

        /* get the data */
        if (U1STAbits.URXDA == 1)
        {
            *b = U1RXREG;   /* get data from UART RX FIFO */
            break;
        }
    }
}

void readPM(WORD length, DWORD_VAL sourceAddr)
{
    WORD bytesRead = 0;
    DWORD_VAL temp;

    /* Read length instructions from flash */
    while (bytesRead < (length * PM_INSTR_SIZE))
    {
        /* read flash */
        temp.Val = ReadLatch(sourceAddr.word.HW, sourceAddr.word.LW);

        /* put bytes in global respnse buffer */
        buffer[bytesRead+5] = temp.v[0];
        buffer[bytesRead+6] = temp.v[1];
        buffer[bytesRead+7] = temp.v[2];
        buffer[bytesRead+8] = temp.v[3];
    
        /* increment byte counter by 4 bytes per instruction */
        bytesRead+=PM_INSTR_SIZE; 
        
        /* increment address counter  by 2 */
        sourceAddr.Val = sourceAddr.Val + 2;
    }
}

void writePM(WORD length, DWORD_VAL sourceAddr)
{
    WORD bytesWritten;
    DWORD_VAL data;

    bytesWritten = 0;
    
    /* write length rows to flash */
    while ((bytesWritten) < (length * PM_ROW_SIZE))
    {
        /* get data to write from buffer, 
        note: first 5 buffer locations are cmd, len, addr */
        data.v[0] = buffer[bytesWritten + 5];   
        data.v[1] = buffer[bytesWritten + 6];
        data.v[2] = buffer[bytesWritten + 7];
        data.v[3] = buffer[bytesWritten + 8];

        /* 4 bytes per instruction: low word, high byte, phantom byte */
        bytesWritten += PM_INSTR_SIZE;

        if ( (sourceAddr.Val > BOOT_LOADER_END) && 
             (sourceAddr.Val < CONFIG_START) )
        {
            WriteLatch(sourceAddr.word.HW, 
                       sourceAddr.word.LW, 
                       data.word.HW, 
                       data.word.LW);
        }

        /* write to flash memory if complete row is finished */
        if ((bytesWritten % PM_ROW_SIZE) == 0)
            WriteMem(PM_ROW_WRITE);

        /* increment addr by 2 */
        sourceAddr.Val = sourceAddr.Val + 2;  
    }
}

void erasePM(WORD length, DWORD_VAL sourceAddr)
{
    WORD i = 0;
    
    while (i < length)
    {
        ++i;
    
        /* erase app memory only */
        if ( (sourceAddr.Val > BOOT_LOADER_END) && 
             (sourceAddr.Val < CONFIG_START) )
        {
            Erase(sourceAddr.word.HW, sourceAddr.word.LW, PM_PAGE_ERASE);
        }
        
        /* increment by a page */
        sourceAddr.Val += (PM_PAGE_SIZE / 2);
    }
}

void ioMap()
{
    /* From periph.c	*/

    /* Disable Watch Dog Timer */
    RCONbits.SWDTEN = 0;

    /*************************** Map Pins UART 1 ****************************/
    RPINR18bits.U1RXR = 24;  /* Make Pin RP24 U1RX */
    RPOR5bits.RP11R = 3;     /* Make Pin RP11 U1TX */

    /*************************** Map Pins UART 2 ****************************/
    RPINR19bits.U2RXR = 22; // Make Pin RP22 U2RX
    RPOR11bits.RP23R = 5;   // Make Pin RP23 U2TX  

    /*************************** Map Pins UART 3 ****************************/
    RPINR17bits.U3RXR = 20; // Make Pin RP20 U3RX
    RPOR12bits.RP25R = 28;   // Make Pin RP25 U3TX  

    /*************************** Map Pins UART 4 ****************************/
    RPINR27bits.U4RXR = 3; // Make Pin RP3 U4RX
    RPOR2bits.RP4R = 30;   // Make Pin RP4 U4TX  
    
    /************************** Map SPI1 Bus Pins ***************************/
    RPINR20bits.SDI1R = 19;	// SDI1 (MISO) RP19
    RPOR13bits.RP27R = 7;	// SDO1 (MOSI) RP27
    RPOR13bits.RP26R = 8;	// SCK1 (SCK)  RP26


    /* delay 1 cycle before set output bits */
    Nop();

    /********************** Disable Unused Peripherals **********************/
    /* PMD1 */
    _ADC1MD = 1;	/* A/D module off	*/
    _SPI1MD = 1;	/* SPI1 off		*/
    _SPI2MD = 1;	/* SPI2 off		*/
    _U1MD = 0;		/* UART 1-4 on		*/
    _U2MD = 0;
    _I2C1MD = 1;	/* I2C modules off	*/
    
    _T1MD = 0;		/* Timers 1-3 on	*/
    _T2MD = 0;
    _T3MD = 0;
    _T4MD = 1;		/* Timers 4-5 off	*/
    _T5MD = 1;
    
    /* PMD2 */
    _OC1MD = 1;		/* Output comparators off*/
    _OC2MD = 1;
    _OC3MD = 1;
    _OC4MD = 1;
    _OC5MD = 1;
    _OC6MD = 1;
    _OC7MD = 1;
    _OC8MD = 1;
    _IC1MD = 1;		/* Input comparators off*/
    _IC2MD = 1;
    _IC3MD = 1;
    _IC4MD = 1;
    _IC5MD = 1;
    _IC6MD = 1;
    _IC7MD = 1;
    _IC8MD = 1;

    /* PMD3 */
    _I2C2MD = 1;	/* I2C modules off	*/
    _I2C3MD = 1;
    _U3MD = 0;		/* UART 3 on		*/
    _PMPMD = 1;
    _RTCCMD = 1;
    _CMPMD = 1;
    _CRCMD = 1;

    /* PMD4 */
    _LVDMD = 1;
    _CTMUMD = 1;
    _REFOMD = 1;
    _U4MD = 0;		/* UART4 on		*/
    _I2C2MD = 1;

    /* PMD5 */
    _OC9MD = 1;		/* Output comparators off*/
    _IC9MD = 1;		/* Input comparators off*/
    
    /* PMD6 */
    _SPI3MD = 1;	/* SPI3 bus off		*/


    /* From dig_io.c	*/
    /* set outputs here */

	TRISBbits.TRISB0 = 0;	//Unused
	TRISBbits.TRISB1 = 0;	//Unused
        TRISBbits.TRISB2 = 0;	//Unused
	TRISBbits.TRISB3 = 0;	//Unused
	TRISBbits.TRISB4 = 0;	//Unused
        TRISBbits.TRISB5 = 0;	//Unused

        TRISCbits.TRISC13 = 0;	//Unused
        TRISCbits.TRISC14 = 0;	//Unused

	TRISDbits.TRISD6 = 0;	//UART_1_*SHDN
	TRISDbits.TRISD7 = 0;	//UART_2_*SHDN
	TRISDbits.TRISD8 = 0;	//UART_3_*SHDN
        TRISDbits.TRISD11 = 0;	//Unused

	TRISEbits.TRISE0 = 0;	//INSTR_1_ON
	TRISEbits.TRISE1 = 0;	//INSTR_1_OFF
	TRISEbits.TRISE2 = 0;	//INSTR_2_ON
	TRISEbits.TRISE3 = 0;	//INSTR_2_OFF
        TRISEbits.TRISE4 = 0;	//INSTR_3_ON
        TRISEbits.TRISE5 = 0;	//INSTR_3_OFF
	TRISEbits.TRISE6 = 0;	//INSTR_4_ON
	TRISEbits.TRISE7 = 0;	//INSTR_4_OFF

	TRISFbits.TRISF4 = 0;	//Unused
	TRISFbits.TRISF5 = 0;	//LED
	TRISFbits.TRISF6 = 0;	//DC-DC_CNTL

	TRISGbits.TRISG2 = 0;	//ADC_SHDN
	TRISGbits.TRISG6 = 0;	//SPI_CS

       
    /* set inputs here */
   
	TRISFbits.TRISF0 = 1;	//INSTR_1_STATE
	TRISFbits.TRISF1 = 1;	//INSTR_2_STATE
	TRISFbits.TRISF2 = 1;	//INSTR_3_STATE
        TRISFbits.TRISF3 = 1;	//INSTR_4_STATE

        TRISGbits.TRISG3 = 1;	//ADC_BUSY

	TRISBbits.TRISB8  = 1;  //Hardware Config switches
	TRISBbits.TRISB9  = 1;
	TRISBbits.TRISB10 = 1;
	TRISBbits.TRISB11 = 1;
	TRISBbits.TRISB12 = 1;
	TRISBbits.TRISB13 = 1;
	TRISBbits.TRISB14 = 1;
	TRISBbits.TRISB15 = 1;



    /* put out bits in default state here */
    Nop();
    INSTR_1_ON = 0;  // 1 on
    Nop();
    INSTR_2_ON = 0;  // 2 on
    Nop();
    INSTR_3_ON = 0;  // 3 on
    Nop();
    INSTR_4_ON = 1;  // 4 on
    Nop();
    INSTR_1_OFF = 0; // 1 off
    Nop();
    INSTR_2_OFF = 0; // 2 off
    Nop();
    INSTR_3_OFF = 0; // 3 off
    Nop();
    INSTR_4_OFF = 1; // 4 off
    Nop();
    UART_1_SHDN = 1;  // uart 1 shdn
    Nop();
    UART_2_SHDN = 1;
    Nop();
    UART_3_SHDN = 1;
    Nop();
    LED_BIT = 0;
    Nop();
    DC_CTRL = 0;
    Nop();
    ADC_SHDN = 1;
    Nop();
    SPI_CS = 1;
    Nop();
			//Remaining output bits unused, initialized high	
    OUT_BIT_14 = 1;
    Nop();
    OUT_BIT_15 = 1;
    Nop();
    OUT_BIT_16 = 1;
    Nop();
    OUT_BIT_17 = 1;
    Nop();
    OUT_BIT_18 = 1;
    Nop();
    OUT_BIT_19 = 1;
    Nop();
    OUT_BIT_20 = 1;
    Nop();
    OUT_BIT_21 = 1;
    Nop();
    OUT_BIT_22 = 1;
    Nop();
    OUT_BIT_23 = 1;
    Nop();
    OUT_BIT_24 = 1;
    Nop();


    /*********************************************/
    /*           Baud Rate Calculation           */
    /*********************************************/
    /*                                           */
    /* BRGH = 0                                  */
    /*                                           */
    /*   UxBRG = ( Fcy / ( 16 * baud )) - 1      */
    /*                                           */
    /* BRGH = 1                                  */
    /*                                           */
    /*   UxBRG = ( Fcy / ( 4 * baud )) - 1       */
    /*                                           */
    /*   Current Settings                        */
    /*   Fosc = 3.6864 MHz                       */
    /*   Fcy = Fosc / 2                          */
    /*   BRGH = 1                                */
    /*                                           */
    /*    ___________________________            */
    /*   |  Baud          |  UxBRG  |            */
    /*   ----------------------------            */
    /*   |  19200        |   23     |            */
    /*   ----------------------------            */
    /*   |  38400        |   11     |            */
    /*   ----------------------------            */
    /*   |  57600        |   7      |            */
    /*   ----------------------------            */
    /*   |  115200       |   3      |            */
    /*   ---------------------------             */
    /*                                           */
    /*********************************************/

    /* 38400 baud */
    U1BRG = 11;
    
    /* No parity, one stop bit, no autobaud, polled */
    U1MODEbits.UARTEN = 1;
    U1MODEbits.ABAUD = 0;
    U1MODEbits.BRGH = 1;
    U1STA = 0x0400;

    /* clear any pending interrupts just in case */
    _U1RXIF = 0;
    _U1TXIF = 0;

    /* disable receive interrupts, disable transmit interrupts */
    _U1RXIE = 0;
    _U1TXIE = 0;
}