#include <stdio.h>
#include <string.h>
#include "platform.h"
#include "xparameters.h"
#include "xiic.h"
#include "xil_io.h"
#include "aes256.h"
#include "xhdcp22_common.h"

#if defined (XPAR_XUARTLITE_NUM_INSTANCES)
#include "xuartlite_l.h"
#else
#include "xuartps.h"
#endif

/************************** Constant Definitions *****************************/
#define XHDCP_IIC_BASEADDR			XPAR_IIC_0_BASEADDR
#define EEPROM_ADDRESS				0x50	 /* 0xA0 as an 8 bit number */
#define PAGE_SIZE					16

#if defined (XPAR_XUARTLITE_NUM_INSTANCES)
#define UART_BASE_ADDRESS XPAR_MB_SS_0_AXI_UARTLITE_BASEADDR
#else
#define UART_BASE_ADDRESS XPAR_XUARTPS_0_BASEADDR
#endif

#define SIGNATURE_OFFSET			0
#define HDCP22_LC128_OFFSET			16
#define HDCP22_CERTIFICATE_OFFSET	32
#define HDCP14_KEY1_OFFSET			1024
#define HDCP14_KEY2_OFFSET			1536

#define IIC_SCLK_RATE		100000

/************************** Function Prototypes ******************************/
u8 EnterPassword (u8 *Password);

void Decrypt (u8 *CipherBuffer, u8 *PlainBuffer, u8 *Key, u16 Length);
void Encrypt (u8 *PlainBuffer, u8 *CipherBuffer, u8 *Key, u16 Length);
u16 Verify (u16 Address, u8 *BufferPtr, u16 Length, u8 *Key, u8 Encrypted);
u16 Store (u16 Address, u8 *BufferPtr, u16 Length);
u16 Get (u16 Address, u8 *BufferPtr, u16 Length);
void Erase (void);
u16 Round16 (u16 Size);
unsigned EepromWriteByte(u16 Address, u8 *BufferPtr, u16 ByteCount);
unsigned EepromReadByte(u16 Address, u8 *BufferPtr, u16 ByteCount);

/************************** Variable Definitions **************************/

int ErrorCount;			  /* The Error Count */

u8 WriteBuffer[PAGE_SIZE];	  /* Write buffer for writing a page */
u8 ReadBuffer[PAGE_SIZE];	  /* Read buffer for reading a page */


