[src/trunk]: src/sys/arch Check for hypervisor mode in cortex_init and exit i...

[skrll <skrll%NetBSD.org@localhost>]

details:   https://anonhg.NetBSD.org/src/rev/8f7253fe9f72
branches:  trunk
changeset: 342340:8f7253fe9f72
user:      skrll <skrll%NetBSD.org@localhost>
date:      Thu Dec 17 08:02:42 2015 +0000

description:
Check for hypervisor mode in cortex_init and exit if the cpu started
there.

Needed by latest RPI firmware.

diffstat:

 sys/arch/arm/cortex/a9_mpsubr.S  |  25 +++++++++++++++++++++----
 sys/arch/evbarm/rpi/rpi2_start.S |   9 ++-------
 2 files changed, 23 insertions(+), 11 deletions(-)

diffs (72 lines):

diff -r cdbb1a360237 -r 8f7253fe9f72 sys/arch/arm/cortex/a9_mpsubr.S
--- a/sys/arch/arm/cortex/a9_mpsubr.S   Thu Dec 17 04:36:56 2015 +0000
+++ b/sys/arch/arm/cortex/a9_mpsubr.S   Thu Dec 17 08:02:42 2015 +0000
@@ -1,4 +1,4 @@
-/*     $NetBSD: a9_mpsubr.S,v 1.44 2015/11/25 08:39:45 skrll Exp $     */
+/*     $NetBSD: a9_mpsubr.S,v 1.45 2015/12/17 08:02:42 skrll Exp $     */
 /*-
  * Copyright (c) 2012 The NetBSD Foundation, Inc.
  * All rights reserved.
@@ -347,9 +347,26 @@
 cortex_init:
        mov     r10, lr                         // save lr
 
-       cpsid   if, #PSR_SVC32_MODE             // SVC32 with no interrupts
-        mov    r0, #0
-        msr    spsr_sxc, r0                    // set SPSR[23:8] to known value
+       /* Leave HYP mode and move into supervisor mode with IRQs/FIQs disabled. */
+       mrs     r0, cpsr
+       and     r0, r0, #(PSR_MODE)     /* Mode is in the low 5 bits of CPSR */
+       teq     r0, #(PSR_HYP32_MODE)   /* Hyp Mode? */
+       bne     1f
+
+       /* Ensure that IRQ, and FIQ will be disabled after eret */
+       mrs     r0, cpsr
+       bic     r0, r0, #(PSR_MODE)
+       orr     r0, r0, #(PSR_SVC32_MODE)
+       orr     r0, r0, #(I32_bit | F32_bit)
+       msr     spsr_cxsf, r0
+       /* Exit hypervisor mode */
+       adr     lr, 1f
+       msr     elr_hyp, lr
+       eret
+1:
+
+       mov     r0, #0
+       msr     spsr_sxc, r0                    // set SPSR[23:8] to known value
 
 #if 0
        mrc     p14, 0, r0, c0, c0, 0           // MIDR read
diff -r cdbb1a360237 -r 8f7253fe9f72 sys/arch/evbarm/rpi/rpi2_start.S
--- a/sys/arch/evbarm/rpi/rpi2_start.S  Thu Dec 17 04:36:56 2015 +0000
+++ b/sys/arch/evbarm/rpi/rpi2_start.S  Thu Dec 17 08:02:42 2015 +0000
@@ -38,7 +38,7 @@
 
 #include "assym.h"
 
-RCSID("$NetBSD: rpi2_start.S,v 1.2 2015/04/18 11:03:31 skrll Exp $")
+RCSID("$NetBSD: rpi2_start.S,v 1.3 2015/12/17 08:02:42 skrll Exp $")
 
 #if defined(VERBOSE_INIT_ARM)
 #define        XPUTC(n)        mov r0, n; bl plputc
@@ -78,11 +78,6 @@
 #ifdef __ARMEB__
        setend  be                      /* force big endian */
 #endif
-       mov     r9, #0
-
-       /* Move into supervisor mode and disable IRQs/FIQs. */
-       cpsid   if, #PSR_SVC32_MODE
-
        /*
         * Save any arguments passed to us.
         */
@@ -101,7 +96,7 @@
        stmia   r4, {r0-r3}             // Save the arguments
 
        /*
-        * Turn on the SMP bit
+        * Setup the CPU
         */
        bl      cortex_init