vboot2: implement select_firmware for pre-romstage verification

This patch has a basic structure of vboot2 integration. It supports only Nyans,
which have bootblock architecture and romstage architecture are
compatible from linker's perspective.

TEST=Built with VBOOT2_VERIFY_FIRMWARE on/off. Booted Nyan Blaze.
BUG=None
BRANCH=none
Signed-off-by: Daisuke Nojiri <dnojiri@chromium.org>

Original-Change-Id: I4bbd4d0452604943b376bef20ea8a258820810aa
Original-Reviewed-on: https://chromium-review.googlesource.com/204522
Original-Reviewed-by: Daisuke Nojiri <dnojiri@chromium.org>
Original-Commit-Queue: Daisuke Nojiri <dnojiri@chromium.org>
Original-Tested-by: Daisuke Nojiri <dnojiri@chromium.org>
(cherry picked from commit a6bce0cbed34def60386f3d9aece59e739740c58)
Signed-off-by: Marc Jones <marc.jones@se-eng.com>

Change-Id: I63ddfbf463c8a83120828ec8ab994f8146f90001
Reviewed-on: http://review.coreboot.org/8160
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>
Tested-by: build bot (Jenkins)

16 files changed, 423 insertions, 3 deletions

diff --git a/src/arch/arm/Makefile.inc b/src/arch/arm/Makefile.inc
index 6f65a71c75..96c8b494b2 100644
--- a/src/arch/arm/Makefile.inc
+++ b/src/arch/arm/Makefile.inc
@@ -68,6 +68,18 @@ $(objcbfs)/bootblock.debug: $(src)/arch/arm/bootblock.ld $(obj)/ldoptions $$(boo
 endif # CONFIG_ARCH_BOOTBLOCK_ARM
 
 ###############################################################################
+# verification stage
+###############################################################################
+
+verstage-y += early_console.c
+verstage-y += div0.c
+verstage-y += eabi_compat.c
+verstage-y += memset.S
+verstage-y += memcpy.S
+verstage-y += memmove.S
+verstage-y += stages.c
+
+###############################################################################
 # romstage
 ###############################################################################
 
diff --git a/src/arch/arm/armv7/Makefile.inc b/src/arch/arm/armv7/Makefile.inc
index 7e67178aa8..0919a93022 100644
--- a/src/arch/arm/armv7/Makefile.inc
+++ b/src/arch/arm/armv7/Makefile.inc
@@ -46,6 +46,18 @@ bootblock-S-ccopts += $(armv7_asm_flags)
 
 endif # CONFIG_ARCH_BOOTBLOCK_ARMV7
 
+################################################################################
+## verification stage
+################################################################################
+
+verstage-c-ccopts += $(armv7_flags)
+verstage-S-ccopts += $(armv7_asm_flags)
+verstage-y += cache.c
+verstage-y += cpu.S
+verstage-y += exception.c
+verstage-y += exception_asm.S
+verstage-y += mmu.c
+
 ###############################################################################
 # romstage
 ###############################################################################
diff --git a/src/arch/arm/bootblock.ld b/src/arch/arm/bootblock.ld
index 150bf2df6b..c1a6ccf575 100644
--- a/src/arch/arm/bootblock.ld
+++ b/src/arch/arm/bootblock.ld
@@ -50,6 +50,7 @@ SECTIONS
 	} : to_load = 0xff
 
 	preram_cbmem_console = CONFIG_CBMEM_CONSOLE_PRERAM_BASE;
