Developer firmware template and refactor common codes

This commit defines a template of developer firmware, and factors out
common codes that will be used between normal and developer firmware.

BUG=chromium-os:12191
TEST=manual

1. CROSS_COMPILE=armv7a-cros-linux-gnueabi- ./MAKEALL chromeos
2. Run normal and recovery boot flow on kaen

Change-Id: Ib3db0aaa09a5ca964d521af6323ab4366fb7c9bd
Reviewed-on: http://gerrit.chromium.org/gerrit/586
Reviewed-by: Che-Liang Chiou <clchiou@chromium.org>
Tested-by: Che-Liang Chiou <clchiou@chromium.org>

12 files changed, 525 insertions, 356 deletions

diff --git a/common/Makefile b/common/Makefile
index e4c989c4c2..bff84c97b5 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -76,6 +76,7 @@ COBJS-$(CONFIG_CMD_CONSOLE) += cmd_console.o
 COBJS-$(CONFIG_CMD_CPLBINFO) += cmd_cplbinfo.o
 COBJS-$(CONFIG_CMD_CROS) += cmd_cros.o
 COBJS-$(CONFIG_CMD_CROS_BOOTSTUB) += cmd_cros_bootstub.o
+COBJS-$(CONFIG_CMD_CROS_DEVELOPER_FIRMWARE) += cmd_cros_developer_firmware.o
 COBJS-$(CONFIG_CMD_CROS_NORMAL_FIRMWARE) += cmd_cros_normal_firmware.o
 COBJS-$(CONFIG_CMD_CROS_REC) += cmd_cros_rec.o
 COBJS-$(CONFIG_CMD_CROS_TPM) += cmd_vboot.o
diff --git a/common/cmd_cros.c b/common/cmd_cros.c
index 6deab3171b..71bcb7de78 100644
--- a/common/cmd_cros.c
+++ b/common/cmd_cros.c
@@ -19,6 +19,7 @@
 #include <chromeos/gbb_bmpblk.h>
 #include <chromeos/gpio.h>
 #include <chromeos/load_firmware_helper.h>
+#include <chromeos/load_kernel_helper.h>
 #include <chromeos/vboot_nvstorage_helper.h>
 #include <chromeos/os_storage.h>
 
diff --git a/common/cmd_cros_developer_firmware.c b/common/cmd_cros_developer_firmware.c
new file mode 100644
index 0000000000..3d044905a8
--- /dev/null
+++ b/common/cmd_cros_developer_firmware.c
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2011 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ */
+
+/* Implementation of developer firmware of Chrome OS Verify Boot */
+
+#include <common.h>
+#include <command.h>
+#include <mmc.h>
+#include <chromeos/firmware_storage.h>
+#include <chromeos/gbb_bmpblk.h>
+#include <chromeos/load_firmware_helper.h>
+#include <chromeos/load_kernel_helper.h>
+#include <chromeos/os_storage.h>
+#include <chromeos/vboot_nvstorage_helper.h>
+
+#include <bmpblk_header.h>
+
+#define PREFIX "cros_developer_firmware: "
+
+int do_cros_normal_firmware(cmd_tbl_t *cmdtp, int flag, int argc,
+		char * const argv[]);
+
+static void beep(void)
+{
+	/* TODO: implement beep */
+	debug(PREFIX "beep\n");
+}
+
+static int is_ctrlu(int c)
+{
+	return 0;
+}
+
+static int is_ctrld(int c)
+{
+	return 0;
+}
+
+static int is_escape(int c)
+{
+	return 0;
+}
+
+static int is_space(int c)
+{
+	return 0;
+}
+
+static int is_enter(int c)
+{
+	return 0;
+}
+
+int do_cros_developer_firmware(cmd_tbl_t *cmdtp, int flag, int argc,
+		char * const argv[])
+{
+	firmware_storage_t file;
+	void *gbb_data = NULL;
+	uint64_t gbb_size = 0;
+	ulong start = 0, time = 0;
+	int c, is_after_20_seconds = 0;
+
+	if (firmware_storage_init(&file) ||
+			load_gbb(&file, &gbb_data, &gbb_size)) {
+		/*
+		 * FIXME: We can't read gbb and so can't show a face on screen;
+		 * how should we do when this happen? Trap in infinite loop?
+		 */
+		debug(PREFIX "cannot load gbb\n");
+		while (1);
+	}
+
+	/* we don't care whether close operation fails */
+	file.close(file.context);
+
+	if (display_screen_in_bmpblk(gbb_data, SCREEN_DEVELOPER_MODE)) {
+		debug(PREFIX "cannot display stuff on LCD screen\n");
+	}
+
+	start = get_timer(0);
+	while (1) {
+		time = get_timer(start);
+
+		/* Beep twice when time > 20 seconds */
+		if (!is_after_20_seconds && time > 20 * CONFIG_SYS_HZ) {
+			beep(); udelay(500); beep();
+			is_after_20_seconds = 1;
+			continue;
+		}
+
+		/* Fall back to normal firmware when time > 30 seconds */
+		if (time > 30 * CONFIG_SYS_HZ)
+			break;
+
+		c = getc();
+		debug(PREFIX "getc() == 0x%x\n", c);
+
+		if (is_ctrlu(c)) {
+			/* TODO: load and boot kernel from USB or SD card */
+		}
+
+		/* Fall back to normal firmware when user pressed Ctrl-D */
+		if (is_ctrld(c))
+			break;
+
+		if (is_space(c) || is_enter(c) || is_escape(c))
+			reboot_to_recovery_mode(NULL,
+					VBNV_RECOVERY_RW_DEV_SCREEN);
+
+		udelay(100);
+	}
+
+	return do_cros_normal_firmware(NULL, 0, 0, NULL);
+}
+
+U_BOOT_CMD(cros_developer_firmware, 1, 1, do_cros_developer_firmware,
+		"verified boot developer firmware", NULL);
diff --git a/common/cmd_cros_normal_firmware.c b/common/cmd_cros_normal_firmware.c
index eb51aedf2a..392faeecff 100644
--- a/common/cmd_cros_normal_firmware.c
+++ b/common/cmd_cros_normal_firmware.c
@@ -18,6 +18,7 @@
 #include <chromeos/firmware_storage.h>
 #include <chromeos/gpio.h>
 #include <chromeos/load_firmware_helper.h>
