diff --git a/inc/cpu.h b/inc/cpu.h
index c1ab1c6..d5bd29b 100644
--- a/inc/cpu.h
+++ b/inc/cpu.h
@@ -52,8 +52,9 @@
 size_t cpu_index(struct cpu *c);
 void cpu_irq_enable(struct cpu *c);
 void cpu_irq_disable(struct cpu *c);
-bool cpu_on(struct cpu *c);
+bool cpu_on(struct cpu *c, size_t entry, size_t arg);
 void cpu_off(struct cpu *c);
+struct cpu *cpu_find(size_t id);
 
 void vcpu_init(struct vcpu *vcpu, struct vm *vm);
 void vcpu_on(struct vcpu *vcpu);
diff --git a/inc/mm.h b/inc/mm.h
index df027dd..a6f637e 100644
--- a/inc/mm.h
+++ b/inc/mm.h
@@ -50,6 +50,7 @@
 bool mm_ptable_unmap_hypervisor(struct mm_ptable *t, int mode);
 
 bool mm_init(void);
+bool mm_cpu_init(void);
 bool mm_map(vaddr_t begin, vaddr_t end, paddr_t paddr, int mode);
 bool mm_unmap(vaddr_t begin, vaddr_t end, int mode);
 void mm_defrag(void);
diff --git a/src/api.c b/src/api.c
index ae429f0..93b14c2 100644
--- a/src/api.c
+++ b/src/api.c
@@ -94,7 +94,7 @@
 }
 
 /**
- * Puts current vcpu in wait for interrupt mode, and returns to the primary
+ * Puts the current vcpu in wait for interrupt mode, and returns to the primary
  * vm.
  */
 struct vcpu *api_wait_for_interrupt(void)
@@ -132,7 +132,7 @@
 
 	/*
 	 * Check that both pages are acessible from the VM, i.e., ensure that
-	 * the caller isn't try to use another VM's memory.
+	 * the caller isn't trying to use another VM's memory.
 	 */
 	if (!mm_ptable_is_mapped(&vm->ptable, recv, 0) ||
 	    !mm_ptable_is_mapped(&vm->ptable, send, 0)) {
diff --git a/src/arch/aarch64/handler.c b/src/arch/aarch64/handler.c
index 3eec780..2e5e75e 100644
--- a/src/arch/aarch64/handler.c
+++ b/src/arch/aarch64/handler.c
@@ -2,6 +2,7 @@
 #include "arch_api.h"
 #include "cpu.h"
 #include "dlog.h"
+#include "psci.h"
 #include "vm.h"
 
 #include "msr.h"
@@ -11,6 +12,9 @@
 	struct vcpu *new;
 };
 
+int32_t smc(size_t arg0, size_t arg1, size_t arg2, size_t arg3);
+void cpu_entry(struct cpu *c);
+
 void irq_current(void)
 {
 	dlog("IRQ from current\n");
@@ -50,24 +54,121 @@
 	}
 }
 
