#include <msp430.h>                      // Generic MSP430 Device Include
#include "stdio.h"
#include "string.h"
#include "bsl.h"
#include "uart.h"

uint8_t BSL_buffer[255]; 
uint8_t buffer[8];
//*****************************************************************************
//
// ! BSL Entry Sequence
// ! Forces target to enter BSL mode
//
//*****************************************************************************
void Host_BSL_entry_sequence()
{
    /* BSL reset sequence
     *
     *            H                        +-------------+
     * P1.3 RST   L -----------------------+
     *
     *            H        +-----+      +-----+
     * P1.4 TEST  L--------+     +------+     +---------+
     *
     *            H -------+     +------+     +---------+
     * P1.5 TCK   L        +-----+      +-----+
     */

    /* RST low, TEST low */

   P1OUT &= ~BIT3;
   P1OUT &= ~BIT4;
    __delay_cycles(BSL_DELAY);
    __delay_cycles(BSL_DELAY);
    __delay_cycles(BSL_DELAY);
    /* RST low, TEST high ,TCK low*/
    P1OUT |= BIT4;
    P1OUT &= ~BIT5;
    __delay_cycles(BSL_DELAY);
    /* RST low, TEST low ,TCK high*/
    P1OUT &= ~BIT4;
    P1OUT |= BIT5;
    __delay_cycles(BSL_DELAY);
    /* RST low, TEST high ,TCK low*/
    P1OUT |= BIT4;
    P1OUT &= ~BIT5;
    __delay_cycles(BSL_DELAY);
    /* RST high, TEST high ,TCK low*/
    P1OUT |= BIT3;
    __delay_cycles(BSL_DELAY);
    /* RST high, TEST low ,TCK high*/
    P1OUT &= ~BIT4;
    P1OUT |= BIT5;
    __delay_cycles(BSL_DELAY);
    __delay_cycles(BSL_DELAY);
    __delay_cycles(BSL_DELAY);

    // Set RST, TST and to input
   // P1DIR &= ~BIT3;
   // P1DIR &= ~BIT4;
}

//*****************************************************************************
//
// ! BSL Entry Sequence
// ! Unlock BSL for programming
// ! If first time, assume blank device.
// ! This will cause a mass erase and destroy previous password.
// ! When programming complete, issue BSL_readPassword to retrieve new one.
//
//*****************************************************************************
BSL_error_t Host_BSL_loadPassword(uint8_t* pPassword)
{
    BSL_error_t bsl_err = eBSL_success;
    uart_error_t uart_ack;
    uint16_t ui16CRC;

    BSL_buffer[0] = (uint8_t)PACKET_HEADER;
    BSL_buffer[1] = LSB(PASSWORD_SIZE + CMD_BYTE);
    BSL_buffer[2] = 0x00;
    BSL_buffer[3] = CMD_RX_PASSWORD;

    memcpy(&BSL_buffer[4], pPassword, PASSWORD_SIZE);

    // Calculate CRC on the PAYLOAD (CMD + PASSWORD)
    ui16CRC = softwareCRC(&BSL_buffer[3], PASSWORD_SIZE + CMD_BYTE);

    // Insert the CRC into the packet
    BSL_buffer[PASSWORD_RX_PACKET_SIZE - CRC_BYTES] = LSB(ui16CRC);
 //   BSL_buffer[PASSWORD_RX_PACKET_SIZE - CRC_BYTES] = 0;
    BSL_buffer[PASSWORD_RX_PACKET_SIZE - CRC_BYTES + 1] = MSB(ui16CRC);

    // Write the packet to the target
    uart_ack = UART_writeBuffer(BSL_buffer, HDR_LEN_CRC_BYTES + CMD_BYTE + PASSWORD_SIZE);
    if(uart_ack != uart_noError)
    {
        P4OUT |= BIT6;
    }

    bsl_err = Host_BSL_getResponse();

   return(bsl_err);
}