+#include <chromeos/load_kernel_helper.h>
 #include <chromeos/os_storage.h>
 #include <chromeos/vboot_nvstorage_helper.h>
 
@@ -29,12 +30,6 @@
 
 #define PREFIX "cros_normal_firmware: "
 
-/* defined in common/cmd_source.c */
-int source(ulong addr, const char *fit_uname);
-
-/* defined in common/cmd_bootm.c */
-int do_bootm(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]);
-
 #define DEVICE_TYPE		"mmc"
 #define DEVICE_NAME		"mmcblk" DEVICE_NUMBER_STRING "p"
 #define DEVICE_NUMBER_STRING	"0"
@@ -67,57 +62,6 @@ int initialize_drive(void)
 	return 0;
 }
 
-/* This function should never return */
-static void reboot_to_recovery_mode(VbNvContext *nvcxt, uint32_t reason)
-{
-	debug(PREFIX "store recovery cookie in recovery field\n");
-	if (VbNvSet(nvcxt, VBNV_RECOVERY_REQUEST, reason) ||
-			VbNvTeardown(nvcxt) ||
-			(nvcxt->raw_changed && write_nvcontext(nvcxt))) {
-		/* FIXME: bring up a sad face? */
-		debug(PREFIX "error: cannot write recovery cookie");
-		printf("Please reset and press recovery button when reboot.\n");
-		while (1);
-	}
-
-	debug(PREFIX "reboot to recovery mode\n");
-	reset_cpu(0);
-
-	debug(PREFIX "error: reset_cpu() returned\n");
-	while (1);
-}
-
-/* This function should never return */
-void boot_kernel(LoadKernelParams *params)
-{
-	char load_address[32];
-	char *argv[2] = { "bootm", load_address };
-
-	debug(PREFIX "boot_kernel\n");
-	debug(PREFIX "kernel_buffer:      0x%p\n",
-			params->kernel_buffer);
-	debug(PREFIX "bootloader_address: 0x%08x\n",
-			(int) params->bootloader_address);
-
-	if (load_kernel_config(params->bootloader_address)) {
-		debug(PREFIX "error: load kernel config failed\n");
-		return;
-	}
-
-	/*
-	 * FIXME: So far bootloader in kernel partition isn't really a
-	 * bootloader; instead, it is merely a u-boot scripts that sets kernel
-	 * parameters. And therefore we still have to boot kernel to here
-	 * by calling do_bootm.
-	 */
-	sprintf(load_address, "0x%p", params->kernel_buffer);
-	debug(PREFIX "run command: %s %s\n", argv[0], argv[1]);
-	do_bootm(NULL, 0, sizeof(argv)/sizeof(*argv), argv);
-
-	debug(PREFIX "error: do_bootm() returned\n");
-	while (1);
-}
-
 int do_cros_normal_firmware(cmd_tbl_t *cmdtp, int flag, int argc,
 		char * const argv[])
 {
diff --git a/common/cmd_cros_rec.c b/common/cmd_cros_rec.c
index 0da55d3c77..f05819491d 100644
--- a/common/cmd_cros_rec.c
+++ b/common/cmd_cros_rec.c
@@ -19,6 +19,7 @@
 #include <usb.h>
 #include <chromeos/firmware_storage.h>
 #include <chromeos/load_firmware_helper.h>
+#include <chromeos/load_kernel_helper.h>
 #include <chromeos/gbb_bmpblk.h>
 #include <chromeos/gpio.h>
 #include <chromeos/os_storage.h>
@@ -93,14 +94,6 @@ static int write_log(void)
 	return 0;
 }
 
-static uint32_t get_cur_stack_addr(void)
-{
-	uint32_t local_var;
-	uint32_t addr = (uint32_t)&local_var;
-
-	return addr;
-}
-
 /*
  * Initializes the memory region that needs to be cleared.
  */