+/**
+ * Handles PSCI requests received via HVC or SMC instructions from the primary
+ * VM only.
+ *
+ * Returns true if the request was a PSCI one, false otherwise.
+ */
+static bool psci_handler(uint32_t func, size_t arg0, size_t arg1, size_t arg2,
+			 long *ret)
+{
+	struct cpu *c;
+	int32_t sret;
+
+	switch (func & ~PSCI_CONVENTION_MASK) {
+	case PSCI_VERSION:
+		/* Version is 0.2. */
+		*ret = 2;
+		break;
+
+	case PSCI_MIGRATE_INFO_TYPE:
+		/* Trusted OS does not require migration. */
+		*ret = 2;
+		break;
+
+	case PSCI_SYSTEM_OFF:
+		smc(PSCI_SYSTEM_OFF, 0, 0, 0);
+		for (;;) {
+		}
+		break;
+
+	case PSCI_SYSTEM_RESET:
+		smc(PSCI_SYSTEM_RESET, 0, 0, 0);
+		for (;;) {
+		}
+		break;
+
+	case PSCI_AFFINITY_INFO:
+		c = cpu_find(arg0);
+		if (!c) {
+			*ret = PSCI_RETURN_INVALID_PARAMETERS;
+			break;
+		}
+
+		if (arg1 != 0) {
+			*ret = PSCI_RETURN_NOT_SUPPORTED;
+			break;
+		}
+
+		sl_lock(&c->lock);
+		if (c->is_on) {
+			*ret = 0; /* ON */
+		} else {
+			*ret = 1; /* OFF */
+		}
+		sl_unlock(&c->lock);
+		break;
+
+	case PSCI_CPU_OFF:
+		cpu_off(cpu());
+		smc(PSCI_CPU_OFF, 0, 0, 0);
+		for (;;) {
+		}
+		break;
+
+	case PSCI_CPU_ON:
+		c = cpu_find(arg0);
+		if (!c) {
+			*ret = PSCI_RETURN_INVALID_PARAMETERS;
+			break;
+		}
+
+		if (cpu_on(c, arg1, arg2)) {
+			*ret = PSCI_RETURN_ALREADY_ON;
+			break;
+		}
+
+		/*
+		 * There's a race when turning a CPU on when it's in the
+		 * process of turning off. We need to loop here while it is
+		 * reported that the CPU is on (because it's about to turn
+		 * itself off).
+		 */
+		do {
+			sret = smc(PSCI_CPU_ON, arg0, (size_t)&cpu_entry,
+				   (size_t)c);
+		} while (sret == PSCI_RETURN_ALREADY_ON);
+
+		if (sret == PSCI_RETURN_SUCCESS) {
+			*ret = PSCI_RETURN_SUCCESS;
+		} else {
+			dlog("Unexpected return from PSCI_CPU_ON: 0x%x\n",
+			     sret);
+			*ret = PSCI_RETURN_INTERNAL_FAILURE;
+		}
+		break;
+
+	default:
+		return false;
+	}
+
+	return true;
+}
+
 struct hvc_handler_return hvc_handler(size_t arg0, size_t arg1, size_t arg2,
 				      size_t arg3)
 {
-	(void)arg3;
-
 	struct hvc_handler_return ret;
 
 	ret.new = NULL;
 
-	switch (arg0) {
-	case 0x84000000: /* PSCI_VERSION */
-		ret.user_ret = 2;
-		break;
+	if (cpu()->current->vm == &primary_vm &&
+	    psci_handler(arg0, arg1, arg2, arg3, &ret.user_ret)) {
+		return ret;
+	}
 
-	case 0x84000006: /* PSCI_MIGRATE */
-		ret.user_ret = 2;
-		break;
-
+	switch ((uint32_t)arg0 & ~PSCI_CONVENTION_MASK) {
 	case HF_VM_GET_COUNT:
 		ret.user_ret = api_vm_get_count();
 		break;
@@ -119,6 +220,7 @@
 {
 	struct cpu *c = cpu();
 	struct vcpu *vcpu = c->current;
+	long ret;
 
 	switch (esr >> 26) {
 	case 0x01: /* EC = 000001, WFI or WFE. */
@@ -159,6 +261,18 @@
 			/* do nothing */
 		}
 
+	case 0x17: /* EC = 010111, SMC instruction. */
+		if (vcpu->vm != &primary_vm ||
+		    !psci_handler(vcpu->regs.r[0], vcpu->regs.r[1],
+				  vcpu->regs.r[2], vcpu->regs.r[3], &ret)) {
+			dlog("Unsupported SMC call: 0x%x\n", vcpu->regs.r[0]);
+			ret = -1;
+		}
+
+		/* Skip the SMC instruction. */
+		vcpu->regs.pc += (esr & (1u << 25)) ? 4 : 2;
+		break;
+
 	default:
 		dlog("Unknown lower sync exception pc=0x%x, esr=0x%x, "
 		     "ec=0x%x\n",
@@ -168,5 +282,7 @@
 		}
 	}
 
+	vcpu->regs.r[0] = ret;
+
 	return NULL;
 }
diff --git a/src/arch/aarch64/inc/arch_cpu.h b/src/arch/aarch64/inc/arch_cpu.h
index 1a3055f..6acee6e 100644
--- a/src/arch/aarch64/inc/arch_cpu.h
+++ b/src/arch/aarch64/inc/arch_cpu.h
@@ -65,13 +65,19 @@
 		  (0xf << 6); /* DAIF bits set; disable interrupts. */
 	r->pc = pc;
 	r->r[0] = arg;
+	/* TODO: Determine if we need to set TSW. */
 	r->lazy.hcr_el2 = (1u << 31) | /* RW bit. */
+			  (1u << 21) | /* TACR, trap access to ACTRL_EL1. */
+			  (1u << 19) | /* TSC, trap SMC instructions. */
+			  (1u << 20) | /* TIDCP, trap impl-defined funct. */
 			  (1u << 2) |  /* PTW, Protected Table Walk. */
 			  (1u << 0);   /* VM: enable stage-2 translation. */
 
 	if (!is_primary) {
-		r->lazy.hcr_el2 |= (7u << 3) | /* AMO, IMO, FMO bits. */
-				   (3u << 13); /* TWI, TWE bits. */
+		r->lazy.hcr_el2 |= (7u << 3) |  /* AMO, IMO, FMO bits. */
+				   (1u << 9) |  /* FB bit. */
+				   (1u << 10) | /* BSU bits set to inner-sh. */
+				   (3u << 13);  /* TWI, TWE bits. */
 	}
 }
 
@@ -92,29 +98,4 @@
 	r->lazy.hcr_el2 &= ~(1u << 7);
 }
 