u8 HdcpSignature[16] = {"xilinx_hdcp_keys"};
u8 Hdcp22Lc128[] =
{
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

u8 Hdcp22Key[] =
{
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};


u8 Hdcp14Key1[] =
{
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

u8 Hdcp14Key2[] =
{
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

int main()
{
	u16 BytesWritten;
	u16 Result;
    u8 CipherBuffer[1024];
    u8 Password[32];
    u8 Key[32];

    init_platform();

    xil_printf("\r\n\r\nHDCP Key EEPROM v1.0\r\n");
	xil_printf("This tool encrypts and stores the HDCP 2.2 certificate and  \r\n");
	xil_printf("HDCP 1.4 keys into the EEPROM on the HDMI FMC board\r\n\r\n");

	EnterPassword(Password);

	// Generate password hash
	XHdcp22Cmn_Sha256Hash(Password, sizeof(Password), Key);

	xil_printf("\r\n");

	// Signature
	xil_printf("Encrypt signature ");
	Encrypt(HdcpSignature, CipherBuffer, Key, sizeof(HdcpSignature));
	xil_printf(" done\r\n");

	xil_printf("Write signature ");
	BytesWritten = Store(SIGNATURE_OFFSET, CipherBuffer, Round16(sizeof(HdcpSignature)));
	xil_printf("(%d bytes)\r\n", BytesWritten);

	xil_printf("Verify signature ");
	Result = Verify(SIGNATURE_OFFSET, HdcpSignature, sizeof(HdcpSignature), Key, TRUE);
	if (Result == sizeof(HdcpSignature))
		xil_printf("ok (%d bytes)\r\n", Result);
	else
		xil_printf("error (%d bytes)\r\n", Result);

	// Encrypt LC128
	xil_printf("Encrypt LC128 ");
	Encrypt(Hdcp22Lc128, CipherBuffer, Key, sizeof(Hdcp22Lc128));
	xil_printf(" done\r\n");

	// Write LC128
	xil_printf("Write LC128 ");
	BytesWritten = Store(HDCP22_LC128_OFFSET, CipherBuffer, Round16(sizeof(Hdcp22Lc128)));
	xil_printf("(%d bytes)\r\n", BytesWritten);

	// Verify LC128
	xil_printf("Verify lc128 ");
	Result = Verify(HDCP22_LC128_OFFSET, Hdcp22Lc128, sizeof(Hdcp22Lc128), Key, TRUE);
	if (Result == sizeof(Hdcp22Lc128))
		xil_printf("ok (%d bytes)\r\n", Result);
	else
		xil_printf("error (%d bytes)\r\n", Result);

	// Encrypt HDCP2.2 ceritificate
	xil_printf("Encrypt HDCP 2.2 Certificate ");
	Encrypt(Hdcp22Key, CipherBuffer, Key, sizeof(Hdcp22Key));
	xil_printf(" done\r\n");

	// Write Key
	xil_printf("Write HDCP 2.2 Certificate ");
	BytesWritten = Store(HDCP22_CERTIFICATE_OFFSET, CipherBuffer, Round16(sizeof(Hdcp22Key)));
	xil_printf("(%d bytes)\r\n", BytesWritten);

	// Verify Key
	xil_printf("Verify HDCP 2.2 Certificate ");
	Result = Verify(HDCP22_CERTIFICATE_OFFSET, Hdcp22Key, sizeof(Hdcp22Key), Key, TRUE);
	if (Result == sizeof(Hdcp22Key))
		xil_printf("ok (%d bytes)\r\n", Result);
	else
		xil_printf("error (%d bytes)\r\n", Result);

	// Encrypt HDCP 1.4 key A
	xil_printf("Encrypt HDCP 1.4 Key A ");
	Encrypt(Hdcp14Key1, CipherBuffer, Key, sizeof(Hdcp14Key1));
	xil_printf(" done\r\n");

	// Write Key
	xil_printf("Write HDCP 1.4 Key A ");
	BytesWritten = Store(HDCP14_KEY1_OFFSET, CipherBuffer, Round16(sizeof(Hdcp14Key1)));
	xil_printf("(%d bytes)\r\n", BytesWritten);

	// Verify Key
	xil_printf("Verify HDCP 1.4 Key A ");
	Result = Verify(HDCP14_KEY1_OFFSET, Hdcp14Key1, sizeof(Hdcp14Key1), Key, TRUE);
	if (Result == sizeof(Hdcp14Key1))
		xil_printf("ok (%d bytes)\r\n", Result);
	else
		xil_printf("error (%d bytes)\r\n", Result);

	// Encrypt HDCP 1.4 key B
	xil_printf("Encrypt HDCP 1.4 Key A ");
	Encrypt(Hdcp14Key2, CipherBuffer, Key, sizeof(Hdcp14Key2));
	xil_printf(" done\r\n");

	// Write Key
	xil_printf("Write HDCP 1.4 Key B ");
	BytesWritten = Store(HDCP14_KEY2_OFFSET, CipherBuffer, Round16(sizeof(Hdcp14Key2)));
	xil_printf("(%d bytes)\r\n", BytesWritten);

	// Verify Key
	xil_printf("Verify HDCP 1.4 Key B ");
	Result = Verify(HDCP14_KEY2_OFFSET, Hdcp14Key2, sizeof(Hdcp14Key2), Key, TRUE);
	if (Result == sizeof(Hdcp14Key1))
		xil_printf("ok (%d bytes)\r\n", Result);
	else
		xil_printf("error (%d bytes)\r\n", Result);

	xil_printf("\r\nHDCP Key EEPROM completed.\r\n");
	xil_printf("This application is stopped.\r\n");
	cleanup_platform();
    return 0;
}

u8 EnterPassword (u8 *Password)
{
	u8 Data;
	u8 i;
	u8 *PasswordPtr;

	// Assign pointer
	PasswordPtr = Password;

	// Clear password
	memset(PasswordPtr, 0x00, 32);

	xil_printf("Enter Password ->");

	i = 0;
	while (1) {
		/* Check if the UART has any data */

#if defined (XPAR_XUARTLITE_NUM_INSTANCES)
		if (!XUartLite_IsReceiveEmpty(UART_BASE_ADDRESS)) {
			/* Read data from uart */
			Data = XUartLite_RecvByte(UART_BASE_ADDRESS);

			/* Send response to user */
			XUartLite_SendByte(UART_BASE_ADDRESS, '.');
#else
		if (XUartPs_IsReceiveData(UART_BASE_ADDRESS)) {
			/* Read data from uart */
			Data = XUartPs_RecvByte(UART_BASE_ADDRESS);

			/* Send response to user */
			XUartPs_SendByte(UART_BASE_ADDRESS, '.');
#endif

			/* Execute */
			if ((Data == '\n') || (Data == '\r')) {
				return TRUE;
			}

			/* Store Data */
			else {
				if (i >= 32)
					return TRUE;
				else {
					*(PasswordPtr + i) = Data;
					i++;
				}
			}
		}
	}
}


u16 Round16 (u16 Size)
{
	if (Size % 16)
		return ((Size/16)+1) * 16;
	else
		return Size;
};

void Decrypt (u8 *CipherBufferPtr, u8 *PlainBufferPtr, u8 *Key, u16 Length)
{
    u8 i;
    u8 *AesBufferPtr;
    u16 AesLength;
    aes256_context ctx;

    // Assign local Pointer
    AesBufferPtr = CipherBufferPtr;

    // Initialize AES256
    aes256_init(&ctx, Key);

    AesLength = Length/16;
    if (Length % 16) {
	AesLength++;
    }

    for (i=0; i<AesLength; i++)
    {
	// Decrypt
	aes256_decrypt_ecb(&ctx, AesBufferPtr);

		// Increment pointer
	AesBufferPtr += 16;	// The aes always encrypts 16 bytes
    }

    // Done
    aes256_done(&ctx);

   // Clear Buffer
    memset(PlainBufferPtr, 0x00, Length);

    // Copy buffers
    memcpy(PlainBufferPtr, CipherBufferPtr, Length);
}

void Encrypt (u8 *PlainBufferPtr, u8 *CipherBufferPtr, u8 *Key, u16 Length)
{
    u8 i;
    u16 AesLength;
    u8 *AesBufferPtr;

    aes256_context ctx;

    // Assign local Pointer
    AesBufferPtr = CipherBufferPtr;

    // Clear Buffer
    memset(AesBufferPtr, 0x00, Length);

    // Copy buffers
    memcpy(AesBufferPtr, PlainBufferPtr, Length);

    // Initialize AES256
    aes256_init(&ctx, Key);

    AesLength = Length/16;
    if (Length % 16) {
	AesLength++;
    }

    for (i=0; i<AesLength; i++)
    {
	// Encrypt
	aes256_encrypt_ecb(&ctx, AesBufferPtr);

	// Increment bufferpointer
	AesBufferPtr += 16;	// The aes always encrypts 16 bytes
    }

    // Done
    aes256_done(&ctx);
}

void Erase (void)
{
	u8 Empty[1024];

	// Clear Buffer
    memset(Empty, 0x00, sizeof(Empty));

    // Erase EEPROM
    Store(0, Empty, sizeof(Empty));
}

u16 Store (u16 Address, u8 *BufferPtr, u16 Length)
{
	u16 i;
	u8 BytesWritten;
	u16 TotalBytesWritten;
	u8 *Ptr;

	Ptr = BufferPtr;
	TotalBytesWritten = 0;
	for (i=0; i<(Length / PAGE_SIZE); i++)
	{
		BytesWritten = EepromWriteByte(Address, Ptr, PAGE_SIZE);
		TotalBytesWritten += BytesWritten;
		Address += BytesWritten;
		Ptr += BytesWritten;
	}

	if (Length > TotalBytesWritten)
	{
		BytesWritten = EepromWriteByte(Address, Ptr, (Length - TotalBytesWritten));
		TotalBytesWritten += BytesWritten;
	//	xil_printf("wr : %0d\r\n", i, BytesWritten);
	}
	return TotalBytesWritten;
}

u16 Get (u16 Address, u8 *BufferPtr, u16 Length)
{
	u16 i;
	u8 BytesRead;
	u16 TotalBytesRead;
	u8 *Ptr;

	Ptr = BufferPtr;
	TotalBytesRead = 0;
	for (i=0; i<(Length / PAGE_SIZE); i++)
	{
		BytesRead = EepromReadByte(Address, Ptr, PAGE_SIZE);
		TotalBytesRead += BytesRead;
		Address += BytesRead;
		Ptr += BytesRead;
	}

	if (Length > TotalBytesRead)
	{
		BytesRead = EepromReadByte(Address, Ptr, (Length - TotalBytesRead));
		TotalBytesRead += BytesRead;
	//	xil_printf("rd : %0d\r\n", i, BytesRead);
	}
	return TotalBytesRead;
}

u16 Verify (u16 Address, u8 *BufferPtr, u16 Length, u8 *Key, u8 Encrypted)
{
	u16 i;
	u8 Error;
	u8 ScratchBuffer[1024];
	u8 CipherBuffer[1024];

	if (Encrypted)
	{
		Get(Address, CipherBuffer, Round16(Length));
		Decrypt(CipherBuffer, ScratchBuffer, Key, Round16(Length));
	}

	else
	{
		Get(Address, ScratchBuffer, Length);
	}

	i = 0;
	Error = 0;
	do
	{
		if (BufferPtr[i] != ScratchBuffer[i])
			Error = 1;
		else
			i++;
	}
	while ((i<Length) && !Error);
	return i;
}

/*****************************************************************************/
/**
* This function writes a buffer of bytes to the IIC serial EEPROM.
*
* @param	Address contains the address in the EEPROM to write to.
* @param	BufferPtr contains the address of the data to write.
* @param	ByteCount contains the number of bytes in the buffer to be written.
*		Note that this should not exceed the page size of the EEPROM as
*		noted by the constant PAGE_SIZE.
*
* @return	The number of bytes written, a value less than that which was
*		specified as an input indicates an error.
*
* @note		None.
*
****************************************************************************/
unsigned EepromWriteByte(u16 Address, u8 *BufferPtr, u16 ByteCount)
{
	volatile unsigned SentByteCount;
	volatile unsigned AckByteCount;
	u8 WriteBuffer[sizeof(Address) + PAGE_SIZE];
	int Index;


	/*
	 * A temporary write buffer must be used which contains both the address
	 * and the data to be written, put the address in first based upon the
	 * size of the address for the EEPROM.
	 */
	WriteBuffer[0] = (u8)(Address >> 8);
	WriteBuffer[1] = (u8)(Address);

	/*
	 * Put the data in the write buffer following the address.
	 */
	for (Index = 0; Index < ByteCount; Index++) {
		WriteBuffer[sizeof(Address) + Index] = BufferPtr[Index];
	}

	/*
	 * Set the address register to the specified address by writing
	 * the address to the device, this must be tried until it succeeds
	 * because a previous write to the device could be pending and it
	 * will not ack until that write is complete.
	 */
	do {
		SentByteCount = XIic_Send(XHDCP_IIC_BASEADDR,
					EEPROM_ADDRESS,
					(u8 *)&Address, sizeof(Address),
					XIIC_STOP);
		if (SentByteCount != sizeof(Address)) {

			/* Send is aborted so reset Tx FIFO */
			XIic_WriteReg(XHDCP_IIC_BASEADDR,  XIIC_CR_REG_OFFSET,
					XIIC_CR_TX_FIFO_RESET_MASK);
			XIic_WriteReg(XHDCP_IIC_BASEADDR, XIIC_CR_REG_OFFSET,
					XIIC_CR_ENABLE_DEVICE_MASK);
		}

	} while (SentByteCount != sizeof(Address));

	/*
	 * Write a page of data at the specified address to the EEPROM.
	 */
	SentByteCount = XIic_Send(XHDCP_IIC_BASEADDR, EEPROM_ADDRESS,
				  WriteBuffer, sizeof(Address) + ByteCount,
				  XIIC_STOP);

	/*
	 * Wait for the write to be complete by trying to do a write and
	 * the device will not ack if the write is still active.
	 */
	do {
		AckByteCount = XIic_Send(XHDCP_IIC_BASEADDR, EEPROM_ADDRESS,
					(u8 *)&Address, sizeof(Address),
					XIIC_STOP);
		if (AckByteCount != sizeof(Address)) {

			/* Send is aborted so reset Tx FIFO */
			XIic_WriteReg(XHDCP_IIC_BASEADDR,  XIIC_CR_REG_OFFSET,
					XIIC_CR_TX_FIFO_RESET_MASK);
			XIic_WriteReg(XHDCP_IIC_BASEADDR, XIIC_CR_REG_OFFSET,
					XIIC_CR_ENABLE_DEVICE_MASK);
		}

	} while (AckByteCount != sizeof(Address));


	/*
	 * Return the number of bytes written to the EEPROM
	 */
	return SentByteCount - sizeof(Address);
}

/*****************************************************************************/
/**
* This function reads a number of bytes from the IIC serial EEPROM into a
* specified buffer.
*
* @param	Address contains the address in the EEPROM to read from.
* @param	BufferPtr contains the address of the data buffer to be filled.
* @param	ByteCount contains the number of bytes in the buffer to be read.
*		This value is not constrained by the page size of the device
*		such that up to 64K may be read in one call.
*
* @return	The number of bytes read. A value less than the specified input
*		value indicates an error.
*
* @note		None.
*
****************************************************************************/
unsigned EepromReadByte(u16 Address, u8 *BufferPtr, u16 ByteCount)
{
	volatile unsigned ReceivedByteCount;
	u16 StatusReg;
	u8 WriteBuffer[sizeof(Address)];

	WriteBuffer[0] = (u8)(Address >> 8);
	WriteBuffer[1] = (u8)(Address);

	/*
	 * Set the address register to the specified address by writing
	 * the address to the device, this must be tried until it succeeds
	 * because a previous write to the device could be pending and it
	 * will not ack until that write is complete.
	 */
	do {
		StatusReg = XIic_ReadReg(XHDCP_IIC_BASEADDR, XIIC_SR_REG_OFFSET);
		if(!(StatusReg & XIIC_SR_BUS_BUSY_MASK)) {
			ReceivedByteCount = XIic_Send(XHDCP_IIC_BASEADDR,
							EEPROM_ADDRESS,
							WriteBuffer,
							sizeof(Address),
							XIIC_STOP);

			if (ReceivedByteCount != sizeof(Address)) {

				/* Send is aborted so reset Tx FIFO */
				XIic_WriteReg(XHDCP_IIC_BASEADDR,
						XIIC_CR_REG_OFFSET,
						XIIC_CR_TX_FIFO_RESET_MASK);
				XIic_WriteReg(XHDCP_IIC_BASEADDR,
						XIIC_CR_REG_OFFSET,
						XIIC_CR_ENABLE_DEVICE_MASK);
			}
		}

	} while (ReceivedByteCount != sizeof(Address));

	/*
	 * Read the number of bytes at the specified address from the EEPROM.
	 */
	ReceivedByteCount = XIic_Recv(XHDCP_IIC_BASEADDR, EEPROM_ADDRESS,
					BufferPtr, ByteCount, XIIC_STOP);

	/*
	 * Return the number of bytes read from the EEPROM.
	 */
	return ReceivedByteCount;
}
