2 files changed, 91 insertions, 2 deletions

diff --git a/arch/arm/boards/raspberry-pi/lowlevel.c b/arch/arm/boards/raspberry-pi/lowlevel.c
index 091bfb8f4b..82b51b5b15 100644
--- a/arch/arm/boards/raspberry-pi/lowlevel.c
+++ b/arch/arm/boards/raspberry-pi/lowlevel.c
@@ -4,7 +4,10 @@
 #include <asm/cache.h>
 #include <common.h>
 #include <linux/sizes.h>
+#include <asm/unaligned.h>
 #include <mach/platform.h>
+#include <debug_ll.h>
+#include <mach/mbox.h>
 #include <of.h>
 
 #include "lowlevel.h"
@@ -45,8 +48,6 @@ static inline void start_raspberry_pi(unsigned long memsize, void *fdt,
 {
 	unsigned long endmem = rpi_stack_top(memsize);
 
-	arm_cpu_lowlevel_init();
-
 	copy_vc_fdt((void *)endmem, vc_fdt, VIDEOCORE_FDT_SZ);
 
 	fdt += get_runtime_offset();
@@ -64,20 +65,102 @@ extern char __dtb_z_bcm2837_rpi_cm3_start[];
 
 RPI_ENTRY_FUNCTION(start_raspberry_pi1, SZ_128M, r2)
 {
+	arm_cpu_lowlevel_init();
+
 	start_raspberry_pi(SZ_128M, __dtb_z_bcm2835_rpi_start, (void *)r2);
 }
 
 RPI_ENTRY_FUNCTION(start_raspberry_pi2, SZ_512M, r2)
 {
+	arm_cpu_lowlevel_init();
+
 	start_raspberry_pi(SZ_512M, __dtb_z_bcm2836_rpi_2_start, (void *)r2);
 }
 
 RPI_ENTRY_FUNCTION(start_raspberry_pi3, SZ_512M, r2)
 {
+	arm_cpu_lowlevel_init();
+
 	start_raspberry_pi(SZ_512M, __dtb_z_bcm2837_rpi_3_start, (void *)r2);
 }
 
 RPI_ENTRY_FUNCTION(start_raspberry_pi_cm3, SZ_512M, r2)
 {
+	arm_cpu_lowlevel_init();
+
 	start_raspberry_pi(SZ_512M, __dtb_z_bcm2837_rpi_cm3_start, (void *)r2);
 }
+
+#define DT_IF_ENABLED(dt, cfg) \
+	(IS_ENABLED(cfg) ? (dt) : NULL)
+
+static void *rpi_get_board_fdt(int rev)
+{
+	if (!(rev & 0x800000))
+		return DT_IF_ENABLED(__dtb_z_bcm2835_rpi_start, CONFIG_MACH_RPI);
+
+	switch (((rev >> 4) & 0xff)) {
+	case BCM2835_BOARD_REV_A:
+	case BCM2835_BOARD_REV_B:
+	case BCM2835_BOARD_REV_A_PLUS:
+	case BCM2835_BOARD_REV_B_PLUS:
+	case BCM2835_BOARD_REV_CM1:
+	case BCM2835_BOARD_REV_ZERO:
+	case BCM2835_BOARD_REV_ZERO_W:
+		return DT_IF_ENABLED(__dtb_z_bcm2835_rpi_start, CONFIG_MACH_RPI);
+
+	case BCM2836_BOARD_REV_2_B:
+		return DT_IF_ENABLED(__dtb_z_bcm2836_rpi_2_start, CONFIG_MACH_RPI2);
+
+	case BCM2837_BOARD_REV_3_B:
+	case BCM2837B0_BOARD_REV_3B_PLUS:
+	case BCM2837B0_BOARD_REV_3A_PLUS:
+	case BCM2837B0_BOARD_REV_ZERO_2:
+		return DT_IF_ENABLED(__dtb_z_bcm2837_rpi_3_start, CONFIG_MACH_RPI3);
+
+	case BCM2837_BOARD_REV_CM3:
+	case BCM2837B0_BOARD_REV_CM3_PLUS:
+		return DT_IF_ENABLED(__dtb_z_bcm2837_rpi_cm3_start, CONFIG_MACH_RPI_CM3);
+	}
+
+	return NULL;
+}
+
+RPI_ENTRY_FUNCTION(start_raspberry_pi_generic, SZ_128M, vc_fdt)
+{
+	void *fdt = NULL;
+	ssize_t memsize;
+	int rev;
+
+	arm_cpu_lowlevel_init();
+
+	debug_ll_init();
+
+	putc_ll('>');
+
+	relocate_to_current_adr();
+	setup_c();
+
+	memsize = rpi_get_arm_mem();
+	if (memsize < 0) {
+		pr_warn("mbox: failed to query ARM memory size. 128M assumed.\n");
+		memsize = SZ_128M;
+	}
+
+	rev = rpi_get_board_rev();
+	if (rev >= 0) {
+		pr_debug("Detected revision %08x\n", rev);
+		fdt = rpi_get_board_fdt(rev);
+	}
+
+	if (!fdt) {
+		fdt = (void *)vc_fdt;
+
+		pr_warn("Unknown Rpi board with rev %08x.\n", rev);
+
+		if (get_unaligned_be32(fdt) != 0xd00dfeed)
+			panic("No suitable built-in or videocore-supplied DT\n");
+	}
+
+	start_raspberry_pi(memsize, fdt, (void *)vc_fdt);
+}
diff --git a/arch/arm/mach-bcm283x/include/mach/debug_ll.h b/arch/arm/mach-bcm283x/include/mach/debug_ll.h
index 4bfa5abc7c..db23112aa0 100644
--- a/arch/arm/mach-bcm283x/include/mach/debug_ll.h
+++ b/arch/arm/mach-bcm283x/include/mach/debug_ll.h
@@ -66,6 +66,12 @@ static inline void debug_ll_init(void)
 	debug_ll_ns16550_init(divisor);
 }
 
+#else
+
+static inline void debug_ll_init(void)
+{
+}
+
 #endif
 
 #endif /* __MACH_BCM2835_DEBUG_LL_H__ */