newlib/libgloss/aarch64/cpu-init/rdimon-aem-el3.S

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)