/*
 * Copyright (C) 2016 Texas Instruments Incorporated - http://www.ti.com/
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

//******************************************************************************
// Version history:
// 1.0 07/17             Initial version. (Nima Eskandari)
// 1.1 07/17             Added Comments. (Nima Eskandari)
//----------------------------------------------------------------------------
//   Designed 2017 by Texas Instruments
//
//   Nima Eskandari
//   Texas Instruments Inc.
//   August 2017
//   Built with CCS Version: Code Composer Studio v7
//******************************************************************************

#include <stdio.h>
#include <stdbool.h>
#include <stddef.h>
#include <unistd.h>
#include <uart_if.h>
#include <gpio_if.h>
#include <config.h>
#include <bsl.h>
#include <utils.h>
#include <Board.h>
#include <debug.h>

//*****************************************************************************
// Buffer for send and receive ************************************************
//*****************************************************************************


uint8_t sendBuffer[270] = {0};
uint8_t receiveBuffer[270] = {0};


//*****************************************************************************
// Commands *******************************************************************
//*****************************************************************************

#define PASSWORD_LENGTH 	0x20
#define RX_PASSWORD_L1L2	0x24
#define ERASE_SEGMENT_L1L2	0x04
#define ERASE_MAIN_L1L2		0x04
#define MASS_ERASE_L1L2		0x04
#define ERASE_CHECK_L1L2	0x04
#define SET_MEM_OFFSET_L1L2	0x04
#define LOAD_PC_L1L2		0x04
#define TX_DATA_BLOCK_L1L2	0x04


#define RX_PASSWORD 		0x10
#define SET_MEM_OFFSET 		0x21  
#define TX_DATA_BLOCK		0x14
#define RX_DATA_BLOCK		0x12
#define ERASE_SEGMENT		0x16
#define ERASE_MAIN			0x16
#define MASS_ERASE			0x18
#define ERASE_CHECK			0x1C
#define LOAD_PC				0x1A

#define HEADER				0x80
#define DATA_ACK			0x90
#define DATA_NAK			0xA0

#define PASSWORD_ADDRESS 	0xffe0

#define GetCKL(cs)			(uint8_t)(cs)
#define GetCKH(cs)			(uint8_t)(cs >> 8)

bool SendSync()
{
	uint8_t uartError = 0;
	uint8_t ackResult = 0;
	UART_SendByte(HEADER);
	ackResult = UART_ReadByteWithTimeout(&uartError);
	MsDelay(5);
	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{

#if DEBUG
	    Debug_UART_printStringWithNumber("Sync failed: 0x%x\r\n", ackResult);
#endif
		return false;
	}
}

//*****************************************************************************
// Write The Default Password *************************************************
// Uses the write password function to write the default password (0xFF) ******
//*****************************************************************************

bool WritePasswordDefault()
{
	uint8_t password[PASSWORD_LENGTH] = {0};
	uint16_t loopIndex = 0;

	for (loopIndex = 0; loopIndex < PASSWORD_LENGTH; loopIndex++)
	{
		password[loopIndex] = 0xFF;
	}

	return WritePassword(password, PASSWORD_LENGTH);
}


//*****************************************************************************
// Write the BSL password *****************************************************
// password: The password for BSL *********************************************
// passwordSize: The size of the password array *******************************
//*****************************************************************************

bool WritePassword(uint8_t* password, uint16_t passwordSize)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;
	if (!SendSync())
	{
		return false;
	}

	if (passwordSize != PASSWORD_LENGTH)
	{
#if DEBUG
	    Debug_UART_printStringWithNumber("Password is incorrect size: 0x%x\r\n", passwordSize);
#endif
		return false;
	}
	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = RX_PASSWORD;
	sendBuffer[2] = RX_PASSWORD_L1L2;
	sendBuffer[3] = RX_PASSWORD_L1L2;

	sendBuffer[4] = (uint8_t)(PASSWORD_ADDRESS & 0x00ff);
	sendBuffer[5] = (uint8_t)((PASSWORD_ADDRESS >> 8) & 0x00ff);
	sendBuffer[6] = (uint8_t)(PASSWORD_LENGTH & 0x00ff);
	sendBuffer[7] = (uint8_t)((PASSWORD_LENGTH >> 8) & 0x00ff);

	memcpy(&sendBuffer[8], password, PASSWORD_LENGTH);

	checksum = CalculateChecksum(sendBuffer, (uint16_t)(PASSWORD_LENGTH + 4 + 4));
	sendBuffer[PASSWORD_LENGTH + 4 + 4] = GetCKL(checksum);
	sendBuffer[PASSWORD_LENGTH + 4 + 5] = GetCKH(checksum);

	UART_SendByteArray(sendBuffer, PASSWORD_LENGTH + 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}
}


//*****************************************************************************
// Read the MSP memory ********************************************************
// startAddress: The address to start reading *********************************
// lenght: The length of the memory block to be read **************************
// dataResult: The array to contain the data read *****************************
//*****************************************************************************

bool ReadMemory(uint16_t startAddress, uint8_t lenght, uint8_t * dataResult)
{
	uint16_t checksum = 0;
	uint16_t loopIndex = 0;
	uint8_t uartError;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = TX_DATA_BLOCK;
	sendBuffer[2] = TX_DATA_BLOCK_L1L2;
	sendBuffer[3] = TX_DATA_BLOCK_L1L2;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = (uint8_t)(lenght & 0x00ff);
	sendBuffer[7] = (uint8_t)((0 >> 8) & 0x00ff);

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	MsDelay(5);
	UART_ReadByteArrayWithTimeout(receiveBuffer, 4 + lenght + 2, &uartError);

	if (receiveBuffer[0] != HEADER)
	{
#if DEBUG
	    Debug_UART_printStringWithNumber("HEADER is incorrect: 0x%x\r\n", receiveBuffer[0]);
#endif
		return false;
	}
	else
	{
		for (loopIndex = 0; loopIndex < lenght; loopIndex ++)
		{
			dataResult[loopIndex] = receiveBuffer[4 + loopIndex];
		}

		checksum = ((uint16_t)receiveBuffer[4 + lenght]) | (((uint16_t)receiveBuffer[4 + lenght + 1]) << 8);

		if (checksum != CalculateChecksum(receiveBuffer, 4 + lenght))
		{
#if DEBUG
		    Debug_UART_printStringWithNumber("Checksum is incorrect: 0x%x\r\n", checksum);
#endif
		}
	}

	return true;
}


//*****************************************************************************
// Write data to MSP memory ***************************************************
// startAddress: The address to start the memory write ************************
// length: The length of the data to be writtem *******************************
// data: The array containing the data to write ******************************* 
//*****************************************************************************


bool WriteMemory(uint16_t startAddress, uint8_t lenght, uint8_t * data)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = RX_DATA_BLOCK;
	sendBuffer[2] = lenght + 4;
	sendBuffer[3] = lenght + 4;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = (uint8_t)(lenght & 0x00ff);
	sendBuffer[7] = (uint8_t)((0 >> 8) & 0x00ff);

	memcpy(&sendBuffer[8], data, lenght);

	checksum = CalculateChecksum(sendBuffer, lenght + 8);
	sendBuffer[lenght + 8] = (uint8_t)(checksum);
	sendBuffer[lenght + 9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, lenght + 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}

}

//*****************************************************************************
// Write large data array to memory. This includes data with length larger ****
// than MAX_CHUNK_SIZE ********************************************************
// startAddress: The address to start the memory write ************************
// length: The length of the data to be writtem *******************************
// data: The array containing the data to write ******************************* 
//*****************************************************************************

#define MAX_CHUNK_SIZE  40 //45 causes CC2640 Driver to break
bool WriteLargeDataToMemory(uint16_t startAddress, uint32_t length, uint8_t * data)
{
	uint32_t currentAddress = startAddress;
	uint32_t currentLength = length;
	uint8_t * currentData = data;
	bool done = false;
	bool result = true;

	while(!done)
	{
		if (currentLength <= 0)
		{
			done = true;
		}
		else if (currentLength < MAX_CHUNK_SIZE)
		{
			result = WriteMemory(currentAddress, currentLength, currentData);
			if (!result)
			{
				return result;
			}
			done = true;
		}
		else
		{
			result = WriteMemory(currentAddress, MAX_CHUNK_SIZE, currentData);
			if (!result)
			{
				return result;
			}
			currentAddress += MAX_CHUNK_SIZE;
			currentData += MAX_CHUNK_SIZE;
			currentLength -= MAX_CHUNK_SIZE;
		}
	}

	return true;
}

//*****************************************************************************
// Perform a mass erase *******************************************************
//*****************************************************************************


bool MassErase()
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = MASS_ERASE;
	sendBuffer[2] = MASS_ERASE_L1L2;
	sendBuffer[3] = MASS_ERASE_L1L2;

	sendBuffer[4] = 0;
	sendBuffer[5] = 0;
	sendBuffer[6] = 0x06;
	sendBuffer[7] = 0xA5;

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}

}

bool EraseMainInfo(uint16_t startAddress)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = ERASE_MAIN;
	sendBuffer[2] = ERASE_MAIN_L1L2;
	sendBuffer[3] = ERASE_MAIN_L1L2;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = 0x04;
	sendBuffer[7] = 0xA5;

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}


}


bool EraseSegment(uint16_t startAddress)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = ERASE_SEGMENT;
	sendBuffer[2] = ERASE_SEGMENT_L1L2;
	sendBuffer[3] = ERASE_SEGMENT_L1L2;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = 0x02;
	sendBuffer[7] = 0xA5;

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}


}


bool EraseCheck(uint16_t startAddress, uint16_t length)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = ERASE_CHECK;
	sendBuffer[2] = ERASE_CHECK_L1L2;
	sendBuffer[3] = ERASE_CHECK_L1L2;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = (uint8_t)(length & 0x00ff);
	sendBuffer[7] = (uint8_t)((length >> 8) & 0x00ff);

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}


}


//*****************************************************************************
// Load the program counter with the specified address ************************
//*****************************************************************************

bool LoadPC(uint16_t startAddress)
{
	uint8_t ackResult = 0;
	uint16_t checksum = 0;

	if (!SendSync())
	{
		return false;
	}

	sendBuffer[0] = (uint8_t)(HEADER);
	sendBuffer[1] = LOAD_PC;
	sendBuffer[2] = LOAD_PC_L1L2;
	sendBuffer[3] = LOAD_PC_L1L2;

	sendBuffer[4] = (uint8_t)(startAddress & 0x00ff);
	sendBuffer[5] = (uint8_t)((startAddress >> 8) & 0x00ff);
	sendBuffer[6] = 0;
	sendBuffer[7] = 0;

	checksum = CalculateChecksum(sendBuffer, 8);
	sendBuffer[8] = (uint8_t)(checksum);
	sendBuffer[9] = (uint8_t)(checksum >> 8);

	UART_SendByteArray(sendBuffer, 4 + 6);
	ackResult = UART_ReadByte();
	MsDelay(5);

	if (ackResult == DATA_ACK)
	{
		return true;
	}
	else
	{
		return false;
	}

}

uint16_t CalculateChecksum(uint8_t data[], uint16_t length)
{
  uint16_t* i_data;
  uint16_t checksum= 0;
  uint16_t i= 0;

  i_data= (uint16_t*)data;

  for (i= 0; i < length/2; i++)
  { 
    checksum^= i_data[i];    /* xor-ing   */
  }
  return(checksum ^ 0xffff); /* inverting */
}

