int main(int argc, char **argv) {
  int ret;
  rsa_t rsa;
  uint32_t mc, vf;
  datum_t em, m;
  uint8_t EM[256];

  mlockall(MCL_CURRENT|MCL_FUTURE);

  rsa_init(&rsa);
  mc = vf = 0;
  em.data = (uint8_t *)EM;
  em.size = (uint32_t)sizeof(EM);

/*
generate these with gentests.pl
NOTE: in some cases, the RSA signing operation will produce a signature which
is 1 or more bytes less in length than N.  In these cases, it must be padded
on the left with zeros.
*/
#include "tests.c"

  rsa_free(&rsa);

  munlockall();

  printf("\nTest run completed with %d miscompares and %d verification failures.\n\n", mc, vf);

  return 0;
}

int
rsa_verify (const uint8_t *pubkey, const uint8_t *hash, const uint8_t *sig)
{
  int r;

  rsa_init (&rsa_ctx, RSA_PKCS_V15, 0);
  rsa_ctx.len = KEY_CONTENT_LEN;
  mpi_lset (&rsa_ctx.E, 0x10001);
  mpi_read_binary (&rsa_ctx.N, pubkey, KEY_CONTENT_LEN);

  DEBUG_INFO ("RSA verify...");

  r = rsa_pkcs1_verify (&rsa_ctx, RSA_PUBLIC, SIG_RSA_SHA256, 32, hash, sig);

  rsa_free (&rsa_ctx);
  if (r < 0)
    {
      DEBUG_INFO ("fail:");
      DEBUG_SHORT (r);
      return r;
    }
  else
    {
      DEBUG_INFO ("verified.\r\n");
      return 0;
    }
}

int rsa_sign(const char *private_path, const void *data, const int size,
	     uint8_t **sigp, uint *sig_len)
{
	RSA *rsa;
	int ret;

	ret = rsa_init();

	if (ret)
		return ret;

	ret = rsa_get_priv_key(private_path, &rsa);
	if (ret)
		goto err_priv;

	ret = rsa_sign_with_key(rsa, data, size, sigp, sig_len);
	if (ret)
		goto err_sign;

	RSA_free(rsa);
	rsa_remove();

	return ret;

err_sign:
	RSA_free(rsa);
err_priv:
	rsa_remove();
	return ret;	
}

static int rsa_genkey (lua_State *L) {
    rsa_context rsa;
    havege_state hs;
    int ret=0;
    
    rsa_init( &rsa, RSA_PKCS_V15, 0, havege_rand, &hs );
    
    if( ( ret = rsa_gen_key( &rsa, KEY_SIZE, EXPONENT ) ) != 0 )
    {
        luaL_error(L, "Error generating key (%d)", ret);
    }
    
    /* Public Key */
    if(ret = push_public_key(L, &rsa))
    {
    	luaL_error(L, "failed to obtain public key: error %d", ret );
    }
    
    /* Private Key */
    if(ret = push_private_key(L, &rsa))
    {
    	luaL_error(L, "failed to obtain private key: error %d", ret );
    }
    
    rsa_free( &rsa );
    
    return 2;
}

uint8_t *
rsa_genkey (void)
{
  int r;
  uint8_t index = 0;
  uint8_t *p_q_modulus = (uint8_t *)malloc (KEY_CONTENT_LEN*2);
  uint8_t *p = p_q_modulus;
  uint8_t *q = p_q_modulus + KEY_CONTENT_LEN/2;
  uint8_t *modulus = p_q_modulus + KEY_CONTENT_LEN;

  if (p_q_modulus == NULL)
    return NULL;

  rsa_init (&rsa_ctx, RSA_PKCS_V15, 0);
  r = rsa_gen_key (&rsa_ctx, random_byte, &index,
		   KEY_CONTENT_LEN * 8, RSA_EXPONENT);
  if (r < 0)
    {
      free (p_q_modulus);
      rsa_free (&rsa_ctx);
      return NULL;
    }

  mpi_write_binary (&rsa_ctx.P, p, KEY_CONTENT_LEN/2);
  mpi_write_binary (&rsa_ctx.Q, q, KEY_CONTENT_LEN/2);
  mpi_write_binary (&rsa_ctx.N, modulus, KEY_CONTENT_LEN);
  rsa_free (&rsa_ctx);
  return p_q_modulus;
}