-/* TODO: Figure out what to do with this. */
-int32_t smc(size_t arg0, size_t arg1, size_t arg2, size_t arg3);
-
-static inline void arch_cpu_on(size_t id, void *ctx)
-{
-	void cpu_entry(void *ctx);
-	int32_t ret;
-
-	/*
-	 * There's a race when turning a CPU on when it's in the process of
-	 * turning off. We need to loop here while it is reported that the CPU
-	 * is on (because it's about to turn itself off).
-	 */
-	do {
-		/* CPU_ON */
-		ret = smc(0xC4000003, id, (size_t)&cpu_entry, (size_t)ctx);
-	} while (ret == -4); /* ALREADY_ON */
-}
-
-static inline void arch_cpu_off(void)
-{
-	/* CPU_OFF */
-	smc(0xC4000002, 0, 0, 0);
-}
-
 #endif /* _ARCH_CPU_H */
diff --git a/src/arch/aarch64/inc/arch_mm.h b/src/arch/aarch64/inc/arch_mm.h
index 3473f37..8662f78 100644
--- a/src/arch/aarch64/inc/arch_mm.h
+++ b/src/arch/aarch64/inc/arch_mm.h
@@ -176,7 +176,7 @@
 }
 
 uint64_t arch_mm_mode_to_attrs(int mode);
-bool arch_mm_init(paddr_t table);
+bool arch_mm_init(paddr_t table, bool first);
 int arch_mm_max_level(int mode);
 
 #endif /* _ARCH_MM_H */