//*****************************************************************************
//
// ! Host_BSL_writeMemory
// ! Writes memory section to target
//
//*****************************************************************************
BSL_error_t Host_BSL_writeMemory(uint32_t addr, const uint8_t* data,  uint32_t len)
{
    BSL_error_t bsl_err = eBSL_success;
    uart_error_t uart_ack;
    uint16_t ui16DataLength;
    uint16_t ui16PayloadSize;
    uint16_t ui16PacketSize;
    uint16_t ui16CRC;
    uint16_t ui16BytesToWrite = len;
    uint32_t TargetAddress = addr;

  //  pSection->checksum = softwareCRC(pSection->pMemory, pSection->mem_size);

    while(ui16BytesToWrite > 0)
    {
        __delay_cycles(200000); //allow target deal with the packet send before

        if(ui16BytesToWrite >= MAX_PAYLOAD_DATA_SIZE)
            ui16DataLength = MAX_PAYLOAD_DATA_SIZE;
        else
            ui16DataLength = ui16BytesToWrite;

        ui16BytesToWrite = ui16BytesToWrite - ui16DataLength;

        // Add (1byte) command + (3 bytes) 3 ADDRS = 4 bytes to the payload
        ui16PayloadSize = (CMD_ADDRS_BYTES + ui16DataLength);

        BSL_buffer[0] = PACKET_HEADER;
        BSL_buffer[1] = LSB(ui16PayloadSize);  // typically 4 + MAX_PAYLOAD SIZE
        BSL_buffer[2] = MSB(ui16PayloadSize);
        BSL_buffer[3] = (uint8_t)CMD_RX_DATA_BLOCK;
        BSL_buffer[4] = ((uint8_t) (TargetAddress & 0xFF));
        BSL_buffer[5] = ((uint8_t) (TargetAddress >> 8));
        BSL_buffer[6] = ((uint8_t) (TargetAddress >> 16));  // Upper ADDRS byte always 0

        // Bump up the target address by 2x the number of bytes sent for the next packet
        TargetAddress += ui16DataLength;

        // Copy the data into the buffer
        memcpy(&BSL_buffer[7], data, ui16DataLength);

        data +=ui16DataLength;

        // Calculate CRC on the PAYLOAD
        ui16CRC = softwareCRC(&BSL_buffer[3], ui16PayloadSize);

        // Calculate the packet length
        ui16PacketSize = HDR_LEN_CRC_BYTES + ui16PayloadSize;

        // Insert the CRC into the packet at the end
        BSL_buffer[ui16PacketSize - CRC_BYTES] = LSB(ui16CRC);
        BSL_buffer[ui16PacketSize - CRC_BYTES + 1] = MSB(ui16CRC);

        // Write the packet to the target
//        I2C_error = I2CWrite(TARGET_MCU_ADDRS, BSL_buffer, ui16PacketSize);
        uart_ack = UART_writeBuffer(BSL_buffer, ui16PacketSize);
        if(uart_ack != uart_noError)
        {
            P4OUT |= BIT6;
        }

        // Check operation was complete
         bsl_err = Host_BSL_getResponse();
         if(bsl_err != eBSL_success)
             break;


    }// end while

    return(bsl_err);
}



//*****************************************************************************
//
// ! softwareCRC
// ! Can be used on MSP430 and non-MSP platforms
// ! This functions computes the 16-bit CRC (same as BSL on MSP target)
//
//*****************************************************************************
uint16_t softwareCRC(const uint8_t* data, uint8_t length)
{

    uint16_t crc = 0xFFFF;
    uint8_t i;
    uint8_t x;

    for(i = 0; i < length; i++)
    {
        x = ((crc >> 8) ^ data[i]) & 0xff;
        x ^= x >> 4;
        crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x;
    }

    return(crc);
}

//*****************************************************************************
//
// ! Host_BSL_getResponse
// ! For those function calls that don't return specific data.
// ! Returns errors.
//
//*****************************************************************************
BSL_error_t Host_BSL_getResponse(void)
{
 //   packet_error_t packet_err = eBSL_packet_ack;
    BSL_error_t bsl_err = eBSL_success;
 //   uint8_t uart_ack;
 //   uint8_t coreCommand;


    UART_readBuffer(buffer, (ACK_BYTE + HDR_LEN_CRC_BYTES + CMD_BYTE));


 //   packet_err = buffer[RESPONSE_ACK_POS];

    bsl_err = buffer[4];

 //   coreCommand = buffer[RESPONSE_CMD_POS];

    return(bsl_err);
}

void SetPC(const uint8_t* PCaddress)
{
    uart_error_t uart_ack;
    uint16_t ui16CRC;
    uint16_t ui16PacketSize;

    BSL_buffer[0] = (uint8_t)PACKET_HEADER;
    BSL_buffer[1] = LSB(PC_BYTES + CMD_BYTE);
    BSL_buffer[2] = 0x00;
    BSL_buffer[3] = CMD_LOAD_PC;
    BSL_buffer[4] = *PCaddress++;
    BSL_buffer[5] = *PCaddress;
    BSL_buffer[6] = 0x00;  // Upper ADDRS byte always 0

    ui16CRC = softwareCRC(&BSL_buffer[3], PC_BYTES + CMD_BYTE);

    BSL_buffer[7] = LSB(ui16CRC);
    BSL_buffer[8] = MSB(ui16CRC);

    ui16PacketSize = PC_BYTES + CMD_BYTE + HDR_LEN_CRC_BYTES;

    uart_ack = UART_writeBuffer(BSL_buffer, ui16PacketSize);
 //   if(uart_ack != uart_noError)
 //   {
 //       P4OUT |= BIT6;
//    }

}