int rsa_sign(struct image_sign_info *info,
	     const struct image_region region[], int region_count,
	     uint8_t **sigp, uint *sig_len)
{
	RSA *rsa;
	int ret;

	ret = rsa_init();
	if (ret)
		return ret;

	ret = rsa_get_priv_key(info->keydir, info->keyname, &rsa);
	if (ret)
		goto err_priv;
	ret = rsa_sign_with_key(rsa, info->checksum, region,
				region_count, sigp, sig_len);
	if (ret)
		goto err_sign;

	RSA_free(rsa);
	rsa_remove();

	return ret;

err_sign:
	RSA_free(rsa);
err_priv:
	rsa_remove();
	return ret;
}

void init_rsa_context_with_public_key(rsa_context *rsa,
                                      const unsigned char *pubkey)
{
  rsa_init(rsa, RSA_PKCS_V15, RSA_RAW, NULL, NULL);

  rsa->len = 256;
  mpi_read_binary(&rsa->N, pubkey + 33, 256);
  mpi_read_string(&rsa->E, 16, "10001");
}

int main (void){
	main_setup();

	for(;;){
		welcome_msg(algo_name);
		rsa_init();
		cmd_interface(cmdlist);
	}
}

static void *rsa_alloc_wrap( void )
{
    void *ctx = polarssl_malloc( sizeof( rsa_context ) );

    if( ctx != NULL )
        rsa_init( (rsa_context *) ctx, 0, 0 );

    return ctx;
}

/**
*  Decrypts a string and removes the padding using either private or public key. 
* (depending on mode).
*  @param ciphertext: binary string to be decrypted.
*  @param key: table containing either the public or the private key, as generated by gen_key.
*  @return  The original message (if everything works ok).
*  @see  rsa_genkey
*/
static int luarsa_pkcs1_decrypt (lua_State *L) {
	int res = 0;
	int mode;
    size_t lmsg, lresult;
    rsa_context rsa;
    char *message = (char*)luaL_checklstring(L, 1, &lmsg); /* ciphertext */
    char result[KEY_SIZE];
    
    rsa_init( &rsa, RSA_PKCS_V15, 0, NULL, NULL ); 
    

    mode = processKey(L, 2, &rsa); /* keytable */
    
    rsa.len = lmsg;

    memset(result, 0, KEY_SIZE);
    printf("\nMode==%s\n", mode==RSA_PUBLIC ? "RSA_PUBLIC" : "RSA_PRIVATE" );
    printf("Size==%d\n", lmsg );
    printf("Crypt.Size==%d\n", rsa.len );
    
    printf("ver: %d\n", rsa.ver);
    printf("len: %d\n", rsa.len);
    printf("padding: %d\n", rsa.padding);
    printf("hash_id: %d\n", rsa.hash_id);
    
    mpi_print("N:%s\n", &rsa.N);
    mpi_print("E:%s\n", &rsa.E);
    
    if(mode!=RSA_PUBLIC) {
        mpi_print("D:%s\n", &rsa.D);
        mpi_print("P:%s\n", &rsa.P);
        mpi_print("Q:%s\n", &rsa.Q);
        mpi_print("DP:%s\n", &rsa.DP);
        mpi_print("DQ:%s\n", &rsa.DQ);
        mpi_print("QP:%s\n", &rsa.QP);

        //mpi_print("RN:%s\n", &rsa.RN);
        //mpi_print("RP:%s\n", &rsa.RP);
        //mpi_print("RQ:%s\n", &rsa.RQ);
    }
    
    // pass rsa context and ciphertext to decryption engine
    res = rsa_pkcs1_decrypt(&rsa, RSA_PRIVATE, &lmsg, message, result);
    printf("Orig.Size==%d\n", lmsg );
    
    if(res) {
    	luaL_error(L, "Error during cipher (%d)", res);
    }
    
    // push encrypted result buffer
    lua_pushlstring(L, result, lmsg); /* ciphertext */

    rsa_free( &rsa );
    
    return 1;
}

int chiffrer_rsa(char* data, char* sortie, int taille_data )
{
    FILE *f;
    int ret;
    size_t i;
	rsa_context rsa;
    entropy_context entropy;
    ctr_drbg_context ctr_drbg;
    char *pers = "rsa_encrypt";
	
    printf( "[i] Seeding the random number generator\n" );

    entropy_init( &entropy );
    if( ( ret = ctr_drbg_init( &ctr_drbg, entropy_func, &entropy,
                               (unsigned char *) pers, strlen( pers ) ) ) != 0 )
    {
        printf( "[-] ctr_drbg_init returned %d\n", ret );
        goto exit;
    }

    printf( "[i] Reading private key\n" );


    rsa_init( &rsa, RSA_PKCS_V15, 0 );
    
    if( ( ret = mpi_read_string( &rsa.N, RSA_N_BASE, RSA_N ) ) != 0 ||
        ( ret = mpi_read_string( &rsa.D, RSA_D_BASE, RSA_D ) ) != 0 )
    {
        printf( "[-] mpi_read_file returned %d\n", ret );
        goto exit;
    }

    rsa.len = ( mpi_msb( &rsa.N ) + 7 ) >> 3;


    /*
     * Calculate the RSA encryption of the hash.
     */
    printf( "[i] Generating the RSA encrypted value (%d/%d)\n", rsa.len, taille_data );
    fflush( stdout );

    if( ( ret = rsa_pkcs1_encrypt( &rsa, ctr_drbg_random, &ctr_drbg,
                                   RSA_PRIVATE, taille_data,
                                   data, sortie ) ) != 0 )
    {
        printf( "[-] rsa_pkcs1_encrypt returned %d\n\n", ret );
        goto exit;
    }
    printf( "[i] Cryptogramme copie\n");

exit:
    return( ret );
}

