[platform][zynq] trap the second cpu in lk's image to reclaim the last 64k of sram

Change-Id: I7eb6787f967f037670bddbe0eeaa4fb384e04d83

platform/zynq/include/platform/zynq.h

@@ -36,7 +36,7 @@
 #define SDRAM_APERTURE_SIZE (0x3ff00000)
 #define SRAM_BASE           (0x0)
 #define SRAM_APERTURE_SIZE  (0x00040000)
-#define SRAM_SIZE           (0x00030000) /* only 192KB mapped currently */
+#define SRAM_SIZE           (0x00040000)
 
 /* hardware base addresses */
 #define UART0_BASE (0xe0000000)

platform/zynq/platform.c

@@ -326,7 +326,6 @@ void platform_early_init(void)
     /* zynq manual says this is mandatory for cache init */
     *REG32(SLCR_BASE + 0xa1c) = 0x020202;
 
-
     /* early initialize the uart so we can printf */
     uart_init_early();
 
@@ -336,6 +335,45 @@ void platform_early_init(void)
     /* initialize the timer block */
     arm_cortex_a9_timer_init(CPUPRIV_BASE, zynq_get_arm_timer_freq());
 
+    /* bump the 2nd cpu into our code space and remap the top SRAM block */
+    if (KERNEL_LOAD_OFFSET != 0) {
+        /* construct a trampoline to get the 2nd cpu up to the trap routine */
+
+        /* figure out the offset of the trampoline routine in physical space from address 0 */
+        extern void platform_reset(void);
+        addr_t tramp = (addr_t)&platform_reset;
+        tramp -= KERNEL_BASE;
+        tramp += MEMBASE;
+
+        /* stuff in a ldr pc, [nextaddrress], and a target address */
+        uint32_t *ptr = (uint32_t *)KERNEL_BASE;
+
+        ptr[0] = 0xe51ff004; // ldr pc, [pc, #-4]
+        ptr[1] = tramp;
+        arch_clean_invalidate_cache_range((addr_t)ptr, 8);
+    }
+
+    /* reset the 2nd cpu, letting it go through its reset vector (at 0x0 physical) */
+    SLCR_REG(A9_CPU_RST_CTRL) |= (1<<1); // reset cpu 1
+    spin(10);
+    SLCR_REG(A9_CPU_RST_CTRL) &= ~(1<<1); // unreset cpu 1
+
+    /* wait for the 2nd cpu to reset, go through the usual reset vector, and get trapped by our code */
+    /* see platform/zynq/reset.S */
+    extern volatile int __cpu_trapped;
+    uint count = 100000;
+    while (--count) {
+        arch_clean_invalidate_cache_range((addr_t)&__cpu_trapped, sizeof(__cpu_trapped));
+        if (__cpu_trapped != 0)
+            break;
+    }
+    if (count == 0) {
+        panic("ZYNQ: failed to trap 2nd cpu\n");
+    }
+
+    /* bounce the 4th sram region down to lower address */
+    SLCR_REG(OCM_CFG) &= ~0xf; /* all banks at low address */
+
     /* add the main memory arena */
 #if !ZYNQ_CODE_IN_SDRAM && SDRAM_SIZE != 0
     /* In the case of running from SRAM, and we are using SDRAM,
@@ -376,6 +414,9 @@ void platform_quiesce(void)
 #endif
 
     platform_stop_timer();
+
+    /* stop the 2nd cpu and hold in reset */
+    SLCR_REG(A9_CPU_RST_CTRL) |= (1<<1); // reset cpu 1
 }
 
 #if WITH_LIB_CONSOLE

platform/zynq/rules.mk

@@ -45,7 +45,7 @@ endif
 
 ifeq ($(ZYNQ_USE_SRAM),1)
 MEMBASE := 0x0
-MEMSIZE := 0x30000 # 3 * 64K
+MEMSIZE := 0x40000 # 4 * 64K
 
 GLOBAL_DEFINES += \
 	ZYNQ_CODE_IN_SRAM=1

platform/zynq/start.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 Travis Geiselbrecht
+ * Copyright (c) 2014-2015 Travis Geiselbrecht
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files
@@ -23,13 +23,43 @@
 #include <asm.h>
 #include <platform/zynq.h>
 
+/* code run at the very beginning of the system, attempting to trap the 2nd cpu */
+FUNCTION(platform_reset)
+    /* figure out our cpu number */
+    mrc     p15, 0, r12, c0, c0, 5 /* MPIDR */
+
+    /* mask off the bottom 8 bits to test cpu number */
+    ubfx    r12, r12, #0, #8
+
+    /* if we're the 0th cpu, continue to arm_reset */
+    teq     r12, #0
+    beq     arm_reset
+
+    /* bump the cpu counter */
+    adr     r12, __cpu_trapped
+    mov     r11, #1
+    str     r11, [r12]
+    dsb
+
+0:
+    /* stay trapped here forever */
+    wfe
+    b       0b
+
+    ldr pc, foo
+foo:
+    .word       0xa
+
+DATA(__cpu_trapped)
+    .word     0
+
+#if 0
+/* disabled for now */
+
 /* this code attempts to remap sram to 0xfffc0000 - 0xffffffff and
    branch the cpu into the equivalent spot. Assumes the cpu is running
    at the initial 0 based mapping */
 
-/* disabled for now */
-#if 0
-
 /* a spot of the top bank of OCM memory for us to run our code from
    needs to be below where the second cpu is running (0xffffe00-0xfffffff0) */
 #define TARGET_SPOT 0xfffff800
@@ -86,3 +116,4 @@ FUNCTION(platform_reset)
 .ltorg
 #endif
 
+