+	verstage_preram_cbmem_console = CONFIG_CBMEM_CONSOLE_PRERAM_BASE;
 
 	/DISCARD/ : {
 		*(.comment)
diff --git a/src/arch/arm/libgcc/Makefile.inc b/src/arch/arm/libgcc/Makefile.inc
index a1d325dfad..5e8858adc2 100644
--- a/src/arch/arm/libgcc/Makefile.inc
+++ b/src/arch/arm/libgcc/Makefile.inc
@@ -25,6 +25,10 @@ ifeq ($(CONFIG_ARCH_BOOTBLOCK_ARM),y)
 bootblock-y += $(libgcc_files)
 endif
 
+ifeq ($(CONFIG_ARCH_VERSTAGE_ARM),y)
+verstage-y += $(libgcc_files)
+endif
+
 ifeq ($(CONFIG_ARCH_ROMSTAGE_ARM),y)
 romstage-y += $(libgcc_files)
 endif
diff --git a/src/console/Makefile.inc b/src/console/Makefile.inc
index d339500ea2..64d980bb43 100644
--- a/src/console/Makefile.inc
+++ b/src/console/Makefile.inc
@@ -6,6 +6,10 @@ ramstage-y += die.c
 smm-$(CONFIG_DEBUG_SMI) += init.c console.c vtxprintf.c printk.c
 smm-$(CONFIG_SMM_TSEG) += die.c
 
+verstage-y += vtxprintf.c
+verstage-y += console.c
+verstage-y += die.c
+
 romstage-y += vtxprintf.c printk.c
 romstage-y += init.c console.c
 romstage-y += post.c
diff --git a/src/include/reset.h b/src/include/reset.h
index 9f117dbda1..9430ffef56 100644
--- a/src/include/reset.h
+++ b/src/include/reset.h
@@ -7,5 +7,6 @@ void hard_reset(void);
 #define hard_reset() do {} while(0)
 #endif
 void soft_reset(void);
+void cpu_reset(void);
 
 #endif
diff --git a/src/lib/Makefile.inc b/src/lib/Makefile.inc
index 9b5f56bb74..946012dda2 100644
--- a/src/lib/Makefile.inc
+++ b/src/lib/Makefile.inc
@@ -24,6 +24,12 @@ bootblock-$(CONFIG_COMMON_CBFS_SPI_WRAPPER) += cbfs_spi.c
 bootblock-y += memchr.c
 bootblock-y += memcmp.c
 
+verstage-y += delay.c
+verstage-y += cbfs.c
+verstage-y += memcmp.c
+verstage-$(CONFIG_CONSOLE_CBMEM) += cbmem_console.c
+
+
 romstage-y += memchr.c
 romstage-y += memcmp.c
 $(foreach arch,$(ARCH_SUPPORTED),\
diff --git a/src/mainboard/google/nyan_blaze/Makefile.inc b/src/mainboard/google/nyan_blaze/Makefile.inc
index b66cd555fb..02b3977088 100644
--- a/src/mainboard/google/nyan_blaze/Makefile.inc
+++ b/src/mainboard/google/nyan_blaze/Makefile.inc
@@ -32,6 +32,8 @@ bootblock-y += bootblock.c
 bootblock-y += pmic.c
 bootblock-y += reset.c
 
+verstage-y += reset.c
+
 romstage-y += reset.c
 romstage-y += romstage.c
 romstage-y += sdram_configs.c
diff --git a/src/mainboard/google/nyan_blaze/romstage.c b/src/mainboard/google/nyan_blaze/romstage.c
index 4e023654e7..480abd640d 100644
--- a/src/mainboard/google/nyan_blaze/romstage.c
+++ b/src/mainboard/google/nyan_blaze/romstage.c
@@ -228,7 +228,11 @@ static void __attribute__((noinline)) romstage(void)
 	cbmemc_reinit();
 #endif
 
+#if CONFIG_VBOOT2_VERIFY_FIRMWARE
+	// vboot_create_handoff((void *)CONFIG_VBOOT_WORK_BUFFER_ADDRESS);
+#else
 	vboot_verify_firmware(romstage_handoff_find_or_add());
+#endif
 
 	timestamp_add(TS_START_COPYRAM, timestamp_get());
 	void *entry = cbfs_load_stage(CBFS_DEFAULT_MEDIA,
diff --git a/src/soc/nvidia/tegra124/Kconfig b/src/soc/nvidia/tegra124/Kconfig
index f0cef82889..a8fcbfb3f6 100644
--- a/src/soc/nvidia/tegra124/Kconfig
+++ b/src/soc/nvidia/tegra124/Kconfig
@@ -46,10 +46,12 @@ config BOOTBLOCK_ROM_OFFSET
 
 config CBFS_HEADER_ROM_OFFSET
 	hex "offset of master CBFS header in ROM"
+	default 0x1e000 if VBOOT2_VERIFY_FIRMWARE
 	default 0x18000
 
 config CBFS_ROM_OFFSET
 	hex "offset of CBFS data in ROM"
+	default 0x1e080 if VBOOT2_VERIFY_FIRMWARE
 	default 0x18080
 
 config SYS_SDRAM_BASE
@@ -62,6 +64,7 @@ config BOOTBLOCK_BASE
 
 config ROMSTAGE_BASE
 	hex
+	default 0x4002d000 if VBOOT2_VERIFY_FIRMWARE
 	default 0x4002c000
 
 config RAMSTAGE_BASE
@@ -94,6 +97,14 @@ config CBFS_CACHE_SIZE
 	hex "size of CBFS cache data"
 	default 0x00017fe0
 
+config VBOOT_WORK_BUFFER_ADDRESS
+	hex "memory address of vboot work buffer"
+	default 0x40018000
+
+config VBOOT_WORK_BUFFER_SIZE
+	hex "size of vboot work buffer"
+	default 0x00004000
+
 config TEGRA124_MODEL_TD570D
 	bool "TD570D"
 
diff --git a/src/soc/nvidia/tegra124/Makefile.inc b/src/soc/nvidia/tegra124/Makefile.inc
index b306412956..4b0ec9649f 100644
--- a/src/soc/nvidia/tegra124/Makefile.inc
+++ b/src/soc/nvidia/tegra124/Makefile.inc
@@ -21,6 +21,14 @@ bootblock-$(CONFIG_CONSOLE_SERIAL) += uart.c
 endif
 
 verstage-y += verstage.c
+verstage-y += cbfs.c
+verstage-y += dma.c
+verstage-y += monotonic_timer.c
+verstage-y += spi.c
+verstage-y += timer.c
+verstage-$(CONFIG_CONSOLE_SERIAL_UART) += uart.c
+verstage-y += ../tegra/gpio.c
+verstage-y += ../tegra/pinmux.c
 
 romstage-y += cbfs.c
 romstage-y += cbmem.c
diff --git a/src/soc/nvidia/tegra124/verstage.c b/src/soc/nvidia/tegra124/verstage.c
index 234a89d0b2..d85fc5c8cf 100644
--- a/src/soc/nvidia/tegra124/verstage.c
+++ b/src/soc/nvidia/tegra124/verstage.c
@@ -1,9 +1,15 @@
 #include "verstage.h"
+#include <vendorcode/google/chromeos/chromeos.h>
 
 /**
  * Stage entry point
  */
 void vboot_main(void)
 {
-	for(;;);
+	/* Stub to force arm_init_caches to the top, before any stack/memory
+	 * accesses */
+	asm volatile ("bl arm_init_caches"
+		      ::: "r0","r1","r2","r3","r4","r5","ip");
+
+	select_firmware();
 }
diff --git a/src/vendorcode/google/chromeos/Makefile.inc b/src/vendorcode/google/chromeos/Makefile.inc
index 12d35b64c8..cb3d9a68b8 100644
--- a/src/vendorcode/google/chromeos/Makefile.inc
+++ b/src/vendorcode/google/chromeos/Makefile.inc
@@ -95,7 +95,33 @@ $(VB_LIB):
 endif
 
 ifeq ($(CONFIG_VBOOT2_VERIFY_FIRMWARE),y)
+VB_SOURCE := vboot_reference
 VERSTAGE_LIB = $(obj)/vendorcode/google/chromeos/verstage.a
+
+INCLUDES += -I$(VB_SOURCE)/firmware/2lib/include
+INCLUDES += -I$(VB_SOURCE)/firmware/include
+verstage-y += vboot_main.c fmap.c chromeos.c
+
+VB_FIRMWARE_ARCH := $(ARCHDIR-$(ARCH-VERSTAGE-y))
+VB2_LIB = $(obj)/external/vboot_reference/vboot_fw2.a
+VBOOT_CFLAGS += $(patsubst -I%,-I$(top)/%,$(filter-out -include $(src)/include/kconfig.h, $(CFLAGS_verstage)))
+VBOOT_CFLAGS += $(verstage-c-ccopts)
+VBOOT_CFLAGS += -include $(top)/src/include/kconfig.h -Wno-missing-prototypes
+VBOOT_CFLAGS += -DVBOOT_DEBUG
+
+$(VB2_LIB): $(obj)/config.h
+	@printf "    MAKE       $(subst $(obj)/,,$(@))\n"
+	$(Q)FIRMWARE_ARCH=$(VB_FIRMWARE_ARCH) \
+	CC="$(CC_verstage)" \
+		CFLAGS="$(VBOOT_CFLAGS)" VBOOT2="y" \
+		make -C $(VB_SOURCE) \
+		BUILD=$(top)/$(dir $(VB2_LIB)) \
+		V=$(V) \
+		fwlib2
+	mv $@ $@.tmp
+	@printf "    OBJCOPY    $(subst $(obj)/,,$(@))\n"
+	$(OBJCOPY_verstage) --prefix-symbols=verstage_ $@.tmp $@
+
 $(VERSTAGE_LIB): $$(verstage-objs)
 	@printf "    AR         $(subst $(obj)/,,$(@))\n"
 	$(AR_verstage) rc $@.tmp $(verstage-objs)
diff --git a/src/vendorcode/google/chromeos/chromeos.h b/src/vendorcode/google/chromeos/chromeos.h
index a2ecac849f..a799528d53 100644
--- a/src/vendorcode/google/chromeos/chromeos.h
+++ b/src/vendorcode/google/chromeos/chromeos.h
@@ -46,7 +46,7 @@ struct romstage_handoff;
 
 /* TODO(shawnn): Remove these CONFIGs and define default weak functions
  * that can be overridden in the platform / MB code. */
-#if CONFIG_VBOOT_VERIFY_FIRMWARE
+#if CONFIG_VBOOT_VERIFY_FIRMWARE || CONFIG_VBOOT2_VERIFY_FIRMWARE
 /*
  * This is a dual purpose routine. If dest is non-NULL the region at
  * offset_addr will be read into the area pointed to by dest.  If dest
@@ -67,7 +67,8 @@ static inline int vboot_get_handoff_info(void **addr, uint32_t *size)
 	return -1;
 }
 static inline int vboot_skip_display_init(void) { return 0; }
-#endif
+#endif /* CONFIG_VBOOT_VERIFY_FIRMWARE || CONFIG_VBOOT2_VERIFY_FIRMWARE */
+
 int vboot_get_sw_write_protect(void);
 
 #include "gnvs.h"
@@ -85,4 +86,8 @@ static inline void chromeos_ram_oops_init(chromeos_acpi_t *chromeos) {}
 static inline void chromeos_reserve_ram_oops(struct device *dev, int idx) {}
 #endif /* CONFIG_CHROMEOS_RAMOOPS */
 
+#if CONFIG_VBOOT2_VERIFY_FIRMWARE
+void select_firmware(void);
+#endif
+
 #endif
diff --git a/src/vendorcode/google/chromeos/vboot_handoff.c b/src/vendorcode/google/chromeos/vboot_handoff.c
index 7ef2c7ee5a..7ea21ea61b 100644
--- a/src/vendorcode/google/chromeos/vboot_handoff.c
+++ b/src/vendorcode/google/chromeos/vboot_handoff.c
@@ -105,6 +105,12 @@ int __attribute((weak)) vboot_get_sw_write_protect(void)
 	return 0;
 }
 
+#if CONFIG_VBOOT2_VERIFY_FIRMWARE
+
+void *vboot_get_payload(int *len) { return NULL; }
+
+#else /* CONFIG_VBOOT2_VERIFY_FIRMWARE */
+
 static void *vboot_get_payload(size_t *len)
 {
 	struct vboot_handoff *vboot_handoff;
@@ -131,6 +137,7 @@ static void *vboot_get_payload(size_t *len)
 
 	return (void *)fwc->address;
 }
+#endif
 
 static int vboot_locate_payload(struct payload *payload)
 {
diff --git a/src/vendorcode/google/chromeos/vboot_main.c b/src/vendorcode/google/chromeos/vboot_main.c
new file mode 100644
index 0000000000..9779d35eea
--- /dev/null
+++ b/src/vendorcode/google/chromeos/vboot_main.c
@@ -0,0 +1,311 @@
+#include <2api.h>
+#include <2struct.h>
+#include <arch/stages.h>
+#include <cbfs.h>
+#include <console/console.h>
+#include <console/vtxprintf.h>
+#include <reset.h>
+#include <string.h>
+
+#include "chromeos.h"
+#include "fmap.h"
+
+#define VBDEBUG(format, args...) \
+	printk(BIOS_INFO, "%s():%d: " format,  __func__, __LINE__, ## args)
+#define TODO_BLOCK_SIZE 8192
+#define MAX_PARSED_FW_COMPONENTS 5
+#define ROMSTAGE_INDEX 2
+
+struct component_entry {
+	uint32_t offset;
+	uint32_t size;
+} __attribute__((packed));
+
+struct components {
+	uint32_t num_components;
+	struct component_entry entries[MAX_PARSED_FW_COMPONENTS];
+} __attribute__((packed));
+
+struct vboot_region {
+	uintptr_t offset_addr;
+	int32_t size;
+};
+
+/* exports */
+
+void vb2ex_printf(const char *func, const char *fmt, ...)
+{
+	va_list args;
+
+	printk(BIOS_INFO, "VB2:%s() ", func);
+	va_start(args, fmt);
+	printk(BIOS_INFO, fmt, args);
+	va_end(args);
+
+	return;
+}
+
+int vb2ex_tpm_clear_owner(struct vb2_context *ctx)
+{
+	VBDEBUG("Clearing owner\n");
+	return VB2_ERROR_UNKNOWN;
+}
+
+int vb2ex_read_resource(struct vb2_context *ctx,
+			enum vb2_resource_index index,
+			uint32_t offset,
+			void *buf,
+			uint32_t size)
+{
+	VBDEBUG("Reading resource\n");
+	return VB2_ERROR_UNKNOWN;
+}
+
+/* locals */
+
+static void locate_region(const char *name, struct vboot_region *region)
+{
+	region->size = find_fmap_entry(name, (void **)&region->offset_addr);
+	VBDEBUG("Located %s @%x\n", name, region->offset_addr);
+}
+
+static int is_slot_a(struct vb2_context *ctx)
+{
+	return !(ctx->flags & VB2_CONTEXT_FW_SLOT_B);
+}
+
+static int in_ro(void)
+{
+	/* TODO: Implement */
+	return 1;
+}
+
+static void reboot(void)
+{
+	cpu_reset();
+}
+
+static void recovery(void)
+{
+	void *entry;
+
+	if (!in_ro())
+		reboot();
+
+	entry = cbfs_load_stage(CBFS_DEFAULT_MEDIA, "fallback/romstage");
+	if (entry != (void *)-1)
+		stage_exit(entry);
+
+	for(;;);
+}
+
+static int hash_body(struct vb2_context *ctx, struct vboot_region *fw_main)
+{
+	uint32_t expected_size;
+	uint8_t block[TODO_BLOCK_SIZE];
+	size_t block_size = sizeof(block);
+	uintptr_t offset;
+	int rv;
+
+	expected_size = fw_main->size;
+	offset= fw_main->offset_addr;
+
+	/* Start the body hash */
+	rv = vb2api_init_hash(ctx, VB2_HASH_TAG_FW_BODY, &expected_size);
+	if (rv) {
+		return rv;
+	}
+
+	/* Extend over the body */
+	while (expected_size) {
+		void *b;
+		if (block_size > expected_size)
+			block_size = expected_size;
+
+		b = vboot_get_region(offset, block_size, block);
+		if (b == NULL)
+			return VB2_ERROR_UNKNOWN;
+		rv = vb2api_extend_hash(ctx, b, block_size);
+		if (rv)
+			return rv;
+
+		expected_size -= block_size;
+		offset+= block_size;
+	}
+
+	/* Check the result */
+	rv = vb2api_check_hash(ctx);
+	if (rv) {
+		return rv;
+	}
+
+	return VB2_SUCCESS;
+}
+
+static int locate_fw_components(struct vb2_context *ctx,
+                                 struct vboot_region *fw_main,
+                                 struct components *fw_info)
+{
+	if (is_slot_a(ctx))
+		locate_region("FW_MAIN_A", fw_main);
+	else
+		locate_region("FW_MAIN_B", fw_main);
+	if (fw_main->size < 0)
+		return 1;
+
+	if (vboot_get_region(fw_main->offset_addr,
+	                     sizeof(*fw_info), fw_info) == NULL)
+		return 1;
+	return 0;
+}
+
+static struct cbfs_stage *load_stage(struct vb2_context *ctx,
+                                     int stage_index,
+                                     struct vboot_region *fw_main,
+                                     struct components *fw_info)
+{
+	struct cbfs_stage *stage;
+	uint32_t fc_addr;
+	uint32_t fc_size;
+
+	/* Check for invalid address. */
+	fc_addr = fw_main->offset_addr + fw_info->entries[stage_index].offset;
+	fc_size = fw_info->entries[stage_index].size;
+	if (fc_addr == 0 || fc_size == 0) {
+		VBDEBUG("romstage address invalid.\n");
+		return NULL;
+	}
+
+	/* Loading to cbfs cache. This stage data must be retained until it's
+	 * decompressed. */
+	stage = vboot_get_region(fc_addr, fc_size, NULL);
+
+	if (stage == NULL) {
+		VBDEBUG("Unable to load a stage.\n");
+		return NULL;
+	}
+
+	return stage;
+}
+
+static void enter_stage(struct cbfs_stage *stage)
+{
+	/* Stages rely the below clearing so that the bss is initialized. */
+	memset((void *) (uintptr_t)stage->load, 0, stage->memlen);
+
+	if (cbfs_decompress(stage->compression,
+	                    (unsigned char *)stage + sizeof(*stage),
+	                    (void *) (uintptr_t) stage->load,
+	                    stage->len))
+		return;
+
+	VBDEBUG("Jumping to entry @%llx.\n", stage->entry);
+	stage_exit((void *)(uintptr_t)stage->entry);
+}
+
+/**
+ * Save non-volatile and/or secure data if needed.
+ */
+static void save_if_needed(struct vb2_context *ctx)
+{
+	if (ctx->flags & VB2_CONTEXT_NVDATA_CHANGED) {
+		VBDEBUG("Saving nvdata\n");
+		//save_vbnv(ctx->nvdata);
+		ctx->flags &= ~VB2_CONTEXT_NVDATA_CHANGED;
+	}
+	if (ctx->flags & VB2_CONTEXT_SECDATA_CHANGED) {
+		VBDEBUG("Saving secdata\n");
+		//antirollback_write_space_firmware(ctx);
+		ctx->flags &= ~VB2_CONTEXT_SECDATA_CHANGED;
+	}
+}
+
+void __attribute__((noinline)) select_firmware(void)
+{
+	struct vb2_context ctx;
+	uint8_t *workbuf = (uint8_t *)CONFIG_VBOOT_WORK_BUFFER_ADDRESS;
+	struct vboot_region fw_main;
+	struct components fw_info;
+	struct cbfs_stage *stage;
+	int rv;
+
+	console_init();
+
+	/* Set up context */
+	memset(&ctx, 0, sizeof(ctx));
+	ctx.workbuf = workbuf;
+	ctx.workbuf_size = CONFIG_VBOOT_WORK_BUFFER_SIZE;
+	memset(ctx.workbuf, 0, ctx.workbuf_size);
+
+	/* Read nvdata from a non-volatile storage */
+	//read_vbnv(ctx.nvdata);
+
+	/* Read secdata from TPM. Initialize TPM if secdata not found. We don't
+	 * check the return value here because vb2api_fw_phase1 will catch
+	 * invalid secdata and tell us what to do (=reboot). */
+	//antirollback_read_space_firmware(&ctx);
+
+	//if (get_developer_mode_switch())
+	//	ctx.flags |= VB2_CONTEXT_FORCE_DEVELOPER_MODE;
+	//if (get_recovery_mode_switch()) {
+	//	clear_recovery_mode_switch();
+	//	ctx.flags |= VB2_CONTEXT_FORCE_RECOVERY_MODE;
+	//}
+
+	/* Do early init */
+	VBDEBUG("Phase 1\n");
+	rv = vb2api_fw_phase1(&ctx);
+	if (rv) {
+		VBDEBUG("Recovery requested (%x)\n", rv);
+		/* If we need recovery mode, leave firmware selection now */
+		save_if_needed(&ctx);
+		recovery();
+	}
+
+	/* Determine which firmware slot to boot */
+	VBDEBUG("Phase 2\n");
+	rv = vb2api_fw_phase2(&ctx);
+	if (rv) {
+		VBDEBUG("Reboot requested (%x)\n", rv);
+		save_if_needed(&ctx);
+		reboot();
+	}
+
+	/* Try that slot */
+	VBDEBUG("Phase 3\n");
+	rv = vb2api_fw_phase3(&ctx);
+	if (rv) {
+		VBDEBUG("Reboot requested (%x)\n", rv);
+		save_if_needed(&ctx);
+		reboot();
+	}
+
+	VBDEBUG("Phase 4\n");
+	rv = locate_fw_components(&ctx, &fw_main, &fw_info);
+	if (rv) {
+		VBDEBUG("Failed to locate firmware components\n");
+		reboot();
+	}
+	rv = hash_body(&ctx, &fw_main);
+	stage = load_stage(&ctx, ROMSTAGE_INDEX, &fw_main, &fw_info);
+	if (stage == NULL) {
+		VBDEBUG("Failed to load stage\n");
+		reboot();
+	}
+	save_if_needed(&ctx);
+	if (rv) {
+		VBDEBUG("Reboot requested (%x)\n", rv);
+		reboot();
+	}
+
+	/* TODO: Do we need to lock secdata? */
+	VBDEBUG("Locking TPM\n");
+
+	/* Load next stage and jump to it */
+	VBDEBUG("Jumping to rw-romstage @%llx\n", stage->entry);
+	enter_stage(stage);
+
+	/* Shouldn't reach here */
+	VBDEBUG("Halting\n");
+	for(;;);
+}