int
rsa_decrypt (const uint8_t *input, uint8_t *output, int msg_len,
	     struct key_data *kd)
{
  mpi P1, Q1, H;
  int r;
  int output_len;

  DEBUG_INFO ("RSA decrypt:");
  DEBUG_WORD ((uint32_t)&output_len);

  mpi_init (&P1, &Q1, &H, NULL);
  rsa_init (&rsa_ctx, RSA_PKCS_V15, 0);

  rsa_ctx.len = msg_len;
  DEBUG_WORD (msg_len);

  mpi_lset (&rsa_ctx.E, 0x10001);
  mpi_read_binary (&rsa_ctx.P, &kd->data[0], KEY_CONTENT_LEN / 2);
  mpi_read_binary (&rsa_ctx.Q, &kd->data[KEY_CONTENT_LEN/2],
		   KEY_CONTENT_LEN / 2);
#if 0 /* Using CRT, we don't use N */
  mpi_mul_mpi (&rsa_ctx.N, &rsa_ctx.P, &rsa_ctx.Q);
#endif
  mpi_sub_int (&P1, &rsa_ctx.P, 1);
  mpi_sub_int (&Q1, &rsa_ctx.Q, 1);
  mpi_mul_mpi (&H, &P1, &Q1);
  mpi_inv_mod (&rsa_ctx.D , &rsa_ctx.E, &H);
  mpi_mod_mpi (&rsa_ctx.DP, &rsa_ctx.D, &P1);
  mpi_mod_mpi (&rsa_ctx.DQ, &rsa_ctx.D, &Q1);
  mpi_inv_mod (&rsa_ctx.QP, &rsa_ctx.Q, &rsa_ctx.P);
  mpi_free (&P1, &Q1, &H, NULL);

  DEBUG_INFO ("RSA decrypt ...");

  r = rsa_pkcs1_decrypt (&rsa_ctx, RSA_PRIVATE, &output_len,
			 input, output, MAX_RES_APDU_DATA_SIZE);
  rsa_free (&rsa_ctx);
  if (r < 0)
    {
      DEBUG_INFO ("fail:");
      DEBUG_SHORT (r);
      return r;
    }
  else
    {
      res_APDU_size = output_len;
      DEBUG_INFO ("done.\r\n");
      GPG_SUCCESS ();
      return 0;
    }
}

uint8_t *rsa_apply(uint8_t *input, int inlen, int *outlen, int mode) {
  rsa_context trsa;
  const char *pers = "rsa_encrypt";
  int rc;

  entropy_context entropy;
  ctr_drbg_context ctr_drbg;
  entropy_init(&entropy);
  if ((rc = ctr_drbg_init(&ctr_drbg, entropy_func, &entropy, (const unsigned char *)pers,
                          strlen(pers))) != 0)
    debug(1, "ctr_drbg_init returned %d\n", rc);

  rsa_init(&trsa, RSA_PKCS_V21, POLARSSL_MD_SHA1); // padding and hash id get overwritten
  // BTW, this seems to reset a lot of parameters in the rsa_context
  rc = x509parse_key(&trsa, (unsigned char *)super_secret_key, strlen(super_secret_key), NULL, 0);
  if (rc != 0)
    debug(1, "Error %d reading the private key.");

  uint8_t *out = NULL;

  switch (mode) {
  case RSA_MODE_AUTH:
    trsa.padding = RSA_PKCS_V15;
    trsa.hash_id = POLARSSL_MD_NONE;
    debug(2, "rsa_apply encrypt");
    out = malloc(trsa.len);
    rc = rsa_pkcs1_encrypt(&trsa, ctr_drbg_random, &ctr_drbg, RSA_PRIVATE, inlen, input, out);
    if (rc != 0)
      debug(1, "rsa_pkcs1_encrypt error %d.", rc);
    *outlen = trsa.len;
    break;
  case RSA_MODE_KEY:
    debug(2, "rsa_apply decrypt");
    trsa.padding = RSA_PKCS_V21;
    trsa.hash_id = POLARSSL_MD_SHA1;
    out = malloc(trsa.len);
#if POLARSSL_VERSION_NUMBER >= 0x01020900
    rc = rsa_pkcs1_decrypt(&trsa, ctr_drbg_random, &ctr_drbg, RSA_PRIVATE, (size_t *)outlen, input,
                           out, trsa.len);
#else
    rc = rsa_pkcs1_decrypt(&trsa, RSA_PRIVATE, outlen, input, out, trsa.len);
#endif
    if (rc != 0)
      debug(1, "decrypt error %d.", rc);
    break;
  default:
    die("bad rsa mode");
  }
  rsa_free(&trsa);
  debug(2, "rsa_apply exit");
  return out;
}

