#include "layer1.h"


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("%hhx", UDSbuf[i]);
  printf(" :	fuse secret\n" );  	

  for (int i = 0; i < UDS_DGST_SIZE; i++)
  		printf("%hhx", 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("layer1.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("%hhx",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("%hhx",CDID[i]);
    printf(" :  CDID\n");
  
  free(UDS_ID);
  free(FW_ID);
  mbedtls_sha256_free(&sha_ctx);
  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("%hhx",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("%hhx",CDIKEY[i]);
  printf(" :  CDIKEY\n");

  free(KEYIN);

  return RIOTSUCCESS;
}



  //firt generate ECC/RSA key. - Done ECC
  //check for deterministic consistency	- inconsistent
  //seed RNGs with CDI
  //let's see how it goes

  //add entropy source?
  //seed RNG
  //create ctx
  //init
  //gen keypair


/* To use HW TRNG /dev/random as the source of entropy add source to entropy contxt -
*/

int use_dev_random(void *data, unsigned char *output,
                             size_t len, size_t *olen )
{
    FILE *file;
    size_t ret, left = len;
    unsigned char *p = output;
    ((void) data);

    *olen = 0;

    file = fopen( "/dev/random", "rb" );
    if( file == NULL )
        return( MBEDTLS_ERR_ENTROPY_SOURCE_FAILED );

    while( left > 0 )
    {
        /* /dev/random can return much less than requested. If so, try again */
        ret = fread( p, 1, left, file );
        if( ret == 0 && ferror( file ) )
        {
            fclose( file );
            return( MBEDTLS_ERR_ENTROPY_SOURCE_FAILED );
        }

        p += ret;
        left -= ret;
        sleep( 1 );
    }
    fclose( file );
    *olen = len;

    return( 0 );
}


int seedRNGSource(void *data, unsigned char *output, size_t len)
{
  //srand(); lib fun call //https://stackoverflow.com/questions/55927662/generate-every-time-same-rsa-key-with-c
  //ctr_drbg    //programs/test/benchmark.c:705
  //hmac_drbg

	//Fill entropy accum with CDI and pass to DRBG


  mbedtls_entropy_context * p_ent = data;
  printf("manual update entropy with CDI\n");
      
  if(memcpy(output, p_ent -> accumulator.buffer + ACCUM_BUFF_OFFSET , ENTROPY_LEN) < 0)
  	return( MBEDTLS_ERR_ENTROPY_SOURCE_FAILED );

  len = ENTROPY_LEN;


  return 0;

  //  ((void) data);
  //  printf("Direct return CDI to drbg\n");
  
  // len = sizeof(CDI);
  // memcpy(output, CDI, len);
  //      for(int i = 0; i < len; i++)
  //   printf("0x%hhx,",output[i]);
  //  printf(" :  CDIKEY\n"); 
  //  printf("%d\n", (int)len);
  // return 0;

}




int deriveECCKeyPair(KeyDrv_context * KD_ctx)
{

	printf("inside deriveECCKeyPair layer1\n\n");

	int ret = 0;
  unsigned char pubkeybuf[100];
  size_t pubkeysize;
  char privkeybuf[100];
  size_t privkeysize;
  char privkeybuf2[100];
  size_t privkey2size;


  mbedtls_ecp_group ecpGrp;
  mbedtls_ecp_group_init(&ecpGrp);
  mbedtls_ecp_group_load(&ecpGrp, ECC_CURVE);

  mbedtls_mpi secret;
  mbedtls_mpi_init(&secret);

  mbedtls_ecp_point Public;
  mbedtls_ecp_point_init(&Public);

  mbedtls_entropy_context entropyCtx;
  mbedtls_entropy_init(&entropyCtx);

  mbedtls_ctr_drbg_context drbgCtx;
  mbedtls_ctr_drbg_init(&drbgCtx);

 
  if (KD_ctx->ENT_MODE == HW_TRNG)		//HW RNG
  {
  	printf("using /dev/random.... this may take a moment\n");
  	mbedtls_entropy_add_source( &entropyCtx, use_dev_random,
                               NULL, ENTROPY_LEN, MBEDTLS_ENTROPY_SOURCE_STRONG );
  	
  	mbedtls_ctr_drbg_seed(&drbgCtx, mbedtls_entropy_func, &entropyCtx,
                             (const unsigned char *) KD_ctx->phrase, strlen(KD_ctx->phrase) );
  }

  else if (KD_ctx->ENT_MODE == DETERM)			// Deterministic derviation with seed
  {	
  	printf("Seeding entropy accumulator....\n");
    if(mbedtls_entropy_update_manual(&entropyCtx, KD_ctx->seed, ENTROPY_LEN) < 0)
    {
    	perror("Accumulator seed failed\n");
    	return RIOTFAILURE;
    }

    if(mbedtls_ctr_drbg_seed(&drbgCtx, seedRNGSource, &entropyCtx,
    				(const unsigned char *) KD_ctx->phrase, sizeof(&KD_ctx->phrase)) < 0)
    {
    	perror("drbg seed failed\v");
    	return RIOTFAILURE;
    }
	}

	else 						//regular SW accumulator used for key derivation
	{
		printf("Accumulating entropy ...\n");
		mbedtls_entropy_update_manual(&entropyCtx, KD_ctx->seed, ENTROPY_LEN);
		mbedtls_ctr_drbg_seed(&drbgCtx, mbedtls_entropy_func, &entropyCtx,
                              (const unsigned char *) KD_ctx->phrase, strlen(KD_ctx->phrase) );
	}

  if(mbedtls_ecp_gen_keypair(&ecpGrp, &secret, &Public, 
  				mbedtls_ctr_drbg_random, &drbgCtx) <0)
  {
  	perror("ECP gen keypair failed\n");
  	return RIOTFAILURE;
  }

  ret = mbedtls_ecp_tls_write_point(&ecpGrp, &Public, MBEDTLS_ECP_PF_UNCOMPRESSED,
   				&pubkeysize, pubkeybuf, sizeof(pubkeybuf));
  if(ret < 0)
  {
  	perror("ECP write point failure\n");
  	return RIOTFAILURE;
  }

	//printf("%zu : pubkeysize\n", pubkeysize );
	for(int i = 0; i < pubkeysize; i++)
		printf("%hhx",pubkeybuf[i]);
	printf(" :  PubKey\n");


  ret = mbedtls_mpi_write_string(&secret, 16, privkeybuf, sizeof(privkeybuf), &privkeysize);
  if(ret < 0)
  {
  	printf("%d\n", ret);
  	perror("MPI write point to string failure\n");
  	return RIOTFAILURE;
  }

	printf("%s :  PrivKey\n",privkeybuf);

	//copy keys to parent function
  mbedtls_ecp_copy(&KD_ctx->Public, &Public);
  mbedtls_mpi_copy(&KD_ctx->secret, &secret);		/* Make SK NULL for Identitiy key generation */



    //what now? how to obtain the keys in PEM/DER/bin format?

    //following experiment better documented in ECC_trial/PKtoPEM.c
    /**
    mbedtls_pk_context ec_pkCtx;
    mbedtls_pk_init(&ec_pkCtx);

    mbedtls_pk_setup(&ec_pkCtx, mbedtls_pk_info_from_type(MBEDTLS_PK_ECKEY));
    mbedtls_ecp_keypair *pk_ecp = mbedtls_pk_ec( ec_pkCtx );

	mbedtls_ecp_copy(&pk_ecp->Q, &Public);
	mbedtls_mpi_copy(&pk_ecp->d, &secret);    

	mbedtls_mpi_write_string(&pk_ecp->d, 16, privkeybuf2, sizeof(privkeybuf2), &privkey2size);
	printf("%s :  wrapped PrivKey\n",privkeybuf2);



	ret = 0;
	unsigned char privBuf[16000];
    unsigned char * d_b = privBuf;
    memset(privBuf, 0, 16000);
    unsigned char pubBuf[16000];
    unsigned char * Q_b = pubBuf;
    memset(pubBuf, 0, 16000);
    ret = mbedtls_pk_write_key_pem(&ec_pkCtx, privBuf, 16000);
    printf("write priv pem ret value = %d\n", ret);

    printf("%s\n", privBuf);

    ret = mbedtls_pk_write_pubkey_pem(&ec_pkCtx, pubBuf, 16000);
    printf("write pub pem ret value = %d\n", ret);

    printf("%s\n", pubBuf);    

	**/


    mbedtls_mpi_free(&secret);
    mbedtls_ecp_point_free(&Public);
    mbedtls_ecp_group_free(&ecpGrp);
    mbedtls_entropy_free(&entropyCtx);
   	mbedtls_ctr_drbg_free(&drbgCtx);

   	printf("leaving deriveECCKeyPair layer1\n\n");

    return 0;
}

int deriveRSAKeyPair(void)
{	
	printf("inside deriveRSAKeyPair layer1\n\n");

	mbedtls_rsa_context rsaCtx;
	mbedtls_rsa_init(&rsaCtx,MBEDTLS_RSA_PKCS_V21, RSA_HASH_ID);

	mbedtls_entropy_context entropyCtx;
  mbedtls_entropy_init(&entropyCtx);

  mbedtls_ctr_drbg_context drbgCtx;
  mbedtls_ctr_drbg_init(&drbgCtx);

    //Seed drbg with secret data now?
    //move "private" string to n param
  int ret = mbedtls_ctr_drbg_seed(&drbgCtx, mbedtls_entropy_func, &entropyCtx,
    				(const unsigned char *) "Private", sizeof("Private"));

	mbedtls_rsa_gen_key(&rsaCtx,mbedtls_ctr_drbg_random, &drbgCtx,
					(unsigned int) 256, 65537);

	//is the key generated and stored in the contxt struct?
    //what now? how to obtain the keys in PEM/DER/bin format?

	printf("leaving deriveRSAKeyPair layer1\n\n");

	return 0;
}