@@ -203,8 +196,6 @@ static int test_clear_mem_regions(void)
  */
 static void clear_ram_not_in_use(void)
 {
-	uint32_t stack_top = get_cur_stack_addr();
-
 	init_mem_region(0, PHYS_SDRAM_1_SIZE);
 
 	/* Excludes the firmware text + data + bss regions. */
diff --git a/include/chromeos/load_kernel_helper.h b/include/chromeos/load_kernel_helper.h
new file mode 100644
index 0000000000..794d7c5572
--- /dev/null
+++ b/include/chromeos/load_kernel_helper.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2011 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ */
+
+#ifndef CHROMEOS_LOAD_KERNEL_HELPER_H_
+#define CHROMEOS_LOAD_KERNEL_HELPER_H_
+
+#include <linux/types.h>
+
+#include <load_kernel_fw.h>
+#include <vboot_nvstorage.h>
+
+/*
+ * Given the bootloader_addres, load the kernel config as bootargs.
+ * Return zero if success and non-zero if error.
+ */
+int load_kernel_config(uint64_t bootloader_address);
+
+/*
+ * Wrapper of LoadKernel() function. Returns the return value of LoadKernel().
+ *
+ * See vboot_reference/firmware/include/load_kernel_fw.h for documentation.
+ *
+ * If <shared_data_blob> is NULL, it will use a system default location.
+ */
+int load_kernel_wrapper(LoadKernelParams *params,
+		void *gbb_data, uint64_t gbb_size, uint64_t boot_flags,
+		VbNvContext *nvcxt, uint8_t *shared_data_blob);
+
+/*
+ * Actual wrapper implementation. Most callers invoke it with
+ * 'bypass_load_kernel' set to False. If it is set to True - the shared memory
+ * is intialized, but the actual kernel load function is not invoked. This
+ * facilitates debugging when the kernel is loaded by some other means (for
+ * instance netbooted)
+ */
+int load_kernel_wrapper_core(LoadKernelParams *params, void *gbb_data,
+			     uint64_t gbb_size, uint64_t boot_flags,
+			     VbNvContext *nvcxt, uint8_t *shared_data_blob,
+			     int bypass_load_kernel);
+
+/*
+ * Boot kernel specified at the address in <params>. This function never
+ * returns.
+ */
+void boot_kernel(LoadKernelParams *params);
+
+#endif /* CHROMEOS_LOAD_KERNEL_HELPER_H_ */
diff --git a/include/chromeos/os_storage.h b/include/chromeos/os_storage.h
index 46f4e6e88d..a9f5fa2674 100644
--- a/include/chromeos/os_storage.h
+++ b/include/chromeos/os_storage.h
@@ -8,15 +8,12 @@
  * Software Foundation.
  */
 
-#ifndef __CHROMEOS_BOOT_DEVICE_IMPL_H__
-#define __CHROMEOS_BOOT_DEVICE_IMPL_H__
+#ifndef CHROMEOS_OS_STORAGE_H_
+#define CHROMEOS_OS_STORAGE_H_
 
 #include <linux/types.h>
 #include <part.h>
 
-#include <load_kernel_fw.h>
-#include <vboot_nvstorage.h>
-
 /* Set boot device.
  *
  * Set partition number in argument part (starting from 1).  Pass part=0 for
@@ -37,33 +34,4 @@ ulong get_limit(void);
 uint64_t get_bytes_per_lba(void);
 uint64_t get_ending_lba(void);
 
-/*
- * Given the bootloader_addres, load the kernel config as bootargs.
- * Return zero if success and non-zero if error.
- */
-int load_kernel_config(uint64_t bootloader_address);
-
-/*
- * Wrapper of LoadKernel() function. Returns the return value of LoadKernel().
- *
- * See vboot_reference/firmware/include/load_kernel_fw.h for documentation.
- *
- * If <shared_data_blob> is NULL, it will use a system default location.
- */
-int load_kernel_wrapper(LoadKernelParams *params,
-		void *gbb_data, uint64_t gbb_size, uint64_t boot_flags,
-		VbNvContext *nvcxt, uint8_t *shared_data_blob);
-
-/*
- * Actual wrapper implementation. Most callers invoke it with
- * 'bypass_load_kernel' set to False. If it is set to True - the shared memory
- * is intialized, but the actual kernel load function is not invoked. This
- * facilitates debugging when the kernel is loaded by some other means (for
- * instance netbooted)
- */
-int load_kernel_wrapper_core(LoadKernelParams *params, void *gbb_data,
-			     uint64_t gbb_size, uint64_t boot_flags,
-			     VbNvContext *nvcxt, uint8_t *shared_data_blob,
-			     int bypass_load_kernel);
-
-#endif /* __CHROMEOS_BOOT_DEVICE_IMPL_H__ */
+#endif /* CHROMEOS_OS_STORAGE_H_ */
diff --git a/include/chromeos/vboot_nvstorage_helper.h b/include/chromeos/vboot_nvstorage_helper.h
index 4fe29f4f1a..f897feaa8a 100644
--- a/include/chromeos/vboot_nvstorage_helper.h
+++ b/include/chromeos/vboot_nvstorage_helper.h
@@ -23,14 +23,21 @@
  *
  * See vboot_reference/firmware/include/vboot_nvstorage.h
  */
-int read_nvcontext(VbNvContext *vnc);
-int write_nvcontext(VbNvContext *vnc);
+int read_nvcontext(VbNvContext *nvcxt);
+int write_nvcontext(VbNvContext *nvcxt);
 
 /*
- * Clear the recovery request in the non-volatile storage.
+ * Set the recovery request in the non-volatile storage.
  *
  * Return zero if success, non-zero if fail.
  */
 int clear_recovery_request(void);
 
+/*
+ * Set the recovery request to <reason> and reboot. This function never returns.
+ *
+ * If <nvcxt> is NULL, this function loads the nvcxt from non-volatile storage.
+ */
+void reboot_to_recovery_mode(VbNvContext *nvcxt, uint32_t reason);
+
 #endif /* CHROMEOS_VBOOT_NVSTORAGE_HELPER_H_ */
diff --git a/lib/chromeos/Makefile b/lib/chromeos/Makefile
index ba51df39d3..3bdf0c9a55 100644
--- a/lib/chromeos/Makefile
+++ b/lib/chromeos/Makefile
@@ -16,6 +16,7 @@ COBJS-$(CONFIG_CHROMEOS) += firmware_storage.o
 COBJS-$(CONFIG_CHROMEOS) += fmap.o
 COBJS-$(CONFIG_CHROMEOS) += get_firmware_body.o
 COBJS-$(CONFIG_CHROMEOS) += load_firmware_helper.o
+COBJS-$(CONFIG_CHROMEOS) += load_kernel_helper.o
 COBJS-$(CONFIG_CHROMEOS) += os_storage.o
 COBJS-$(CONFIG_CHROMEOS) += tlcl_stub.o
 COBJS-$(CONFIG_CHROMEOS) += utility.o
diff --git a/lib/chromeos/load_kernel_helper.c b/lib/chromeos/load_kernel_helper.c
new file mode 100644
index 0000000000..401f722b00
--- /dev/null
+++ b/lib/chromeos/load_kernel_helper.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (c) 2011 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ */
+
+#include <common.h>
+#include <part.h>
+#include <chromeos/gpio.h>
+#include <chromeos/load_kernel_helper.h>
+#include <chromeos/os_storage.h>
+
+/* TODO For load fmap; remove when not used */
+#include <chromeos/firmware_storage.h>
+
+/* TODO For strcpy; remove when not used */
+#include <linux/string.h>
+
+/* TODO remove when not used */
+extern uint64_t get_nvcxt_lba(void);
+
+/* defined in common/cmd_bootm.c */
+int do_bootm(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]);
+
+#include <load_kernel_fw.h>
+#include <vboot_nvstorage.h>
+#include <vboot_struct.h>
+
+/* This is used to keep u-boot and kernel in sync */
+#define SHARED_MEM_VERSION 1
+
+#undef PREFIX
+#define PREFIX "load_kernel_wrapper: "
+
+int load_kernel_wrapper_core(LoadKernelParams *params,
+			     void *gbb_data, uint64_t gbb_size,
+			     uint64_t boot_flags, VbNvContext *nvcxt,
+			     uint8_t *shared_data_blob,
+			     int bypass_load_kernel)
+{
+	/*
+	 * TODO(clchiou): Hack for bringing up factory; preserve recovery
+	 * reason before LoadKernel destroys it. Remove when not needed.
+	 */
+	uint32_t reason = 0;
+	VbNvGet(nvcxt, VBNV_RECOVERY_REQUEST, &reason);
+
+	int status = LOAD_KERNEL_NOT_FOUND;
+	block_dev_desc_t *dev_desc;
+
+	memset(params, '\0', sizeof(*params));
+
+	if (!bypass_load_kernel) {
+		dev_desc = get_bootdev();
+		if (!dev_desc) {
+			debug(PREFIX "get_bootdev fail\n");
+			goto EXIT;
+		}
+	}
+
+	params->gbb_data = gbb_data;
+	params->gbb_size = gbb_size;
+
+	params->boot_flags = boot_flags;
+	params->shared_data_blob = shared_data_blob ? shared_data_blob :
+			(uint8_t *) CONFIG_VB_SHARED_DATA_BLOB;
+	params->shared_data_size = CONFIG_VB_SHARED_DATA_SIZE;
+
+	params->bytes_per_lba = get_bytes_per_lba();
+	params->ending_lba = get_ending_lba();
+
+	params->kernel_buffer = (uint8_t *) CONFIG_LOADADDR;
+	params->kernel_buffer_size = CONFIG_MAX_KERNEL_SIZE;
+
+	params->nv_context = nvcxt;
+
+	debug(PREFIX "call LoadKernel() with parameters...\n");
+	debug(PREFIX "shared_data_blob:     0x%p\n",
+			params->shared_data_blob);
+	debug(PREFIX "bytes_per_lba:        %d\n",
+			(int) params->bytes_per_lba);
+	debug(PREFIX "ending_lba:           0x%08x\n",
+			(int) params->ending_lba);
+	debug(PREFIX "kernel_buffer:        0x%p\n",
+			params->kernel_buffer);
+	debug(PREFIX "kernel_buffer_size:   0x%08x\n",
+			(int) params->kernel_buffer_size);
+	debug(PREFIX "boot_flags:           0x%08x\n",
+			(int) params->boot_flags);
+
+	if (!bypass_load_kernel) {
+		status = LoadKernel(params);
+	} else {
+		status = LOAD_KERNEL_SUCCESS;
+		params->partition_number = 2;
+	}
+
+EXIT:
+	debug(PREFIX "LoadKernel status: %d\n", status);
+	if (status == LOAD_KERNEL_SUCCESS) {
+		debug(PREFIX "partition_number:   0x%08x\n",
+				(int) params->partition_number);
+		debug(PREFIX "bootloader_address: 0x%08x\n",
+				(int) params->bootloader_address);
+		debug(PREFIX "bootloader_size:    0x%08x\n",
+				(int) params->bootloader_size);
+
+		if (params->partition_number == 2) {
+			setenv("kernelpart", "2");
+			setenv("rootpart", "3");
+		} else if (params->partition_number == 4) {
+			setenv("kernelpart", "4");
+			setenv("rootpart", "5");
+		} else {
+			debug(PREFIX "unknown kernel partition: %d\n",
+					(int) params->partition_number);
+			status = LOAD_KERNEL_NOT_FOUND;
+		}
+	}
+
+	/*
+	 * TODO(clchiou): This is an urgent hack for bringing up factory. We
+	 * fill in data that will be used by kernel at last 1MB space.
+	 *
+	 * Rewrite this part after the protocol specification between
+	 * Chrome OS firmware and kernel is finalized.
+	 */
+	if (status == LOAD_KERNEL_SUCCESS) {
+		DECLARE_GLOBAL_DATA_PTR;
+
+		void *kernel_shared_data = (void*)
+			gd->bd->bi_dram[CONFIG_NR_DRAM_BANKS-1].start +
+			gd->bd->bi_dram[CONFIG_NR_DRAM_BANKS-1].size - SZ_1M;
+
+		struct {
+			uint32_t total_size;
+			uint8_t  signature[10];
+			uint16_t version;
+			uint64_t nvcxt_lba;
+			uint16_t vbnv[2];
+			uint8_t  nvcxt_cache[VBNV_BLOCK_SIZE];
+			uint8_t  write_protect_sw;
+			uint8_t  recovery_sw;
+			uint8_t  developer_sw;
+			uint8_t  binf[5];
+			uint32_t chsw;
+			uint8_t  hwid[256];
+			uint8_t  fwid[256];
+			uint8_t  frid[256];
+			uint32_t fmap_base;
+			uint8_t  shared_data_body[CONFIG_LENGTH_FMAP];
+		} __attribute__((packed)) *sd = kernel_shared_data;
+
+		int i;
+
+		debug(PREFIX "kernel shared data at %p\n", kernel_shared_data);
+
+		memset(sd, '\0', sizeof(*sd));
+
+		strcpy((char*) sd->signature, "CHROMEOS");
+		sd->version = SHARED_MEM_VERSION;
+
+		/*
+		 * chsw bit value
+		 *   bit 0x00000002 : recovery button pressed
+		 *   bit 0x00000020 : developer mode enabled
+		 *   bit 0x00000200 : firmware write protect disabled
+		 */
+		if (params->boot_flags & BOOT_FLAG_RECOVERY)
+			sd->chsw |= 0x002;
+		if (params->boot_flags & BOOT_FLAG_DEVELOPER)
+			sd->chsw |= 0x020;
+		sd->chsw |= 0x200; /* so far write protect is disabled */
+
+		strcpy((char*) sd->hwid, CONFIG_CHROMEOS_HWID);
+		strcpy((char*) sd->fwid, "ARM Firmware ID");
+		strcpy((char*) sd->frid, "ARM Read-Only Firmware ID");
+
+		sd->binf[0] = 0; /* boot reason; always 0 */
+		if (params->boot_flags & BOOT_FLAG_RECOVERY) {
+			sd->binf[1] = 0; /* active main firmware */
+			sd->binf[3] = 0; /* active firmware type */
+		} else {
+			sd->binf[1] = 1; /* active main firmware */
+			sd->binf[3] = 1; /* active firmware type */
+		}
+		sd->binf[2] = 0; /* active EC firmware */
+		sd->binf[4] = reason;
+
+		sd->write_protect_sw =
+			is_firmware_write_protect_gpio_asserted();
+		sd->recovery_sw = is_recovery_mode_gpio_asserted();
+		sd->developer_sw = is_developer_mode_gpio_asserted();
+
+		sd->vbnv[0] = 0;
+		sd->vbnv[1] = VBNV_BLOCK_SIZE;
+
+		firmware_storage_t file;
+		firmware_storage_init(&file);
+		firmware_storage_read(&file,
+				CONFIG_OFFSET_FMAP, CONFIG_LENGTH_FMAP,
+				sd->shared_data_body);
+		file.close(file.context);
+		sd->fmap_base = (uint32_t)sd->shared_data_body;
+
+		sd->total_size = sizeof(*sd);
+
+		sd->nvcxt_lba = get_nvcxt_lba();
+
+		memcpy(sd->nvcxt_cache,
+				params->nv_context->raw, VBNV_BLOCK_SIZE);
+
+		debug(PREFIX "chsw     %08x\n", sd->chsw);
+		for (i = 0; i < 5; i++)
+			debug(PREFIX "binf[%2d] %08x\n", i, sd->binf[i]);
+		debug(PREFIX "vbnv[ 0] %08x\n", sd->vbnv[0]);
+		debug(PREFIX "vbnv[ 1] %08x\n", sd->vbnv[1]);
+		debug(PREFIX "fmap     %08llx\n", sd->fmap_start_address);
+		debug(PREFIX "nvcxt    %08llx\n", sd->nvcxt_lba);
+		debug(PREFIX "nvcxt_c  ");
+		for (i = 0; i < VBNV_BLOCK_SIZE; i++)
+			debug("%02x", sd->nvcxt_cache[i]);
+		putc('\n');
+	}
+
+	return status;
+}
+
+/* Maximum kernel command-line size */
+#define CROS_CONFIG_SIZE 4096
+
+/* Size of the x86 zeropage table */
+#define CROS_PARAMS_SIZE 4096
+
+int load_kernel_config(uint64_t bootloader_address)
+{
+	char buf[80 + CROS_CONFIG_SIZE];
+
+	strcpy(buf, "setenv bootargs ${bootargs} ");
+
+	/* Use the bootloader address to find the kernel config location. */
+	strcat(buf, (char *)(bootloader_address - CROS_PARAMS_SIZE -
+			CROS_CONFIG_SIZE));
+
+	/*
+	 * Use run_command instead of setenv because we need variable
+	 * substitutions.
+	 * TODO: Do more variable substitutions for the bug:
+	 * http://crosbug.com/14022
+	 */
+	if (run_command(buf, 0)) {
+		debug(PREFIX "run_command(%s) fail\n", buf);
+		return 1;
+	}
+	return 0;
+}
+
+int load_kernel_wrapper(LoadKernelParams *params,
+			void *gbb_data, uint64_t gbb_size,
+			uint64_t boot_flags, VbNvContext *nvcxt,
+			uint8_t *shared_data_blob)
+{
+	return load_kernel_wrapper_core(params, gbb_data, gbb_size, boot_flags,
+					nvcxt, shared_data_blob, 0);
+}
+
+void boot_kernel(LoadKernelParams *params)
+{
+	char load_address[32];
+	char *argv[2] = { "bootm", load_address };
+
+	debug(PREFIX "boot_kernel\n");
+	debug(PREFIX "kernel_buffer:      0x%p\n",
+			params->kernel_buffer);
+	debug(PREFIX "bootloader_address: 0x%08x\n",
+			(int) params->bootloader_address);
+
+	if (load_kernel_config(params->bootloader_address)) {
+		debug(PREFIX "error: load kernel config failed\n");
+		return;
+	}
+
+	/*
+	 * FIXME: So far bootloader in kernel partition isn't really a
+	 * bootloader; instead, it is merely a u-boot scripts that sets kernel
+	 * parameters. And therefore we still have to boot kernel to here
+	 * by calling do_bootm.
+	 */
+	sprintf(load_address, "0x%p", params->kernel_buffer);
+	debug(PREFIX "run command: %s %s\n", argv[0], argv[1]);
+	do_bootm(NULL, 0, sizeof(argv)/sizeof(*argv), argv);
+
+	debug(PREFIX "error: do_bootm() returned\n");
+	while (1);
+}
diff --git a/lib/chromeos/os_storage.c b/lib/chromeos/os_storage.c
index e02d910663..c07daf6a6c 100644
--- a/lib/chromeos/os_storage.c
+++ b/lib/chromeos/os_storage.c
@@ -11,30 +11,14 @@
 #include <common.h>
 #include <malloc.h>
 #include <part.h>
