package/boot/uboot-sunxi/patches/002-uboot-jwrdegoede-psci-support.patch

   1 From 86f31982ac62e80fe586cad2e0a49a7b22e3d4ee Mon Sep 17 00:00:00 2001
   2 From: Marc Zyngier <marc.zyngier@arm.com>
   3 Date: Sat, 7 Dec 2013 11:19:07 +0000
   4 Subject: [PATCH] ARM: HYP/non-sec: move switch to non-sec to the last boot
   5  phase
   6
   7 Having the switch to non-secure in the "prep" phase is causing
   8 all kind of troubles, as that stage can be called multiple times.
   9
  10 Instead, move the switch to non-secure to the last possible phase,
  11 when there is no turning back anymore.
  12
  13 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
  14 ---
  15  arch/arm/lib/bootm.c | 5 +++--
  16  1 file changed, 3 insertions(+), 2 deletions(-)
  17
  18 diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
  19 index 47ee070..10634a4 100644
  20 --- a/arch/arm/lib/bootm.c
  21 +++ b/arch/arm/lib/bootm.c
  22 @@ -242,7 +242,6 @@ static void boot_prep_linux(bootm_headers_t *images)
  23                 printf("FDT and ATAGS support not compiled in - hanging\n");
  24                 hang();
  25         }
  26 -       do_nonsec_virt_switch();
  27  }
  28
  29  /* Subcommand: GO */
  30 @@ -287,8 +286,10 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
  31         else
  32                 r2 = gd->bd->bi_boot_params;
  33
  34 -       if (!fake)
  35 +       if (!fake) {
  36 +               do_nonsec_virt_switch();
  37                 kernel_entry(0, machid, r2);
  38 +       }
  39  #endif
  40  }
  41
  42 From c26d288f6cbc6d53219001d42476f314c403257b Mon Sep 17 00:00:00 2001
  43 From: Marc Zyngier <marc.zyngier@arm.com>
  44 Date: Sat, 7 Dec 2013 11:19:08 +0000
  45 Subject: [PATCH] ARM: HYP/non-sec: add a barrier after setting SCR.NS==1
  46
  47 A CP15 instruction execution can be reordered, requiring an
  48 isb to be sure it is executed in program order.
  49
  50 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
  51 ---
  52  arch/arm/cpu/armv7/nonsec_virt.S | 1 +
  53  1 file changed, 1 insertion(+)
  54
  55 diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
  56 index 6367e09..12de5c2 100644
  57 --- a/arch/arm/cpu/armv7/nonsec_virt.S
  58 +++ b/arch/arm/cpu/armv7/nonsec_virt.S
  59 @@ -46,6 +46,7 @@ _secure_monitor:
  60  #endif
  61
  62         mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)
  63 +       isb
  64
  65  #ifdef CONFIG_ARMV7_VIRT
  66         mrceq   p15, 0, r0, c12, c0, 1          @ get MVBAR value
  67 From 06feeea3c84cc58ff3d5c19f6a430886495f86ce Mon Sep 17 00:00:00 2001
  68 From: Marc Zyngier <marc.zyngier@arm.com>
  69 Date: Sat, 7 Dec 2013 11:19:09 +0000
  70 Subject: [PATCH] ARM: non-sec: reset CNTVOFF to zero
  71
  72 Before switching to non-secure, make sure that CNTVOFF is set
  73 to zero on all CPUs. Otherwise, kernel running in non-secure
  74 without HYP enabled (hence using virtual timers) may observe
  75 timers that are not synchronized, effectively seeing time
  76 going backward...
  77
  78 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
  79 ---
  80  arch/arm/cpu/armv7/nonsec_virt.S | 9 ++++++++-
  81  1 file changed, 8 insertions(+), 1 deletion(-)
  82
  83 diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
  84 index 12de5c2..b5c946f 100644
  85 --- a/arch/arm/cpu/armv7/nonsec_virt.S
  86 +++ b/arch/arm/cpu/armv7/nonsec_virt.S
  87 @@ -38,10 +38,10 @@ _secure_monitor:
  88         bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
  89         orr     r1, r1, #0x31                   @ enable NS, AW, FW bits
  90
  91 -#ifdef CONFIG_ARMV7_VIRT
  92         mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
  93         and     r0, r0, #CPUID_ARM_VIRT_MASK    @ mask virtualization bits
  94         cmp     r0, #(1 << CPUID_ARM_VIRT_SHIFT)
  95 +#ifdef CONFIG_ARMV7_VIRT
  96         orreq   r1, r1, #0x100                  @ allow HVC instruction
  97  #endif
  98
  99 @@ -52,7 +52,14 @@ _secure_monitor:
 100         mrceq   p15, 0, r0, c12, c0, 1          @ get MVBAR value
 101         mcreq   p15, 4, r0, c12, c0, 0          @ write HVBAR
 102  #endif
 103 +       bne     1f
 104
 105 +       @ Reset CNTVOFF to 0 before leaving monitor mode
 106 +       mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
 107 +       ands    r0, r0, #CPUID_ARM_GENTIMER_MASK        @ test arch timer bits
 108 +       movne   r0, #0
 109 +       mcrrne  p15, 4, r0, r0, c14             @ Reset CNTVOFF to zero
 110 +1:
 111         movs    pc, lr                          @ return to non-secure SVC
 112
 113  _hyp_trap:
 114 From 054bcf5147ff5a20298bce5b3bdfbf3e1c797594 Mon Sep 17 00:00:00 2001
 115 From: Marc Zyngier <marc.zyngier@arm.com>
 116 Date: Sat, 7 Dec 2013 11:19:10 +0000
 117 Subject: [PATCH] ARM: add missing HYP mode constant
 118
 119 In order to be able to use the various mode constants (far more
 120 readable than random hex values), add the missing HYP and A
 121 values.
 122
 123 Also update arm/lib/interrupts.c to display HYP instead of an
 124 unknown value.
 125
 126 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 127 ---
 128  arch/arm/include/asm/proc-armv/ptrace.h | 2 ++
 129  arch/arm/lib/interrupts.c               | 2 +-
 130  2 files changed, 3 insertions(+), 1 deletion(-)
 131
 132 diff --git a/arch/arm/include/asm/proc-armv/ptrace.h b/arch/arm/include/asm/proc-armv/ptrace.h
 133 index 21aef58..71df5a9 100644
 134 --- a/arch/arm/include/asm/proc-armv/ptrace.h
 135 +++ b/arch/arm/include/asm/proc-armv/ptrace.h
 136 @@ -38,12 +38,14 @@ struct pt_regs {
 137  #define IRQ_MODE       0x12
 138  #define SVC_MODE       0x13
 139  #define ABT_MODE       0x17
 140 +#define HYP_MODE       0x1a
 141  #define UND_MODE       0x1b
 142  #define SYSTEM_MODE    0x1f
 143  #define MODE_MASK      0x1f
 144  #define T_BIT          0x20
 145  #define F_BIT          0x40
 146  #define I_BIT          0x80
 147 +#define A_BIT          0x100
 148  #define CC_V_BIT       (1 << 28)
 149  #define CC_C_BIT       (1 << 29)
 150  #define CC_Z_BIT       (1 << 30)
 151 diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
 152 index 758b013..f6b7c03 100644
 153 --- a/arch/arm/lib/interrupts.c
 154 +++ b/arch/arm/lib/interrupts.c
 155 @@ -103,7 +103,7 @@ void show_regs (struct pt_regs *regs)
 156         "UK12_26",      "UK13_26",      "UK14_26",      "UK15_26",
 157         "USER_32",      "FIQ_32",       "IRQ_32",       "SVC_32",
 158         "UK4_32",       "UK5_32",       "UK6_32",       "ABT_32",
 159 -       "UK8_32",       "UK9_32",       "UK10_32",      "UND_32",
 160 +       "UK8_32",       "UK9_32",       "HYP_32",       "UND_32",
 161         "UK12_32",      "UK13_32",      "UK14_32",      "SYS_32",
 162         };
 163
 164 From 213a8d9b7e613210d3c7d8b99c95b454ad0527d8 Mon Sep 17 00:00:00 2001
 165 From: Marc Zyngier <marc.zyngier@arm.com>
 166 Date: Sat, 7 Dec 2013 11:19:11 +0000
 167 Subject: [PATCH] ARM: HYP/non-sec: add separate section for secure code
 168
 169 In anticipation of refactoring the HYP/non-secure code to run
 170 from secure RAM, add a new linker section that will contain that
 171 code.
 172
 173 Nothing is using it just yet.
 174
 175 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 176 ---
 177  arch/arm/config.mk      |  2 +-
 178  arch/arm/cpu/u-boot.lds | 30 ++++++++++++++++++++++++++++++
 179  arch/arm/lib/sections.c |  2 ++
 180  3 files changed, 33 insertions(+), 1 deletion(-)
 181
 182 diff --git a/arch/arm/config.mk b/arch/arm/config.mk
 183 index 66ecc2e..2bdfca5 100644
 184 --- a/arch/arm/config.mk
 185 +++ b/arch/arm/config.mk
 186 @@ -113,7 +113,7 @@ endif
 187  ifdef CONFIG_ARM64
 188  OBJCOPYFLAGS += -j .text -j .rodata -j .data -j .u_boot_list -j .rela.dyn
 189  else
 190 -OBJCOPYFLAGS += -j .text -j .rodata -j .hash -j .data -j .got.plt -j .u_boot_list -j .rel.dyn
 191 +OBJCOPYFLAGS += -j .text -j .secure_text -j .rodata -j .hash -j .data -j .got.plt -j .u_boot_list -j .rel.dyn
 192  endif
 193
 194  ifneq ($(CONFIG_IMX_CONFIG),)
 195 diff --git a/arch/arm/cpu/u-boot.lds b/arch/arm/cpu/u-boot.lds
 196 index 33c1f99..f45885d 100644
 197 --- a/arch/arm/cpu/u-boot.lds
 198 +++ b/arch/arm/cpu/u-boot.lds
 199 @@ -7,6 +7,8 @@
 200   * SPDX-License-Identifier:    GPL-2.0+
 201   */
 202
 203 +#include <config.h>
 204 +
 205  OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
 206  OUTPUT_ARCH(arm)
 207  ENTRY(_start)
 208 @@ -22,6 +24,34 @@ SECTIONS
 209                 *(.text*)
 210         }
 211
 212 +#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT) || defined(CONFIG_ARMV7_PSCI)
 213 +
 214 +#ifndef CONFIG_ARMV7_SECURE_BASE
 215 +#define CONFIG_ARMV7_SECURE_BASE
 216 +#endif
 217 +
 218 +       .__secure_start : {
 219 +               . = ALIGN(0x1000);
 220 +               *(.__secure_start)
 221 +       }
 222 +
 223 +       .secure_text CONFIG_ARMV7_SECURE_BASE :
 224 +               AT(ADDR(.__secure_start) + SIZEOF(.__secure_start))
 225 +       {
 226 +               *(._secure.text)
 227 +       }
 228 +
 229 +       . = LOADADDR(.__secure_start) +
 230 +               SIZEOF(.__secure_start) +
 231 +               SIZEOF(.secure_text);
 232 +
 233 +       __secure_end_lma = .;
 234 +       .__secure_end : AT(__secure_end_lma) {
 235 +               *(.__secure_end)
 236 +               LONG(0x1d1071c);        /* Must output something to reset LMA */
 237 +       }
 238 +#endif
 239 +
 240         . = ALIGN(4);
 241         .rodata : { *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.rodata*))) }
 242
 243 diff --git a/arch/arm/lib/sections.c b/arch/arm/lib/sections.c
 244 index 5b30bcb..a1205c3 100644
 245 --- a/arch/arm/lib/sections.c
 246 +++ b/arch/arm/lib/sections.c
 247 @@ -25,4 +25,6 @@ char __image_copy_start[0] __attribute__((section(".__image_copy_start")));
 248  char __image_copy_end[0] __attribute__((section(".__image_copy_end")));
 249  char __rel_dyn_start[0] __attribute__((section(".__rel_dyn_start")));
 250  char __rel_dyn_end[0] __attribute__((section(".__rel_dyn_end")));
 251 +char __secure_start[0] __attribute__((section(".__secure_start")));
 252 +char __secure_end[0] __attribute__((section(".__secure_end")));
 253  char _end[0] __attribute__((section(".__end")));
 254 From d7ebd8f57e84ea92ef0cf55080f0acec9c6d1ace Mon Sep 17 00:00:00 2001
 255 From: Marc Zyngier <marc.zyngier@arm.com>
 256 Date: Sat, 7 Dec 2013 11:19:12 +0000
 257 Subject: [PATCH] ARM: HYP/non-sec: allow relocation to secure RAM
 258
 259 The current non-sec switching code suffers from one major issue:
 260 it cannot run in secure RAM, as a large part of u-boot still needs
 261 to be run while we're switched to non-secure.
 262
 263 This patch reworks the whole HYP/non-secure strategy by:
 264 - making sure the secure code is the *last* thing u-boot executes
 265   before entering the payload
 266 - performing an exception return from secure mode directly into
 267   the payload
 268 - allowing the code to be dynamically relocated to secure RAM
 269   before switching to non-secure.
 270
 271 This involves quite a bit of horrible code, specially as u-boot
 272 relocation is quite primitive.
 273
 274 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 275 ---
 276  arch/arm/cpu/armv7/nonsec_virt.S | 161 +++++++++++++++++++--------------------
 277  arch/arm/cpu/armv7/virt-v7.c     |  59 +++++---------
 278  arch/arm/include/asm/armv7.h     |  10 ++-
 279  arch/arm/include/asm/secure.h    |  26 +++++++
 280  arch/arm/lib/bootm.c             |  22 +++---
 281  5 files changed, 138 insertions(+), 140 deletions(-)
 282  create mode 100644 arch/arm/include/asm/secure.h
 283
 284 diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
 285 index b5c946f..2a43e3c 100644
 286 --- a/arch/arm/cpu/armv7/nonsec_virt.S
 287 +++ b/arch/arm/cpu/armv7/nonsec_virt.S
 288 @@ -10,10 +10,13 @@
 289  #include <linux/linkage.h>
 290  #include <asm/gic.h>
 291  #include <asm/armv7.h>
 292 +#include <asm/proc-armv/ptrace.h>
 293
 294  .arch_extension sec
 295  .arch_extension virt
 296
 297 +       .pushsection ._secure.text, "ax"
 298 +
 299         .align  5
 300  /* the vector table for secure state and HYP mode */
 301  _monitor_vectors:
 302 @@ -22,51 +25,86 @@ _monitor_vectors:
 303         adr pc, _secure_monitor
 304         .word 0
 305         .word 0
 306 -       adr pc, _hyp_trap
 307 +       .word 0
 308         .word 0
 309         .word 0
 310
 311 +.macro is_cpu_virt_capable     tmp
 312 +       mrc     p15, 0, \tmp, c0, c1, 1         @ read ID_PFR1
 313 +       and     \tmp, \tmp, #CPUID_ARM_VIRT_MASK        @ mask virtualization bits
 314 +       cmp     \tmp, #(1 << CPUID_ARM_VIRT_SHIFT)
 315 +.endm
 316 +
 317  /*
 318   * secure monitor handler
 319   * U-boot calls this "software interrupt" in start.S
 320   * This is executed on a "smc" instruction, we use a "smc #0" to switch
 321   * to non-secure state.
 322 - * We use only r0 and r1 here, due to constraints in the caller.
 323 + * r0, r1, r2: passed to the callee
 324 + * ip: target PC
 325   */
 326  _secure_monitor:
 327 -       mrc     p15, 0, r1, c1, c1, 0           @ read SCR
 328 -       bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
 329 -       orr     r1, r1, #0x31                   @ enable NS, AW, FW bits
 330 +       mrc     p15, 0, r5, c1, c1, 0           @ read SCR
 331 +       bic     r5, r5, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
 332 +       orr     r5, r5, #0x31                   @ enable NS, AW, FW bits
 333
 334 -       mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
 335 -       and     r0, r0, #CPUID_ARM_VIRT_MASK    @ mask virtualization bits
 336 -       cmp     r0, #(1 << CPUID_ARM_VIRT_SHIFT)
 337 +       mov     r6, #SVC_MODE                   @ default mode is SVC
 338 +       is_cpu_virt_capable r4
 339  #ifdef CONFIG_ARMV7_VIRT
 340 -       orreq   r1, r1, #0x100                  @ allow HVC instruction
 341 +       orreq   r5, r5, #0x100                  @ allow HVC instruction
 342 +       moveq   r6, #HYP_MODE                   @ Enter the kernel as HYP
 343  #endif
 344
 345 -       mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)
 346 +       mcr     p15, 0, r5, c1, c1, 0           @ write SCR (with NS bit set)
 347         isb
 348
 349 -#ifdef CONFIG_ARMV7_VIRT
 350 -       mrceq   p15, 0, r0, c12, c0, 1          @ get MVBAR value
 351 -       mcreq   p15, 4, r0, c12, c0, 0          @ write HVBAR
 352 -#endif
 353         bne     1f
 354
 355         @ Reset CNTVOFF to 0 before leaving monitor mode
 356 -       mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
 357 -       ands    r0, r0, #CPUID_ARM_GENTIMER_MASK        @ test arch timer bits
 358 -       movne   r0, #0
 359 -       mcrrne  p15, 4, r0, r0, c14             @ Reset CNTVOFF to zero
 360 +       mrc     p15, 0, r4, c0, c1, 1           @ read ID_PFR1
 361 +       ands    r4, r4, #CPUID_ARM_GENTIMER_MASK        @ test arch timer bits
 362 +       movne   r4, #0
 363 +       mcrrne  p15, 4, r4, r4, c14             @ Reset CNTVOFF to zero
 364  1:
 365 -       movs    pc, lr                          @ return to non-secure SVC
 366 -
 367 -_hyp_trap:
 368 -       mrs     lr, elr_hyp     @ for older asm: .byte 0x00, 0xe3, 0x0e, 0xe1
 369 -       mov pc, lr                              @ do no switch modes, but
 370 -                                               @ return to caller
 371 -
 372 +       mov     lr, ip
 373 +       mov     ip, #(F_BIT | I_BIT | A_BIT)    @ Set A, I and F
 374 +       tst     lr, #1                          @ Check for Thumb PC
 375 +       orrne   ip, ip, #T_BIT                  @ Set T if Thumb
 376 +       orr     ip, ip, r6                      @ Slot target mode in
 377 +       msr     spsr_cxfs, ip                   @ Set full SPSR
 378 +       movs    pc, lr                          @ ERET to non-secure
 379 +
 380 +ENTRY(_do_nonsec_entry)
 381 +       mov     ip, r0
 382 +       mov     r0, r1
 383 +       mov     r1, r2
 384 +       mov     r2, r3
 385 +       smc     #0
 386 +ENDPROC(_do_nonsec_entry)
 387 +
 388 +.macro get_cbar_addr   addr
 389 +#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
 390 +       ldr     \addr, =CONFIG_ARM_GIC_BASE_ADDRESS
 391 +#else
 392 +       mrc     p15, 4, \addr, c15, c0, 0       @ read CBAR
 393 +       bfc     \addr, #0, #15                  @ clear reserved bits
 394 +#endif
 395 +.endm
 396 +
 397 +.macro get_gicd_addr   addr
 398 +       get_cbar_addr   \addr
 399 +       add     \addr, \addr, #GIC_DIST_OFFSET  @ GIC dist i/f offset
 400 +.endm
 401 +
 402 +.macro get_gicc_addr   addr, tmp
 403 +       get_cbar_addr   \addr
 404 +       is_cpu_virt_capable \tmp
 405 +       movne   \tmp, #GIC_CPU_OFFSET_A9        @ GIC CPU offset for A9
 406 +       moveq   \tmp, #GIC_CPU_OFFSET_A15       @ GIC CPU offset for A15/A7
 407 +       add     \addr, \addr, \tmp
 408 +.endm
 409 +
 410 +#ifndef CONFIG_ARMV7_PSCI
 411  /*
 412   * Secondary CPUs start here and call the code for the core specific parts
 413   * of the non-secure and HYP mode transition. The GIC distributor specific
 414 @@ -74,31 +112,21 @@ _hyp_trap:
 415   * Then they go back to wfi and wait to be woken up by the kernel again.
 416   */
 417  ENTRY(_smp_pen)
 418 -       mrs     r0, cpsr
 419 -       orr     r0, r0, #0xc0
 420 -       msr     cpsr, r0                        @ disable interrupts
 421 -       ldr     r1, =_start
 422 -       mcr     p15, 0, r1, c12, c0, 0          @ set VBAR
 423 +       cpsid   i
 424 +       cpsid   f
 425
 426         bl      _nonsec_init
 427 -       mov     r12, r0                         @ save GICC address
 428 -#ifdef CONFIG_ARMV7_VIRT
 429 -       bl      _switch_to_hyp
 430 -#endif
 431 -
 432 -       ldr     r1, [r12, #GICC_IAR]            @ acknowledge IPI
 433 -       str     r1, [r12, #GICC_EOIR]           @ signal end of interrupt
 434
 435         adr     r0, _smp_pen                    @ do not use this address again
 436         b       smp_waitloop                    @ wait for IPIs, board specific
 437  ENDPROC(_smp_pen)
 438 +#endif
 439
 440  /*
 441   * Switch a core to non-secure state.
 442   *
 443   *  1. initialize the GIC per-core interface
 444   *  2. allow coprocessor access in non-secure modes
 445 - *  3. switch the cpu mode (by calling "smc #0")
 446   *
 447   * Called from smp_pen by secondary cores and directly by the BSP.
 448   * Do not assume that the stack is available and only use registers
 449 @@ -108,38 +136,23 @@ ENDPROC(_smp_pen)
 450   * though, but we check this in C before calling this function.
 451   */
 452  ENTRY(_nonsec_init)
 453 -#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
 454 -       ldr     r2, =CONFIG_ARM_GIC_BASE_ADDRESS
 455 -#else
 456 -       mrc     p15, 4, r2, c15, c0, 0          @ read CBAR
 457 -       bfc     r2, #0, #15                     @ clear reserved bits
 458 -#endif
 459 -       add     r3, r2, #GIC_DIST_OFFSET        @ GIC dist i/f offset
 460 +       get_gicd_addr   r3
 461 +
 462         mvn     r1, #0                          @ all bits to 1
 463         str     r1, [r3, #GICD_IGROUPRn]        @ allow private interrupts
 464
 465 -       mrc     p15, 0, r0, c0, c0, 0           @ read MIDR
 466 -       ldr     r1, =MIDR_PRIMARY_PART_MASK
 467 -       and     r0, r0, r1                      @ mask out variant and revision
 468 +       get_gicc_addr   r3, r1
 469
 470 -       ldr     r1, =MIDR_CORTEX_A7_R0P0 & MIDR_PRIMARY_PART_MASK
 471 -       cmp     r0, r1                          @ check for Cortex-A7
 472 -
 473 -       ldr     r1, =MIDR_CORTEX_A15_R0P0 & MIDR_PRIMARY_PART_MASK
 474 -       cmpne   r0, r1                          @ check for Cortex-A15
 475 -
 476 -       movne   r1, #GIC_CPU_OFFSET_A9          @ GIC CPU offset for A9
 477 -       moveq   r1, #GIC_CPU_OFFSET_A15         @ GIC CPU offset for A15/A7
 478 -       add     r3, r2, r1                      @ r3 = GIC CPU i/f addr
 479 -
 480 -       mov     r1, #1                          @ set GICC_CTLR[enable]
 481 +       mov     r1, #3                          @ Enable both groups
 482         str     r1, [r3, #GICC_CTLR]            @ and clear all other bits
 483         mov     r1, #0xff
 484         str     r1, [r3, #GICC_PMR]             @ set priority mask register
 485
 486 +       mrc     p15, 0, r0, c1, c1, 2
 487         movw    r1, #0x3fff
 488 -       movt    r1, #0x0006
 489 -       mcr     p15, 0, r1, c1, c1, 2           @ NSACR = all copros to non-sec
 490 +       movt    r1, #0x0004
 491 +       orr     r0, r0, r1
 492 +       mcr     p15, 0, r0, c1, c1, 2           @ NSACR = all copros to non-sec
 493
 494  /* The CNTFRQ register of the generic timer needs to be
 495   * programmed in secure state. Some primary bootloaders / firmware
 496 @@ -157,21 +170,9 @@ ENTRY(_nonsec_init)
 497
 498         adr     r1, _monitor_vectors
 499         mcr     p15, 0, r1, c12, c0, 1          @ set MVBAR to secure vectors
 500 -
 501 -       mrc     p15, 0, ip, c12, c0, 0          @ save secure copy of VBAR
 502 -
 503         isb
 504 -       smc     #0                              @ call into MONITOR mode
 505 -
 506 -       mcr     p15, 0, ip, c12, c0, 0          @ write non-secure copy of VBAR
 507 -
 508 -       mov     r1, #1
 509 -       str     r1, [r3, #GICC_CTLR]            @ enable non-secure CPU i/f
 510 -       add     r2, r2, #GIC_DIST_OFFSET
 511 -       str     r1, [r2, #GICD_CTLR]            @ allow private interrupts
 512
 513         mov     r0, r3                          @ return GICC address
 514 -
 515         bx      lr
 516  ENDPROC(_nonsec_init)
 517
 518 @@ -183,18 +184,10 @@ ENTRY(smp_waitloop)
 519         ldr     r1, [r1]
 520         cmp     r0, r1                  @ make sure we dont execute this code
 521         beq     smp_waitloop            @ again (due to a spurious wakeup)
 522 -       mov     pc, r1
 523 +       mov     r0, r1
 524 +       b       _do_nonsec_entry
 525  ENDPROC(smp_waitloop)
 526  .weak smp_waitloop
 527  #endif
 528
 529 -ENTRY(_switch_to_hyp)
 530 -       mov     r0, lr
 531 -       mov     r1, sp                          @ save SVC copy of LR and SP
 532 -       isb
 533 -       hvc #0                   @ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
 534 -       mov     sp, r1
 535 -       mov     lr, r0                          @ restore SVC copy of LR and SP
 536 -
 537 -       bx      lr
 538 -ENDPROC(_switch_to_hyp)
 539 +       .popsection
 540 diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
 541 index 2cd604f..6500030 100644
 542 --- a/arch/arm/cpu/armv7/virt-v7.c
 543 +++ b/arch/arm/cpu/armv7/virt-v7.c
 544 @@ -13,17 +13,10 @@
 545  #include <asm/armv7.h>
 546  #include <asm/gic.h>
 547  #include <asm/io.h>
 548 +#include <asm/secure.h>
 549
 550  unsigned long gic_dist_addr;
 551
 552 -static unsigned int read_cpsr(void)
 553 -{
 554 -       unsigned int reg;
 555 -
 556 -       asm volatile ("mrs %0, cpsr\n" : "=r" (reg));
 557 -       return reg;
 558 -}
 559 -
 560  static unsigned int read_id_pfr1(void)
 561  {
 562         unsigned int reg;
 563 @@ -72,6 +65,18 @@ static unsigned long get_gicd_base_address(void)
 564  #endif
 565  }
 566
 567 +static void relocate_secure_section(void)
 568 +{
 569 +#ifdef CONFIG_ARMV7_SECURE_BASE
 570 +       size_t sz = __secure_end - __secure_start;
 571 +
 572 +       memcpy((void *)CONFIG_ARMV7_SECURE_BASE, __secure_start, sz);
 573 +       flush_dcache_range(CONFIG_ARMV7_SECURE_BASE,
 574 +                          CONFIG_ARMV7_SECURE_BASE + sz + 1);
 575 +       invalidate_icache_all();
 576 +#endif
 577 +}
 578 +
 579  static void kick_secondary_cpus_gic(unsigned long gicdaddr)
 580  {
 581         /* kick all CPUs (except this one) by writing to GICD_SGIR */
 582 @@ -83,35 +88,7 @@ void __weak smp_kick_all_cpus(void)
 583         kick_secondary_cpus_gic(gic_dist_addr);
 584  }
 585
 586 -int armv7_switch_hyp(void)
 587 -{
 588 -       unsigned int reg;
 589 -
 590 -       /* check whether we are in HYP mode already */
 591 -       if ((read_cpsr() & 0x1f) == 0x1a) {
 592 -               debug("CPU already in HYP mode\n");
 593 -               return 0;
 594 -       }
 595 -
 596 -       /* check whether the CPU supports the virtualization extensions */
 597 -       reg = read_id_pfr1();
 598 -       if ((reg & CPUID_ARM_VIRT_MASK) != 1 << CPUID_ARM_VIRT_SHIFT) {
 599 -               printf("HYP mode: Virtualization extensions not implemented.\n");
 600 -               return -1;
 601 -       }
 602 -
 603 -       /* call the HYP switching code on this CPU also */
 604 -       _switch_to_hyp();
 605 -
 606 -       if ((read_cpsr() & 0x1F) != 0x1a) {
 607 -               printf("HYP mode: switch not successful.\n");
 608 -               return -1;
 609 -       }
 610 -
 611 -       return 0;
 612 -}
 613 -
 614 -int armv7_switch_nonsec(void)
 615 +int armv7_init_nonsec(void)
 616  {
 617         unsigned int reg;
 618         unsigned itlinesnr, i;
 619 @@ -147,11 +124,13 @@ int armv7_switch_nonsec(void)
 620         for (i = 1; i <= itlinesnr; i++)
 621                 writel((unsigned)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
 622
 623 -       smp_set_core_boot_addr((unsigned long)_smp_pen, -1);
 624 +#ifndef CONFIG_ARMV7_PSCI
 625 +       smp_set_core_boot_addr((unsigned long)secure_ram_addr(_smp_pen), -1);
 626         smp_kick_all_cpus();
 627 +#endif
 628
 629         /* call the non-sec switching code on this CPU also */
 630 -       _nonsec_init();
 631 -
 632 +       relocate_secure_section();
 633 +       secure_ram_addr(_nonsec_init)();
 634         return 0;
 635  }
 636 diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
 637 index 395444e..11476dd 100644
 638 --- a/arch/arm/include/asm/armv7.h
 639 +++ b/arch/arm/include/asm/armv7.h
 640 @@ -78,13 +78,17 @@ void v7_outer_cache_inval_range(u32 start, u32 end);
 641
 642  #if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
 643
 644 -int armv7_switch_nonsec(void);
 645 -int armv7_switch_hyp(void);
 646 +int armv7_init_nonsec(void);
 647
 648  /* defined in assembly file */
 649  unsigned int _nonsec_init(void);
 650 +void _do_nonsec_entry(void *target_pc, unsigned long r0,
 651 +                     unsigned long r1, unsigned long r2);
 652  void _smp_pen(void);
 653 -void _switch_to_hyp(void);
 654 +
 655 +extern char __secure_start[];
 656 +extern char __secure_end[];
 657 +
 658  #endif /* CONFIG_ARMV7_NONSEC || CONFIG_ARMV7_VIRT */
 659
 660  #endif /* ! __ASSEMBLY__ */
 661 diff --git a/arch/arm/include/asm/secure.h b/arch/arm/include/asm/secure.h
 662 new file mode 100644
 663 index 0000000..effdb18
 664 --- /dev/null
 665 +++ b/arch/arm/include/asm/secure.h
 666 @@ -0,0 +1,26 @@
 667 +#ifndef __ASM_SECURE_H
 668 +#define __ASM_SECURE_H
 669 +
 670 +#include <config.h>
 671 +
 672 +#ifdef CONFIG_ARMV7_SECURE_BASE
 673 +/*
 674 + * Warning, horror ahead.
 675 + *
 676 + * The target code lives in our "secure ram", but u-boot doesn't know
 677 + * that, and has blindly added reloc_off to every relocation
 678 + * entry. Gahh. Do the opposite conversion. This hack also prevents
 679 + * GCC from generating code veeners, which u-boot doesn't relocate at
 680 + * all...
 681 + */
 682 +#define secure_ram_addr(_fn) ({                                                \
 683 +                       DECLARE_GLOBAL_DATA_PTR;                        \
 684 +                       void *__fn = _fn;                               \
 685 +                       typeof(_fn) *__tmp = (__fn - gd->reloc_off);    \
 686 +                       __tmp;                                          \
 687 +               })
 688 +#else
 689 +#define secure_ram_addr(_fn)   (_fn)
 690 +#endif
 691 +
 692 +#endif
 693 diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
 694 index 10634a4..61aa14e 100644
 695 --- a/arch/arm/lib/bootm.c
 696 +++ b/arch/arm/lib/bootm.c
 697 @@ -20,6 +20,7 @@
 698  #include <libfdt.h>
 699  #include <fdt_support.h>
 700  #include <asm/bootm.h>
 701 +#include <asm/secure.h>
 702  #include <linux/compiler.h>
 703
 704  #if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
 705 @@ -184,27 +185,17 @@ static void setup_end_tag(bd_t *bd)
 706
 707  __weak void setup_board_tags(struct tag **in_params) {}
 708
 709 +#ifdef CONFIG_ARM64
 710  static void do_nonsec_virt_switch(void)
 711  {
 712 -#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
 713 -       if (armv7_switch_nonsec() == 0)
 714 -#ifdef CONFIG_ARMV7_VIRT
 715 -               if (armv7_switch_hyp() == 0)
 716 -                       debug("entered HYP mode\n");
 717 -#else
 718 -               debug("entered non-secure state\n");
 719 -#endif
 720 -#endif
 721 -
 722 -#ifdef CONFIG_ARM64
 723         smp_kick_all_cpus();
 724         flush_dcache_all();     /* flush cache before swtiching to EL2 */
 725         armv8_switch_to_el2();
 726  #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
 727         armv8_switch_to_el1();
 728  #endif
 729 -#endif
 730  }
 731 +#endif
 732
 733  /* Subcommand: PREP */
 734  static void boot_prep_linux(bootm_headers_t *images)
 735 @@ -287,8 +278,13 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
 736                 r2 = gd->bd->bi_boot_params;
 737
 738         if (!fake) {
 739 -               do_nonsec_virt_switch();
 740 +#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
 741 +               armv7_init_nonsec();
 742 +               secure_ram_addr(_do_nonsec_entry)(kernel_entry,
 743 +                                                 0, machid, r2);
 744 +#else
 745                 kernel_entry(0, machid, r2);
 746 +#endif
 747         }
 748  #endif
 749  }
 750 From 8ea1554da4a6e556d3213a77cf59daa1c154bdb5 Mon Sep 17 00:00:00 2001
 751 From: Marc Zyngier <marc.zyngier@arm.com>
 752 Date: Sat, 7 Dec 2013 11:19:13 +0000
 753 Subject: [PATCH] ARM: HYP/non-sec: add generic ARMv7 PSCI code
 754
 755 Implement core support for PSCI. As this is generic code, it doesn't
 756 implement anything really useful (all the functions are returning
 757 Not Implemented).
 758
 759 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 760 ---
 761  arch/arm/cpu/armv7/Makefile |   4 ++
 762  arch/arm/cpu/armv7/psci.S   | 105 ++++++++++++++++++++++++++++++++++++++++++++
 763  arch/arm/include/asm/psci.h |  35 +++++++++++++++
 764  3 files changed, 144 insertions(+)
 765  create mode 100644 arch/arm/cpu/armv7/psci.S
 766  create mode 100644 arch/arm/include/asm/psci.h
 767
 768 diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
 769 index 6f17771..0cf5c45 100644
 770 --- a/arch/arm/cpu/armv7/Makefile
 771 +++ b/arch/arm/cpu/armv7/Makefile
 772 @@ -24,6 +24,10 @@ obj-y        += nonsec_virt.o
 773  obj-y  += virt-v7.o
 774  endif
 775
 776 +ifneq ($(CONFIG_ARMV7_PSCI),)
 777 +obj-y  += psci.o
 778 +endif
 779 +
 780  obj-$(CONFIG_KONA) += kona-common/
 781  obj-$(CONFIG_OMAP_COMMON) += omap-common/
 782  obj-$(CONFIG_TEGRA) += tegra-common/
 783 diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
 784 new file mode 100644
 785 index 0000000..a9341e0
 786 --- /dev/null
 787 +++ b/arch/arm/cpu/armv7/psci.S
 788 @@ -0,0 +1,105 @@
 789 +/*
 790 + * Copyright (C) 2013 - ARM Ltd
 791 + * Author: Marc Zyngier <marc.zyngier@arm.com>
 792 + *
 793 + * This program is free software; you can redistribute it and/or modify
 794 + * it under the terms of the GNU General Public License version 2 as
 795 + * published by the Free Software Foundation.
 796 + *
 797 + * This program is distributed in the hope that it will be useful,
 798 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 799 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 800 + * GNU General Public License for more details.
 801 + *
 802 + * You should have received a copy of the GNU General Public License
 803 + * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 804 + */
 805 +
 806 +#include <config.h>
 807 +#include <linux/linkage.h>
 808 +#include <asm/psci.h>
 809 +
 810 +       .pushsection ._secure.text, "ax"
 811 +
 812 +       .arch_extension sec
 813 +
 814 +       .align  5
 815 +       .globl _psci_vectors
 816 +_psci_vectors:
 817 +       b       default_psci_vector     @ reset
 818 +       b       default_psci_vector     @ undef
 819 +       b       _smc_psci               @ smc
 820 +       b       default_psci_vector     @ pabort
 821 +       b       default_psci_vector     @ dabort
 822 +       b       default_psci_vector     @ hyp
 823 +       b       default_psci_vector     @ irq
 824 +       b       psci_fiq_enter          @ fiq
 825 +
 826 +ENTRY(psci_fiq_enter)
 827 +       movs    pc, lr
 828 +ENDPROC(psci_fiq_enter)
 829 +.weak psci_fiq_enter
 830 +
 831 +ENTRY(default_psci_vector)
 832 +       movs    pc, lr
 833 +ENDPROC(default_psci_vector)
 834 +.weak default_psci_vector
 835 +
 836 +ENTRY(psci_cpu_suspend)
 837 +ENTRY(psci_cpu_off)
 838 +ENTRY(psci_cpu_on)
 839 +ENTRY(psci_migrate)
 840 +       mov     r0, #ARM_PSCI_RET_NI    @ Return -1 (Not Implemented)
 841 +       mov     pc, lr
 842 +ENDPROC(psci_migrate)
 843 +ENDPROC(psci_cpu_on)
 844 +ENDPROC(psci_cpu_off)
 845 +ENDPROC(psci_cpu_suspend)
 846 +.weak psci_cpu_suspend
 847 +.weak psci_cpu_off
 848 +.weak psci_cpu_on
 849 +.weak psci_migrate
 850 +
 851 +_psci_table:
 852 +       .word   ARM_PSCI_FN_CPU_SUSPEND
 853 +       .word   psci_cpu_suspend
 854 +       .word   ARM_PSCI_FN_CPU_OFF
 855 +       .word   psci_cpu_off
 856 +       .word   ARM_PSCI_FN_CPU_ON
 857 +       .word   psci_cpu_on
 858 +       .word   ARM_PSCI_FN_MIGRATE
 859 +       .word   psci_migrate
 860 +       .word   0
 861 +       .word   0
 862 +
 863 +_smc_psci:
 864 +       push    {r3-r7,lr}
 865 +
 866 +       @ Switch to secure
 867 +       mrc     p15, 0, r7, c1, c1, 0
 868 +       bic     r4, r7, #1
 869 +       mcr     p15, 0, r4, c1, c1, 0
 870 +       isb
 871 +
 872 +       adr     r4, _psci_table
 873 +1:     ldr     r5, [r4]        @ Load PSCI function ID
 874 +       ldr     r6, [r4, #4]    @ Load target PC
 875 +       cmp     r5, #0          @ If reach the end, bail out
 876 +       mvneq   r0, #0          @ Return -1 (Not Implemented)
 877 +       beq     2f
 878 +       cmp     r0, r5          @ If not matching, try next entry
 879 +       addne   r4, r4, #8
 880 +       bne     1b
 881 +       cmp     r6, #0          @ Not implemented
 882 +       moveq   r0, #ARM_PSCI_RET_NI
 883 +       beq     2f
 884 +
 885 +       blx     r6              @ Execute PSCI function
 886 +
 887 +       @ Switch back to non-secure
 888 +       mcr     p15, 0, r7, c1, c1, 0
 889 +
 890 +2:     pop     {r3-r7, lr}
 891 +       movs    pc, lr          @ Return to the kernel
 892 +
 893 +       .popsection
 894 diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h
 895 new file mode 100644
 896 index 0000000..704b4b0
 897 --- /dev/null
 898 +++ b/arch/arm/include/asm/psci.h
 899 @@ -0,0 +1,35 @@
 900 +/*
 901 + * Copyright (C) 2013 - ARM Ltd
 902 + * Author: Marc Zyngier <marc.zyngier@arm.com>
 903 + *
 904 + * This program is free software; you can redistribute it and/or modify
 905 + * it under the terms of the GNU General Public License version 2 as
 906 + * published by the Free Software Foundation.
 907 + *
 908 + * This program is distributed in the hope that it will be useful,
 909 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 910 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 911 + * GNU General Public License for more details.
 912 + *
 913 + * You should have received a copy of the GNU General Public License
 914 + * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 915 + */
 916 +
 917 +#ifndef __ARM_PSCI_H__
 918 +#define __ARM_PSCI_H__
 919 +
 920 +/* PSCI interface */
 921 +#define ARM_PSCI_FN_BASE               0x95c1ba5e
 922 +#define ARM_PSCI_FN(n)                 (ARM_PSCI_FN_BASE + (n))
 923 +
 924 +#define ARM_PSCI_FN_CPU_SUSPEND                ARM_PSCI_FN(0)
 925 +#define ARM_PSCI_FN_CPU_OFF            ARM_PSCI_FN(1)
 926 +#define ARM_PSCI_FN_CPU_ON             ARM_PSCI_FN(2)
 927 +#define ARM_PSCI_FN_MIGRATE            ARM_PSCI_FN(3)
 928 +
 929 +#define ARM_PSCI_RET_SUCCESS           0
 930 +#define ARM_PSCI_RET_NI                        (-1)
 931 +#define ARM_PSCI_RET_INVAL             (-2)
 932 +#define ARM_PSCI_RET_DENIED            (-3)
 933 +
 934 +#endif /* __ARM_PSCI_H__ */
 935 From 0ca6171c385fed00125b320592ee94922f44f13a Mon Sep 17 00:00:00 2001
 936 From: Marc Zyngier <marc.zyngier@arm.com>
 937 Date: Sat, 7 Dec 2013 11:19:14 +0000
 938 Subject: [PATCH] ARM: HYP/non-sec: add the option for a second-stage monitor
 939
 940 Allow the switch to a second stage secure monitor just before
 941 switching to non-secure.
 942
 943 This allows a resident piece of firmware to be active once the
 944 kernel has been entered (the u-boot monitor is dead anyway,
 945 its pages being reused).
 946
 947 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 948 ---
 949  arch/arm/cpu/armv7/nonsec_virt.S | 13 +++++++++++--
 950  1 file changed, 11 insertions(+), 2 deletions(-)
 951
 952 diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
 953 index 2a43e3c..745670e 100644
 954 --- a/arch/arm/cpu/armv7/nonsec_virt.S
 955 +++ b/arch/arm/cpu/armv7/nonsec_virt.S
 956 @@ -44,10 +44,19 @@ _monitor_vectors:
 957   * ip: target PC
 958   */
 959  _secure_monitor:
 960 +#ifdef CONFIG_ARMV7_PSCI
 961 +       ldr     r5, =_psci_vectors              @ Switch to the next monitor
 962 +       mcr     p15, 0, r5, c12, c0, 1
 963 +       isb
 964 +
 965 +       @ Obtain a secure stack, and configure the PSCI backend
 966 +       bl      psci_arch_init
 967 +#endif
 968 +
 969         mrc     p15, 0, r5, c1, c1, 0           @ read SCR
 970 -       bic     r5, r5, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
 971 +       bic     r5, r5, #0x4a                   @ clear IRQ, EA, nET bits
 972         orr     r5, r5, #0x31                   @ enable NS, AW, FW bits
 973 -
 974 +                                               @ FIQ preserved for secure mode
 975         mov     r6, #SVC_MODE                   @ default mode is SVC
 976         is_cpu_virt_capable r4
 977  #ifdef CONFIG_ARMV7_VIRT
 978 From 9aa373162eb2cc0055a6e4ecd46977c911de1124 Mon Sep 17 00:00:00 2001
 979 From: Ma Haijun <mahaijuns@gmail.com>
 980 Date: Sat, 15 Feb 2014 12:51:10 +0000
 981 Subject: [PATCH] ARM: convert arch_fixup_memory_node to a generic FDT fixup
 982  function
 983
 984 Some architecture needs extra device tree setup. Instead of adding
 985 yet another hook, convert arch_fixup_memory_node to be a generic
 986 FDT fixup function.
 987
 988 [maz: collapsed 3 patches into one, rewrote commit message]
 989
 990 Signed-off-by: Ma Haijun <mahaijuns@gmail.com>
 991 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
 992 ---
 993  arch/arm/lib/bootm-fdt.c | 2 +-
 994  arch/arm/lib/bootm.c     | 2 +-
 995  common/image-fdt.c       | 7 +++++--
 996  include/common.h         | 6 +++---
 997  4 files changed, 10 insertions(+), 7 deletions(-)
 998
 999 diff --git a/arch/arm/lib/bootm-fdt.c b/arch/arm/lib/bootm-fdt.c
1000 index e40691d..8394e15 100644
1001 --- a/arch/arm/lib/bootm-fdt.c
1002 +++ b/arch/arm/lib/bootm-fdt.c
1003 @@ -20,7 +20,7 @@
1004
1005  DECLARE_GLOBAL_DATA_PTR;
1006
1007 -int arch_fixup_memory_node(void *blob)
1008 +int arch_fixup_fdt(void *blob)
1009  {
1010         bd_t *bd = gd->bd;
1011         int bank;
1012 diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
1013 index 61aa14e..4cff6b0 100644
1014 --- a/arch/arm/lib/bootm.c
1015 +++ b/arch/arm/lib/bootm.c
1016 @@ -357,7 +357,7 @@ void boot_prep_vxworks(bootm_headers_t *images)
1017         if (images->ft_addr) {
1018                 off = fdt_path_offset(images->ft_addr, "/memory");
1019                 if (off < 0) {
1020 -                       if (arch_fixup_memory_node(images->ft_addr))
1021 +                       if (arch_fixup_fdt(images->ft_addr))
1022                                 puts("## WARNING: fixup memory failed!\n");
1023                 }
1024         }
1025 diff --git a/common/image-fdt.c b/common/image-fdt.c
1026 index a54a919..6f074de 100644
1027 --- a/common/image-fdt.c
1028 +++ b/common/image-fdt.c
1029 @@ -445,7 +445,7 @@ __weak int ft_verify_fdt(void *fdt)
1030         return 1;
1031  }
1032
1033 -__weak int arch_fixup_memory_node(void *blob)
1034 +__weak int arch_fixup_fdt(void *blob)
1035  {
1036         return 0;
1037  }
1038 @@ -462,7 +462,10 @@ int image_setup_libfdt(bootm_headers_t *images, void *blob,
1039                 puts(" - must RESET the board to recover.\n");
1040                 return -1;
1041         }
1042 -       arch_fixup_memory_node(blob);
1043 +       if (arch_fixup_fdt(blob) < 0) {
1044 +               puts("ERROR: arch specific fdt fixup failed");
1045 +               return -1;
1046 +       }
1047         if (IMAGE_OF_BOARD_SETUP)
1048                 ft_board_setup(blob, gd->bd);
1049         fdt_fixup_ethernet(blob);
1050 diff --git a/include/common.h b/include/common.h
1051 index cbd3c9e..700b015 100644
1052 --- a/include/common.h
1053 +++ b/include/common.h
1054 @@ -326,14 +326,14 @@ int arch_early_init_r(void);
1055  void board_show_dram(ulong size);
1056
1057  /**
1058 - * arch_fixup_memory_node() - Write arch-specific memory information to fdt
1059 + * arch_fixup_fdt() - Write arch-specific information to fdt
1060   *
1061 - * Defined in arch/$(ARCH)/lib/bootm.c
1062 + * Defined in arch/$(ARCH)/lib/bootm-fdt.c
1063   *
1064   * @blob:      FDT blob to write to
1065   * @return 0 if ok, or -ve FDT_ERR_... on failure
1066   */
1067 -int arch_fixup_memory_node(void *blob);
1068 +int arch_fixup_fdt(void *blob);
1069
1070  /* common/flash.c */
1071  void flash_perror (int);
1072 From ccdf689da800c9f1c5226146e936b071c7ec8800 Mon Sep 17 00:00:00 2001
1073 From: Marc Zyngier <marc.zyngier@arm.com>
1074 Date: Sat, 7 Dec 2013 11:19:15 +0000
1075 Subject: [PATCH] ARM: HYP/non-sec/PSCI: emit DT nodes
1076
1077 Generate the PSCI node in the device tree.
1078
1079 Also add a reserve section for the "secure" code that lives in
1080 in normal RAM, so that the kernel knows it'd better not trip on
1081 it.
1082
1083 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
1084 ---
1085  arch/arm/cpu/armv7/Makefile  |   1 +
1086  arch/arm/cpu/armv7/virt-dt.c | 100 +++++++++++++++++++++++++++++++++++++++++++
1087  arch/arm/include/asm/armv7.h |   1 +
1088  arch/arm/lib/bootm-fdt.c     |  11 ++++-
1089  4 files changed, 111 insertions(+), 2 deletions(-)
1090  create mode 100644 arch/arm/cpu/armv7/virt-dt.c
1091
1092 diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
1093 index 0cf5c45..93a5a69 100644
1094 --- a/arch/arm/cpu/armv7/Makefile
1095 +++ b/arch/arm/cpu/armv7/Makefile
1096 @@ -22,6 +22,7 @@ endif
1097  ifneq ($(CONFIG_ARMV7_NONSEC)$(CONFIG_ARMV7_VIRT),)
1098  obj-y  += nonsec_virt.o
1099  obj-y  += virt-v7.o
1100 +obj-y  += virt-dt.o
1101  endif
1102
1103  ifneq ($(CONFIG_ARMV7_PSCI),)
1104 diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c
1105 new file mode 100644
1106 index 0000000..0b0d6a7
1107 --- /dev/null
1108 +++ b/arch/arm/cpu/armv7/virt-dt.c
1109 @@ -0,0 +1,100 @@
1110 +/*
1111 + * Copyright (C) 2013 - ARM Ltd
1112 + * Author: Marc Zyngier <marc.zyngier@arm.com>
1113 + *
1114 + * This program is free software; you can redistribute it and/or modify
1115 + * it under the terms of the GNU General Public License version 2 as
1116 + * published by the Free Software Foundation.
1117 + *
1118 + * This program is distributed in the hope that it will be useful,
1119 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
1120 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
1121 + * GNU General Public License for more details.
1122 + *
1123 + * You should have received a copy of the GNU General Public License
1124 + * along with this program.  If not, see <http://www.gnu.org/licenses/>.
1125 + */
1126 +
1127 +#include <common.h>
1128 +#include <stdio_dev.h>
1129 +#include <linux/ctype.h>
1130 +#include <linux/types.h>
1131 +#include <asm/global_data.h>
1132 +#include <libfdt.h>
1133 +#include <fdt_support.h>
1134 +#include <asm/armv7.h>
1135 +#include <asm/psci.h>
1136 +
1137 +static int fdt_psci(void *fdt)
1138 +{
1139 +#ifdef CONFIG_ARMV7_PSCI
1140 +       int nodeoff;
1141 +       int tmp;
1142 +
1143 +       nodeoff = fdt_path_offset(fdt, "/cpus");
1144 +       if (nodeoff < 0) {
1145 +               printf("couldn't find /cpus\n");
1146 +               return nodeoff;
1147 +       }
1148 +
1149 +       /* add 'enable-method = "psci"' to each cpu node */
1150 +       for (tmp = fdt_first_subnode(fdt, nodeoff);
1151 +            tmp >= 0;
1152 +            tmp = fdt_next_subnode(fdt, tmp)) {
1153 +               const struct fdt_property *prop;
1154 +               int len;
1155 +
1156 +               prop = fdt_get_property(fdt, tmp, "device_type", &len);
1157 +               if (!prop)
1158 +                       continue;
1159 +               if (len < 4)
1160 +                       continue;
1161 +               if (strcmp(prop->data, "cpu"))
1162 +                       continue;
1163 +
1164 +               fdt_setprop_string(fdt, tmp, "enable-method", "psci");
1165 +       }
1166 +
1167 +       nodeoff = fdt_path_offset(fdt, "/psci");
1168 +       if (nodeoff < 0) {
1169 +               nodeoff = fdt_path_offset(fdt, "/");
1170 +               if (nodeoff < 0)
1171 +                       return nodeoff;
1172 +
1173 +               nodeoff = fdt_add_subnode(fdt, nodeoff, "psci");
1174 +               if (nodeoff < 0)
1175 +                       return nodeoff;
1176 +       }
1177 +
1178 +       tmp = fdt_setprop_string(fdt, nodeoff, "compatible", "arm,psci");
1179 +       if (tmp)
1180 +               return tmp;
1181 +       tmp = fdt_setprop_string(fdt, nodeoff, "method", "smc");
1182 +       if (tmp)
1183 +               return tmp;
1184 +       tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_suspend", ARM_PSCI_FN_CPU_SUSPEND);
1185 +       if (tmp)
1186 +               return tmp;
1187 +       tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_off", ARM_PSCI_FN_CPU_OFF);
1188 +       if (tmp)
1189 +               return tmp;
1190 +       tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_on", ARM_PSCI_FN_CPU_ON);
1191 +       if (tmp)
1192 +               return tmp;
1193 +       tmp = fdt_setprop_u32(fdt, nodeoff, "migrate", ARM_PSCI_FN_MIGRATE);
1194 +       if (tmp)
1195 +               return tmp;
1196 +#endif
1197 +       return 0;
1198 +}
1199 +
1200 +int armv7_update_dt(void *fdt)
1201 +{
1202 +#ifndef CONFIG_ARMV7_SECURE_BASE
1203 +       /* secure code lives in RAM, keep it alive */
1204 +       fdt_add_mem_rsv(fdt, (unsigned long)__secure_start,
1205 +                       __secure_end - __secure_start);
1206 +#endif
1207 +
1208 +       return fdt_psci(fdt);
1209 +}
1210 diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
1211 index 11476dd..323f282 100644
1212 --- a/arch/arm/include/asm/armv7.h
1213 +++ b/arch/arm/include/asm/armv7.h
1214 @@ -79,6 +79,7 @@ void v7_outer_cache_inval_range(u32 start, u32 end);
1215  #if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
1216
1217  int armv7_init_nonsec(void);
1218 +int armv7_update_dt(void *fdt);
1219
1220  /* defined in assembly file */
1221  unsigned int _nonsec_init(void);
1222 diff --git a/arch/arm/lib/bootm-fdt.c b/arch/arm/lib/bootm-fdt.c
1223 index 8394e15..ccb76c7 100644
1224 --- a/arch/arm/lib/bootm-fdt.c
1225 +++ b/arch/arm/lib/bootm-fdt.c
1226 @@ -17,13 +17,14 @@
1227
1228  #include <common.h>
1229  #include <fdt_support.h>
1230 +#include <asm/armv7.h>
1231
1232  DECLARE_GLOBAL_DATA_PTR;
1233
1234  int arch_fixup_fdt(void *blob)
1235  {
1236         bd_t *bd = gd->bd;
1237 -       int bank;
1238 +       int bank, ret;
1239         u64 start[CONFIG_NR_DRAM_BANKS];
1240         u64 size[CONFIG_NR_DRAM_BANKS];
1241
1242 @@ -32,5 +33,11 @@ int arch_fixup_fdt(void *blob)
1243                 size[bank] = bd->bi_dram[bank].size;
1244         }
1245
1246 -       return fdt_fixup_memory_banks(blob, start, size, CONFIG_NR_DRAM_BANKS);
1247 +       ret = fdt_fixup_memory_banks(blob, start, size, CONFIG_NR_DRAM_BANKS);
1248 +       if (ret)
1249 +               return ret;
1250 +
1251 +#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
1252 +       return armv7_update_dt(blob);
1253 +#endif
1254  }
1255 From d5ee64675e6481e4f29e48e494ea132cd74786c8 Mon Sep 17 00:00:00 2001
1256 From: Marc Zyngier <marc.zyngier@arm.com>
1257 Date: Sat, 7 Dec 2013 11:19:17 +0000
1258 Subject: [PATCH] sunxi: HYP/non-sec: add sun7i PSCI backend
1259
1260 So far, only supporting the CPU_ON method.
1261 Other functions can be added later.
1262
1263 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
1264 ---
1265  arch/arm/cpu/armv7/sunxi/Makefile |   3 +
1266  arch/arm/cpu/armv7/sunxi/psci.S   | 162 ++++++++++++++++++++++++++++++++++++++
1267  include/configs/sun7i.h           |   6 ++
1268  3 files changed, 171 insertions(+)
1269  create mode 100644 arch/arm/cpu/armv7/sunxi/psci.S
1270
1271 diff --git a/arch/arm/cpu/armv7/sunxi/Makefile b/arch/arm/cpu/armv7/sunxi/Makefile
1272 index 19e4b17..0df6772 100644
1273 --- a/arch/arm/cpu/armv7/sunxi/Makefile
1274 +++ b/arch/arm/cpu/armv7/sunxi/Makefile
1275 @@ -32,6 +32,9 @@ obj-y += cpu_info.o
1276  ifdef CONFIG_CMD_WATCHDOG
1277  obj-$(CONFIG_CMD_WATCHDOG)     += cmd_watchdog.o
1278  endif
1279 +ifdef CONFIG_ARMV7_PSCI
1280 +obj-y  += psci.o
1281 +endif
1282  endif
1283
1284  ifdef CONFIG_SPL_BUILD
1285 diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S
1286 new file mode 100644
1287 index 0000000..0084c81
1288 --- /dev/null
1289 +++ b/arch/arm/cpu/armv7/sunxi/psci.S
1290 @@ -0,0 +1,162 @@
1291 +/*
1292 + * Copyright (C) 2013 - ARM Ltd
1293 + * Author: Marc Zyngier <marc.zyngier@arm.com>
1294 + *
1295 + * Based on code by Carl van Schaik <carl@ok-labs.com>.
1296 + *
1297 + * This program is free software; you can redistribute it and/or modify
1298 + * it under the terms of the GNU General Public License version 2 as
1299 + * published by the Free Software Foundation.
1300 + *
1301 + * This program is distributed in the hope that it will be useful,
1302 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
1303 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
1304 + * GNU General Public License for more details.
1305 + *
1306 + * You should have received a copy of the GNU General Public License
1307 + * along with this program.  If not, see <http://www.gnu.org/licenses/>.
1308 + */
1309 +
1310 +#include <config.h>
1311 +#include <asm/psci.h>
1312 +#include <asm/arch/cpu.h>
1313 +
1314 +/*
1315 + * Memory layout:
1316 + *
1317 + * SECURE_RAM to text_end :
1318 + *     ._secure_text section
1319 + * text_end to ALIGN_PAGE(text_end):
1320 + *     nothing
1321 + * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
1322 + *     1kB of stack per CPU (4 CPUs max).
1323 + */
1324 +
1325 +       .pushsection ._secure.text, "ax"
1326 +
1327 +       .arch_extension sec
1328 +
1329 +#define        ONE_MS                  (CONFIG_SYS_CLK_FREQ / 1000)
1330 +#define        TEN_MS                  (10 * ONE_MS)
1331 +
1332 +.macro timer_wait      reg, ticks
1333 +       @ Program CNTP_TVAL
1334 +       movw    \reg, #(\ticks & 0xffff)
1335 +       movt    \reg, #(\ticks >> 16)
1336 +       mcr     p15, 0, \reg, c14, c2, 0
1337 +       isb
1338 +       @ Enable physical timer, mask interrupt
1339 +       mov     \reg, #3
1340 +       mcr     p15, 0, \reg, c14, c2, 1
1341 +       @ Poll physical timer until ISTATUS is on
1342 +1:     isb
1343 +       mrc     p15, 0, \reg, c14, c2, 1
1344 +       ands    \reg, \reg, #4
1345 +       bne     1b
1346 +       @ Disable timer
1347 +       mov     \reg, #0
1348 +       mcr     p15, 0, \reg, c14, c2, 1
1349 +       isb
1350 +.endm
1351 +
1352 +.globl psci_arch_init
1353 +psci_arch_init:
1354 +       mrc     p15, 0, r5, c1, c1, 0   @ Read SCR
1355 +       bic     r5, r5, #1              @ Secure mode
1356 +       mcr     p15, 0, r5, c1, c1, 0   @ Write SCR
1357 +       isb
1358 +
1359 +       mrc     p15, 0, r4, c0, c0, 5   @ MPIDR
1360 +       and     r4, r4, #3              @ cpu number in cluster
1361 +       mov     r5, #400                @ 1kB of stack per CPU
1362 +       mul     r4, r4, r5
1363 +
1364 +       adr     r5, text_end            @ end of text
1365 +       add     r5, r5, #0x2000         @ Skip two pages
1366 +       lsr     r5, r5, #12             @ Align to start of page
1367 +       lsl     r5, r5, #12
1368 +       sub     sp, r5, r4              @ here's our stack!
1369 +
1370 +       bx      lr
1371 +
1372 +       @ r1 = target CPU
1373 +       @ r2 = target PC
1374 +.globl psci_cpu_on
1375 +psci_cpu_on:
1376 +       adr     r0, _target_pc
1377 +       str     r2, [r0]
1378 +       dsb
1379 +
1380 +       movw    r0, #(SUNXI_CPUCFG_BASE & 0xffff)
1381 +       movt    r0, #(SUNXI_CPUCFG_BASE >> 16)
1382 +
1383 +       @ CPU mask
1384 +       and     r1, r1, #3      @ only care about first cluster
1385 +       mov     r4, #1
1386 +       lsl     r4, r4, r1
1387 +
1388 +       adr     r6, _sunxi_cpu_entry
1389 +       str     r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)
1390 +
1391 +       @ Assert reset on target CPU
1392 +       mov     r6, #0
1393 +       lsl     r5, r1, #6      @ 64 bytes per CPU
1394 +       add     r5, r5, #0x40   @ Offset from base
1395 +       add     r5, r5, r0      @ CPU control block
1396 +       str     r6, [r5]        @ Reset CPU
1397 +
1398 +       @ l1 invalidate
1399 +       ldr     r6, [r0, #0x184]
1400 +       bic     r6, r6, r4
1401 +       str     r6, [r0, #0x184]
1402 +
1403 +       @ Lock CPU
1404 +       ldr     r6, [r0, #0x1e4]
1405 +       bic     r6, r6, r4
1406 +       str     r6, [r0, #0x1e4]
1407 +
1408 +       @ Release power clamp
1409 +       movw    r6, #0x1ff
1410 +       movt    r6, #0
1411 +1:     lsrs    r6, r6, #1
1412 +       str     r6, [r0, #0x1b0]
1413 +       bne     1b
1414 +
1415 +       timer_wait r1, TEN_MS
1416 +
1417 +       @ Clear power gating
1418 +       ldr     r6, [r0, #0x1b4]
1419 +       bic     r6, r6, #1
1420 +       str     r6, [r0, #0x1b4]
1421 +
1422 +       @ Deassert reset on target CPU
1423 +       mov     r6, #3
1424 +       str     r6, [r5]
1425 +
1426 +       @ Unlock CPU
1427 +       ldr     r6, [r0, #0x1e4]
1428 +       orr     r6, r6, r4
1429 +       str     r6, [r0, #0x1e4]
1430 +
1431 +       mov     r0, #ARM_PSCI_RET_SUCCESS       @ Return PSCI_RET_SUCCESS
1432 +       mov     pc, lr
1433 +
1434 +_target_pc:
1435 +       .word   0
1436 +
1437 +_sunxi_cpu_entry:
1438 +       @ Set SMP bit
1439 +       mrc     p15, 0, r0, c1, c0, 1
1440 +       orr     r0, r0, #0x40
1441 +       mcr     p15, 0, r0, c1, c0, 1
1442 +       isb
1443 +
1444 +       bl      _nonsec_init
1445 +       bl      psci_arch_init
1446 +
1447 +       adr     r0, _target_pc
1448 +       ldr     r0, [r0]
1449 +       b       _do_nonsec_entry
1450 +
1451 +text_end:
1452 +       .popsection
1453 diff --git a/include/configs/sun7i.h b/include/configs/sun7i.h
1454 index 11cc9ea..bae7b37 100644
1455 --- a/include/configs/sun7i.h
1456 +++ b/include/configs/sun7i.h
1457 @@ -22,6 +22,12 @@
1458  #define CONFIG_BOARD_POSTCLK_INIT 1
1459  #endif
1460
1461 +#define CONFIG_ARMV7_VIRT              1
1462 +#define CONFIG_ARMV7_NONSEC            1
1463 +#define CONFIG_ARMV7_PSCI              1
1464 +#define CONFIG_ARMV7_PSCI_NR_CPUS      2
1465 +#define CONFIG_ARMV7_SECURE_BASE       SUNXI_SRAM_B_BASE
1466 +
1467  /*
1468   * Include common sunxi configuration where most the settings are
1469   */
1470 From a74a847c3727209a45c30a80c01b930938941dd4 Mon Sep 17 00:00:00 2001
1471 From: Marc Zyngier <marc.zyngier@arm.com>
1472 Date: Sat, 7 Dec 2013 11:19:18 +0000
1473 Subject: [PATCH] sunxi: HYP/non-sec: configure CNTFRQ on all CPUs
1474
1475 CNTFRQ needs to be properly configured on all CPUs. Otherwise,
1476 virtual machines hoping to find valuable information on secondary
1477 CPUs will be disapointed...
1478
1479 Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
1480 ---
1481  include/configs/sun7i.h | 1 +
1482  1 file changed, 1 insertion(+)
1483
1484 diff --git a/include/configs/sun7i.h b/include/configs/sun7i.h
1485 index bae7b37..58a254b 100644
1486 --- a/include/configs/sun7i.h
1487 +++ b/include/configs/sun7i.h
1488 @@ -27,6 +27,7 @@
1489  #define CONFIG_ARMV7_PSCI              1
1490  #define CONFIG_ARMV7_PSCI_NR_CPUS      2
1491  #define CONFIG_ARMV7_SECURE_BASE       SUNXI_SRAM_B_BASE
1492 +#define CONFIG_SYS_CLK_FREQ            24000000
1493
1494  /*
1495   * Include common sunxi configuration where most the settings are