#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "common.h"
#include "tht_memory_map_defines.h"


// Add the following Function
 
void UsbEPConfig(U32 ep, U32 dir, U32 stall, U32 setup, U32 MPS, U32 format, U32 xbsa, U32 ybsa, U32 Bufsize, U32 Tbyte)
{
 
  	U32 DWORD0 = 0;
  	U32 DWORD1 = 0;
	U32 DWORD2 = 0; 
  	U32 DWORD3 = 0;
  	U32 localEp = ep;
  	U32 DWORD0_addr,DWORD1_addr,DWORD2_addr,DWORD3_addr ;
 
  	DWORD0_addr = OTG_EP_BASE + (16*((localEp*2) + dir)); 
  	DWORD1_addr = DWORD0_addr + 4;         
  	DWORD2_addr = DWORD1_addr + 4;      
  	DWORD3_addr = DWORD2_addr + 4;
  
  
  	/*Configuring DWORD0*/
  	DWORD0 = (stall << 31)| (setup << 30) | (0 << 26) | (MPS<< 16) | (format << 14) | 0;
  	*(VP_U32)DWORD0_addr = DWORD0;
   
      
  	/*Configuring DWORD1*/
  	DWORD1 = (ybsa << 16)  | (xbsa << 2) |0 ;
  	*(VP_U32)DWORD1_addr = DWORD1;
  
  
  	/*Configuring DWORD2*/
 	/* reserved */
 

  	/*Configuring DWORD3*/
  	//Bufsize = buffersize - 1
  	DWORD3 = (Bufsize << 21) | Tbyte;  
  	*(VP_U32)DWORD3_addr = DWORD3;
 
  	/*Configure EP Table Entry*/
 
}
 
U8 _gUseUSB = 1; // default is use USB
U8 _gFirstAccess = 1;

void CheckFlashComplete(U32 StatusCode)
{
 	U32 temp = 0;
 	U32 USBTimeOut = 0x100000;
 	U32 EpStartAddress = 0;

// Change to determine whether UART/USB to give the feedback

	if (_gFirstAccess)
	{
		// UART 
    	//**************************************************************
		// Send through UART
		*(VP_U32)UART1_UTXD_1 = (U8)StatusCode & 0xFF; // Lewis, UART1_TXDATA > UART1_UTXD_1
		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF00)>>8);
		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF0000)>>16);
		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF000000)>>24);
	 	
		while (!(*(VP_U32)UART1_USR2_1 & 0x4000)); // Lewis, UART1_SR2 > UART1_USR2_1
		_gFirstAccess = 0;
	}

	if (_gUseUSB)
 	{
 		//**************************************************************
 		// Send through USB
 		UsbEPConfig(0x2, 0x1,   0,  0, 32, 0x2, (0*16),  0x200,   (0x200-1),    4);
 	
 		//read the Start Address (xbsa, ybsa)
 		EpStartAddress = ((*(VP_U32)(OTG_EP_BASE+(16*5)+4))&0x0000FFFF);
   
 		*(VP_U32)(OTG_DATA_BASE + EpStartAddress + (0*0x4)) = StatusCode;
    
    	//Set/Clear Fill Status
    	*(VP_U32)(OTG_FUNC_XFILL_STAT) = 0x20; //write to toggle the correct EP's X buffer status
 
 		//clear XY interrupt
 		temp = *(VP_U32)OTG_FUNC_XINT_STAT;
 		*(VP_U32)OTG_FUNC_XINT_STAT = temp;
 
 		temp = *(VP_U32)OTG_FUNC_YINT_STAT;
 		*(VP_U32)OTG_FUNC_YINT_STAT = temp;
 
 		//ready EP2
 		*(VP_U32)(OTG_FUNC_EP_RDY) = 0x20;
 
	 	//check for EP2 IN transfer done
	 	while (1)
	 	{
	  		if (*(VP_U32)OTG_FUNC_EP_DSTAT & 0x20)
	   		{
	   		 	//clear EP0 IN transfer done 
	 			*(VP_U32)OTG_FUNC_EP_DSTAT = 0x20;
	 			//clear XY interrupt
	 			temp = *(VP_U32)OTG_FUNC_XINT_STAT;
	 			*(VP_U32)OTG_FUNC_XINT_STAT = temp;
 
	 			temp = *(VP_U32)OTG_FUNC_YINT_STAT;
	 			*(VP_U32)OTG_FUNC_YINT_STAT = temp;
	 			break;
	  		}
	  		else
	   			USBTimeOut -- ;
 
  			if (!USBTimeOut)
   			{
   				_gUseUSB = 0;	
   				break;
 			}
 		}
	}
	else
	{
		// UART 
	    //**************************************************************
 		// Send through UART
 		*(VP_U32)UART1_UTXD_1 = (U8)StatusCode & 0xFF; // Lewis, UART1_TXDATA > UART1_UTXD_1
 		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF00)>>8);
 		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF0000)>>16);
 		*(VP_U32)UART1_UTXD_1 = (U8)((StatusCode & 0xFF000000)>>24);
 	
 		while (!(*(VP_U32)UART1_USR2_1 & 0x4000)); // Lewis, UART1_SR2 > UART1_USR2_1
 	}
 // end UART
 

 
	//**************************************************************
 
}

void CheckHABStatus(void)
{
	U32 temp = 0;
	U32 HAB_Status = 0;


	//read 0xFFFFE800 and feedback to PC host
	HAB_Status = *(P_U32)0xFFFFE800;
	CheckFlashComplete(HAB_Status);
		
}