//*****************************************************************************
// Enter BSL mode using RST and TST pins **************************************
//*****************************************************************************

void BSLEntrySequence()
{

	GPIO_IF_setOutputHighOnPin(Board_TEST_PIN);
	GPIO_IF_setOutputHighOnPin(Board_RESET_PIN);
	MsDelay(200);

	GPIO_IF_setOutputLowOnPin(Board_TEST_PIN);
	usDelay(800);
	GPIO_IF_setOutputLowOnPin(Board_RESET_PIN);
	usDelay(20);

	GPIO_IF_setOutputHighOnPin(Board_TEST_PIN);
	usDelay(20);
	GPIO_IF_setOutputLowOnPin(Board_TEST_PIN);
	usDelay(20);

	GPIO_IF_setOutputHighOnPin(Board_TEST_PIN);
	usDelay(20);
	GPIO_IF_setOutputHighOnPin(Board_RESET_PIN);
	usDelay(20);
	GPIO_IF_setOutputLowOnPin(Board_TEST_PIN);
	usDelay(20);

}

//*****************************************************************************
// Reset the MSP. Exits the BSL Mode ******************************************
//*****************************************************************************

void Reset()
{
	GPIO_IF_setOutputLowOnPin(Board_TEST_PIN);
	GPIO_IF_setOutputHighOnPin(Board_RESET_PIN);
	MsDelay(200);

	GPIO_IF_setOutputLowOnPin(Board_RESET_PIN);
	usDelay(100);
	GPIO_IF_setOutputHighOnPin(Board_RESET_PIN);
	usDelay(100);
}
