diff --git a/build/toolchain/embedded.gni b/build/toolchain/embedded.gni
index e74fb6f..462bbfa 100644
--- a/build/toolchain/embedded.gni
+++ b/build/toolchain/embedded.gni
@@ -362,13 +362,13 @@
                              "gic_version",
                              "gicd_base_address",
                              "gicr_base_address",
+                             "max_cpus",
                              "toolchain_args",
                            ])
     cpu = "${invoker.cpu}+fp"
 
     # Nonsense values because they are required but shouldn't be used.
     heap_pages = 0
-    max_cpus = 1
     max_vms = 1
   }
 }
diff --git a/src/BUILD.gn b/src/BUILD.gn
index 5ca57ab..045d1f3 100644
--- a/src/BUILD.gn
+++ b/src/BUILD.gn
@@ -50,7 +50,6 @@
     "abort.c",
     "api.c",
     "cpu.c",
-    "fdt_handler.c",
     "mm.c",
     "mpool.c",
     "panic.c",
@@ -59,6 +58,7 @@
 
   deps = [
     ":fdt",
+    ":fdt_handler",
     ":memiter",
     ":std",
     "//src/arch/${plat_arch}/hypervisor",
@@ -93,6 +93,19 @@
   ]
 }
 
+source_set("fdt_handler") {
+  sources = [
+    "fdt_handler.c",
+  ]
+  deps = [
+    ":fdt",
+  ]
+
+  if (is_debug) {
+    deps += [ ":dlog" ]
+  }
+}
+
 # Flattened Device Tree (FDT) utilities.
 source_set("fdt") {
   sources = [
diff --git a/test/hftest/BUILD.gn b/test/hftest/BUILD.gn
index a18a252..3900c36 100644
--- a/test/hftest/BUILD.gn
+++ b/test/hftest/BUILD.gn
@@ -110,6 +110,7 @@
     "hftest_common.c",
   ]
   deps = [
+    "//src:fdt_handler",
     "//src:memiter",
     "//src:panic",
     "//src:std",
diff --git a/test/hftest/hftest_common.c b/test/hftest/hftest_common.c
index 4a3f0df..8c8d334 100644
--- a/test/hftest/hftest_common.c
+++ b/test/hftest/hftest_common.c
@@ -18,6 +18,8 @@
 
 #include "hf/arch/vm/power_mgmt.h"
 
+#include "hf/boot_params.h"
+#include "hf/fdt_handler.h"
 #include "hf/memiter.h"
 #include "hf/std.h"
 
@@ -131,12 +133,13 @@
 }
 
 static void run_test(hftest_test_fn set_up, hftest_test_fn test,
-		     hftest_test_fn tear_down)
+		     hftest_test_fn tear_down, const struct fdt_header *fdt)
 {
 	/* Prepare the context. */
 	struct hftest_context *ctx = hftest_get_context();
 	memset_s(ctx, sizeof(*ctx), 0, sizeof(*ctx));
 	ctx->abort = abort;
+	ctx->fdt = fdt;
 
 	/* Run any set up functions. */
 	if (set_up) {
@@ -166,7 +169,8 @@
 /**
  * Runs the given test case.
  */
-void hftest_run(struct memiter suite_name, struct memiter test_name)
+void hftest_run(struct memiter suite_name, struct memiter test_name,
+		const struct fdt_header *fdt)
 {
 	size_t i;
 	bool found_suite = false;
@@ -209,7 +213,7 @@
 		case HFTEST_KIND_TEST:
 			if (memiter_iseq(&test_name, test->name)) {
 				run_test(suite_set_up, test->fn,
-					 suite_tear_down);
+					 suite_tear_down, fdt);
 				return;
 			}
 			break;
@@ -244,3 +248,38 @@
 	HFTEST_LOG("");
 	HFTEST_LOG("    Run the named test from the named test suite.");
 }
+
+static uintptr_t vcpu_index_to_id(size_t index)
+{
+	/* For now we use indices as IDs for vCPUs. */
+	return index;
+}
+
+/**
+ * Get the ID of the CPU with the given index.
+ */
+uintptr_t hftest_get_cpu_id(size_t index)
+{
+	struct boot_params params;
+	struct fdt_node n;
+	const struct fdt_header *fdt = hftest_get_context()->fdt;
+
+	if (fdt == NULL) {
+		/*
+		 * We must be in a service VM, so apply the mapping that Hafnium
+		 * uses for vCPU IDs.
+		 */
+		return vcpu_index_to_id(index);
+	}
+
+	/* Find physical CPU ID from FDT. */
+	if (!fdt_root_node(&n, fdt)) {
+		FAIL("FDT failed validation.");
+	}
+	if (!fdt_find_child(&n, "")) {
+		FAIL("Unable to find FDT root node.");
+	}
+	fdt_find_cpus(&n, params.cpu_ids, &params.cpu_count);
+
+	return params.cpu_ids[index];
+}
diff --git a/test/hftest/inc/hftest.h b/test/hftest/inc/hftest.h
index d22cadf..bcffe5c 100644
--- a/test/hftest/inc/hftest.h
+++ b/test/hftest/inc/hftest.h
@@ -90,5 +90,7 @@
  */
 #define HFTEST_LOG_INDENT "    "
 
+uintptr_t hftest_get_cpu_id(size_t index);
+
 /* Above this point is the public API. Now include the implementation. */
 #include <hftest_impl.h>
diff --git a/test/hftest/inc/hftest_common.h b/test/hftest/inc/hftest_common.h
index fa35aab..d141076 100644
--- a/test/hftest/inc/hftest_common.h
+++ b/test/hftest/inc/hftest_common.h
@@ -14,6 +14,7 @@
  * limitations under the License.
  */
 
+#include "hf/fdt.h"
 #include "hf/memiter.h"
 
 #include "hftest_impl.h"
@@ -22,5 +23,6 @@
 void hftest_use_list(struct hftest_test list[], size_t count);
 
 void hftest_json(void);
-void hftest_run(struct memiter suite_name, struct memiter test_name);
+void hftest_run(struct memiter suite_name, struct memiter test_name,
+		const struct fdt_header *fdt);
 void hftest_help(void);
diff --git a/test/hftest/inc/hftest_impl.h b/test/hftest/inc/hftest_impl.h
index 13b49f1..64eeac4 100644
--- a/test/hftest/inc/hftest_impl.h
+++ b/test/hftest/inc/hftest_impl.h
@@ -18,6 +18,7 @@
 
 #include <stdnoreturn.h>
 
+#include "hf/fdt.h"
 #include "hf/spci.h"
 #include "hf/std.h"
 
@@ -133,6 +134,9 @@
 	uint32_t failures;
 	noreturn void (*abort)(void);
 
+	/* These are used in primary VMs. */
+	const struct fdt_header *fdt;
+
 	/* These are used in services. */
 	struct spci_message *send;
 	struct spci_message *recv;
diff --git a/test/hftest/linux_main.c b/test/hftest/linux_main.c
index 3cb58d1..6ff258b 100644
--- a/test/hftest/linux_main.c
+++ b/test/hftest/linux_main.c
@@ -15,6 +15,7 @@
  */
 
 #include <stdalign.h>
+#include <stddef.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
@@ -54,7 +55,7 @@
 
 		memiter_init(&suite_name, argv[2], strnlen_s(argv[2], 64));
 		memiter_init(&test_name, argv[3], strnlen_s(argv[3], 64));
-		hftest_run(suite_name, test_name);
+		hftest_run(suite_name, test_name, NULL);
 		return;
 	}
 
