synquacer: Add RNG pseudo TA

diff --git a/core/arch/arm/include/arm64.h b/core/arch/arm/include/arm64.h
index 2c1fd8c..0cf14c0 100644
--- a/core/arch/arm/include/arm64.h
+++ b/core/arch/arm/include/arm64.h
@@ -305,6 +305,10 @@  DEFINE_REG_READ_FUNC_(cntfrq, uint32_t, cntfrq_el0)
 DEFINE_REG_READ_FUNC_(cntpct, uint64_t, cntpct_el0)
 DEFINE_REG_READ_FUNC_(cntkctl, uint32_t, cntkctl_el1)
 DEFINE_REG_WRITE_FUNC_(cntkctl, uint32_t, cntkctl_el1)
+DEFINE_REG_READ_FUNC_(cntps_ctl, uint32_t, cntps_ctl_el1)
+DEFINE_REG_WRITE_FUNC_(cntps_ctl, uint32_t, cntps_ctl_el1)
+DEFINE_REG_READ_FUNC_(cntps_cval, uint32_t, cntps_cval_el1)
+DEFINE_REG_WRITE_FUNC_(cntps_cval, uint32_t, cntps_cval_el1)
 
 DEFINE_REG_READ_FUNC_(pmccntr, uint64_t, pmccntr_el0)
 
diff --git a/core/arch/arm/plat-synquacer/main.c b/core/arch/arm/plat-synquacer/main.c
index c3aac4c..df29f99 100644
--- a/core/arch/arm/plat-synquacer/main.c
+++ b/core/arch/arm/plat-synquacer/main.c
@@ -18,6 +18,7 @@ 
 #include <sm/optee_smc.h>
 #include <tee/entry_fast.h>
 #include <tee/entry_std.h>
+#include <rng_pta.h>
 
 static void main_fiq(void);
 
@@ -38,6 +39,7 @@  static struct pl011_data console_data;
 
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, THERMAL_SENSOR_BASE, CORE_MMU_DEVICE_SIZE);
 
 const struct thread_handlers *generic_boot_get_handlers(void)
 {
@@ -46,7 +48,7 @@  const struct thread_handlers *generic_boot_get_handlers(void)
 
 static void main_fiq(void)
 {
-	panic();
+	gic_it_handle(&gic_data);
 }
 
 void console_init(void)
@@ -66,12 +68,34 @@  void main_init_gic(void)
 	if (!gicd_base)
 		panic();
 
-	/* Initialize GIC */
-	gic_init(&gic_data, 0, gicd_base);
+	/* On ARMv8-A, GIC configuration is initialized in TF-A */
+	gic_init_base_addr(&gic_data, 0, gicd_base);
+
 	itr_init(&gic_data.chip);
 }
 
-void main_secondary_init_gic(void)
+static enum itr_return timer_itr_cb(struct itr_handler *h __unused)
 {
-	gic_cpu_init(&gic_data);
+	/* Reset timer for next FIQ */
+	generic_timer_handler();
+
+	/* Collect entropy on each timer FIQ */
+	rng_collect_entropy();
+
+	return ITRR_HANDLED;
+}
+
+static struct itr_handler timer_itr = {
+	.it = IT_SEC_TIMER,
+	.flags = ITRF_TRIGGER_LEVEL,
+	.handler = timer_itr_cb,
+};
+
+static TEE_Result init_timer_itr(void)
+{
+	itr_add(&timer_itr);
+	itr_enable(IT_SEC_TIMER);
+
+	return TEE_SUCCESS;
 }
+driver_init(init_timer_itr);
diff --git a/core/arch/arm/plat-synquacer/platform_config.h b/core/arch/arm/plat-synquacer/platform_config.h
index 4d6d545..8a91ddb 100644
--- a/core/arch/arm/plat-synquacer/platform_config.h
+++ b/core/arch/arm/plat-synquacer/platform_config.h
@@ -19,6 +19,9 @@ 
 #define CONSOLE_UART_CLK_IN_HZ		62500000
 #define CONSOLE_BAUDRATE		115200
 
+#define THERMAL_SENSOR_BASE		0x54190000
+#define IT_SEC_TIMER			29
+
 #define DRAM0_BASE			0x80000000
 
 /* Platform specific defines */
