200 lines
5.4 KiB
ArmAsm
200 lines
5.4 KiB
ArmAsm
/* Copyright (c) 2009, 2010, 2011, 2012 ARM Ltd. All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions
|
|
are met:
|
|
1. Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
2. Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
3. The name of the company may not be used to endorse or promote
|
|
products derived from this software without specific prior written
|
|
permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR IMPLIED
|
|
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
|
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
|
|
IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
|
|
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
|
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
|
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
|
|
|
|
#include "newlib.h"
|
|
#include "svc.h"
|
|
|
|
/* ANSI concatenation macros. */
|
|
#define CONCAT(a, b) CONCAT2(a, b)
|
|
#define CONCAT2(a, b) a ## b
|
|
|
|
#ifdef __USER_LABEL_PREFIX__
|
|
#define FUNCTION( name ) CONCAT (__USER_LABEL_PREFIX__, name)
|
|
#else
|
|
#error __USER_LABEL_PREFIX is not defined
|
|
#endif
|
|
|
|
|
|
.text
|
|
.align 2
|
|
_init_vectors:
|
|
/* Installs a table of exception vectors to catch and handle all
|
|
exceptions by terminating the process with a diagnostic. */
|
|
adr x0, vectors
|
|
msr vbar_el3, x0
|
|
msr vbar_el2, x0
|
|
msr vbar_el1, x0
|
|
ret
|
|
|
|
curr_sp0_sync:
|
|
curr_sp0_irq:
|
|
curr_sp0_fiq:
|
|
curr_sp0_serror:
|
|
curr_spx_sync:
|
|
curr_spx_irq:
|
|
curr_spx_fiq:
|
|
curr_spx_serror:
|
|
lower_a64_sync:
|
|
lower_a64_irq:
|
|
lower_a64_fiq:
|
|
lower_a64_serror:
|
|
lower_a32_sync:
|
|
lower_a32_irq:
|
|
lower_a32_fiq:
|
|
lower_a32_serror:
|
|
mov x0, 2
|
|
adr x1, .LC3
|
|
mov x2, 26
|
|
bl FUNCTION (write)
|
|
mov x0, 126
|
|
b FUNCTION (exit) /* Cannot return. */
|
|
.LC3:
|
|
.string "Terminated by exception.\n"
|
|
|
|
.macro ventry label
|
|
.align 7
|
|
b \label
|
|
.endm
|
|
|
|
/* AArch64 Exception Model -- 3.5.5 Exception Vectors. */
|
|
|
|
.align 12
|
|
vectors:
|
|
/* Current EL with SP0. */
|
|
ventry curr_sp0_sync /* Synchronous */
|
|
ventry curr_sp0_irq /* Irq/vIRQ */
|
|
ventry curr_sp0_fiq /* Fiq/vFIQ */
|
|
ventry curr_sp0_serror /* SError/VSError */
|
|
|
|
/* Current EL with SPx. */
|
|
ventry curr_spx_sync /* Synchronous */
|
|
ventry curr_spx_irq /* IRQ/vIRQ */
|
|
ventry curr_spx_fiq /* FIQ/vFIQ */
|
|
ventry curr_spx_serror /* SError/VSError */
|
|
|
|
/* Lower EL using AArch64. */
|
|
ventry lower_a64_sync /* Synchronous */
|
|
ventry lower_a64_irq /* IRQ/vIRQ */
|
|
ventry lower_a64_fiq /* FIQ/vFIQ */
|
|
ventry lower_a64_serror /* SError/VSError */
|
|
|
|
/* Lower EL using AArch32. */
|
|
ventry lower_a32_sync /* Synchronous */
|
|
ventry lower_a32_irq /* IRQ/vIRQ */
|
|
ventry lower_a32_fiq /* FIQ/vFIQ */
|
|
ventry lower_a32_serror /* SError/VSError */
|
|
|
|
|
|
.text
|
|
.align 2
|
|
_flat_map:
|
|
/* Page table setup (identity mapping). */
|
|
adrp x0, ttb
|
|
add x0, x0, :lo12:ttb
|
|
msr ttbr0_el3, x0
|
|
adr x1, . /* phys address */
|
|
bic x1, x1, #(1 << 30) - 1 /* 1GB block alignment */
|
|
add x2, x0, x1, lsr #(30 - 3) /* offset in level 1 page
|
|
table */
|
|
mov x3, #0x401 /* page table attributes
|
|
(AF, block) */
|
|
orr x1, x1, x3
|
|
mov x3, #(1 << 30) /* 1GB block */
|
|
str x1, [x2], #8 /* 1st GB */
|
|
add x1, x1, x3
|
|
str x1, [x2] /* 2nd GB */
|
|
|
|
/* Setup/enable the MMU. */
|
|
|
|
/* RES1, RES1, 40-bit PA, 39-bit VA, inner/outer cacheable WB */
|
|
ldr x0, =(1 << 31) | (1 << 23) | (2 << 16) | 25 | (3 << 10) | (3 << 8)
|
|
msr tcr_el3, x0
|
|
|
|
mov x0, #0xee /* Inner/outer cacheable WB */
|
|
msr mair_el3, x0
|
|
isb
|
|
|
|
mrs x0, sctlr_el3
|
|
ldr x1, =0x100d /* bits I(12) SA(3) C(2) M(0) */
|
|
bic x0, x0, #(1 << 1) /* clear bit A(1) */
|
|
bic x0, x0, #(1 << 19) /* clear WXN */
|
|
orr x0, x0, x1 /* set bits */
|
|
|
|
dsb sy
|
|
msr sctlr_el3, x0
|
|
isb
|
|
|
|
/* Determine if SVE is available. */
|
|
mrs x0, id_aa64pfr0_el1
|
|
tbz x0, 32, .Lnosve
|
|
|
|
/* set up CPTR_EL3.TZ to 1. */
|
|
mrs x0, cptr_el3
|
|
|
|
/* TZ is bit 8 of CPTR_EL3. */
|
|
orr x0, x0, #0x100
|
|
msr cptr_el3, x0
|
|
isb
|
|
|
|
/* set up vector lenght in ZCR_EL3 (4 LSB). */
|
|
mov x2, #0xF
|
|
|
|
/* Try to set the maximum value supported by the architecture (2048).
|
|
SVE Arch.
|
|
|
|
"If this field is set to a value that is not supported by the
|
|
implementation then reading the register must return the highest
|
|
supported vector length that is less than the value written." */
|
|
mrs x1, s3_6_c1_c2_0 /* mrs x1, zcr_el3. */
|
|
bfi x1, x2, 0, 4
|
|
msr s3_6_c1_c2_0, x1 /* msr zcr_el3, x1. */
|
|
isb
|
|
.Lnosve:
|
|
ret
|
|
|
|
.data
|
|
.align 12
|
|
ttb:
|
|
.space 4096, 0
|
|
|
|
|
|
.text
|
|
.align 2
|
|
.global FUNCTION (_cpu_init_hook)
|
|
.type FUNCTION (_cpu_init_hook), %function
|
|
.cfi_sections .debug_frame
|
|
FUNCTION (_cpu_init_hook):
|
|
.cfi_startproc
|
|
str x30, [sp, -16]!
|
|
.cfi_def_cfa_offset 16
|
|
.cfi_offset 30, -16
|
|
bl _init_vectors
|
|
bl _flat_map
|
|
ldr x30, [sp], 16
|
|
.cfi_restore 30
|
|
ret
|
|
.cfi_endproc
|
|
.size FUNCTION (_cpu_init_hook), .-FUNCTION (_cpu_init_hook)
|