diff --git a/test/hftest/standalone_main.c b/test/hftest/standalone_main.c
index b4938e6..c1bfa1a 100644
--- a/test/hftest/standalone_main.c
+++ b/test/hftest/standalone_main.c
@@ -84,7 +84,7 @@
 			HFTEST_LOG("Unable to parse test.");
 			return;
 		}
-		hftest_run(suite_name, test_name);
+		hftest_run(suite_name, test_name, fdt);
 		return;
 	}
 
diff --git a/test/vmapi/primary_only/faults.c b/test/vmapi/primary_only/faults.c
index f24e61f..1adeb5b 100644
--- a/test/vmapi/primary_only/faults.c
+++ b/test/vmapi/primary_only/faults.c
@@ -61,8 +61,8 @@
 
 	/* Start secondary cpu while holding lock. */
 	sl_lock(&s.lock);
-	EXPECT_EQ(cpu_start(1, other_stack, sizeof(other_stack), rx_reader,
-			    (uintptr_t)&s),
+	EXPECT_EQ(cpu_start(hftest_get_cpu_id(1), other_stack,
+			    sizeof(other_stack), rx_reader, (uintptr_t)&s),
 		  true);
 
 	/* Wait for CPU to release the lock. */
diff --git a/test/vmapi/primary_only/primary_only.c b/test/vmapi/primary_only/primary_only.c
index 6cb8f46..d715d19 100644
--- a/test/vmapi/primary_only/primary_only.c
+++ b/test/vmapi/primary_only/primary_only.c
@@ -117,9 +117,10 @@
 
 	/* Start secondary while holding lock. */
 	sl_lock(&lock);
-	EXPECT_EQ(cpu_start(1, other_stack, sizeof(other_stack), vm_cpu_entry,
-			    (uintptr_t)&lock),
-		  true);
+	EXPECT_EQ(
+		cpu_start(hftest_get_cpu_id(1), other_stack,
+			  sizeof(other_stack), vm_cpu_entry, (uintptr_t)&lock),
+		true);
 
 	/* Wait for CPU to release the lock. */
 	sl_lock(&lock);
diff --git a/test/vmapi/primary_with_secondaries/run_race.c b/test/vmapi/primary_with_secondaries/run_race.c
index 2062de1..bd832c7 100644
--- a/test/vmapi/primary_with_secondaries/run_race.c
+++ b/test/vmapi/primary_with_secondaries/run_race.c
@@ -89,8 +89,8 @@
 	SERVICE_SELECT(SERVICE_VM0, "check_state", mb.send);
 
 	/* Start second vCPU. */
-	ASSERT_TRUE(cpu_start(1, stack, sizeof(stack), vm_cpu_entry,
-			      (uintptr_t)&mb));
+	ASSERT_TRUE(cpu_start(hftest_get_cpu_id(1), stack, sizeof(stack),
+			      vm_cpu_entry, (uintptr_t)&mb));
 
 	/* Run on a loop until the secondary VM is done. */
 	EXPECT_TRUE(run_loop(&mb));