diff --git a/arm/Makefile.arm64 b/arm/Makefile.arm64
index e176974..18a8f30 100644
--- a/arm/Makefile.arm64
+++ b/arm/Makefile.arm64
@@ -20,8 +20,10 @@
 cflatobjs += lib/arm64/processor.o
 cflatobjs += lib/arm64/spinlock.o
 cflatobjs += lib/arm64/gic-v3-its.o lib/arm64/gic-v3-its-cmd.o
+cflatobjs += lib/arm64/ffa_utils.o
 cflatobjs += lib/arm64/ffa.o
 cflatobjs += lib/arm64/call.o
+cflatobjs += lib/arm64/ffa_mailbox.o
 
 
 OBJDIRS += lib/arm64
@@ -31,6 +33,7 @@
 tests += $(TEST_DIR)/micro-bench.flat
 tests += $(TEST_DIR)/cache.flat
 tests += $(TEST_DIR)/ffa.flat
+tests += $(TEST_DIR)/ffa_mailbox.flat
 tests += $(TEST_DIR)/hello.flat
 tests += $(TEST_DIR)/null.flat
 
diff --git a/arm/ffa.c b/arm/ffa.c
index c03768b..30c92e6 100644
--- a/arm/ffa.c
+++ b/arm/ffa.c
@@ -3,16 +3,7 @@
  * TODO: Figure out the right way of specifying the license header.
  */
 