diff --git a/src/arch/aarch64/inc/psci.h b/src/arch/aarch64/inc/psci.h
new file mode 100644
index 0000000..22f859f
--- /dev/null
+++ b/src/arch/aarch64/inc/psci.h
@@ -0,0 +1,45 @@
+#ifndef _PSCI_H
+#define _PSCI_H
+
+/* clang-format off */
+
+#define PSCI_CONVENTION_MASK (1u << 30)
+
+/* The following are function identifiers for PSCI. */
+#define PSCI_VERSION                 0x84000000
+#define PSCI_CPU_SUSPEND             0x84000001
+#define PSCI_CPU_OFF                 0x84000002
+#define PSCI_CPU_ON                  0x84000003
+#define PSCI_AFFINITY_INFO           0x84000004
+#define PSCI_MIGRATE                 0x84000005
+#define PSCI_MIGRATE_INFO_TYPE       0x84000006
+#define PSCI_MIGRATE_INFO_UP_CPU     0x84000007
+#define PSCI_SYSTEM_OFF              0x84000008
+#define PSCI_SYSTEM_RESET            0x84000009
+#define PSCI_FEATURES                0x8400000a
+#define PSCI_CPU_FREEZE              0x8400000b
+#define PSCI_CPU_DEFAULT_SUSPEND     0x8400000c
+#define PSCI_NODE_HW_STATE           0x8400000d
+#define PSCI_SYSTEM_SUSPEND          0x8400000e
+#define PSCI_SET_SYSPEND_MODE        0x8400000f
+#define PSCI_STAT_RESIDENCY          0x84000010
+#define PSCI_STAT_COUNT              0x84000011
+#define PSCI_SYSTEM_RESET2           0x84000012
+#define PSCI_MEM_PROTECT             0x84000013
+#define PSCI_MEM_PROTECT_CHECK_RANGE 0x84000014
+
+/* The following are return codes for PSCI. */
+#define PSCI_RETURN_SUCCESS            0
+#define PSCI_RETURN_NOT_SUPPORTED      -1
+#define PSCI_RETURN_INVALID_PARAMETERS -2
+#define PSCI_RETURN_DENIED             -3
+#define PSCI_RETURN_ALREADY_ON         -4
+#define PSCI_RETURN_ON_PENDING         -5
+#define PSCI_RETURN_INTERNAL_FAILURE   -6
+#define PSCI_NOT_PRESENT               -7
+#define PSCI_DISABLE                   -8
+#define PSCI_INVALID_ADDRESS           -9
+
+/* clang-format on */
+
+#endif /* _PSCI_H */
diff --git a/src/arch/aarch64/mm.c b/src/arch/aarch64/mm.c
index c5c1b4f..53506bc 100644
--- a/src/arch/aarch64/mm.c
+++ b/src/arch/aarch64/mm.c
@@ -144,7 +144,7 @@
 	return mm_max_s2_level;
 }
 
-bool arch_mm_init(paddr_t table)
+bool arch_mm_init(paddr_t table, bool first)
 {
 	static const int pa_bits_table[16] = {32, 36, 40, 42, 44, 48};
 	uint64_t features = read_msr(id_aa64mmfr0_el1);
@@ -165,7 +165,9 @@
 		return false;
 	}
 
-	dlog("Supported bits in physical address: %d\n", pa_bits);
+	if (first) {
+		dlog("Supported bits in physical address: %d\n", pa_bits);
+	}
 
 	/*
 	 * Determine sl0 based on the number of bits. The maximum value is given
@@ -179,7 +181,9 @@
 		sl0 = 1;
 	}
 
-	dlog("Number of page table levels: %d\n", mm_max_s2_level + 1);
+	if (first) {
+		dlog("Number of page table levels: %d\n", mm_max_s2_level + 1);
+	}
 
 	v = (1u << 31) |	       /* RES1. */
 	    ((features & 0xf) << 16) | /* PS, matching features. */
diff --git a/src/cpu.c b/src/cpu.c
index 581bc11..6cd0a2b 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -1,5 +1,6 @@
 #include "cpu.h"
 
+#include "api.h"
 #include "arch_cpu.h"
 #include "dlog.h"
 #include "std.h"