diff --git a/core/arch/arm/plat-synquacer/rng_pta.c b/core/arch/arm/plat-synquacer/rng_pta.c
new file mode 100644
index 0000000..cdda51d
--- /dev/null
+++ b/core/arch/arm/plat-synquacer/rng_pta.c
@@ -0,0 +1,249 @@ 
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2018, Linaro Limited
+ */
+
+#include <crypto/crypto.h>
+#include <kernel/delay.h>
+#include <kernel/pseudo_ta.h>
+#include <mm/core_memprot.h>
+#include <io.h>
+#include <string.h>
+#include <rng_pta.h>
+#include <rng_pta_client.h>
+
+#define PTA_NAME "rng.pta"
+
+#define ENTROPY_POOL_SIZE	4096
+
+#define RAW_DATA_POOL_SIZE	896
+#define RAW_DATA_PER_SAMPLE	7
+
+#define LSB_MASK		0x1
+
+#define MAX_ONES_COUNT		672
+#define MIN_ONES_COUNT		224
+
+static uint8_t entropy_pool[ENTROPY_POOL_SIZE] = {0};
+static uint32_t entropy_size;
+
+static uint8_t raw_data_pool[RAW_DATA_POOL_SIZE] = {0};
+static uint32_t raw_data_size;
+
+/* Used to monitor health of entropy source */
+static uint32_t ones_count;
+
+static void pool_add_entropy(uint8_t *entropy, uint32_t size)
+{
+	uint32_t copy_size;
+
+	if (entropy_size >= ENTROPY_POOL_SIZE)
+		return;
+
+	if ((ENTROPY_POOL_SIZE - entropy_size) >= size)
+		copy_size = size;
+	else
+		copy_size = ENTROPY_POOL_SIZE - raw_data_size;
+
+	memcpy((entropy_pool + entropy_size), entropy, copy_size);
+
+	entropy_size += copy_size;
+}
+
+static void pool_get_entropy(uint8_t *buf, uint32_t size)
+{
+	uint32_t off;
+
+	if (size > entropy_size)
+		return;
+
+	off = entropy_size - size;
+
+	memcpy(buf, &entropy_pool[off], size);
+	entropy_size -= size;
+}
+
+static uint32_t pool_get_entropy_sz(void)
+{
+	uint32_t sz;
+
+	sz = entropy_size;
+
+	return sz;
+}
+
+static bool health_test(void)
+{
+	bool result = false;
+
+	/*
+	 * Basic heatlh test to check if one's count lies in 25% to 75%
+	 * of collected raw data from sensor readings.
+	 */
+	if ((ones_count <= MAX_ONES_COUNT) && (ones_count >= MIN_ONES_COUNT))
+		result = true;
+	else
+		result = false;
+
+	ones_count = 0;
+
+	return result;
+}
+
+static void pool_add_raw_data(uint8_t *raw_data, uint32_t size)
+{
+	uint32_t copy_size;
+	uint8_t entropy_sha256[TEE_SHA256_HASH_SIZE];
+	TEE_Result res;
+
+	if (raw_data_size >= RAW_DATA_POOL_SIZE) {
+		/* Check if health test passes then only add entropy */
+		if (health_test() == true) {
+			/*
+			 * Use vetted conditioner SHA256 as per NIST.SP.800-90B
+			 * to condition raw data from entropy source.
+			 * Here we have assumed that entropy source provides
+			 * 4 bits per 7 sensor readings per sample.
+			 * Also as per NIST.SP.800-90B, to get full entropy
+			 * from vetted conditioner, we need to supply double of
+			 * input entropy. So with full entropy (8 bits per byte)
+			 * we will get yield as one byte of output data for
+			 * every 28 sensor readings.
+			 * For 32 bytes of SHA256 output data, we need to supply
+			 * 896 bytes of raw input data.
+			 */
+			res = hash_sha256_compute(entropy_sha256, raw_data_pool,
+						  raw_data_size);
+			if (res == TEE_SUCCESS)
+				pool_add_entropy(entropy_sha256,
+						 TEE_SHA256_HASH_SIZE);
+		}
+
+		raw_data_size = 0;
+	}
+
+	if ((RAW_DATA_POOL_SIZE - raw_data_size) >= size)
+		copy_size = size;
+	else
+		copy_size = RAW_DATA_POOL_SIZE - raw_data_size;
+
+	memcpy((raw_data_pool + raw_data_size), raw_data, copy_size);
+
+	raw_data_size += copy_size;
+}
+
+void rng_collect_entropy(void)
+{
+	uint8_t raw_data[RAW_DATA_PER_SAMPLE] = {0};
+
+	raw_data[0] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE0 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[0] & LSB_MASK;
+
+	raw_data[1] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE1 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[1] & LSB_MASK;
+
+	raw_data[2] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE2 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[2] & LSB_MASK;
+
+	raw_data[3] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE3 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[3] & LSB_MASK;
+
+	raw_data[4] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE4 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[4] & LSB_MASK;
+
+	raw_data[5] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE5 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[5] & LSB_MASK;
+
+	raw_data[6] = (uint8_t)read32((vaddr_t)
+				      phys_to_virt_io(THERMAL_SENSOR_BASE6 +
+						      TEMP_DATA_REG_OFFSET));
+	ones_count += raw_data[6] & LSB_MASK;
+
+	pool_add_raw_data(raw_data, RAW_DATA_PER_SAMPLE);
+
+	if (pool_get_entropy_sz() >= ENTROPY_POOL_SIZE)
+		generic_timer_stop();
+}
+
+static TEE_Result rng_get_entropy(uint32_t types,
+				  TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint8_t *e = NULL;
+	uint32_t pool_size = 0, rq_size = 0;
+
+	if (types != TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
+				     TEE_PARAM_TYPE_NONE,
+				     TEE_PARAM_TYPE_NONE,
+				     TEE_PARAM_TYPE_NONE)) {
+		EMSG("bad parameters types: 0x%" PRIx32, types);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rq_size = params[0].memref.size;
+
+	if ((rq_size == 0) || (rq_size > ENTROPY_POOL_SIZE))
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	e = (uint8_t *)params[0].memref.buffer;
+	if (!e)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	pool_size = pool_get_entropy_sz();
+
+	if (pool_size < rq_size) {
+		params[0].memref.size = pool_size;
+		pool_get_entropy(e, pool_size);
+	} else {
+		params[0].memref.size = rq_size;
+		pool_get_entropy(e, rq_size);
+	}
+
+	/* Enable timer FIQ to fetch entropy */
+	generic_timer_start();
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Trusted Application Entry Points
+ */
+static TEE_Result open_session(uint32_t param_types __unused,
+			       TEE_Param params[TEE_NUM_PARAMS] __unused,
+			       void **session_context __unused)
+{
+	DMSG("open entry point for pseudo-TA \"%s\"", PTA_NAME);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result invoke_command(void *pSessionContext __unused,
+				 uint32_t nCommandID, uint32_t nParamTypes,
+				 TEE_Param pParams[TEE_NUM_PARAMS])
+{
+	FMSG("command entry point for pseudo-TA \"%s\"", PTA_NAME);
+
+	switch (nCommandID) {
+	case PTA_CMD_GET_ENTROPY:
+		return rng_get_entropy(nParamTypes, pParams);
+	default:
+		break;
+	}
+
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+pseudo_ta_register(.uuid = PTA_RNG_UUID, .name = PTA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .open_session_entry_point = open_session,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/plat-synquacer/rng_pta.h b/core/arch/arm/plat-synquacer/rng_pta.h
new file mode 100644
index 0000000..6119466
--- /dev/null
+++ b/core/arch/arm/plat-synquacer/rng_pta.h
@@ -0,0 +1,25 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2018, Linaro Limited
+ */
+
+#ifndef __RNG_PTA_H
+#define __RNG_PTA_H
+
+#define THERMAL_SENSOR_BASE0		0x54190800
+#define THERMAL_SENSOR_BASE1		0x54190880
+#define THERMAL_SENSOR_BASE2		0x54190900
+#define THERMAL_SENSOR_BASE3		0x54190980
+#define THERMAL_SENSOR_BASE4		0x54190a00
+#define THERMAL_SENSOR_BASE5		0x54190a80
+#define THERMAL_SENSOR_BASE6		0x54190b00
+
+#define TEMP_DATA_REG_OFFSET		0x34
+
+void rng_collect_entropy(void);
+
+void generic_timer_start(void);
+void generic_timer_stop(void);
+void generic_timer_handler(void);
+
+#endif /* __RNG_PTA_H */
diff --git a/core/arch/arm/plat-synquacer/rng_pta_client.h b/core/arch/arm/plat-synquacer/rng_pta_client.h
new file mode 100644
index 0000000..ddd398c
--- /dev/null
+++ b/core/arch/arm/plat-synquacer/rng_pta_client.h
@@ -0,0 +1,22 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2018, Linaro Limited
+ */
+
+#ifndef __RNG_PTA_CLIENT_H
+#define __RNG_PTA_CLIENT_H
+
+#define PTA_RNG_UUID { 0xab7a617c, 0xb8e7, 0x4d8f, \
+		{ 0x83, 0x01, 0xd0, 0x9b, 0x61, 0x03, 0x6b, 0x64 } }
+
+/*
+ * PTA_CMD_GET_ENTROPY - Get Entropy from RNG using Thermal Sensor
+ *
+ * param[0] (inout memref) - Entropy buffer memory reference
+ * param[1] unused
+ * param[2] unused
+ * param[3] unused
+ */
+#define PTA_CMD_GET_ENTROPY		0x0
+
+#endif /* __RNG_PTA_CLIENT_H */
diff --git a/core/arch/arm/plat-synquacer/sub.mk b/core/arch/arm/plat-synquacer/sub.mk
index 8ddc2fd..013e57d 100644
--- a/core/arch/arm/plat-synquacer/sub.mk
+++ b/core/arch/arm/plat-synquacer/sub.mk
@@ -1,2 +1,4 @@ 
 global-incdirs-y += .
 srcs-y += main.c
+srcs-y += rng_pta.c
+srcs-y += timer_fiq.c
diff --git a/core/arch/arm/plat-synquacer/timer_fiq.c b/core/arch/arm/plat-synquacer/timer_fiq.c
new file mode 100644
index 0000000..9f60df1
--- /dev/null
+++ b/core/arch/arm/plat-synquacer/timer_fiq.c
@@ -0,0 +1,41 @@ 
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2018, Linaro Limited
+ */
+
+#include <arm.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <io.h>
+#include <kernel/panic.h>
+#include <kernel/misc.h>
+#include <rng_pta.h>
+
+void generic_timer_start(void)
+{
+	uint64_t cval;
+	uint32_t ctl = 1;
+
+	/* The timer will fire every 2 ms */
+	cval = read_cntpct() + (read_cntfrq() / 500);
+	write_cntps_cval(cval);
+
+	/* Enable the secure physical timer */
+	write_cntps_ctl(ctl);
+}
+
+void generic_timer_stop(void)
+{
+	/* Disable the timer */
+	write_cntps_ctl(0);
+}
+
+void generic_timer_handler(void)
+{
+	/* Ensure that the timer did assert the interrupt */
+	assert((read_cntps_ctl() >> 2));
+
+	/* Disable the timer and reprogram it */
+	write_cntps_ctl(0);
+	generic_timer_start();
+}
diff --git a/core/include/crypto/crypto.h b/core/include/crypto/crypto.h
index 2018d3c..e0e6474 100644
--- a/core/include/crypto/crypto.h
+++ b/core/include/crypto/crypto.h
@@ -247,6 +247,17 @@  TEE_Result crypto_acipher_ecc_shared_secret(struct ecc_keypair *private_key,
 					    unsigned long *secret_len);
 
 /*
+ * Computes a SHA-256 hash, doesn't require crypto_init() to be called in
+ * advance and has as few dependencies as possible.
+ *
+ * This function is primarily used by hash_sha256_check and could be used
+ * inside interrupt context where the crypto library can't be used due to
+ * mutex handling.
+ */
+TEE_Result hash_sha256_compute(uint8_t *digest, const uint8_t *data,
+		size_t data_size);
+
+/*
  * Verifies a SHA-256 hash, doesn't require crypto_init() to be called in
  * advance and has as few dependencies as possible.
  *
diff --git a/core/lib/libtomcrypt/src/tee_ltc_provider.c b/core/lib/libtomcrypt/src/tee_ltc_provider.c
index 0c35a34..ced14cb 100644
--- a/core/lib/libtomcrypt/src/tee_ltc_provider.c
+++ b/core/lib/libtomcrypt/src/tee_ltc_provider.c
@@ -2807,11 +2807,10 @@  void tomcrypt_arm_neon_disable(struct tomcrypt_arm_neon_state *state)
 #endif
 
 #if defined(CFG_CRYPTO_SHA256)
-TEE_Result hash_sha256_check(const uint8_t *hash, const uint8_t *data,
-		size_t data_size)
+TEE_Result hash_sha256_compute(uint8_t *digest, const uint8_t *data,
+			       size_t data_size)
 {
 	hash_state hs;
-	uint8_t digest[TEE_SHA256_HASH_SIZE];
 
 	if (sha256_init(&hs) != CRYPT_OK)
 		return TEE_ERROR_GENERIC;
@@ -2819,6 +2818,19 @@  TEE_Result hash_sha256_check(const uint8_t *hash, const uint8_t *data,
 		return TEE_ERROR_GENERIC;
 	if (sha256_done(&hs, digest) != CRYPT_OK)
 		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result hash_sha256_check(const uint8_t *hash, const uint8_t *data,
+			     size_t data_size)
+{
+	uint8_t digest[TEE_SHA256_HASH_SIZE];
+	TEE_Result res;
+
+	res = hash_sha256_compute(digest, data, data_size);
+	if (res != TEE_SUCCESS)
+		return res;
 	if (buf_compare_ct(digest, hash, sizeof(digest)) != 0)
 		return TEE_ERROR_SECURITY;
 	return TEE_SUCCESS;