#include "layer1.h"


//ROM functions

#define UDSFILE "./out/RANDFILE"
#define UDSsize 8						//bytes
#define FW_file "layer2.c"				//use RANDFILE if testing FW_M for consistency
#define FW_size 1000		
////need to find a way to determine FW file size using BIO tools


int createUDS()
{
	int ret = RAND_write_file(UDSFILE);
	if (ret == -1)
		perror("rand write to file failed\n");
	printf("\n %d bytes written to file\n", ret);
	//readUDS();
	return 0;
}

int readUDS(uint8_t* UDS_M)
{
	//1. Create RANDFILE if it does not exist. skip if it exists
	//2. Load RANDFILE
	//3. Read 64 bits to UDSbuf and compute hash into input arg.
	//4. free UDSbuf

	BIO *fp, *out;
	int i;

	//uint8_t UDSbuf[UDSsize] = {0};
	//uint8_t UDS_M[SHA256_DGST_SIZE] = {0};

	uint8_t* UDSbuf = calloc(1,sizeof(uint8_t)*UDSsize);
	//uint8_t* UDS_M = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);


	out = BIO_new_fp(stdout, BIO_NOCLOSE);

	int ret = RAND_load_file(UDSFILE, 8);
	while (ret < 0)
	{
		perror("Could not load seed file\n");
		BIO_printf(out,"Creating new seed file\n");
		createUDS();
		ret = RAND_load_file(UDSFILE, 8);
	}


	fp = BIO_new_file(UDSFILE, "r");
	if(!fp)
		perror("Opening seed file to read failed\n");
	
	if (BIO_read(fp,UDSbuf,UDSsize) < 0)		//Suspecting a ENDian issue in reading. data is half byte reversed
		perror("BIO read failed\n");

	//Compute hash of UDS
	if(SHA256(UDSbuf, UDSsize, UDS_M) == NULL)
		perror("UDS measurement failed\n");

	//Print block. delete later
	for(i = 0; i < UDSsize; i++)
	 	BIO_printf(out,"%x,",UDSbuf[i]);
	BIO_printf(out, "\n");

	BIO_printf(out,"UDS digest\t: ");
	for(i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",UDS_M[i]);
	BIO_printf(out, "\n");

	free(UDSbuf);
	BIO_free(fp);
	BIO_free(out);
	return 1;

}


int readFWID(uint8_t* FW_M)
{

// 	//1. Read layer1.c into memory
// 	//2. Calcualte hash into arg

	BIO *fp, *out;
	int i;

// 	//uint8_t * source;
// 	//FW_size shoudl not be static.
// 	//Use indefinite array or determine FW_size
	uint8_t* source = calloc(1,sizeof(uint8_t)*FW_size);
	out = BIO_new_fp(stdout, BIO_NOCLOSE);

	fp = BIO_new_file(FW_file, "r");
	if(!fp)
		perror("Opening firmware bin to read failed\n");
	
	if (BIO_read(fp,source,FW_size) < 0)		//Suspecting a ENDian issue in reading. data is half byte reversed
		perror("BIO read failed\n");

	//Compute hash of FW
	if(SHA256(source, FW_size, FW_M) == NULL)
		perror("FW measurement failed\n");

	//Print block. delete later
	 // for(i = 0; i < 100; i++)
	 //  	BIO_printf(out,"%x,",source[i]);
	 // BIO_printf(out, "\n");

	BIO_printf(out,"FW digest\t: ");
	for(i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",FW_M[i]);
	BIO_printf(out, "\n");

	free(source);
	BIO_free(fp);
	BIO_free(out);
	return 1;

}



int calcCDID(uint8_t * UDS_M, uint8_t * FW_M, uint8_t * CDID)
{
	//0. internally call readUDS and readFWID? abstraction of UDS against layer2

	//1. create sha256 context
	//2. add UDS hash
	//3. add FW hash
	//4. calc composite hash into CDID arg

	BIO * out = BIO_new_fp(stdout, BIO_NOCLOSE);

	SHA256_CTX *ctx;
	if(!SHA256_Init(ctx))
		perror("SHA init failed\n");

	if(!SHA256_Update(ctx, UDS_M, UDSsize))
		perror("SHA update failed\n");
	if(!SHA256_Update(ctx, FW_M, FW_size))
		perror("SHA update2 failed\n");

 	if(!SHA256_Final(CDID, ctx))
 		perror("SHA close failed\n");

 	//print block

 // 	BIO_printf(out,"UDID_M : ");
	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	BIO_printf(out,"%x",UDS_M[i]);
	// BIO_printf(out, "\n");

	// BIO_printf(out,"FWID_M : ");
	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	BIO_printf(out,"%x",FW_M[i]);
	// BIO_printf(out, "\n");

	BIO_printf(out,"CDI\t\t: ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",CDID[i]);
	BIO_printf(out, "\n");
	BIO_free(out);

	return 1;
}

int _calcCDID(uint8_t * _CDID)
{

	/* 
	***BUG*** : 
	CDI hash value returned is not correct or consistent.
	hash generated by readUDS() and readFWID() are correct and consistent but 
	caclCDID() is broken. 

	Do not use this wrapper function unless fixed
	*/

	BIO * out = BIO_new_fp(stdout, BIO_NOCLOSE);
	BIO_printf(out, "\n");BIO_printf(out, "\n");BIO_printf(out, "\n");

	//step 1 : Derive Device ID
	uint8_t* UDS_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	readUDS(UDS_ID);

	//step 2 : Derive Firmware ID
	uint8_t* FW_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	readFWID(FW_ID);

	////step3 : call calcCDID
	calcCDID(UDS_ID,FW_ID,_CDID);
	BIO_printf(out,"_CDI : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",_CDID[i]);
	BIO_printf(out, " : wrap \n\n\n");


	//attempt 2 : use SHA256 here
	BIO_printf(out,"_UDID : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",UDS_ID[i]);
	BIO_printf(out, "\n");

	BIO_printf(out,"_FWID : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",FW_ID[i]);
	BIO_printf(out, "\n");

	memset(_CDID, 0, SHA256_DGST_SIZE);

	BIO_printf(out,"_CDI 0 : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",_CDID[i]);
	BIO_printf(out, "\n");

	SHA256_CTX *ctx;
	if(!SHA256_Init(ctx))
		perror("SHA init failed\n");

	if(!SHA256_Update(ctx, UDS_ID, UDSsize))
		perror("SHA update failed\n");
	if(!SHA256_Update(ctx, FW_ID, FW_size))
		perror("SHA update2 failed\n");

 	if(!SHA256_Final(_CDID, ctx))
  		perror("SHA close failed\n");


	
	BIO_printf(out,"_UDID : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",UDS_ID[i]);
	BIO_printf(out, "\n");

	BIO_printf(out,"_FWID : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",FW_ID[i]);
	BIO_printf(out, "\n");


	BIO_printf(out,"_CDI : ");
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
	 	BIO_printf(out,"%x",_CDID[i]);
	BIO_printf(out, "\n");
	BIO_free(out);

	return 1;
}