diff --git a/services/std_svc/trng/trng_entropy_pool.c b/services/std_svc/trng/trng_entropy_pool.c
new file mode 100644
index 0000000..ac13b1d
--- /dev/null
+++ b/services/std_svc/trng/trng_entropy_pool.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2021, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <lib/spinlock.h>
+#include <plat/common/plat_trng.h>
+
+/*
+ * # Entropy pool
+ * Note that the TRNG Firmware interface can request up to 192 bits of entropy
+ * in a single call or three 64bit words per call. We have 4 words in the pool
+ * so that when we have 1-63 bits in the pool, and we have a request for
+ * 192 bits of entropy, we don't have to throw out the leftover 1-63 bits of
+ * entropy.
+ */
+#define WORDS_IN_POOL (4)
+static uint64_t entropy[WORDS_IN_POOL];
+/* index in bits of the first bit of usable entropy */
+static uint32_t entropy_bit_index;
+/* then number of valid bits in the entropy pool */
+static uint32_t entropy_bit_size;
+
+static spinlock_t trng_pool_lock;
+
+#define BITS_PER_WORD (sizeof(entropy[0]) * 8)
+#define BITS_IN_POOL (WORDS_IN_POOL * BITS_PER_WORD)
+#define ENTROPY_MIN_WORD (entropy_bit_index / BITS_PER_WORD)
+#define ENTROPY_FREE_BIT (entropy_bit_size + entropy_bit_index)
+#define _ENTROPY_FREE_WORD (ENTROPY_FREE_BIT / BITS_PER_WORD)
+#define ENTROPY_FREE_INDEX (_ENTROPY_FREE_WORD % WORDS_IN_POOL)
+/* ENTROPY_WORD_INDEX(0) includes leftover bits in the lower bits */
+#define ENTROPY_WORD_INDEX(i) ((ENTROPY_MIN_WORD + i) % WORDS_IN_POOL)
+
+/*
+ * Fill the entropy pool until we have at least as many bits as requested.
+ * Returns true after filling the pool, and false if the entropy source is out
+ * of entropy and the pool could not be filled.
+ * Assumes locks are taken.
+ */
+static bool trng_fill_entropy(uint32_t nbits)
+{
+	while (nbits > entropy_bit_size) {
+		bool valid = plat_get_entropy(&entropy[ENTROPY_FREE_INDEX]);
+
+		if (valid) {
+			entropy_bit_size += BITS_PER_WORD;
+			assert(entropy_bit_size <= BITS_IN_POOL);
+		} else {
+			return false;
+		}
+	}
+	return true;
+}
+
+/*
+ * Pack entropy into the out buffer, filling and taking locks as needed.
+ * Returns true on success, false on failure.
+ *
+ * Note: out must have enough space for nbits of entropy
+ */
+bool trng_pack_entropy(uint32_t nbits, uint64_t *out)
+{
+	bool success = true;
+
+	spin_lock(&trng_pool_lock);
+
+	if (!trng_fill_entropy(nbits)) {
+		success = false;
+		goto out;
+	}
+
+	const unsigned int rshift = entropy_bit_index % BITS_PER_WORD;
+	const unsigned int lshift = BITS_PER_WORD - rshift;
+	const int to_fill = ((nbits + BITS_PER_WORD - 1) / BITS_PER_WORD);
+	int word_i;
+
+	for (word_i = 0; word_i < to_fill; word_i++) {
+		/*
+		 * Repack the entropy from the pool into the passed in out
+		 * buffer. This takes the lower bits from the valid upper bits
+		 * of word_i and the upper bits from the lower bits of
+		 * (word_i + 1).
+		 *
+		 * I found the following diagram useful. note: `e` represents
+		 * valid entropy, ` ` represents invalid bits (not entropy) and
+		 * `x` represents valid entropy that must not end up in the
+		 * packed word.
+		 *
+		 *          |---------entropy pool----------|
+		 * C var    |--(word_i + 1)-|----word_i-----|
+		 * bit idx  |7 6 5 4 3 2 1 0|7 6 5 4 3 2 1 0|
+		 *          [x,x,e,e,e,e,e,e|e,e, , , , , , ]
+		 *          |   [e,e,e,e,e,e,e,e]           |
+		 *          |   |--out[word_i]--|           |
+		 *    lshift|---|               |--rshift---|
+		 *
+		 *          ==== Which is implemented as ====
+		 *
+		 *          |---------entropy pool----------|
+		 * C var    |--(word_i + 1)-|----word_i-----|
+		 * bit idx  |7 6 5 4 3 2 1 0|7 6 5 4 3 2 1 0|
+		 *          [x,x,e,e,e,e,e,e|e,e, , , , , , ]
+		 * C expr       << lshift       >> rshift
+		 * bit idx   5 4 3 2 1 0                 7 6
+		 *          [e,e,e,e,e,e,0,0|0,0,0,0,0,0,e,e]
+		 *                ==== bit-wise or ====
+		 *                   5 4 3 2 1 0 7 6
+		 *                  [e,e,e,e,e,e,e,e]
+		 */
+		out[word_i] = 0;
+		out[word_i] |= entropy[ENTROPY_WORD_INDEX(word_i)] >> rshift;
+
+		/*
+		 * Note that a shift of 64 bits is treated as a shift of 0 bits.
+		 * When the shift amount is the same as the BITS_PER_WORD, we
+		 * don't want to include the next word of entropy, so we skip
+		 * the `|=` operation.
+		 */
+		if (lshift != BITS_PER_WORD) {
+			out[word_i] |= entropy[ENTROPY_WORD_INDEX(word_i + 1)]
+				<< lshift;
+		}
+	}
+	const uint64_t mask = ~0ULL >> (BITS_PER_WORD - (nbits % BITS_PER_WORD));
+
+	out[to_fill - 1] &= mask;
+
+	entropy_bit_index = (entropy_bit_index + nbits) % BITS_IN_POOL;
+	entropy_bit_size -= nbits;
+
+out:
+	spin_unlock(&trng_pool_lock);
+
+	return success;
+}
+
+void trng_entropy_pool_setup(void)
+{
+	int i;
+
+	for (i = 0; i < WORDS_IN_POOL; i++) {
+		entropy[i] = 0;
+	}
+	entropy_bit_index = 0;
+	entropy_bit_size = 0;
+}
diff --git a/services/std_svc/trng/trng_entropy_pool.h b/services/std_svc/trng/trng_entropy_pool.h
new file mode 100644
index 0000000..fab2367
--- /dev/null
+++ b/services/std_svc/trng/trng_entropy_pool.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TRNG_ENTROPY_POOL_H
+#define TRNG_ENTROPY_POOL_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+bool trng_pack_entropy(uint32_t nbits, uint64_t *out);
+void trng_entropy_pool_setup(void);
+
+#endif /* TRNG_ENTROPY_POOL_H */
diff --git a/services/std_svc/trng/trng_main.c b/services/std_svc/trng/trng_main.c
new file mode 100644
index 0000000..38aa649
--- /dev/null
+++ b/services/std_svc/trng/trng_main.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <arch_features.h>
+#include <lib/smccc.h>
+#include <services/trng_svc.h>
+#include <smccc_helpers.h>
+
+#include <plat/common/plat_trng.h>
+
+#include "trng_entropy_pool.h"
+
+static const uuid_t uuid_null;
+
+/* handle the RND call in SMC 32 bit mode */
+static uintptr_t trng_rnd32(uint32_t nbits, void *handle)
+{
+	uint32_t mask = ~0U;
+	uint64_t ent[2];
+
+	if (nbits == 0U || nbits > 96U) {
+		SMC_RET1(handle, TRNG_E_INVALID_PARAMS);
+	}
+
+	if (!trng_pack_entropy(nbits, &ent[0])) {
+		SMC_RET1(handle, TRNG_E_NO_ENTROPY);
+	}
+
+	if ((nbits % 32U) != 0U) {
+		mask >>= 32U - (nbits % 32U);
+	}
+
+	switch ((nbits - 1U) / 32U) {
+	case 0:
+		SMC_RET4(handle, TRNG_E_SUCCESS, 0, 0, ent[0] & mask);
+		break; /* unreachable */
+	case 1:
+		SMC_RET4(handle, TRNG_E_SUCCESS, 0, (ent[0] >> 32) & mask,
+			 ent[0] & 0xFFFFFFFF);
+		break; /* unreachable */
+	case 2:
+		SMC_RET4(handle, TRNG_E_SUCCESS, ent[1] & mask,
+			 (ent[0] >> 32) & 0xFFFFFFFF, ent[0] & 0xFFFFFFFF);
+		break; /* unreachable */
+	default:
+		SMC_RET1(handle, TRNG_E_INVALID_PARAMS);
+		break; /* unreachable */
+	}
+}
+
+/* handle the RND call in SMC 64 bit mode */
+static uintptr_t trng_rnd64(uint32_t nbits, void *handle)
+{
+	uint64_t mask = ~0ULL;
+	uint64_t ent[3];
+
+	if (nbits == 0U || nbits > 192U) {
+		SMC_RET1(handle, TRNG_E_INVALID_PARAMS);
+	}
+
+	if (!trng_pack_entropy(nbits, &ent[0])) {
+		SMC_RET1(handle, TRNG_E_NO_ENTROPY);
+	}
+
+	/* Mask off higher bits if only part of register requested */
+	if ((nbits % 64U) != 0U) {
+		mask >>= 64U - (nbits % 64U);
+	}
+
+	switch ((nbits - 1U) / 64U) {
+	case 0:
+		SMC_RET4(handle, TRNG_E_SUCCESS, 0, 0, ent[0] & mask);
+		break; /* unreachable */
+	case 1:
+		SMC_RET4(handle, TRNG_E_SUCCESS, 0, ent[1] & mask, ent[0]);
+		break; /* unreachable */
+	case 2:
+		SMC_RET4(handle, TRNG_E_SUCCESS, ent[2] & mask, ent[1], ent[0]);
+		break; /* unreachable */
+	default:
+		SMC_RET1(handle, TRNG_E_INVALID_PARAMS);
+		break; /* unreachable */
+	}
+}
+
+void trng_setup(void)
+{
+	trng_entropy_pool_setup();
+	plat_entropy_setup();
+}
+
+/* Predicate indicating that a function id is part of TRNG */
+bool is_trng_fid(uint32_t smc_fid)
+{
+	return ((smc_fid == ARM_TRNG_VERSION) ||
+		(smc_fid == ARM_TRNG_FEATURES) ||
+		(smc_fid == ARM_TRNG_GET_UUID) ||
+		(smc_fid == ARM_TRNG_RND32) ||
+		(smc_fid == ARM_TRNG_RND64));
+}
+
+uintptr_t trng_smc_handler(uint32_t smc_fid, u_register_t x1, u_register_t x2,
+			   u_register_t x3, u_register_t x4, void *cookie,
+			   void *handle, u_register_t flags)
+{
+	if (!memcmp(&plat_trng_uuid, &uuid_null, sizeof(uuid_t))) {
+		SMC_RET1(handle, TRNG_E_NOT_IMPLEMENTED);
+	}
+
+	switch (smc_fid) {
+	case ARM_TRNG_VERSION:
+		SMC_RET1(handle, MAKE_SMCCC_VERSION(
+			TRNG_VERSION_MAJOR, TRNG_VERSION_MINOR
+		));
+		break; /* unreachable */
+	case ARM_TRNG_FEATURES:
+		if (is_trng_fid((uint32_t)x1)) {
+			SMC_RET1(handle, TRNG_E_SUCCESS);
+		} else {
+			SMC_RET1(handle, TRNG_E_NOT_SUPPORTED);
+		}
+		break; /* unreachable */
+	case ARM_TRNG_GET_UUID:
+		SMC_UUID_RET(handle, plat_trng_uuid);
+		break; /* unreachable */
+	case ARM_TRNG_RND32:
+		return trng_rnd32((uint32_t)x1, handle);
+	case ARM_TRNG_RND64:
+		return trng_rnd64((uint32_t)x1, handle);
+	default:
+		WARN("Unimplemented TRNG Service Call: 0x%x\n",
+			smc_fid);
+		SMC_RET1(handle, TRNG_E_NOT_IMPLEMENTED);
+		break; /* unreachable */
+	}
+}