-#include <libcflat.h>
-
-#include "arm64/call.h"
-#include "arm64/ffa.h"
-
-/* SMCCC reserves ID 0 for the hypervisor itself. */
-const int HYPERVISOR_ID = 0;
-
-/* For now, assume the primary VM has an ID of 1, same as hafnium */
-const ffa_vm_id_t PRIMARY_VM_ID = 1;
+#include "arm64/ffa_utils.h"
 
 static void test_ffa_primary_basics(void)
 {
diff --git a/arm/ffa_mailbox.c b/arm/ffa_mailbox.c
new file mode 100644
index 0000000..82158f0
--- /dev/null
+++ b/arm/ffa_mailbox.c
@@ -0,0 +1,346 @@
+/*
+ * Adapted from Hafnium.
+ * TODO: Figure out the right way of specifying the license header.
+ */
+
+#include "arm64/ffa_utils.h"
+
+
+static struct mailbox_buffers mb;
+
+#ifndef be16toh
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+
+#define be16toh(v) __builtin_bswap16(v)
+#define be32toh(v) __builtin_bswap32(v)
+#define be64toh(v) __builtin_bswap64(v)
+
+#define htobe16(v) __builtin_bswap16(v)
+#define htobe32(v) __builtin_bswap32(v)
+#define htobe64(v) __builtin_bswap64(v)
+
+#define le16toh(v) (v)
+#define le32toh(v) (v)
+#define le64toh(v) (v)
+
+#define htole16(v) (v)
+#define htole32(v) (v)
+#define htole64(v) (v)
+
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+
+#define be16toh(v) (v)
+#define be32toh(v) (v)
+#define be64toh(v) (v)
+
+#define htobe16(v) (v)
+#define htobe32(v) (v)
+#define htobe64(v) (v)
+
+#define le16toh(v) __builtin_bswap16(v)
+#define le32toh(v) __builtin_bswap32(v)
+#define le64toh(v) __builtin_bswap64(v)
+
+#define htole16(v) __builtin_bswap16(v)
+#define htole32(v) __builtin_bswap32(v)
+#define htole64(v) __builtin_bswap64(v)
+
+#else
+
+/*
+ * __BYTE_ORDER__ != __ORDER_LITTLE_ENDIAN__ &&
+ * __BYTE_ORDER__ != __ORDER_BIG_ENDIAN__
+ */
+
+#error "Unsupported byte order"
+
+#endif
+#endif
+
+
+/**
+ * Reverses the order of the elements in the given array.
+ */
+static void reverse(char *s, size_t len)
+{
+	size_t i;
+
+	for (i = 0; i < len / 2; i++) {
+		char t = s[i];
+		s[i] = s[len - 1 - i];
+		s[len - 1 - i] = t;
+	}
+}
+
+/**
+ * Finds the next lexicographic permutation of the given array, if there is one.
+ */
+static void next_permutation(char *s, size_t len)
+{
+	size_t i;
+	size_t j;
+
+	for (i = len - 2; i < len; i--) {
+		const char t = s[i];
+		if (t >= s[i + 1]) {
+			continue;
+		}
+
+		for (j = len - 1; t >= s[j]; j--) {
+		}
+
+		s[i] = s[j];
+		s[j] = t;
+		reverse(s + i + 1, len - i - 1);
+		return;
+	}
+}
+
+static void service_echo(int count)
+{
+        report_info("%s: %d: VM id %d is running!", __func__, __LINE__, get_vm_id());
+
+	/* Loop, echo messages back to the sender. */
+	while (count--) {
+		struct ffa_value ret = ffa_msg_wait();
+		ffa_vm_id_t target_vm_id = ffa_msg_send_receiver(ret);
+		ffa_vm_id_t source_vm_id = ffa_msg_send_sender(ret);
+		void *send_buf = mb.send;
+		void *recv_buf = mb.recv;
+
+		ASSERT_EQ(ret.func, FFA_MSG_SEND_32);
+		memcpy_s(send_buf, FFA_MSG_PAYLOAD_MAX, recv_buf,
+			 ffa_msg_send_size(ret));
+
+		EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+		ffa_msg_send(target_vm_id, source_vm_id, ffa_msg_send_size(ret),
+			     0);
+	}
+
+        report_info("%s: %d: VM id %d is done!", __func__, __LINE__, get_vm_id());
+}
+
+static void service_relay(int count)
+{
+        report_info("%s: %d: VM id %d is running!", __func__, __LINE__, get_vm_id());
+
+	/*
+	 * Loop, forward messages to the next VM.
+	 *
+	 * The first 32-bits of the message are the little-endian 32-bit ID of
+	 * the VM to forward the message to. This ID will be dropped from the
+	 * message so multiple IDs can be places at the start of the message.
+	 */
+	while (count--) {
+		ffa_vm_id_t *chain;
+		ffa_vm_id_t next_vm_id;
+		void *next_message;
+		uint32_t next_message_size;
+
+		/* Receive the message to relay. */
+		struct ffa_value ret = ffa_msg_wait();
+		ASSERT_EQ(ret.func, FFA_MSG_SEND_32);
+
+		/* Prepare to relay the message. */
+		void *send_buf = mb.send;
+		void *recv_buf = mb.recv;
+		ASSERT_GE(ffa_msg_send_size(ret), sizeof(ffa_vm_id_t));
+
+		chain = (ffa_vm_id_t *)recv_buf;
+		next_vm_id = le16toh(*chain);
+		next_message = chain + 1;
+		next_message_size =
+			ffa_msg_send_size(ret) - sizeof(ffa_vm_id_t);
+
+		/* Send the message to the next stage. */
+		memcpy_s(send_buf, FFA_MSG_PAYLOAD_MAX, next_message,
+			 next_message_size);
+
+		EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+		ffa_msg_send(get_vm_id(), next_vm_id, next_message_size, 0);
+	}
+
+	report_info("%s: %d: VM id %d is done!", __func__, __LINE__, get_vm_id());
+}
+
+static void service_clear_empty(void)
+{
+        report_info("%s: %d: VM id %d is running!", __func__, __LINE__, get_vm_id());
+        report_info("%s: %d: VM id %d is done!", __func__, __LINE__, get_vm_id());
+}
+
+static void echo(void)
+{
+	const char message[] = "Echo this back to me!";
+	struct ffa_value run_res;
+
+	EXPECT(is_primary_vm());
+
+	run_res = ffa_run(SERVICE_VM1, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_WAIT_32);
+	EXPECT_EQ(run_res.arg2, FFA_SLEEP_INDEFINITE);
+
+	/* Set the message, echo it and check it didn't change. */
+	memcpy_s(mb.send, FFA_MSG_PAYLOAD_MAX, message, sizeof(message));
+	EXPECT_EQ(
+		ffa_msg_send(get_vm_id(), SERVICE_VM1, sizeof(message), 0)
+			.func,
+		FFA_SUCCESS_32);
+	run_res = ffa_run(SERVICE_VM1, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_SEND_32);
+	EXPECT_EQ(ffa_msg_send_size(run_res), sizeof(message));
+	EXPECT_EQ(memcmp(mb.recv, message, sizeof(message)), 0);
+	EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+
+	report(true, "%s", __func__);
+}
+
+static void repeated_echo(int count)
+{
+	char message[] = "Echo this back to me!";
+	struct ffa_value run_res;
+
+	while (count--) {
+		/* Run secondary until it reaches the wait for messages. */
+		run_res = ffa_run(SERVICE_VM1, 0);
+		EXPECT_EQ(run_res.func, FFA_MSG_WAIT_32);
+		EXPECT_EQ(run_res.arg2, FFA_SLEEP_INDEFINITE);
+
+		/* Set the message, echo it and check it didn't change. */
+		next_permutation(message, sizeof(message) - 1);
+		memcpy_s(mb.send, FFA_MSG_PAYLOAD_MAX, message,
+			 sizeof(message));
+		EXPECT_EQ(ffa_msg_send(get_vm_id(), SERVICE_VM1,
+				       sizeof(message), 0)
+				  .func,
+			  FFA_SUCCESS_32);
+		run_res = ffa_run(SERVICE_VM1, 0);
+		EXPECT_EQ(run_res.func, FFA_MSG_SEND_32);
+		EXPECT_EQ(ffa_msg_send_size(run_res), sizeof(message));
+		EXPECT_EQ(memcmp(mb.recv, message, sizeof(message)), 0);
+		EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+	}
+
+	report(true, "%s", __func__);
+}
+
+/**
+ * Send a message to relay_a which will forward it to relay_b where it will be
+ * sent back here.
+ */
+static void relay(void)
+{
+	const char message[] = "Send this round the relay!";
+	struct ffa_value run_res;
+
+	run_res = ffa_run(SERVICE_VM1, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_WAIT_32);
+	EXPECT_EQ(run_res.arg2, FFA_SLEEP_INDEFINITE);
+	run_res = ffa_run(SERVICE_VM2, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_WAIT_32);
+	EXPECT_EQ(run_res.arg2, FFA_SLEEP_INDEFINITE);
+
+	/*
+	 * Build the message chain so the message is sent from here to
+	 * SERVICE_VM1, then to SERVICE_VM2 and finally back to here.
+	 */
+	{
+		ffa_vm_id_t *chain = (ffa_vm_id_t *)mb.send;
+		*chain++ = htole32(SERVICE_VM2);
+		*chain++ = htole32(get_vm_id());
+		memcpy_s(chain, FFA_MSG_PAYLOAD_MAX - (2 * sizeof(ffa_vm_id_t)),
+			 message, sizeof(message));
+
+		EXPECT_EQ(
+			ffa_msg_send(
+				get_vm_id(), SERVICE_VM1,
+				sizeof(message) + (2 * sizeof(ffa_vm_id_t)), 0)
+				.func,
+			FFA_SUCCESS_32);
+	}
+
+	/* Let SERVICE_VM1 forward the message. */
+	run_res = ffa_run(SERVICE_VM1, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_SEND_32);
+	EXPECT_EQ(ffa_msg_send_receiver(run_res), SERVICE_VM2);
+	EXPECT_EQ(ffa_msg_send_size(run_res), 0);
+
+	/* Let SERVICE_VM2 forward the message. */
+	run_res = ffa_run(SERVICE_VM2, 0);
+	EXPECT_EQ(run_res.func, FFA_MSG_SEND_32);
+
+	/* Ensure the message is intact. */
+	EXPECT_EQ(ffa_msg_send_receiver(run_res), get_vm_id());
+	EXPECT_EQ(ffa_msg_send_size(run_res), sizeof(message));
+	EXPECT_EQ(memcmp(mb.recv, message, sizeof(message)), 0);
+	EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+
+	report(true, "%s", __func__);
+}
+
+
+/**
+ * Clearing an empty mailbox is an error.
+ */
+static void clear_empty(void)
+{
+	EXPECT_FFA_ERROR(ffa_rx_release(), FFA_DENIED);
+	EXPECT_FFA_ERROR(ffa_rx_release(), FFA_DENIED);
+	EXPECT_FFA_ERROR(ffa_rx_release(), FFA_DENIED);
+	report(true, "%s", __func__);
+}
+
+static void test_echo(void)
+{
+	if (is_primary_vm()) {
+		echo();
+	} else {
+		service_echo(1);
+	}
+}
+
+static void test_repeated_echo(void)
+{
+	int count = 100;
+
+	if (is_primary_vm()) {
+		repeated_echo(count);
+	} else {
+		service_echo(count);
+	}
+}
+
+static void test_relay(void)
+{
+	if (is_primary_vm()) {
+		relay();
+	} else {
+		service_relay(1);
+	}
+}
+
+static void test_clear_empty(void)
+{
+	if (is_primary_vm()) {
+		clear_empty();
+	} else {
+		service_clear_empty();
+	}
+}
+
+int main(void)
+{
+	mb = set_up_mailbox();
+
+	if (0) {
+	test_echo();
+	test_repeated_echo();
+	test_clear_empty();}
+	test_relay();
+
+	if (is_primary_vm()) {
+		return report_summary();
+	} else {
+		return 0;
+	}
+}
diff --git a/arm/unittests.cfg b/arm/unittests.cfg
index 5919cfc..61ed63b 100644
--- a/arm/unittests.cfg
+++ b/arm/unittests.cfg
@@ -248,6 +248,10 @@
 arch = arm64
 groups = nodefault,ffa
 
+[ffa_mailbox]
+file = ffa_mailbox.flat
+arch = arm64
+groups = nodefault,ffa
 
 # Hello world
 [hello]
diff --git a/lib/arm/asm/setup.h b/lib/arm/asm/setup.h
index edf4e86..c8afb24 100644
--- a/lib/arm/asm/setup.h
+++ b/lib/arm/asm/setup.h
@@ -12,7 +12,6 @@
 #define NR_CPUS			511
 extern u64 cpus[NR_CPUS];	/* per-cpu IDs (MPIDRs) */
 extern int nr_cpus;
-extern bool is_secondary_vm;	/* True if running as an FFA secondary VM */
 
 #define MR_F_PRIMARY		(1U << 0)
 #define MR_F_IO			(1U << 1)
diff --git a/lib/arm/io.c b/lib/arm/io.c
index fac3f74..04acb43 100644
--- a/lib/arm/io.c
+++ b/lib/arm/io.c
@@ -18,7 +18,7 @@
 #include <asm/io.h>
 
 #include "io.h"
-#include "arm64/call.h"
+#include "arm64/ffa_utils.h"
 
 /**
  * Sends a character to the debug log for the VM.
@@ -102,7 +102,7 @@
 	spin_lock(&uart_lock);
 	while (*s)
 	{
-		if (!is_secondary_vm)
+		if (is_primary_vm())
 			writeb(*s++, uart0_base);
 		else
 			hf_debug_log(*s++);
@@ -150,7 +150,7 @@
 {
 	chr_testdev_exit(code);
 
-	if (!is_secondary_vm)
+	if (is_primary_vm())
 		psci_system_off();
 	else
 		ffa_yield();
diff --git a/lib/arm/setup.c b/lib/arm/setup.c
index ee9f8d0..6adcae8 100644
--- a/lib/arm/setup.c
+++ b/lib/arm/setup.c
@@ -23,6 +23,7 @@
 #include <asm/processor.h>
 #include <asm/smp.h>
 
+#include "arm64/ffa_utils.h"
 #include "io.h"
 
 #define NR_INITIAL_MEM_REGIONS 16
@@ -34,7 +35,6 @@
 
 u64 cpus[NR_CPUS] = { [0 ... NR_CPUS-1] = (u64)~0 };
 int nr_cpus;
-bool is_secondary_vm;
 
 static struct mem_region __initial_mem_regions[NR_INITIAL_MEM_REGIONS + 1];
 struct mem_region *mem_regions = __initial_mem_regions;
@@ -89,13 +89,31 @@
 	return MR_F_UNKNOWN;
 }
 
+static phys_addr_t get_highest_reserved_addr(const void *fdt)
+{
+        int n;
+        u64 base, size;
+	phys_addr_t highest_reserved = 0;
+
+        /* Process header /memreserve/ fields */
+        for (n = 0; ; n++) {
+                fdt_get_mem_rsv(fdt, n, &base, &size);
+		if (!size)
+			break;
+		if (base > highest_reserved)
+			highest_reserved = base;
+        }
+
+	return highest_reserved;
+}
+
 static void mem_init(phys_addr_t freemem_start)
 {
 	struct dt_pbus_reg regs[NR_INITIAL_MEM_REGIONS];
 	struct mem_region primary, mem = {
 		.start = (phys_addr_t)-1,
 	};
-	phys_addr_t base, top;
+	phys_addr_t base, top, highest_reserved;
 	int nr_regs, nr_io = 0, i;
 
 	/*
@@ -141,6 +159,16 @@
 	assert(primary.end != 0);
 	assert(!(mem.start & ~PHYS_MASK) && !((mem.end - 1) & ~PHYS_MASK));
 
+	if (is_primary_vm()) {
+		highest_reserved = get_highest_reserved_addr(dt_fdt());
+
+		if (highest_reserved < mem.end)
+			mem.end = highest_reserved;
+
+		if (highest_reserved < primary.end)
+			primary.end = highest_reserved;
+	}
+
 	__phys_offset = primary.start;	/* PHYS_OFFSET */
 	__phys_end = primary.end;	/* PHYS_END */
 
@@ -205,16 +233,13 @@
 	mem_init(PAGE_ALIGN((unsigned long)freemem));
 	cpu_init();
 
-	/* Check whether this is running as a secondary VM. */
-	is_secondary_vm = (fdt_node_offset_by_compatible(fdt, -1,
-		"linux,dummy-virt") >= 0);
 
 
 	/* cpu_init must be called before thread_info_init */
 	thread_info_init(current_thread_info(), 0);
 
 	/* mem_init must be called before io_init */
-	if (!is_secondary_vm)
+	if (is_primary_vm())
 		io_init();
 
 	/* finish setup */
diff --git a/lib/arm64/call.c b/lib/arm64/call.c
index e6a5cf3..2274d00 100644
--- a/lib/arm64/call.c
+++ b/lib/arm64/call.c
@@ -64,4 +64,5 @@
 		"x4", "x5", "x6", "x7");
 
 	return r0;