int EsSign::RsaVerify(const u8* hash, const u8* modulus, const u8* signature)
{
	static const u8 public_exponent[3] = { 0x01, 0x00, 0x01 };

	int ret;
	EsSignType type;
	rsa_context rsa;
	int hash_id = 0;
	int hash_len = 0;

	rsa_init(&rsa, RSA_PKCS_V15, hash_id);

	if (hash == NULL || modulus == NULL || signature == NULL) return 1;

	// get signature type
	type = (EsSignType)be_word(*((u32*)(signature)));

	switch (type)
	{
	case(ES_SIGN_RSA4096_SHA1) :
	case(ES_SIGN_RSA4096_SHA256) :
	{
		rsa.len = Crypto::kRsa4096Size;
		hash_id = (type == ES_SIGN_RSA4096_SHA1) ? SIG_RSA_SHA1 : SIG_RSA_SHA256;
		hash_len = (type == ES_SIGN_RSA4096_SHA1) ? Crypto::kSha1HashLen : Crypto::kSha256HashLen;
		break;
	}
	case(ES_SIGN_RSA2048_SHA1) :
	case(ES_SIGN_RSA2048_SHA256) :
	{
		rsa.len = Crypto::kRsa2048Size;
		hash_id = (type == ES_SIGN_RSA2048_SHA1) ? SIG_RSA_SHA1 : SIG_RSA_SHA256;
		hash_len = (type == ES_SIGN_RSA2048_SHA1) ? Crypto::kSha1HashLen : Crypto::kSha256HashLen;
		break;
	}
	default:
		return 1;
	}

	mpi_read_binary(&rsa.E, public_exponent, sizeof(public_exponent));
	mpi_read_binary(&rsa.N, modulus, rsa.len);

	ret = rsa_rsassa_pkcs1_v15_verify(&rsa, RSA_PRIVATE, hash_id, hash_len, hash, signature + 4);

	rsa_free(&rsa);

	return ret;
}

int Crypto::SignRsa2048Sha256(const u8 modulus[kRsa2048Size], const u8 private_exponent[kRsa2048Size], const u8 hash[kSha256HashLen], u8 signature[kRsa2048Size])
{
	int ret;
	rsa_context ctx;
	rsa_init(&ctx, RSA_PKCS_V15, 0);

	ctx.len = kRsa2048Size;
	mpi_read_binary(&ctx.D, private_exponent, ctx.len);
	mpi_read_binary(&ctx.N, modulus, ctx.len);

	ret = rsa_rsassa_pkcs1_v15_sign(&ctx, RSA_PRIVATE, SIG_RSA_SHA256, kSha256HashLen, hash, signature);

	rsa_free(&ctx);

	return ret;
}

int
rsa_sign (const uint8_t *raw_message, uint8_t *output, int msg_len,
	  struct key_data *kd)
{
  mpi P1, Q1, H;
  int r;
  unsigned char temp[RSA_SIGNATURE_LENGTH];

  mpi_init (&P1, &Q1, &H, NULL);
  rsa_init (&rsa_ctx, RSA_PKCS_V15, 0);

