#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)
		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)
{

	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);
	calcCDID(UDS_ID,FW_ID,_CDID);
	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;
}