-}
\ No newline at end of file
+}
+
diff --git a/lib/arm64/call.h b/lib/arm64/call.h
index e1a67f4..d5cbd80 100644
--- a/lib/arm64/call.h
+++ b/lib/arm64/call.h
@@ -257,3 +257,4 @@
 	return ffa_call((struct ffa_value){.func = FFA_FEATURES_32,
 					   .arg1 = function_id});
 }
+
diff --git a/lib/arm64/ffa_mailbox.c b/lib/arm64/ffa_mailbox.c
new file mode 100644
index 0000000..d5322a3
--- /dev/null
+++ b/lib/arm64/ffa_mailbox.c
@@ -0,0 +1,324 @@
+/*
+ * Copyright 2018 The Hafnium Authors.
+ *
+ * Use of this source code is governed by a BSD-style
+ * license that can be found in the LICENSE file or at
+ * https://opensource.org/licenses/BSD-3-Clause.
+ */
+
+#include "ffa_mailbox.h"
+
+#include "call.h"
+
+#include "ffa_utils.h"
+
+static uint8_t send_page[FFA_PAGE_SIZE] __attribute__((aligned(FFA_PAGE_SIZE)));
+static uint8_t recv_page[FFA_PAGE_SIZE] __attribute__((aligned(FFA_PAGE_SIZE)));
+// static_assert(sizeof(send_page) == FFA_PAGE_SIZE, "Send page is not a page.");
+// static_assert(sizeof(recv_page) == FFA_PAGE_SIZE, "Recv page is not a page.");
+
+static uint64_t send_page_addr = (uint64_t)send_page;
+static uint64_t recv_page_addr = (uint64_t)recv_page;
+
+struct mailbox_buffers set_up_mailbox(void)
+{
+	ASSERT_EQ(ffa_rxtx_map(send_page_addr, recv_page_addr).func,
+		  FFA_SUCCESS_32);
+	return (struct mailbox_buffers){
+		.send = send_page,
+		.recv = recv_page,
+	};
+}
+
+/*
+ * Helper function to send memory to a VM then send a message with the retrieve
+ * request it needs to retrieve it.
+ */
+ffa_memory_handle_t send_memory_and_retrieve_request(
+	uint32_t share_func, void *tx_buffer, ffa_vm_id_t sender,
+	ffa_vm_id_t recipient,
+	struct ffa_memory_region_constituent constituents[],
+	uint32_t constituent_count, ffa_memory_region_flags_t flags,
+	enum ffa_data_access send_data_access,
+	enum ffa_data_access retrieve_data_access,
+	enum ffa_instruction_access send_instruction_access,
+	enum ffa_instruction_access retrieve_instruction_access)
+{
+	uint32_t total_length;
+	uint32_t fragment_length;
+	uint32_t msg_size;
+	struct ffa_value ret;
+	const ffa_memory_handle_t INVALID_FRAGMENT_HANDLE = 0xffffffffffffffff;
+	ffa_memory_handle_t fragment_handle = INVALID_FRAGMENT_HANDLE;
+	ffa_memory_handle_t handle;
+	uint32_t remaining_constituent_count;
+	uint32_t sent_length;
+
+	/* Send the first fragment of the memory. */
+	remaining_constituent_count = ffa_memory_region_init(
+		tx_buffer, FFA_MAILBOX_SIZE, sender, recipient, constituents,
+		constituent_count, 0, flags, send_data_access,
+		send_instruction_access, FFA_MEMORY_NORMAL_MEM,
+		FFA_MEMORY_CACHE_WRITE_BACK, FFA_MEMORY_OUTER_SHAREABLE,
+		&total_length, &fragment_length);
+	if (remaining_constituent_count == 0) {
+		EXPECT_EQ(total_length, fragment_length);
+	}
+	switch (share_func) {
+	case FFA_MEM_DONATE_32:
+		ret = ffa_mem_donate(total_length, fragment_length);
+		break;
+	case FFA_MEM_LEND_32:
+		ret = ffa_mem_lend(total_length, fragment_length);
+		break;
+	case FFA_MEM_SHARE_32:
+		ret = ffa_mem_share(total_length, fragment_length);
+		break;
+	default:
+		//FAIL("Invalid share_func %#x.\n", share_func);
+		/* Never reached, but needed to keep clang-analyser happy. */
+		return 0;
+	}
+	sent_length = fragment_length;
+
+	/* Send the remaining fragments. */
+	while (remaining_constituent_count != 0) {
+		//dlog_verbose("%d constituents left to send.\n",
+		//	     remaining_constituent_count);
+		EXPECT_EQ(ret.func, FFA_MEM_FRAG_RX_32);
+		if (fragment_handle == INVALID_FRAGMENT_HANDLE) {
+			fragment_handle = ffa_frag_handle(ret);
+		} else {
+			EXPECT_EQ(ffa_frag_handle(ret), fragment_handle);
+		}
+		EXPECT_EQ(ret.arg3, sent_length);
+		/* Sender MBZ at virtual instance. */
+		EXPECT_EQ(ffa_frag_sender(ret), 0);
+
+		remaining_constituent_count = ffa_memory_fragment_init(
+			tx_buffer, FFA_MAILBOX_SIZE,
+			constituents + constituent_count -
+				remaining_constituent_count,
+			remaining_constituent_count, &fragment_length);
+
+		ret = ffa_mem_frag_tx(fragment_handle, fragment_length);
+		sent_length += fragment_length;
+	}
+
+	EXPECT_EQ(sent_length, total_length);
+	EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+	handle = ffa_mem_success_handle(ret);
+	EXPECT_EQ(handle & FFA_MEMORY_HANDLE_ALLOCATOR_MASK,
+		  FFA_MEMORY_HANDLE_ALLOCATOR_HYPERVISOR);
+	if (fragment_handle != INVALID_FRAGMENT_HANDLE) {
+		EXPECT_EQ(handle, fragment_handle);
+	}
+
+	/*
+	 * Send the appropriate retrieve request to the VM so that it can use it
+	 * to retrieve the memory.
+	 */
+	msg_size = ffa_memory_retrieve_request_init(
+		tx_buffer, handle, sender, recipient, 0, 0,
+		retrieve_data_access, retrieve_instruction_access,
+		FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+		FFA_MEMORY_OUTER_SHAREABLE);
+	EXPECT_LE(msg_size, FFA_MAILBOX_SIZE);
+	EXPECT_EQ(ffa_msg_send(sender, recipient, msg_size, 0).func,
+		  FFA_SUCCESS_32);
+
+	return handle;
+}
+
+/*
+ * Helper function to send memory to a VM then send a message with the retrieve
+ * request it needs to retrieve it, forcing the request to be made in at least
+ * two fragments even if it could fit in one.
+ */
+ffa_memory_handle_t send_memory_and_retrieve_request_force_fragmented(
+	uint32_t share_func, void *tx_buffer, ffa_vm_id_t sender,
+	ffa_vm_id_t recipient,
+	struct ffa_memory_region_constituent constituents[],
+	uint32_t constituent_count, ffa_memory_region_flags_t flags,
+	enum ffa_data_access send_data_access,
+	enum ffa_data_access retrieve_data_access,
+	enum ffa_instruction_access send_instruction_access,
+	enum ffa_instruction_access retrieve_instruction_access)
+{
+	uint32_t total_length;
+	uint32_t fragment_length;
+	uint32_t msg_size;
+	uint32_t remaining_constituent_count;
+	struct ffa_value ret;
+	ffa_memory_handle_t handle;
+
+	/* Send everything except the last constituent in the first fragment. */
+	remaining_constituent_count = ffa_memory_region_init(
+		tx_buffer, FFA_MAILBOX_SIZE, sender, recipient, constituents,
+		constituent_count, 0, flags, send_data_access,
+		send_instruction_access, FFA_MEMORY_NORMAL_MEM,
+		FFA_MEMORY_CACHE_WRITE_BACK, FFA_MEMORY_OUTER_SHAREABLE,
+		&total_length, &fragment_length);
+	EXPECT_EQ(remaining_constituent_count, 0);
+	EXPECT_EQ(total_length, fragment_length);
+	/* Don't include the last constituent in the first fragment. */
+	fragment_length -= sizeof(struct ffa_memory_region_constituent);
+	switch (share_func) {
+	case FFA_MEM_DONATE_32:
+		ret = ffa_mem_donate(total_length, fragment_length);
+		break;
+	case FFA_MEM_LEND_32:
+		ret = ffa_mem_lend(total_length, fragment_length);
+		break;
+	case FFA_MEM_SHARE_32:
+		ret = ffa_mem_share(total_length, fragment_length);
+		break;
+	default:
+		//FAIL("Invalid share_func %#x.\n", share_func);
+		/* Never reached, but needed to keep clang-analyser happy. */
+		return 0;
+	}
+	EXPECT_EQ(ret.func, FFA_MEM_FRAG_RX_32);
+	EXPECT_EQ(ret.arg3, fragment_length);
+	/* Sender MBZ at virtual instance. */
+	EXPECT_EQ(ffa_frag_sender(ret), 0);
+
+	handle = ffa_frag_handle(ret);
+
+	/* Send the last constituent in a separate fragment. */
+	remaining_constituent_count = ffa_memory_fragment_init(
+		tx_buffer, FFA_MAILBOX_SIZE,
+		&constituents[constituent_count - 1], 1, &fragment_length);
+	EXPECT_EQ(remaining_constituent_count, 0);
+	ret = ffa_mem_frag_tx(handle, fragment_length);
+	EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+	EXPECT_EQ(ffa_mem_success_handle(ret), handle);
+
+	/*
+	 * Send the appropriate retrieve request to the VM so that it can use it
+	 * to retrieve the memory.
+	 */
+	msg_size = ffa_memory_retrieve_request_init(
+		tx_buffer, handle, sender, recipient, 0, 0,
+		retrieve_data_access, retrieve_instruction_access,
+		FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+		FFA_MEMORY_OUTER_SHAREABLE);
+	EXPECT_LE(msg_size, FFA_MAILBOX_SIZE);
+	EXPECT_EQ(ffa_msg_send(sender, recipient, msg_size, 0).func,
+		  FFA_SUCCESS_32);
+
+	return handle;
+}
+
+/*
+ * Use the retrieve request from the receive buffer to retrieve a memory region
+ * which has been sent to us. Copies all the fragments into the provided buffer
+ * if any, and checks that the total length of all fragments is no more than
+ * `memory_region_max_size`. Returns the sender, and the handle via a return
+ * parameter.
+ */
+ffa_vm_id_t retrieve_memory_from_message(
+	void *recv_buf, void *send_buf, struct ffa_value msg_ret,
+	ffa_memory_handle_t *handle,
+	struct ffa_memory_region *memory_region_ret,
+	size_t memory_region_max_size)
+{
+	uint32_t msg_size;
+	struct ffa_value ret;
+	struct ffa_memory_region *memory_region;
+	ffa_vm_id_t sender;
+	struct ffa_memory_region *retrieve_request;
+	ffa_memory_handle_t handle_;
+	uint32_t fragment_length;
+	uint32_t total_length;
+	uint32_t fragment_offset;
+
+	EXPECT_EQ(msg_ret.func, FFA_MSG_SEND_32);
+	msg_size = ffa_msg_send_size(msg_ret);
+	sender = ffa_msg_send_sender(msg_ret);
+
+	retrieve_request = (struct ffa_memory_region *)recv_buf;
+	handle_ = retrieve_request->handle;
+	if (handle != NULL) {
+		*handle = handle_;
+	}
+	memcpy_s(send_buf, FFA_MAILBOX_SIZE, recv_buf, msg_size);
+	ffa_rx_release();
+	ret = ffa_mem_retrieve_req(msg_size, msg_size);
+	EXPECT_EQ(ret.func, FFA_MEM_RETRIEVE_RESP_32);
+	total_length = ret.arg1;
+	fragment_length = ret.arg2;
+	EXPECT_GE(fragment_length,
+		  sizeof(struct ffa_memory_region) +
+			  sizeof(struct ffa_memory_access) +
+			  sizeof(struct ffa_composite_memory_region));
+	EXPECT_LE(fragment_length, FFA_MAILBOX_SIZE);
+	EXPECT_LE(fragment_length, total_length);
+	memory_region = (struct ffa_memory_region *)recv_buf;
+	EXPECT_EQ(memory_region->receiver_count, 1);
+	EXPECT_EQ(memory_region->receivers[0].receiver_permissions.receiver,
+		  ffa_id_get().arg2);
+
+	/* Copy into the return buffer. */
+	if (memory_region_ret != NULL) {
+		memcpy_s(memory_region_ret, memory_region_max_size,
+			 memory_region, fragment_length);
+	}
+
+	/*
+	 * Release the RX buffer now that we have read everything we need from
+	 * it.
+	 */
+	memory_region = NULL;
+	EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+
+	/* Retrieve the remaining fragments. */
+	fragment_offset = fragment_length;
+	while (fragment_offset < total_length) {
+		ret = ffa_mem_frag_rx(handle_, fragment_offset);
+		EXPECT_EQ(ret.func, FFA_MEM_FRAG_TX_32);
+		EXPECT_EQ(ffa_frag_handle(ret), handle_);
+		/* Sender MBZ at virtual instance. */
+		EXPECT_EQ(ffa_frag_sender(ret), 0);
+		fragment_length = ret.arg3;
+		EXPECT_GT(fragment_length, 0);
+		ASSERT_LE(fragment_offset + fragment_length,
+			  memory_region_max_size);
+		if (memory_region_ret != NULL) {
+			memcpy_s((uint8_t *)memory_region_ret + fragment_offset,
+				 memory_region_max_size - fragment_offset,
+				 recv_buf, fragment_length);
+		}
+		fragment_offset += fragment_length;
+		EXPECT_EQ(ffa_rx_release().func, FFA_SUCCESS_32);
+	}
+	EXPECT_EQ(fragment_offset, total_length);
+
+	return sender;
+}
+
+/*
+ * Use the retrieve request from the receive buffer to retrieve a memory region
+ * which has been sent to us, expecting it to fail with the given error code.
+ * Returns the sender.
+ */
+ffa_vm_id_t retrieve_memory_from_message_expect_fail(void *recv_buf,
+						     void *send_buf,
+						     struct ffa_value msg_ret,
+						     int32_t expected_error)
+{
+	uint32_t msg_size;
+	struct ffa_value ret;
+	ffa_vm_id_t sender;
+
+	EXPECT_EQ(msg_ret.func, FFA_MSG_SEND_32);
+	msg_size = ffa_msg_send_size(msg_ret);
+	sender = ffa_msg_send_sender(msg_ret);
+
+	memcpy_s(send_buf, FFA_MAILBOX_SIZE, recv_buf, msg_size);
+	ffa_rx_release();
+	ret = ffa_mem_retrieve_req(msg_size, msg_size);
+	EXPECT_FFA_ERROR(ret, expected_error);
+
+	return sender;
+}
diff --git a/lib/arm64/ffa_mailbox.h b/lib/arm64/ffa_mailbox.h
new file mode 100644
index 0000000..8261aed
--- /dev/null
+++ b/lib/arm64/ffa_mailbox.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2018 The Hafnium Authors.
+ *
+ * Use of this source code is governed by a BSD-style
+ * license that can be found in the LICENSE file or at
+ * https://opensource.org/licenses/BSD-3-Clause.
+ */
+
+#pragma once
+
+#include "ffa.h"
+#include "call.h"
+
+
+struct mailbox_buffers {
+	void *send;
+	void *recv;
+};
+
+struct mailbox_buffers set_up_mailbox(void);
+ffa_memory_handle_t send_memory_and_retrieve_request(
+	uint32_t share_func, void *tx_buffer, ffa_vm_id_t sender,
+	ffa_vm_id_t recipient,
+	struct ffa_memory_region_constituent constituents[],
+	uint32_t constituent_count, ffa_memory_region_flags_t flags,
+	enum ffa_data_access send_data_access,
+	enum ffa_data_access retrieve_data_access,
+	enum ffa_instruction_access send_instruction_access,
+	enum ffa_instruction_access retrieve_instruction_access);
+ffa_memory_handle_t send_memory_and_retrieve_request_force_fragmented(
+	uint32_t share_func, void *tx_buffer, ffa_vm_id_t sender,
+	ffa_vm_id_t recipient,
+	struct ffa_memory_region_constituent constituents[],
+	uint32_t constituent_count, ffa_memory_region_flags_t flags,
+	enum ffa_data_access send_data_access,
+	enum ffa_data_access retrieve_data_access,
+	enum ffa_instruction_access send_instruction_access,
+	enum ffa_instruction_access retrieve_instruction_access);
+ffa_vm_id_t retrieve_memory_from_message(
+	void *recv_buf, void *send_buf, struct ffa_value msg_ret,
+	ffa_memory_handle_t *handle,
+	struct ffa_memory_region *memory_region_ret,
+	size_t memory_region_max_size);
+ffa_vm_id_t retrieve_memory_from_message_expect_fail(void *recv_buf,
+						     void *send_buf,
+						     struct ffa_value msg_ret,
+						     int32_t expected_error);
diff --git a/lib/arm64/ffa_utils.c b/lib/arm64/ffa_utils.c
new file mode 100644
index 0000000..5ad75d5
--- /dev/null
+++ b/lib/arm64/ffa_utils.c
@@ -0,0 +1,16 @@
+#include "ffa_utils.h"
+
+ffa_vm_id_t get_vm_id(void)
+{
+        struct ffa_value ret;
+
+        ret = ffa_id_get();
+        ASSERT_EQ(ret.func, FFA_SUCCESS_32);
+
+        return ret.arg2;
+}
+
+bool is_primary_vm(void)
+{
+	return get_vm_id() == PRIMARY_VM_ID;
+}
\ No newline at end of file
diff --git a/lib/arm64/ffa_utils.h b/lib/arm64/ffa_utils.h
new file mode 100644
index 0000000..0339a41
--- /dev/null
+++ b/lib/arm64/ffa_utils.h
@@ -0,0 +1,57 @@
+/*
+ * Adapted from Hafnium.
+ * TODO: Figure out the right way of specifying the license header.
+ */
+
+#pragma once
+
+#include <libcflat.h>
+
+#include "call.h"
+#include "ffa.h"
+#include "ffa_mailbox.h"
+
+#define memcpy_s(dest, destmax, src, count) memcpy(dest, src, count)
+
+#define TEST_ASSERT(a, b, op) \
+do { \
+	if (!((a) op (b))) { \
+		report_info("%s:%d: Assertion failed: %s %s %s", \
+		       __FILE__, __LINE__, #a, #op, #b); \
+		dump_stack(); \
+                assert(0); \
+	} \
+} while (0)
+
+#define EXPECT(a) TEST_ASSERT(a, true, ==)
+#define EXPECT_EQ(a, b) TEST_ASSERT(a, b, ==)
+#define EXPECT_NE(a, b) TEST_ASSERT(a, b, !=)
+#define EXPECT_GT(a, b) TEST_ASSERT(a, b, >)
+#define EXPECT_GE(a, b) TEST_ASSERT(a, b, >=)
+#define EXPECT_LE(a, b) TEST_ASSERT(a, b, <=)
+#define ASSERT_LE(a, b) TEST_ASSERT(a, b, <=)
+#define ASSERT_EQ(a, b) TEST_ASSERT(a, b, ==)
+#define ASSERT_NE(a, b) TEST_ASSERT(a, b, !=)
+#define ASSERT_GE(a, b) TEST_ASSERT(a, b, >=)
+
+#define EXPECT_FFA_ERROR(value, ffa_error)       \
+	do {                                     \
+		struct ffa_value v = (value);    \
+		EXPECT_EQ(v.func, FFA_ERROR_32); \
+		EXPECT_EQ(v.arg2, (ffa_error));  \
+	} while (0)
+
+
+/* SMCCC reserves ID 0 for the hypervisor itself. */
+#define HYPERVISOR_ID 0
+
+/* For now, assume the primary VM has an ID of 1, same as hafnium */
+#define PRIMARY_VM_ID 1
+
+#define SERVICE_VM1 (PRIMARY_VM_ID + 1)
+
+#define SERVICE_VM2 (PRIMARY_VM_ID + 2)
+
+ffa_vm_id_t get_vm_id(void);
+
+bool is_primary_vm(void);
\ No newline at end of file