@@ -16,6 +16,13 @@
#include <asm/assembler.h>
#include <asm/memory.h>
+#define RWDT_CLOCK_ON 0xdeadbeef
+#define RWDT_CLOCK_OFF 0x00000000
+#define SCTLR_MMU 0x01
+#define BOOTROM_ADDRESS 0xE6340000
+#define RWTCSRA_ADDRESS 0xE6020004
+#define RWTCSRA_WOVF 0x10
+
/*
* Reset vector for secondary CPUs.
* This will be mapped at address 0 by SBAR register.
@@ -37,6 +44,73 @@ shmobile_boot_fn:
shmobile_boot_size:
.long . - shmobile_boot_vector
+#ifdef CONFIG_ARCH_RCAR_GEN2
+/*
+ * Reset vector for R-Car Gen2 and RZ/G1 secondary CPUs.
+ * This will be mapped at address 0 by SBAR register.
+ */
+ENTRY(shmobile_boot_vector_gen2)
+/*
+ if (SCTLR_MMU == 1)
+ goto shmobile_smp_continue_gen2;
+*/
+ mrc p15, 0, r1, c1, c0, 0 @ r1 = SCTLR
+ and r0, r1, #SCTLR_MMU
+ cmp r0, #SCTLR_MMU
+ beq shmobile_smp_continue_gen2
+
+/*
+ if (shmobile_wdt_clock_status != RWDT_CLOCK_ON)
+ goto shmobile_smp_continue_gen2;
+*/
+ ldr r0, #shmobile_wdt_clock_status
+ ldr r1, #clock_on
+ cmp r0, r1
+ bne shmobile_smp_continue_gen2
+
+/*
+ if (RWTCSRA_WOVF == 0)
+ goto shmobile_smp_continue_gen2;
+*/
+ ldr r0, rwtcsra
+ mov r1, #0
+ ldrb r1, [r0]
+ and r0, r1, #RWTCSRA_WOVF
+ cmp r0, #RWTCSRA_WOVF
+ bne shmobile_smp_continue_gen2
+
+/*
+ goto bootrom;
+*/
+ ldr r0, bootrom
+ bx r0
+
+shmobile_smp_continue_gen2:
+ ldr r1, shmobile_boot_fn_gen2
+ bx r1
+
+ENDPROC(shmobile_boot_vector_gen2)
+
+ .align 4
+rwtcsra:
+ .word RWTCSRA_ADDRESS
+bootrom:
+ .word BOOTROM_ADDRESS
+clock_on:
+ .word RWDT_CLOCK_ON
+ .globl shmobile_wdt_clock_status
+shmobile_wdt_clock_status:
+ .word RWDT_CLOCK_OFF
+
+ .align 2
+ .globl shmobile_boot_fn_gen2
+shmobile_boot_fn_gen2:
+ .space 4
+ .globl shmobile_boot_size_gen2
+shmobile_boot_size_gen2:
+ .long . - shmobile_boot_vector_gen2
+#endif /* CONFIG_ARCH_RCAR_GEN2 */
+
/*
* Per-CPU SMP boot function/argument selection code based on MPIDR
*/