#include "layer1.h"


#define IDENTITY     "identity"

int readUDS(uint8_t* UDSdigest)
{
  //Read 8 bytes from RANDFILE
	//calculate sha256  	
  //print digest

  FILE *fp = NULL;
  fp = fopen("RANDFILE","rb");
  if (!fp)
  {  
    perror("File open failed\n");
    return -1;    //FILENOTFOUND
  }
  //uint8_t UDSbuf[UDS_SIZE] = {0};	//{0Xe3,0xc5,0x58,0xaa,0x2f,0xd2,0x19,0x25};
  uint8_t *UDSbuf = calloc(1, sizeof(uint8_t)*UDS_SIZE);
  fread(UDSbuf,UDS_SIZE,1,fp);
  fclose(fp);

  uint8_t UDS_ID[UDS_DGST_SIZE] = {0};

  //mbedtls_sha256_ret( &UDSbuf,sizeof(UDSbuf),UDS_ID,0 );
  mbedtls_sha256_ret( UDSbuf,UDS_SIZE,UDSdigest,0 );


	for (int i = 0; i < UDS_SIZE; i++)
  		printf("%x", UDSbuf[i]);
  printf(" :	fuse secret\n" );  	

  for (int i = 0; i < UDS_DGST_SIZE; i++)
  		printf("%x", UDSdigest[i]);
  printf(" :  UDS ID\n" );

  free(UDSbuf);
	return RIOTSUCCESS;

}


int readFWID(uint8_t* FW_M)
{

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

  FILE *fp = NULL;  
  fp = fopen("layer2.c", "r");
  if (!fp)
  {  
    perror("File open failed\n");
    return -1;    //FILENOTFOUND
  }

  fseek(fp, 0L, SEEK_END);
  long FW_size = ftell(fp);
  rewind(fp);

  //uint8_t *source = calloc(1,sizeof(uint8_t)*FW_size);
  uint8_t source[FW_size];

  if (fread(&source, FW_size, 1, fp) < 0)
  {
    perror("File read failed");
    return -1;
  }
  fclose(fp);
  mbedtls_sha256_ret(source,FW_size,FW_M,0 );

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

  //printf("File contnts : %s\n", source);

  for(int i = 0; i < FW_DGST_SIZE; i++)
    printf("%x",FW_M[i]);
  printf(" :  FW digest\n");
  
  //free(source);
  return RIOTSUCCESS;

}

int _calcCDID(uint8_t * CDID)
{
  //function should not expose any internal measurements
  //ONLY CDI should be accessible outside layer 0

  //call readUDS, store measurement
  //call readFWID, store measurement
  //calculate Composite Device Identity
  //return CDI

  uint8_t* UDS_ID = calloc(1,sizeof(uint8_t)*UDS_DGST_SIZE);
  readUDS(UDS_ID);

  uint8_t* FW_ID = calloc(1,sizeof(uint8_t)*FW_DGST_SIZE);
  readFWID(FW_ID);

  mbedtls_sha256_context * sha_ctx;
  mbedtls_sha256_init(sha_ctx);
  if(mbedtls_sha256_starts_ret(sha_ctx,0) < 0 )
  {
    perror("SHA session failed to start\n");
    mbedtls_sha256_free(sha_ctx);
    return -1;
  }

  if(mbedtls_sha256_update_ret(sha_ctx, UDS_ID, UDS_DGST_SIZE) < 0)
  {
    perror("SHA session update failed\n");
    mbedtls_sha256_free(sha_ctx);
    return -1;  
  }

  if(mbedtls_sha256_update_ret(sha_ctx, FW_ID, FW_DGST_SIZE) < 0)
  {
    perror("SHA session update failed\n");
    mbedtls_sha256_free(sha_ctx);
    return -1;  
  }  

  if(mbedtls_sha256_finish_ret(sha_ctx,CDID) < 0)
  {
    perror("SHA session update failed\n");
    mbedtls_sha256_free(sha_ctx);
    return -1;  
  }

    for(int i = 0; i < CDI_DGST_SIZE; i++)
      printf("%x",CDID[i]);
    printf(" :  CDID\n");
  
  free(UDS_ID);
  free(FW_ID);
  return RIOTSUCCESS;

}

/*int mbedtls_hkdf( const mbedtls_md_info_t *md, 
                  const unsigned char *salt, size_t salt_len, 
                  const unsigned char *ikm, size_t ikm_len,
                  const unsigned char *info, size_t info_len,
                  unsigned char *okm, size_t okm_len ); */

int _calcCDIKEY(uint8_t * CDIKEY)
{
  uint8_t* KEYIN = calloc(1,sizeof(uint8_t)*CDI_DGST_SIZE);
  _calcCDID(KEYIN);

  for(int i = 0; i < SHA256_DGST_SIZE; i++)
      printf("%x",KEYIN[i]);
  printf(" :  CDID main\n");

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

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

  mbedtls_hkdf(md_info, salt, sizeof(salt), KEYIN, CDI_DGST_SIZE,
              IDENTITY, sizeof(IDENTITY), CDIKEY, CDI_KEY_SIZE);

  //mbedtls_hkdf_extract( md_info, salt, sizeof(salt),KEYIN, CDI_DGST_SIZE, CDIKEY);

  for(int i = 0; i < CDI_KEY_SIZE; i++)
      printf("%x",CDIKEY[i]);
  printf(" :  CDIKEY\n");

  free(KEYIN);

  return RIOTSUCCESS;
}