  rsa_ctx.len = KEY_CONTENT_LEN;
  mpi_lset (&rsa_ctx.E, 0x10001);
  mpi_read_binary (&rsa_ctx.P, &kd->data[0], rsa_ctx.len / 2);
  mpi_read_binary (&rsa_ctx.Q, &kd->data[KEY_CONTENT_LEN/2], rsa_ctx.len / 2);
#if 0 /* Using CRT, we don't use N */
  mpi_mul_mpi (&rsa_ctx.N, &rsa_ctx.P, &rsa_ctx.Q);
#endif
  mpi_sub_int (&P1, &rsa_ctx.P, 1);
  mpi_sub_int (&Q1, &rsa_ctx.Q, 1);
  mpi_mul_mpi (&H, &P1, &Q1);
  mpi_inv_mod (&rsa_ctx.D , &rsa_ctx.E, &H);
  mpi_mod_mpi (&rsa_ctx.DP, &rsa_ctx.D, &P1);
  mpi_mod_mpi (&rsa_ctx.DQ, &rsa_ctx.D, &Q1);
  mpi_inv_mod (&rsa_ctx.QP, &rsa_ctx.Q, &rsa_ctx.P);
  mpi_free (&P1, &Q1, &H, NULL);

  DEBUG_INFO ("RSA sign...");

  r = rsa_pkcs1_sign (&rsa_ctx, RSA_PRIVATE, SIG_RSA_RAW,
		      msg_len, raw_message, temp);
  memcpy (output, temp, RSA_SIGNATURE_LENGTH);
  rsa_free (&rsa_ctx);
  if (r < 0)
    {
      DEBUG_INFO ("fail:");
      DEBUG_SHORT (r);
      return r;
    }
  else
    {
      res_APDU_size = RSA_SIGNATURE_LENGTH;
      DEBUG_INFO ("done.\r\n");
      GPG_SUCCESS ();
      return 0;
    }
}

// Initialize peripheral UART upgrade procedure
int ws_upgrade_ota_init(void)
{
    ws_upgrade_state = WS_UPGRADE_STATE_IDLE;

    // register memory management function
    brcmcryptoglue_set_allocator(cfa_mm_Alloc);
    brcmcryptoglue_set_deallocator(cfa_mm_Free);

    // initialize padding scheme and hash algorithm
    rsa_init(&rsaCtx, RSA_PKCS_V21, POLARSSL_MD_SHA256);

    mpi_read_binary( &rsaCtx.E, &rsa_pub_key[0], 3);
    mpi_read_binary( &rsaCtx.N, &rsa_pub_key[3], WS_UPGRADE_RSA_SIGNATURE_LEN + 1);

    ws_upgrade_init();
    return TRUE;
}

void init_rsa_context_with_public_key(rsa_context *rsa,
                                      const unsigned char *pubkey)
{
#ifdef USE_MBEDTLS
  mbedtls_rsa_init(rsa, MBEDTLS_RSA_PKCS_V15, 0);
#else
  rsa_init(rsa, RSA_PKCS_V15, RSA_RAW, NULL, NULL);
#endif

#if !defined(USE_MBEDTLS) && (PLATFORM_ID == 6 || PLATFORM_ID == 8)
  rsa->length = 256;
#else
  rsa->len = 256;
#endif
  mpi_read_binary(&rsa->N, pubkey + 33, 256);
  mpi_read_string(&rsa->E, 16, "10001");
}

/* Initialize SSL library
 */
int init_ssl_module(char *logfile) {
	ssl_error_logfile = logfile;

	rsa_init(&rsa, RSA_PKCS_V15, 0);

	entropy_init(&entropy);
	ctr_drbg_init(&ctr_drbg, entropy_func, &entropy, (unsigned char*)"Hiawatha_RND", 10);
	ctr_drbg_set_prediction_resistance(&ctr_drbg, CTR_DRBG_PR_OFF);

	ssl_cache_init(&cache);
	ssl_cache_set_max_entries(&cache, 100);

	if (pthread_mutex_init(&random_mutex, NULL) != 0) {
		return -1;
	} else if (pthread_mutex_init(&cache_mutex, NULL) != 0) {
		return -1;
	}

	return 0;
}

void
crypto_rsa_encrypt(int len, uint8 * in, uint8 * out, uint32 modulus_size, uint8 * modulus, uint8 * exponent)
{
	rsa_context ctx;
	rsa_init(&ctx, 0, 0);
	ctx.len = modulus_size;
	mpi_init(&ctx.N, &ctx.E, NULL);
	mpi_read_binary(&ctx.N, modulus, modulus_size);
	mpi_read_binary(&ctx.E, exponent, SEC_EXPONENT_SIZE);
	ASSERT(!rsa_check_pubkey( &ctx ));

	ASSERT(modulus_size <= SEC_MAX_MODULUS_SIZE);
	uint8 in2[SEC_MAX_MODULUS_SIZE];
	memset(in2, 0, modulus_size - len);
	memcpy(in2 + modulus_size - len, in, len);
	int err = rsa_public(&ctx, in2, out);
	ASSERT(!err);
	mpi_free(&ctx.N, &ctx.E, NULL);
	rsa_free(&ctx);
}