SHARE
TWEET

Untitled

a guest Feb 17th, 2015 1,672 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define CPUID_ARM_VIRT_SHIFT            12
  2. #define CPUID_ARM_VIRT_MASK             (0xF << CPUID_ARM_VIRT_SHIFT)
  3. #define CPUID_ARM_GENTIMER_SHIFT        16
  4. #define CPUID_ARM_GENTIMER_MASK         (0xF << CPUID_ARM_GENTIMER_SHIFT)
  5.  
  6. #define BCM2708_PERI_BASE 0x3f000000
  7.  
  8. .arch_extension sec
  9. .arch_extension virt
  10.  
  11. .section .init
  12. .globl _start
  13. /* the vector table for secure state and HYP mode */
  14. _start:
  15.         b jmp_loader    /* reset */
  16.         .word 0 /* undef */
  17.         adr pc, _secure_monitor
  18.         .word 0
  19.         .word 0
  20.         .word 0
  21.         .word 0
  22.         .word 0
  23.  
  24. /*
  25.  * secure monitor handler
  26.  * U-boot calls this "software interrupt" in start.S
  27.  * This is executed on a "smc" instruction, we use a "smc #0" to switch
  28.  * to non-secure state.
  29.  * We use only r0 and r1 here, due to constraints in the caller.
  30.  */
  31. _secure_monitor:
  32.         mrc     p15, 0, r1, c1, c1, 0           @ read SCR
  33.         bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
  34.         orr     r1, r1, #0x31                   @ enable NS, AW, FW bits
  35.  
  36.         @mrc    p15, 0, r0, c0, c1, 1           @ read ID_PFR1
  37.         @and    r0, r0, #CPUID_ARM_VIRT_MASK    @ mask virtualization bits
  38.         @cmp    r0, #(1 << CPUID_ARM_VIRT_SHIFT)
  39.         orr     r1, r1, #0x100                  @ allow HVC instruction
  40.  
  41.         mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)
  42.  
  43.         mrc     p15, 0, r0, c12, c0, 1          @ get MVBAR value
  44.         mcr     p15, 4, r0, c12, c0, 0          @ write HVBAR
  45.  
  46.         @ Reset CNTVOFF to 0 before leaving monitor mode
  47.         mov     r0, #0
  48.         mcrr    p15, 4, r0, r0, c14             @ Reset CNTVOFF to zero
  49. 1:
  50.         movs    pc, lr                          @ return to non-secure SVC
  51.  
  52. jmp_loader:
  53. @ Check which proc we are and run proc 0 only
  54.  
  55.         mrc p15, 0, r0, c1, c0, 0 @ Read System Control Register
  56.         orr r0, r0, #(1<<2)       @ cache enable
  57.         orr r0, r0, #(1<<12)      @ icache enable
  58.         mcr p15, 0, r0, c1, c0, 0 @ Write System Control Register
  59.  
  60.         mrc p15, 0, r0, c1, c0, 1 @ Read Auxiliary Control Register
  61.         orr r0, r0, #(1<<6)       @ SMP
  62.         mcr p15, 0, r0, c1, c0, 1 @ Write Auxiliary Control Register
  63.  
  64.         mov r0, #1
  65.         mcr p15, 0, r0, c14, c3, 1 @ CNTV_CTL (enable=1, imask=0)
  66.  
  67. @ set to non-sec
  68.         movw    r1, #0x3fff
  69.         movt    r1, #0x0006
  70.         mcr     p15, 0, r1, c1, c1, 2           @ NSACR = all copros to non-sec
  71. @ timer frequency
  72.         ldr     r1, =19200000
  73.         mcr     p15, 0, r1, c14, c0, 0          @ write CNTFRQ
  74.  
  75.         adr     r1, _start
  76.         mcr     p15, 0, r1, c12, c0, 1          @ set MVBAR to secure vectors
  77.         mrc     p15, 0, ip, c12, c0, 0          @ save secure copy of VBAR
  78.  
  79.         isb
  80.         smc     #0                              @ call into MONITOR mode
  81.  
  82.         mcr     p15, 0, ip, c12, c0, 0          @ write non-secure copy of VBAR
  83.  
  84.         mov     r4, #0x8000
  85.         mrc     p15, 0, r0, c0, c0, 5
  86.         ubfx    r0, r0, #0, #2
  87.         cmp     r0, #0
  88.         beq     9f
  89.  
  90.         cmp     r0, #0xff
  91.         bge     10f
  92.  
  93.         ldr     r5, =0x4000008C         @ mbox
  94.         ldr     r3, =0x00000000         @ magic
  95.         str     r3, [r5, r0, lsl #4]
  96.  
  97.         ldr     r5, =0x400000CC         @ mbox
  98. 1:
  99.         ldr     r4, [r5, r0, lsl #4]
  100.         cmp     r4, r3
  101.         beq     1b
  102.  
  103. @ clear mailbox
  104.         str     r4, [r5, r0, lsl #4]
  105.  
  106. 9:
  107.         @ldr    r5, = (BCM2708_PERI_BASE + 0x300c)              @ stc0
  108.         @ldr    r6, = 0xaff0aff0
  109.         @str    r4, [r5, r0, lsl #2]
  110.  
  111.  
  112. @       msr     cpsr_fsxc, #0xd3
  113.         mov     r0, #0
  114.         ldr     r1, =3138               @ BCM2708 machine id
  115.         ldr     r2, =0x100              @ ATAGS
  116.         bx      r4
  117. 10:
  118.         wfi
  119.         b       10b
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top