-#include <chromeos/gpio.h>
 #include <chromeos/os_storage.h>
 
-/* TODO For load fmap; remove when not used */
-#include <chromeos/firmware_storage.h>
-
-/* TODO For strcpy; remove when not used */
-#include <linux/string.h>
-
-/* TODO remove when not used */
-extern uint64_t get_nvcxt_lba(void);
-
 #include <boot_device.h>
-#include <load_kernel_fw.h>
-#include <vboot_nvstorage.h>
-#include <vboot_struct.h>
 
 #define PREFIX "boot_device: "
 
 #define BACKUP_LBA_OFFSET 0x20
 
-/* This is used to keep u-boot and kernel in sync */
-#define SHARED_MEM_VERSION 1
-
 static struct {
 	block_dev_desc_t *dev_desc;
 	ulong offset, limit;
@@ -165,238 +149,3 @@ int BootDeviceWriteLBA(uint64_t lba_start, uint64_t lba_count,
 
 	return 0;
 }
-
-#undef PREFIX
-#define PREFIX "load_kernel_wrapper: "
-
-int load_kernel_wrapper_core(LoadKernelParams *params,
-			     void *gbb_data, uint64_t gbb_size,
-			     uint64_t boot_flags, VbNvContext *nvcxt,
-			     uint8_t *shared_data_blob,
-			     int bypass_load_kernel)
-{
-	/*
-	 * TODO(clchiou): Hack for bringing up factory; preserve recovery
-	 * reason before LoadKernel destroys it. Remove when not needed.
-	 */
-	uint32_t reason = 0;
-	VbNvGet(nvcxt, VBNV_RECOVERY_REQUEST, &reason);
-
-	int status = LOAD_KERNEL_NOT_FOUND;
-	block_dev_desc_t *dev_desc;
-
-	memset(params, '\0', sizeof(*params));
-
-	if (!bypass_load_kernel) {
-		dev_desc = get_bootdev();
-		if (!dev_desc) {
-			debug(PREFIX "get_bootdev fail\n");
-			goto EXIT;
-		}
-	}
-
-	params->gbb_data = gbb_data;
-	params->gbb_size = gbb_size;
-
-	params->boot_flags = boot_flags;
-	params->shared_data_blob = shared_data_blob ? shared_data_blob :
-			(uint8_t *) CONFIG_VB_SHARED_DATA_BLOB;
-	params->shared_data_size = CONFIG_VB_SHARED_DATA_SIZE;
-
-	params->bytes_per_lba = get_bytes_per_lba();
-	params->ending_lba = get_ending_lba();
-
-	params->kernel_buffer = (uint8_t *) CONFIG_LOADADDR;
-	params->kernel_buffer_size = CONFIG_MAX_KERNEL_SIZE;
-
-	params->nv_context = nvcxt;
-
-	debug(PREFIX "call LoadKernel() with parameters...\n");
-	debug(PREFIX "shared_data_blob:     0x%p\n",
-			params->shared_data_blob);
-	debug(PREFIX "bytes_per_lba:        %d\n",
-			(int) params->bytes_per_lba);
-	debug(PREFIX "ending_lba:           0x%08x\n",
-			(int) params->ending_lba);
-	debug(PREFIX "kernel_buffer:        0x%p\n",
-			params->kernel_buffer);
-	debug(PREFIX "kernel_buffer_size:   0x%08x\n",
-			(int) params->kernel_buffer_size);
-	debug(PREFIX "boot_flags:           0x%08x\n",
-			(int) params->boot_flags);
-
-	if (!bypass_load_kernel) {
-		status = LoadKernel(params);
-	} else {
-		status = LOAD_KERNEL_SUCCESS;
-		params->partition_number = 2;
-	}
-
-EXIT:
-	debug(PREFIX "LoadKernel status: %d\n", status);
-	if (status == LOAD_KERNEL_SUCCESS) {
-		debug(PREFIX "partition_number:   0x%08x\n",
-				(int) params->partition_number);
-		debug(PREFIX "bootloader_address: 0x%08x\n",
-				(int) params->bootloader_address);
-		debug(PREFIX "bootloader_size:    0x%08x\n",
-				(int) params->bootloader_size);
-
-		if (params->partition_number == 2) {
-			setenv("kernelpart", "2");
-			setenv("rootpart", "3");
-		} else if (params->partition_number == 4) {
-			setenv("kernelpart", "4");
-			setenv("rootpart", "5");
-		} else {
-			debug(PREFIX "unknown kernel partition: %d\n",
-					(int) params->partition_number);
-			status = LOAD_KERNEL_NOT_FOUND;
-		}
-	}
-
-	/*
-	 * TODO(clchiou): This is an urgent hack for bringing up factory. We
-	 * fill in data that will be used by kernel at last 1MB space.
-	 *
-	 * Rewrite this part after the protocol specification between
-	 * Chrome OS firmware and kernel is finalized.
-	 */
-	if (status == LOAD_KERNEL_SUCCESS) {
-		DECLARE_GLOBAL_DATA_PTR;
-
-		void *kernel_shared_data = (void*)
-			gd->bd->bi_dram[CONFIG_NR_DRAM_BANKS-1].start +
-			gd->bd->bi_dram[CONFIG_NR_DRAM_BANKS-1].size - SZ_1M;
-
-		struct {
-			uint32_t total_size;
-			uint8_t  signature[10];
-			uint16_t version;
-			uint64_t nvcxt_lba;
-			uint16_t vbnv[2];
-			uint8_t  nvcxt_cache[VBNV_BLOCK_SIZE];
-			uint8_t  write_protect_sw;
-			uint8_t  recovery_sw;
-			uint8_t  developer_sw;
-			uint8_t  binf[5];
-			uint32_t chsw;
-			uint8_t  hwid[256];
-			uint8_t  fwid[256];
-			uint8_t  frid[256];
-			uint32_t fmap_base;
-			uint8_t  shared_data_body[CONFIG_LENGTH_FMAP];
-		} __attribute__((packed)) *sd = kernel_shared_data;
-
-		int i;
-
-		debug(PREFIX "kernel shared data at %p\n", kernel_shared_data);
-
-		memset(sd, '\0', sizeof(*sd));
-
-		strcpy((char*) sd->signature, "CHROMEOS");
-		sd->version = SHARED_MEM_VERSION;
-
-		/*
-		 * chsw bit value
-		 *   bit 0x00000002 : recovery button pressed
-		 *   bit 0x00000020 : developer mode enabled
-		 *   bit 0x00000200 : firmware write protect disabled
-		 */
-		if (params->boot_flags & BOOT_FLAG_RECOVERY)
-			sd->chsw |= 0x002;
-		if (params->boot_flags & BOOT_FLAG_DEVELOPER)
-			sd->chsw |= 0x020;
-		sd->chsw |= 0x200; /* so far write protect is disabled */
-
-		strcpy((char*) sd->hwid, CONFIG_CHROMEOS_HWID);
-		strcpy((char*) sd->fwid, "ARM Firmware ID");
-		strcpy((char*) sd->frid, "ARM Read-Only Firmware ID");
-
-		sd->binf[0] = 0; /* boot reason; always 0 */
-		if (params->boot_flags & BOOT_FLAG_RECOVERY) {
-			sd->binf[1] = 0; /* active main firmware */
-			sd->binf[3] = 0; /* active firmware type */
-		} else {
-			sd->binf[1] = 1; /* active main firmware */
-			sd->binf[3] = 1; /* active firmware type */
-		}
-		sd->binf[2] = 0; /* active EC firmware */
-		sd->binf[4] = reason;
-
-		sd->write_protect_sw =
-			is_firmware_write_protect_gpio_asserted();
-		sd->recovery_sw = is_recovery_mode_gpio_asserted();
-		sd->developer_sw = is_developer_mode_gpio_asserted();
-
-		sd->vbnv[0] = 0;
-		sd->vbnv[1] = VBNV_BLOCK_SIZE;
-
-		firmware_storage_t file;
-		firmware_storage_init(&file);
-		firmware_storage_read(&file,
-				CONFIG_OFFSET_FMAP, CONFIG_LENGTH_FMAP,
-				sd->shared_data_body);
-		file.close(file.context);
-		sd->fmap_base = (uint32_t)sd->shared_data_body;
-
-		sd->total_size = sizeof(*sd);
-
-		sd->nvcxt_lba = get_nvcxt_lba();
-
-		memcpy(sd->nvcxt_cache,
-				params->nv_context->raw, VBNV_BLOCK_SIZE);
-
-		debug(PREFIX "chsw     %08x\n", sd->chsw);
-		for (i = 0; i < 5; i++)
-			debug(PREFIX "binf[%2d] %08x\n", i, sd->binf[i]);
-		debug(PREFIX "vbnv[ 0] %08x\n", sd->vbnv[0]);
-		debug(PREFIX "vbnv[ 1] %08x\n", sd->vbnv[1]);
-		debug(PREFIX "fmap     %08llx\n", sd->fmap_start_address);
-		debug(PREFIX "nvcxt    %08llx\n", sd->nvcxt_lba);
-		debug(PREFIX "nvcxt_c  ");
-		for (i = 0; i < VBNV_BLOCK_SIZE; i++)
-			debug("%02x", sd->nvcxt_cache[i]);
-		putc('\n');
-	}
-
-	return status;
-}
-
-/* Maximum kernel command-line size */
-#define CROS_CONFIG_SIZE 4096
-
-/* Size of the x86 zeropage table */
-#define CROS_PARAMS_SIZE 4096
-
-int load_kernel_config(uint64_t bootloader_address)
-{
-	char buf[80 + CROS_CONFIG_SIZE];
-
-	strcpy(buf, "setenv bootargs ${bootargs} ");
-
-	/* Use the bootloader address to find the kernel config location. */
-	strcat(buf, (char *)(bootloader_address - CROS_PARAMS_SIZE -
-			CROS_CONFIG_SIZE));
-
-	/*
-	 * Use run_command instead of setenv because we need variable
-	 * substitutions.
-	 * TODO: Do more variable substitutions for the bug:
-	 * http://crosbug.com/14022
-	 */
-	if (run_command(buf, 0)) {
-		debug(PREFIX "run_command(%s) fail\n", buf);
-		return 1;
-	}
-	return 0;
-}
-
-int load_kernel_wrapper(LoadKernelParams *params,
-			void *gbb_data, uint64_t gbb_size,
-			uint64_t boot_flags, VbNvContext *nvcxt,
-			uint8_t *shared_data_blob)
-{
-	return load_kernel_wrapper_core(params, gbb_data, gbb_size, boot_flags,
-					nvcxt, shared_data_blob, 0);
-}
diff --git a/lib/chromeos/vboot_nvstorage_helper.c b/lib/chromeos/vboot_nvstorage_helper.c
index 35ee971a56..d73615c5d1 100644
--- a/lib/chromeos/vboot_nvstorage_helper.c
+++ b/lib/chromeos/vboot_nvstorage_helper.c
@@ -188,3 +188,33 @@ int clear_recovery_request(void)
 
 	return 0;
 }
+
+void reboot_to_recovery_mode(VbNvContext *nvcxt, uint32_t reason)
+{
+	VbNvContext nvcontext;
+
+	if (!nvcxt) {
+		nvcxt = &nvcontext;
+		if (read_nvcontext(nvcxt) || VbNvSetup(nvcxt)) {
+			debug(PREFIX "cannot read nvcxt\n");
+			goto FAIL;
+		}
+	}
+
+	debug(PREFIX "store recovery cookie in recovery field\n");
+	if (VbNvSet(nvcxt, VBNV_RECOVERY_REQUEST, reason) ||
+			VbNvTeardown(nvcxt) ||
+			(nvcxt->raw_changed && write_nvcontext(nvcxt))) {
+		debug(PREFIX "cannot write back nvcxt");
+		goto FAIL;
+	}
+
+	debug(PREFIX "reboot to recovery mode\n");
+	reset_cpu(0);
+
+	debug(PREFIX "error: reset_cpu() returned\n");
+FAIL:
+	/* FIXME: bring up a sad face? */
+	printf("Please reset and press recovery button when reboot.\n");
+	while (1);
+}