---
 .../hwkey-agent/CA_sample/hwkey-app.c         |   6 +-
 .../hwkey-agent/crypto_service.c              | 146 ++++++++++++++++--
 .../nvidia-sample/hwkey-agent/ekb_helper.c    |  60 ++++---
 trusty/app/nvidia-sample/hwkey-agent/rules.mk |   3 +-
 .../lib/lib/openssl/Crypto-config-trusty.mk   |   5 +
 trusty/lk/common/lib/libc/string/strcspn.c    |  22 +++
 trusty/lk/trusty/lib/trusty/trusty_app.c      |   4 +-
 7 files changed, 205 insertions(+), 41 deletions(-)
 create mode 100644 trusty/lk/common/lib/libc/string/strcspn.c

diff --git a/trusty/app/nvidia-sample/hwkey-agent/CA_sample/hwkey-app.c b/trusty/app/nvidia-sample/hwkey-agent/CA_sample/hwkey-app.c
index b694240..2d3ea6e 100644
--- a/trusty/app/nvidia-sample/hwkey-agent/CA_sample/hwkey-app.c
+++ b/trusty/app/nvidia-sample/hwkey-agent/CA_sample/hwkey-app.c
@@ -283,13 +283,13 @@ static void handle_file_encryption(struct arguments *argus)
 
 	do {
 		int f_rc;
-		uint8_t buff[CRYPTO_SRV_PAYLOAD_SIZE];
+		uint8_t buff[inf_size];
 		bool first_packet = false;
 		bool last_packet = false;
 
-		memset(buff, 0, CRYPTO_SRV_PAYLOAD_SIZE);
+		memset(buff, 0, inf_size);
 
-		f_rc = fread(buff, 1, CRYPTO_SRV_PAYLOAD_SIZE, infptr);
+		f_rc = fread(buff, 1, inf_size, infptr);
 		if (f_rc < 0) {
 			LOG("Unexpected failure when reading input file\n");
 			fail_handler(1);
diff --git a/trusty/app/nvidia-sample/hwkey-agent/crypto_service.c b/trusty/app/nvidia-sample/hwkey-agent/crypto_service.c
index a77767e..9a20fd3 100644
--- a/trusty/app/nvidia-sample/hwkey-agent/crypto_service.c
+++ b/trusty/app/nvidia-sample/hwkey-agent/crypto_service.c
@@ -27,6 +27,104 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <openssl/bn.h>
+#include <openssl/pem.h>
+#include <openssl/ssl.h>
+#include <openssl/rsa.h>
+#include <openssl/evp.h>
+#include <openssl/bio.h>
+#include <openssl/err.h>
+#include <openssl/rand.h>
+
+int padding = RSA_PKCS1_PADDING;
+static char publicKey[] = "-----BEGIN PUBLIC KEY-----\n"
+						  "MIIBIjANBgkqhkiG9w0BAQEFAAOCAQ8AMIIBCgKCAQEAy8Dbv8prpJ/0kKhlGeJY\n"
+						  "ozo2t60EG8L0561g13R29LvMR5hyvGZlGJpmn65+A4xHXInJYiPuKzrKUnApeLZ+\n"
+						  "vw1HocOAZtWK0z3r26uA8kQYOKX9Qt/DbCdvsF9wF8gRK0ptx9M6R13NvBxvVQAp\n"
+						  "fc9jB9nTzphOgM4JiEYvlV8FLhg9yZovMYd6Wwf3aoXK891VQxTr/kQYoq1Yp+68\n"
+						  "i6T4nNq7NWC+UNVjQHxNQMQMzU6lWCX8zyg3yH88OAQkUXIXKfQ+NkvYQ1cxaMoV\n"
+						  "PpY72+eVthKzpMeyHkBn7ciumk5qgLTEJAfWZpe4f4eFZj/Rc8Y8Jj2IS5kVPjUy\n"
+						  "wQIDAQAB\n"
+						  "-----END PUBLIC KEY-----\n";
+
+static char privateKey[] = "-----BEGIN RSA PRIVATE KEY-----\n"
+						   "MIIEowIBAAKCAQEAy8Dbv8prpJ/0kKhlGeJYozo2t60EG8L0561g13R29LvMR5hy\n"
+						   "vGZlGJpmn65+A4xHXInJYiPuKzrKUnApeLZ+vw1HocOAZtWK0z3r26uA8kQYOKX9\n"
+						   "Qt/DbCdvsF9wF8gRK0ptx9M6R13NvBxvVQApfc9jB9nTzphOgM4JiEYvlV8FLhg9\n"
+						   "yZovMYd6Wwf3aoXK891VQxTr/kQYoq1Yp+68i6T4nNq7NWC+UNVjQHxNQMQMzU6l\n"
+						   "WCX8zyg3yH88OAQkUXIXKfQ+NkvYQ1cxaMoVPpY72+eVthKzpMeyHkBn7ciumk5q\n"
+						   "gLTEJAfWZpe4f4eFZj/Rc8Y8Jj2IS5kVPjUywQIDAQABAoIBADhg1u1Mv1hAAlX8\n"
+						   "omz1Gn2f4AAW2aos2cM5UDCNw1SYmj+9SRIkaxjRsE/C4o9sw1oxrg1/z6kajV0e\n"
+						   "N/t008FdlVKHXAIYWF93JMoVvIpMmT8jft6AN/y3NMpivgt2inmmEJZYNioFJKZG\n"
+						   "X+/vKYvsVISZm2fw8NfnKvAQK55yu+GRWBZGOeS9K+LbYvOwcrjKhHz66m4bedKd\n"
+						   "gVAix6NE5iwmjNXktSQlJMCjbtdNXg/xo1/G4kG2p/MO1HLcKfe1N5FgBiXj3Qjl\n"
+						   "vgvjJZkh1as2KTgaPOBqZaP03738VnYg23ISyvfT/teArVGtxrmFP7939EvJFKpF\n"
+						   "1wTxuDkCgYEA7t0DR37zt+dEJy+5vm7zSmN97VenwQJFWMiulkHGa0yU3lLasxxu\n"
+						   "m0oUtndIjenIvSx6t3Y+agK2F3EPbb0AZ5wZ1p1IXs4vktgeQwSSBdqcM8LZFDvZ\n"
+						   "uPboQnJoRdIkd62XnP5ekIEIBAfOp8v2wFpSfE7nNH2u4CpAXNSF9HsCgYEA2l8D\n"
+						   "JrDE5m9Kkn+J4l+AdGfeBL1igPF3DnuPoV67BpgiaAgI4h25UJzXiDKKoa706S0D\n"
+						   "4XB74zOLX11MaGPMIdhlG+SgeQfNoC5lE4ZWXNyESJH1SVgRGT9nBC2vtL6bxCVV\n"
+						   "WBkTeC5D6c/QXcai6yw6OYyNNdp0uznKURe1xvMCgYBVYYcEjWqMuAvyferFGV+5\n"
+						   "nWqr5gM+yJMFM2bEqupD/HHSLoeiMm2O8KIKvwSeRYzNohKTdZ7FwgZYxr8fGMoG\n"
+						   "PxQ1VK9DxCvZL4tRpVaU5Rmknud9hg9DQG6xIbgIDR+f79sb8QjYWmcFGc1SyWOA\n"
+						   "SkjlykZ2yt4xnqi3BfiD9QKBgGqLgRYXmXp1QoVIBRaWUi55nzHg1XbkWZqPXvz1\n"
+						   "I3uMLv1jLjJlHk3euKqTPmC05HoApKwSHeA0/gOBmg404xyAYJTDcCidTg6hlF96\n"
+						   "ZBja3xApZuxqM62F6dV4FQqzFX0WWhWp5n301N33r0qR6FumMKJzmVJ1TA8tmzEF\n"
+						   "yINRAoGBAJqioYs8rK6eXzA8ywYLjqTLu/yQSLBn/4ta36K8DyCoLNlNxSuox+A5\n"
+						   "w6z2vEfRVQDq4Hm4vBzjdi3QfYLNkTiTqLcvgWZ+eX44ogXtdTDO7c+GeMKWz4XX\n"
+						   "uJSUVL5+CVjKLjZEJ6Qc2WZLl94xSwL71E41H4YciVnSCQxVc4Jw\n"
+						   "-----END RSA PRIVATE KEY-----\n";
+static const char rnd_seed[] = "string to make the random number generator think it has entropy";
+static RSA *createRSA(char *key, int pubtype)
+{
+	RSA *rsa = NULL;
+	BIO *bio = NULL;
+
+	bio = BIO_new(BIO_s_mem());
+	if (bio == NULL)
+	{
+		TLOGI("bio is NULL!\n");
+	}
+
+	BIO_puts(bio, key);
+	if (pubtype == 1)
+	{
+		rsa = PEM_read_bio_RSA_PUBKEY(bio, NULL, NULL, NULL);
+	}
+	else
+	{
+		rsa = PEM_read_bio_RSAPrivateKey(bio, NULL, NULL, NULL);
+	}
+	if (rsa == NULL)
+	{
+		TLOGI("wrong again\n");
+	}
+	BIO_free(bio);
+	return rsa;
+}
+static int private_decrypt(unsigned char *enc_data, int data_len, char *key, unsigned char *decrypted)
+{
+	RSA *rsa = createRSA(key, 0);
+	if (rsa != NULL)
+	{
+		int result = RSA_private_decrypt(data_len, enc_data, decrypted, rsa, RSA_PKCS1_PADDING);
+		RSA_free(rsa);
+		return result;
+	}
+	return 0;
+}
+int public_encrypt(unsigned char *data, int data_len, char *key, unsigned char *encrypted)
+{
+	RSA *rsa = createRSA(key, 1);
+	if (rsa != NULL)
+	{
+		int result = RSA_public_encrypt(data_len, data, encrypted, rsa, padding);
+		RSA_free(rsa);
+		return result;
+	}
+	return 0;
+}
+
 void crypto_srv_process_req(iovec_t *ipc_msg, int len)
 {
 	crypto_srv_msg_t *msg = ipc_msg[0].base;
@@ -34,34 +132,54 @@ void crypto_srv_process_req(iovec_t *ipc_msg, int len)
 	int payload_len = msg->payload_len;
 	uint8_t *data = NULL;
 	uint8_t *key_in_ekb;
+	unsigned long err;
+	/*
 	AES_KEY aes_key;
 	int aes_enc_flag;
 	int idx;
+	*/
 
 	key_in_ekb = ekb_get_key(EKB_USER_KEY_DISK_ENCRYPTION);
-	if (key_in_ekb == NULL) {
+	if (key_in_ekb == NULL)
+	{
 		TLOGE("%s: get key in ekb failed\n", __func__);
 		return;
 	}
 
 	data = malloc(CRYPTO_SRV_PAYLOAD_SIZE);
-	if (data == NULL) {
+	if (data == NULL)
+	{
 		TLOGE("%s: malloc failed\n", __func__);
 		return;
 	}
 	memcpy(data, payload, payload_len);
-
-	if (msg->cmd == CRYPTO_SRV_CMD_ENCRYPT) {
-		AES_set_encrypt_key(key_in_ekb, AES_KEY_128_SIZE * 8, &aes_key);
-		aes_enc_flag = AES_ENCRYPT;
-	} else {
-		AES_set_decrypt_key(key_in_ekb, AES_KEY_128_SIZE * 8, &aes_key);
-		aes_enc_flag = AES_DECRYPT;
+	RAND_seed(rnd_seed, sizeof rnd_seed);
+	if (msg->cmd == CRYPTO_SRV_CMD_ENCRYPT)
+	{
+//		int size_ = public_encrypt(data, 1, publicKey, payload);
+		public_encrypt(data, 1, publicKey, payload);
+		err = ERR_get_error();
+		payload =(uint8_t *)ERR_error_string(err,(char *)payload);
+		//payload = ERR_func_error_string(108);
+		//payload = (unsigned char*)ERR_reason_error_string(110);//ERR_lib_error_string(36)-ERR_func_error_string(108)-ERR_reason_error_string(110);
+		//memcpy(msg->payload,encrypted, sizeof(encrypted));
+		if (ERR_reason_error_string(110) == NULL)
+		{
+			msg->cmd = -100;
+		}
+	}
+	else
+	{
+//		int size_ = private_decrypt(data, 1, privateKey, payload);
+		private_decrypt(data, 1, privateKey,payload);
+		err = ERR_get_error();
+		payload = (uint8_t *)ERR_error_string(err, (char *)payload);
+		//payload = (unsigned char*)ERR_reason_error_string(110);//ERR_lib_error_string(36)-ERR_func_error_string(108)-ERR_reason_error_string(110);
+		//memcpy(msg->payload,encrypted, sizeof(encrypted));
+		if (ERR_reason_error_string(110) == NULL)
+		{
+			msg->cmd = -100;
+		}
 	}
-
-	for (idx = 0; idx < payload_len; idx += AES_BLOCK_SIZE)
-		AES_cbc_encrypt(data + idx, payload + idx, AES_BLOCK_SIZE,
-				&aes_key, msg->iv, aes_enc_flag);
-
 	free(data);
 }
diff --git a/trusty/app/nvidia-sample/hwkey-agent/ekb_helper.c b/trusty/app/nvidia-sample/hwkey-agent/ekb_helper.c
index 0e1ca84..e498b27 100644
--- a/trusty/app/nvidia-sample/hwkey-agent/ekb_helper.c
+++ b/trusty/app/nvidia-sample/hwkey-agent/ekb_helper.c
@@ -38,21 +38,24 @@ typedef uint8_t sym_key[AES_KEY_128_SIZE];
 
 sym_key *sym_keys[EKB_USER_KEYS_NUM];
 
-struct ekb_content {
+struct ekb_content
+{
 	uint8_t ekb_cmac[16];
 	uint8_t random_iv[16];
 	uint8_t ekb_ciphertext[16];
 };
 
 /* Facilitates the IOCTL_MAP_EKS_TO_USER ioctl() */
-union ptr_to_int_bridge {
+union ptr_to_int_bridge
+{
 	uint32_t val;
 	void *ptr;
 };
 
 uint8_t *ekb_get_key(uint8_t idx)
 {
-	if (idx >= EKB_USER_KEYS_NUM) {
+	if (idx >= EKB_USER_KEYS_NUM)
+	{
 		return NULL;
 	}
 	return (uint8_t *)sym_keys[idx];
@@ -78,23 +81,25 @@ static int get_ekb(uint8_t **ekb_base, uint32_t *ekb_size)
 		{.ptr = &nsdram_ekb_base},
 		{.ptr = &nsdram_ekb_size},
 		{.ptr = &nsdram_ekb_map_base},
-		{.ptr = &nsdram_ekb_map_size}
-	};
+		{.ptr = &nsdram_ekb_map_size}};
 
-	if ((ekb_base == NULL) || (ekb_size == NULL)) {
+	if ((ekb_base == NULL) || (ekb_size == NULL))
+	{
 		TLOGE("%s: Invalid arguments\n", __func__);
 		return ERR_INVALID_ARGS;
 	}
 
 	/* Map the NSDRAM region containing EKB */
 	rc = ioctl(3, IOCTL_MAP_EKS_TO_USER, params);
-	if (rc != NO_ERROR) {
+	if (rc != NO_ERROR)
+	{
 		TLOGE("%s: failed to map EKB memory (%d)\n", __func__, rc);
 		return ERR_GENERIC;
 	}
 
 	/* Enforce a reasonable bound on the EKB size */
-	if (nsdram_ekb_size > MIN_HEAP_SIZE) {
+	if (nsdram_ekb_size > MIN_HEAP_SIZE)
+	{
 		rc = ERR_TOO_BIG;
 		goto err;
 	}
@@ -103,7 +108,8 @@ static int get_ekb(uint8_t **ekb_base, uint32_t *ekb_size)
 	*ekb_size = nsdram_ekb_size;
 
 	*ekb_base = malloc(*ekb_size);
-	if (*ekb_base == NULL) {
+	if (*ekb_base == NULL)
+	{
 		TLOGE("%s: malloc failed\n", __func__);
 		rc = ERR_NO_MEMORY;
 		goto err;
@@ -112,7 +118,8 @@ static int get_ekb(uint8_t **ekb_base, uint32_t *ekb_size)
 
 err:
 	/* Unmap the NSDRAM region containing EKB */
-	if (munmap(nsdram_ekb_map_base, nsdram_ekb_map_size) != 0) {
+	if (munmap(nsdram_ekb_map_base, nsdram_ekb_map_size) != 0)
+	{
 		TLOGE("%s: failed to unmap EKB\n", __func__);
 		return ERR_GENERIC;
 	}
@@ -135,30 +142,37 @@ int ekb_verification(uint8_t *ekb_ak, uint8_t *ekb_ek)
 
 	/* Get EKB */
 	rc = get_ekb(&ekb_base, &ekb_size);
-	if ((rc != NO_ERROR) || (ekb_base == NULL) || (ekb_size == 0)) {
+	if ((rc != NO_ERROR) || (ekb_base == NULL) || (ekb_size == 0))
+	{
 		TLOGE("%s: failed to get EKB (%d). Exiting\n", __func__, rc);
 		goto ekb_verify_error;
 	}
 
 	/* Convert to EKB layout */
-	ekb = (struct ekb_content*)ekb_base;
-
+	ekb = (struct ekb_content *)ekb_base;
+	TLOGE("ekb->random_iv is:");
+	for (size_t i = 0; i < 16; i++)
+	{
+		fprintf(stderr, "%d", ekb->random_iv[i]);
+	}
+	fprintf(stderr, "\n");
 	/* Caculate the EKB_cmac */
 	cmac = CMAC_CTX_new();
-	if (cmac == NULL) {
+	if (cmac == NULL)
+	{
 		TLOGE("%s: CMAC_CTX_new failed.\n", __func__);
 		rc = ERR_NO_MEMORY;
 		goto ekb_verify_error;
 	}
 
 	CMAC_Init(cmac, ekb_ak, AES_KEY_128_SIZE, EVP_aes_128_cbc(), NULL);
-	CMAC_Update(cmac, ekb->random_iv, sizeof(ekb->random_iv) +
-				sizeof(ekb->ekb_ciphertext) * EKB_USER_KEYS_NUM);
+	CMAC_Update(cmac, ekb->random_iv, sizeof(ekb->random_iv) + sizeof(ekb->ekb_ciphertext) * EKB_USER_KEYS_NUM);
 	CMAC_Final(cmac, ekb_cmac_verify, &cmac_len);
 	CMAC_CTX_free(cmac);
 
 	/* Verify the CMAC */
-	if (memcmp(ekb->ekb_cmac, ekb_cmac_verify, cmac_len)) {
+	if (memcmp(ekb->ekb_cmac, ekb_cmac_verify, cmac_len))
+	{
 		TLOGE("%s: EKB_CMAC verification is not match.\n", __func__);
 		rc = ERR_NOT_VALID;
 		goto ekb_verify_error;
@@ -166,17 +180,21 @@ int ekb_verification(uint8_t *ekb_ak, uint8_t *ekb_ek)
 
 	/* Decrypt the EKB ciphertext */
 	rc = AES_set_decrypt_key(ekb_ek, AES_KEY_128_SIZE * 8, &decrypt_key);
-	if (rc < 0) {
+	if (rc < 0)
+	{
 		TLOGE("%s: AES set decrypt key failed.\n", __func__);
 		goto ekb_verify_error;
 	}
 
-	for (i = 0; i < EKB_USER_KEYS_NUM; ++i) {
+	for (i = 0; i < EKB_USER_KEYS_NUM; ++i)
+	{
 		data = malloc(AES_KEY_128_SIZE);
-		if (data == NULL) {
+		if (data == NULL)
+		{
 			TLOGE("%s: malloc failed\n", __func__);
 			/* free memory that was previously allocated */
-			while (--i >= 0) {
+			while (--i >= 0)
+			{
 				if (sym_keys[i])
 					free((uint8_t *)sym_keys[i]);
 			}
diff --git a/trusty/app/nvidia-sample/hwkey-agent/rules.mk b/trusty/app/nvidia-sample/hwkey-agent/rules.mk
index 0ce9a55..168c9c6 100644
--- a/trusty/app/nvidia-sample/hwkey-agent/rules.mk
+++ b/trusty/app/nvidia-sample/hwkey-agent/rules.mk
@@ -41,7 +41,8 @@ MODULE_SRCS += \
 	$(LOCAL_DIR)/platform/tegra_se/tegra_se.c \
 	$(LOCAL_DIR)/platform/tegra_se/tegra_se_aes.c \
 	$(LOCAL_DIR)/platform/tegra_se/tegra_se_rng1.c \
-	$(LOCAL_DIR)/rng_srv.c
+	$(LOCAL_DIR)/rng_srv.c \
+/opt/Xavier/32.6/public/atf_and_trusty/trusty/trusty/lk/common/lib/libc/string/strcspn.c
 
 MODULE_DEPS += \
 	app/trusty \
diff --git a/trusty/lib/lib/openssl/Crypto-config-trusty.mk b/trusty/lib/lib/openssl/Crypto-config-trusty.mk
index 6d8f197..5bce3f5 100644
--- a/trusty/lib/lib/openssl/Crypto-config-trusty.mk
+++ b/trusty/lib/lib/openssl/Crypto-config-trusty.mk
@@ -353,6 +353,11 @@ common_src_files := \
   crypto/x509/x_attrib.c \
   crypto/x509/x_pubkey.c \
   crypto/x509v3/v3_utl.c \
+  crypto/asn1/d2i_pu.c \
+  crypto/rand/rand_lib.c\
+  crypto/ui/ui_lib.c\
+  crypto/ui/ui_null.c\
+  crypto/ui/ui_openssl.c\
 
 common_c_includes := \
   . \
diff --git a/trusty/lk/common/lib/libc/string/strcspn.c b/trusty/lk/common/lib/libc/string/strcspn.c
new file mode 100644
index 0000000..3b16e33
--- /dev/null
+++ b/trusty/lk/common/lib/libc/string/strcspn.c
@@ -0,0 +1,22 @@
+extern void abort (void);
+extern int inside_main;
+
+__attribute__ ((__noinline__))
+__SIZE_TYPE__
+strcspn (const char *s1, const char *s2)
+{
+  const char *p, *q;
+#if 0
+#ifdef __OPTIMIZE__
+  if (inside_main)
+    abort();
+#endif
+#endif
+  for (p = s1; *p; p++)
+    for (q = s2; *q; q++)
+      if (*p == *q)
+	goto found;
+
+ found:
+  return p - s1;
+}
diff --git a/trusty/lk/trusty/lib/trusty/trusty_app.c b/trusty/lk/trusty/lib/trusty/trusty_app.c
index 69e6573..486bc72 100644
--- a/trusty/lk/trusty/lib/trusty/trusty_app.c
+++ b/trusty/lk/trusty/lib/trusty/trusty_app.c
@@ -22,7 +22,7 @@
  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-//#define DEBUG_LOAD_TRUSTY_APP
+#define DEBUG_LOAD_TRUSTY_APP 1
 
 #include <arch.h>
 #include <assert.h>
@@ -342,7 +342,7 @@ static status_t alloc_address_map(trusty_app_t *trusty_app)
 #if DEBUG_LOAD_TRUSTY_APP
 		dprintf(SPEW,
 			"trusty_app %d: load vaddr 0x%08lx, paddr 0x%08lx,"
-			" rsize 0x%08x, msize 0x%08x, access %c%c%c,"
+			" rsize 0x%08lx, msize 0x%08x, access %c%c%c,"
 			" flags 0x%x\n",
 			trusty_app_idx, vaddr, paddr, size, prg_hdr->p_memsz,
 			flags & UTM_R ? 'r' : '-', flags & UTM_W ? 'w' : '-',
-- 
2.25.1

