2 files changed, 84 insertions, 5 deletions

diff --git a/arch/arm/mach-rockchip/rk3036-board.c b/arch/arm/mach-rockchip/rk3036-board.c
index b63f9c0..bf2b268 100644
--- a/arch/arm/mach-rockchip/rk3036-board.c
+++ b/arch/arm/mach-rockchip/rk3036-board.c
@@ -11,11 +11,50 @@
 #include <asm/io.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/periph.h>
+#include <asm/arch/grf_rk3036.h>
+#include <asm/arch/boot_mode.h>
+#include <asm/arch/sdram_rk3036.h>
 #include <asm/gpio.h>
 #include <dm/pinctrl.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
+#define GRF_BASE	0x20008000
+
+static void setup_boot_mode(void)
+{
+	struct rk3036_grf *const grf = (void *)GRF_BASE;
+	int boot_mode = readl(&grf->os_reg[4]);
+
+	debug("boot mode %x.\n", boot_mode);
+
+	/* Clear boot mode */
+	writel(BOOT_NORMAL, &grf->os_reg[4]);
+
+	switch (boot_mode) {
+	case BOOT_FASTBOOT:
+		printf("enter fastboot!\n");
+		setenv("preboot", "setenv preboot; fastboot usb0");
+		break;
+	case BOOT_UMS:
+		printf("enter UMS!\n");
+		setenv("preboot", "setenv preboot; ums mmc 0");
+		break;
+	}
+}
+
+__weak int rk_board_late_init(void)
+{
+	return 0;
+}
+
+int board_late_init(void)
+{
+	setup_boot_mode();
+
+	return rk_board_late_init();
+}
+
 int board_init(void)
 {
 	return 0;
diff --git a/arch/arm/mach-rockchip/rk3288-board.c b/arch/arm/mach-rockchip/rk3288-board.c
index 6c36bf9..baf9522 100644
--- a/arch/arm/mach-rockchip/rk3288-board.c
+++ b/arch/arm/mach-rockchip/rk3288-board.c
@@ -8,25 +8,65 @@
 #include <clk.h>
 #include <dm.h>
 #include <ram.h>
+#include <syscon.h>
 #include <asm/io.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/periph.h>
+#include <asm/arch/pmu_rk3288.h>
+#include <asm/arch/boot_mode.h>
 #include <asm/gpio.h>
 #include <dm/pinctrl.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
+#define PMU_BASE	0xff730000
+
+static void setup_boot_mode(void)
+{
+	struct rk3288_pmu *const pmu = (void *)PMU_BASE;
+	int boot_mode = readl(&pmu->sys_reg[0]);
+
+	debug("boot mode %x.\n", boot_mode);
+
+	/* Clear boot mode */
+	writel(BOOT_NORMAL, &pmu->sys_reg[0]);
+
+	switch (boot_mode) {
+	case BOOT_FASTBOOT:
+		printf("enter fastboot!\n");
+		setenv("preboot", "setenv preboot; fastboot usb0");
+		break;
+	case BOOT_UMS:
+		printf("enter UMS!\n");
+		setenv("preboot", "setenv preboot; if mmc dev 0;"
+		       "then ums mmc 0; else ums mmc 1;fi");
+		break;
+	}
+}
+
+__weak int rk_board_late_init(void)
+{
+	return 0;
+}
+
+int board_late_init(void)
+{
+	setup_boot_mode();
+
+	return rk_board_late_init();
+}
+
 int board_init(void)
 {
 #ifdef CONFIG_ROCKCHIP_SPL_BACK_TO_BROM
 	struct udevice *pinctrl;
 	int ret;
 
-    /*
-     * We need to implement sdcard iomux here for the further
-     * initlization, otherwise, it'll hit sdcard command sending
-     * timeout exception.
-     */
+	/*
+	 * We need to implement sdcard iomux here for the further
+	 * initlization, otherwise, it'll hit sdcard command sending
+	 * timeout exception.
+	 */
 	ret = uclass_get_device(UCLASS_PINCTRL, 0, &pinctrl);
 	if (ret) {
 		debug("%s: Cannot find pinctrl device\n", __func__);