@@ -24,14 +25,14 @@
 	for (i = 0; i < MAX_CPUS; i++) {
 		struct cpu *c = cpus + i;
 		cpu_init(c);
-		c->id = i; /* TODO: Initialize ID. */
+		c->id = i; /* TODO: Initialize ID based on fdt. */
 		c->stack_bottom = callstacks + STACK_SIZE * (i + 1);
 	}
 }
 
 size_t cpu_index(struct cpu *c)
 {
-	return cpus - c;
+	return c - cpus;
 }
 
 void cpu_init(struct cpu *c)
@@ -60,7 +61,7 @@
 /**
  * Turns CPU on and returns the previous state.
  */
-bool cpu_on(struct cpu *c)
+bool cpu_on(struct cpu *c, size_t entry, size_t arg)
 {
 	bool prev;
 
@@ -70,23 +71,38 @@
 	sl_unlock(&c->lock);
 
 	if (!prev) {
-		/* The CPU is currently off, we need to turn it on. */
-		arch_cpu_on(c->id, c);
+		struct vcpu *vcpu = primary_vm.vcpus + cpu_index(c);
+		arch_regs_init(&vcpu->regs, entry, arg, true);
+		vcpu_on(vcpu);
 	}
 
 	return prev;
 }
 
-/*
- * This must be called only from the same CPU.
+/**
+ * Prepares the CPU for turning itself off.
  */
 void cpu_off(struct cpu *c)
 {
 	sl_lock(&c->lock);
 	c->is_on = false;
 	sl_unlock(&c->lock);
+}
 
-	arch_cpu_off();
+/**
+ * Searches for a CPU based on its id.
+ */
+struct cpu *cpu_find(size_t id)
+{
+	size_t i;
+
+	for (i = 0; i < MAX_CPUS; i++) {
+		if (cpus[i].id == id) {
+			return cpus + i;
+		}
+	}
+
+	return NULL;
 }
 
 void vcpu_init(struct vcpu *vcpu, struct vm *vm)
@@ -95,7 +111,8 @@
 	sl_init(&vcpu->lock);
 	vcpu->vm = vm;
 	vcpu->state = vcpu_state_off;
-	/* TODO: Initialize vmid register. */
+	/* TODO: This needs to be moved to arch-dependent code. */
+	vcpu->regs.lazy.vmpidr_el2 = vcpu - vm->vcpus;
 }
 
 void vcpu_on(struct vcpu *vcpu)
diff --git a/src/load.c b/src/load.c
index 1b75008..7ab012e 100644
--- a/src/load.c
+++ b/src/load.c
@@ -135,8 +135,6 @@
 		vm_start_vcpu(&primary_vm, 0, tmp, kernel_arg, true);
 	}
 
-	vm_set_current(&primary_vm);
-
 	return true;
 }
 
diff --git a/src/main.c b/src/main.c
index 8862ba8..7938fec 100644
--- a/src/main.c
+++ b/src/main.c
@@ -120,5 +120,11 @@
 
 	dlog("Starting up cpu %d\n", cpu_index(c));
 
+	if (!mm_cpu_init()) {
+		panic("mm_cpu_init failed");
+	}
+
+	vm_set_current(&primary_vm);
+
 	return primary_vm.vcpus + cpu_index(c);
 }
diff --git a/src/mm.c b/src/mm.c
index 00c5d3f..f2062c8 100644
--- a/src/mm.c
+++ b/src/mm.c
@@ -461,7 +461,12 @@
 	mm_map((vaddr_t)data_begin, (vaddr_t)data_end, (paddr_t)data_begin,
 	       MM_MODE_R | MM_MODE_W | MM_MODE_NOSYNC);
 
-	return arch_mm_init((paddr_t)ptable.table);
+	return arch_mm_init((paddr_t)ptable.table, true);
+}
+
+bool mm_cpu_init(void)
+{
+	return arch_mm_init((paddr_t)ptable.table, false);
 }
 
 /**