#include "ROMprotocol.h"


/*
	DIMASTATUS ret = 0;
	int len = 0;


if(DEBUG)
{printf("PASS \n");}

if(ret < DIMASUCCESS)
	{
		perror("\n");
		
		//other cleanup
		exit();
	}

*/

DIMASTATUS ROMprotocol()
{

	printf("PASS 10\n");

	DIMASTATUS ret = 0;
	int len = 0;

	if(DEBUG)
	{printf("PASS 11\n");}


	//Calculate DIMA RTM hash
	
	//////////////TODO////////////////
	//this should be calculated on the DIMA bin and remain static. 
	//how to do that?
	//hardcoding junk value for now

	uint8_t RTM_ID[SHA256_DGST_SIZE] = {	0xbe,0x67,0x0a,0xf8,0xf3,0x92,0x0e,0x4f,
											0xba,0x68,0xd1,0x56,0xea,0x34,0x3f,0xbc,
    										0xf1,0xd9,0x30,0xe2,0x33,0xcc,0x28,0xc5,
    										0x7b,0xbb,0x38,0x7d,0xe6,0x66,0xbb,0x1e		};

	//Calculating UDS hash

	FILE *fp = NULL;
	fp = fopen("SecureStorage/RANDFILE", "rb");
	if(!fp)
	{
		perror("DIMAFILENOTFOUND: Unable to access UDS\n");
		
		fclose(fp);
		exit(DIMAFILENOTFOUND);
	}

	if(DEBUG)
{printf("PASS 12\n");}

	uint8_t *UDSbuf = calloc(1, sizeof(uint8_t)*SHA256_DGST_SIZE);
	fread(UDSbuf,UDS_SIZE,1,fp);
	fclose(fp);

if(DEBUG)
{printf("PASS 13\n");}


	uint8_t* UDS_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	ret = mbedtls_sha256_ret( UDSbuf,UDS_SIZE,UDS_ID,0 );
	if(ret < DIMASUCCESS)
	{
		perror("DIMASHAFAILUE:\n");
		
		free(UDSbuf);
		free(UDS_ID);
		exit(DIMAFAILUREUNKWN);
	}

	free(UDSbuf);
	//UDS_ID contains the UDS hash
	//Printer
if(DEBUG)
{
		for (int i = 0; i < UDS_SIZE; i++)
  			printf("%hhx", UDSbuf[i]);
		printf(" :	fuse secret\n" );  	

  		for (int i = 0; i < SHA256_DGST_SIZE; i++)
  			printf("%hhx", UDS_ID[i]);
		printf(" :  UDS ID\n" );
}

	//Calculating CDI SHA256

	uint8_t* CDI = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	mbedtls_sha256_context CDI_ctx;
	mbedtls_sha256_init(&CDI_ctx);

	ret = mbedtls_sha256_starts_ret(&CDI_ctx,0);
	if(ret < DIMASUCCESS)
	{
		perror("DIMASHAFAILURE\n");
		mbedtls_sha256_free(&CDI_ctx);
		exit(DIMASHAFAILURE);
	}

	ret = mbedtls_sha256_update_ret(&CDI_ctx, UDS_ID, SHA256_DGST_SIZE);
	if(ret < DIMASUCCESS)
	{
		perror("DIMASHAFAILURE\n");
		mbedtls_sha256_free(&CDI_ctx);
		exit(DIMASHAFAILURE);
	}

	ret = mbedtls_sha256_update_ret(&CDI_ctx, RTM_ID, SHA256_DGST_SIZE);
	if(ret < DIMASUCCESS)
	{
		perror("DIMASHAFAILURE\n");
		mbedtls_sha256_free(&CDI_ctx);
		exit(DIMASHAFAILURE);
	}

	ret = mbedtls_sha256_finish_ret(&CDI_ctx,CDI);
	if(ret < DIMASUCCESS)
	{
		perror("DIMASHAFAILURE\n");
		mbedtls_sha256_free(&CDI_ctx);
		exit(DIMASHAFAILURE);
	}

	free(UDS_ID);
	//free(RTM_ID);
	mbedtls_sha256_free(&CDI_ctx);

if(DEBUG)
{
	for(int i = 0; i < SHA256_DGST_SIZE; i++)
    	printf("%hhx",CDI[i]);
    printf(" :  CDI\n");
}

if(DEBUG)
{printf("PASS 14\n");}
	//Calculating CDI key, HKDF

	const mbedtls_md_info_t * md_info;
	if(!(md_info = mbedtls_md_info_from_type(HKDF_ALG)))
	{
		perror("DIMAHKDFFAILURE: MD alg type def failed\n");
		exit(DIMAHKDFFAILURE);
	}

	uint8_t salt[32] = {	0x30,0xe2,0x3e,0xcc,0x28,0xc5,0x7b,0xbb,
							0x38,0x7d,0xe6,0x66,0xbb,0xf1,0xd9,0x1e,
    						0xbe,0x67,0x0a,0xf8,0xf3,0x92,0x0e,0xba,
    						0x68,0xd1,0x56,0xea,0x34,0x3f,0xbc,0x4f		};

    uint8_t * CDIKEY = calloc(1, sizeof(uint8_t)*HKDF_KEY_SIZE);
	ret = mbedtls_hkdf(md_info, salt, sizeof(salt), CDI, SHA256_DGST_SIZE,
              IDENTITY, sizeof(IDENTITY), CDIKEY, HKDF_KEY_SIZE);

	if(ret < DIMASUCCESS)
	{
		perror("DIMAHKDFFAILURE\n");
		free(CDI);
		free(CDIKEY);
		exit(DIMAHKDFFAILURE);
	}

	//free(CDI);				

if(DEBUG)
{
	for(int i = 0; i < HKDF_KEY_SIZE; i++)
      printf("%hhx",CDIKEY[i]);
	printf(" :  CDIKEY\n");
}
	
if(DEBUG)
{printf("PASS 15\n");}



	//Every key derivation should start with a new KD contxt
	//setting context for DID

	KeyDrv_context DID_ctx;
	DID_ctx.ENT_MODE = DETERM;
	DID_ctx.PKC_MODE = DFL_PKC;
	DID_ctx.seed = CDIKEY;
	DID_ctx.phrase = IDENTITY;
	DID_ctx.KEY_FORM = DFL_FORM;
	//DID_ctx.pub_file = DFL_PUB;
	//DID_ctx.priv_file = DFL_PRIV;			//Dont save DID priv outside SS

if(DEBUG)
{printf("PASS 16\n");}

	//Deriving and storing DID
	ret = AsymmKeyGen(&DID_ctx);
	if(ret < DIMASUCCESS)
	{
		perror("DIMAFAILURE : DID key gen failed\n");
		free(CDIKEY);
		exit(DIMAFAILURE);
	}

	free(CDIKEY);			//But CDIKey is needed to gen AliasKP 
	//delete DID_ctx??

	//setting context for Alias keys

//////////////////////////deriving alisa keys


	//placeholder FW_ID
	uint8_t FW_ID[SHA256_DGST_SIZE] = {	0xf3,0x92,0x0e,0x4f,0xbe,0x67,0x0a,0xf8,
											0xf1,0xd9,0x30,0xe2,0x33,0xcc,0x28,0xc5,
    										0xba,0x68,0xd1,0x56,0xea,0x34,0x3f,0xbc,
    										0xe6,0x66,0xbb,0x1e,0x7b,0xbb,0x38,0x7d	}; 

    uint8_t * FWKEY = calloc(1, sizeof(uint8_t)*HKDF_KEY_SIZE);

    //create Alias key derivation material using CDI and seed and FW_ID as salt
    //Alternatively do composite hash of CDI and FW_ID to create Alias key der material
    ret = mbedtls_hkdf(md_info, FW_ID, sizeof(FW_ID), CDI, SHA256_DGST_SIZE,
              ALIAS, sizeof(ALIAS), FWKEY, HKDF_KEY_SIZE);

    if(ret < DIMASUCCESS)
	{
		perror("DIMAHKDFFAILURE\n");
		exit(DIMAHKDFFAILURE);
	}

	KeyDrv_context ALIAS_ctx;
	ALIAS_ctx.ENT_MODE = DETERM;
	ALIAS_ctx.PKC_MODE = DFL_PKC;
	ALIAS_ctx.seed = FWKEY;
	ALIAS_ctx.phrase = ALIAS;
	ALIAS_ctx.KEY_FORM = DFL_FORM;

	printf("Generating Alias keys\n");

	ret = AsymmKeyGen(&ALIAS_ctx);
	if(ret < DIMASUCCESS)
	{
		perror("DIMAFAILURE : ALIAS key gen failed\n");
		exit(DIMAFAILURE);
	}


	//session keys?

	free(CDI);
	free(FWKEY);


if(DEBUG)
{printf("PASS 17\n");}




if(DEBUG)
{printf("PASS 100\n");}
	return DIMASUCCESS;

}



/////////////////////////
