#include "layer1.h"


//ROM functions

#define UDSFILE "./out/RANDFILE"
#define UDSsize 8	//bytes
#define FW_file "layer1.c"
#define FW_size 1000		
////need to find a way to determine 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_dig_t] = {0};

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


	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 : ");
	for(i = 0; i < SHA256_dig_t; 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

	//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));
	BIO *fp, *out;
	int buf_size = 0;
	int i;

	out = BIO_new_fp(stdout, BIO_NOCLOSE);
	fp = BIO_new_file(FW_file, "r");

	if(!fp)
		perror("Opening FW to read failed\n");

	if(BIO_eof(fp))
		perror("File empty\n");

	 while(!BIO_eof(fp) && (buf_size < FW_size))
	 {
	 	BIO_read(fp, source[buf_size], 1);
	 	buf_size++;				//buf_size includes EOF
	 	//break;
	 }

	if(SHA256(source, buf_size -1, FW_M) == NULL)
		perror("FW measurement failed\n");


	//print block
	BIO_printf(out,"FW digest : ");
	for(i = 0; i < SHA256_dig_t; 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_dig_t; i++)
	 	BIO_printf(out,"%x",UDS_M[i]);
	BIO_printf(out, "\n");

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

	BIO_printf(out,"CDI : ");
	for(int i = 0; i < SHA256_dig_t; 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_dig_t);
	readUDS(UDS_ID);

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

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

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


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

	return 1;
}