aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorAchin Gupta <achin.gupta@arm.com>2013-10-25 09:08:21 +0100
committerJames Morrissey <james.morrissey@arm.com>2013-10-25 09:37:16 +0100
commit4f6ad66ae9fcc8bcb3b0fcee10b7ab1ffcaf1a56 (patch)
tree475db5d74370cb62b02afab0900774955a59702f
downloadarm-trusted-firmware-4f6ad66ae9fcc8bcb3b0fcee10b7ab1ffcaf1a56.tar.gz
ARMv8 Trusted Firmware release v0.2v0.2
-rw-r--r--Makefile169
-rw-r--r--arch/aarch64/cpu/cpu_helpers.S66
-rw-r--r--arch/system/gic/aarch64/gic_v3_sysregs.S89
-rw-r--r--arch/system/gic/gic.h217
-rw-r--r--arch/system/gic/gic_v2.c426
-rw-r--r--arch/system/gic/gic_v3.c46
-rw-r--r--bl1/aarch64/bl1_arch_setup.c83
-rw-r--r--bl1/aarch64/bl1_entrypoint.S93
-rw-r--r--bl1/aarch64/early_exceptions.S216
-rw-r--r--bl1/bl1.ld.S90
-rw-r--r--bl1/bl1.mk46
-rw-r--r--bl1/bl1_main.c132
-rw-r--r--bl2/aarch64/bl2_arch_setup.c42
-rw-r--r--bl2/aarch64/bl2_entrypoint.S94
-rw-r--r--bl2/bl2.ld.S85
-rw-r--r--bl2/bl2.mk48
-rw-r--r--bl2/bl2_main.c143
-rw-r--r--bl31/aarch64/bl31_arch_setup.c100
-rw-r--r--bl31/aarch64/bl31_entrypoint.S121
-rw-r--r--bl31/aarch64/exception_handlers.c184
-rw-r--r--bl31/aarch64/runtime_exceptions.S248
-rw-r--r--bl31/bl31.ld.S88
-rw-r--r--bl31/bl31.mk55
-rw-r--r--bl31/bl31_main.c76
-rw-r--r--common/bl_common.c516
-rw-r--r--common/psci/psci_afflvl_off.c265
-rw-r--r--common/psci/psci_afflvl_on.c416
-rw-r--r--common/psci/psci_afflvl_suspend.c465
-rw-r--r--common/psci/psci_common.c520
-rw-r--r--common/psci/psci_entry.S159
-rw-r--r--common/psci/psci_main.c190
-rw-r--r--common/psci/psci_private.h147
-rw-r--r--common/psci/psci_setup.c268
-rw-r--r--common/runtime_svc.c50
-rw-r--r--docs/change-log.md76
-rw-r--r--docs/porting-guide.md939
-rw-r--r--docs/user-guide.md961
-rw-r--r--drivers/arm/interconnect/cci-400/cci400.c59
-rw-r--r--drivers/arm/interconnect/cci-400/cci400.h72
-rw-r--r--drivers/arm/peripherals/pl011/console.h39
-rw-r--r--drivers/arm/peripherals/pl011/pl011.c82
-rw-r--r--drivers/arm/peripherals/pl011/pl011.h107
-rw-r--r--drivers/power/fvp_pwrc.c103
-rw-r--r--drivers/power/fvp_pwrc.h76
-rw-r--r--fdts/fvp-base-gicv2-psci.dtbbin0 -> 9061 bytes
-rw-r--r--fdts/fvp-base-gicv2-psci.dts250
-rw-r--r--fdts/fvp-base-gicv2legacy-psci.dtbbin0 -> 9061 bytes
-rw-r--r--fdts/fvp-base-gicv2legacy-psci.dts250
-rw-r--r--fdts/fvp-base-gicv3-psci.dtbbin0 -> 9033 bytes
-rw-r--r--fdts/fvp-base-gicv3-psci.dts250
-rw-r--r--fdts/rtsm_ve-motherboard.dtsi264
-rw-r--r--include/aarch64/arch.h315
-rw-r--r--include/aarch64/arch_helpers.h295
-rw-r--r--include/asm_macros.S82
-rw-r--r--include/bakery_lock.h55
-rw-r--r--include/bl1.h52
-rw-r--r--include/bl2.h48
-rw-r--r--include/bl31.h51
-rw-r--r--include/bl_common.h134
-rw-r--r--include/mmio.h43
-rw-r--r--include/pm.h66
-rw-r--r--include/psci.h166
-rw-r--r--include/runtime_svc.h122
-rw-r--r--include/semihosting.h74
-rw-r--r--include/spinlock.h41
-rw-r--r--lib/arch/aarch64/cache_helpers.S233
-rw-r--r--lib/arch/aarch64/misc_helpers.S274
-rw-r--r--lib/arch/aarch64/sysreg_helpers.S1154
-rw-r--r--lib/arch/aarch64/tlb_helpers.S111
-rw-r--r--lib/mmio.c41
-rw-r--r--lib/non-semihosting/ctype.h60
-rw-r--r--lib/non-semihosting/mem.c103
-rw-r--r--lib/non-semihosting/std.c106
-rw-r--r--lib/non-semihosting/strcmp.c49
-rw-r--r--lib/non-semihosting/string.c40
-rw-r--r--lib/non-semihosting/strlen.c46
-rw-r--r--lib/non-semihosting/strncmp.c52
-rw-r--r--lib/non-semihosting/strncpy.c62
-rw-r--r--lib/non-semihosting/strsep.c74
-rw-r--r--lib/non-semihosting/strtol.c146
-rw-r--r--lib/non-semihosting/strtoull.c117
-rw-r--r--lib/non-semihosting/subr_prf.c557
-rw-r--r--lib/semihosting/aarch64/semihosting_call.S37
-rw-r--r--lib/semihosting/semihosting.c234
-rw-r--r--lib/sync/locks/bakery/bakery_lock.c104
-rw-r--r--lib/sync/locks/exclusive/spinlock.S50
-rw-r--r--license.md26
-rw-r--r--plat/common/aarch64/platform_helpers.S141
-rw-r--r--plat/fvp/aarch64/bl1_plat_helpers.S264
-rw-r--r--plat/fvp/aarch64/fvp_common.c600
-rw-r--r--plat/fvp/aarch64/fvp_helpers.S57
-rw-r--r--plat/fvp/bl1_plat_setup.c169
-rw-r--r--plat/fvp/bl2_plat_setup.c147
-rw-r--r--plat/fvp/bl31_plat_setup.c424
-rw-r--r--plat/fvp/fvp_pm.c372
-rw-r--r--plat/fvp/fvp_topology.c241
-rw-r--r--plat/fvp/platform.h341
-rw-r--r--readme.md125
98 files changed, 17582 insertions, 0 deletions
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..5aa9ee8
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,169 @@
+#
+# Copyright (c) 2013, ARM Limited. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# 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.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+#
+
+# Decrease the verbosity of the make script
+# can be made verbose by passing V=1 at the make command line
+ifdef V
+ KBUILD_VERBOSE = $(V)
+else
+ KBUILD_VERBOSE = 0
+endif
+
+ifeq "$(KBUILD_VERBOSE)" "0"
+ Q=@
+else
+ Q=
+endif
+
+DEBUG ?= 0
+BL_COMMON_OBJS = misc_helpers.o cache_helpers.o tlb_helpers.o \
+ semihosting_call.o mmio.o pl011.o semihosting.o \
+ std.o bl_common.o platform_helpers.o sysreg_helpers.o
+
+ARCH := aarch64
+
+all: $(patsubst %,%.bin,bl1 bl2 bl31) ;
+
+
+#$(info $(filter bl2.%, $(MAKECMDGOALS)))
+#$(info $(filter bl1.%, $(MAKECMDGOALS)))
+#$(info $(MAKECMDGOALS))
+
+$(info Including bl1.mk)
+include bl1/bl1.mk
+
+$(info Including bl2.mk)
+include bl2/bl2.mk
+
+$(info Including bl31.mk)
+include bl31/bl31.mk
+
+OBJS += $(BL_COMMON_OBJS)
+
+INCLUDES += -Ilib/include/ -Iinclude/aarch64/ -Iinclude/ \
+ -Idrivers/arm/interconnect/cci-400/ \
+ -Idrivers/arm/peripherals/pl011/ \
+ -Iplat/fvp -Idrivers/power \
+ -Iarch/system/gic -Icommon/psci
+
+ASFLAGS += -D__ASSEMBLY__ $(INCLUDES)
+CFLAGS := -Wall -std=c99 -c -Os -DDEBUG=$(DEBUG) $(INCLUDES) ${CFLAGS}
+
+LDFLAGS += -O1
+BL1_LDFLAGS := -Map=$(BL1_MAPFILE) --script $(BL1_LINKERFILE) --entry=$(BL1_ENTRY_POINT)
+BL2_LDFLAGS := -Map=$(BL2_MAPFILE) --script $(BL2_LINKERFILE) --entry=$(BL2_ENTRY_POINT)
+BL31_LDFLAGS := -Map=$(BL31_MAPFILE) --script $(BL31_LINKERFILE) --entry=$(BL31_ENTRY_POINT)
+
+
+vpath %.ld.S bl1:bl2:bl31
+vpath %.c bl1:bl2:bl31
+vpath %.c bl1/${ARCH}:bl2/${ARCH}:bl31/${ARCH}
+vpath %.S bl1/${ARCH}:bl2/${ARCH}:bl31/${ARCH}
+
+
+ifneq ($(DEBUG), 0)
+#CFLAGS += -g -O0
+CFLAGS += -g
+# -save-temps -fverbose-asm
+ASFLAGS += -g -Wa,--gdwarf-2
+endif
+
+
+CC = $(CROSS_COMPILE)gcc
+CPP = $(CROSS_COMPILE)cpp
+AS = $(CROSS_COMPILE)gcc
+AR = $(CROSS_COMPILE)ar
+LD = $(CROSS_COMPILE)ld
+OC = $(CROSS_COMPILE)objcopy
+OD = $(CROSS_COMPILE)objdump
+NM = $(CROSS_COMPILE)nm
+PP = $(CROSS_COMPILE)gcc -E $(CFLAGS)
+
+
+distclean: clean
+ @echo " DISTCLEAN"
+ $(Q)rm -rf *.zi
+ $(Q)rm -rf *.dump
+ $(Q)rm -rf *.bin
+ $(Q)rm -f *.axf
+ $(Q)rm -f *.i *.s
+ $(Q)rm -f *.ar
+ $(Q)rm -f *.map
+ $(Q)rm -f *.scf
+ $(Q)rm -f *.txt
+ $(Q)rm -f *.elf
+ $(Q)rm -rf *.bin
+ $(Q)rm -f $(LISTFILE)
+
+clean:
+ @echo " CLEAN"
+ $(Q)rm -f *.o *.ld
+
+.PHONY: dump
+
+dump:
+ @echo " OBJDUMP"
+ $(OD) -d bl1.elf > bl1.dump
+ $(OD) -d bl2.elf > bl2.dump
+ $(OD) -d bl31.elf > bl31.dump
+
+%.o: %.S
+ @echo " AS $<"
+ $(Q)$(AS) $(ASFLAGS) -c $< -o $@
+
+%.o: %.c
+ @echo " CC $<"
+ $(Q)$(CC) $(CFLAGS) -c $< -o $@
+
+%.ld: %.ld.S
+ @echo " LDS $<"
+ $(Q)$(AS) $(ASFLAGS) -P -E $< -o $@
+
+
+bl1.elf: $(OBJS) $(BL1_OBJS) bl1.ld
+ @echo " LD $@"
+ $(Q)$(LD) -o $@ $(LDFLAGS) $(BL1_LDFLAGS) $(OBJS) $(BL1_OBJS)
+ @echo "Built $@ successfully"
+ @echo
+
+bl2.elf: $(OBJS) $(BL2_OBJS) bl2.ld
+ @echo " LD $@"
+ $(Q)$(LD) -o $@ $(LDFLAGS) $(BL2_LDFLAGS) $(OBJS) $(BL2_OBJS)
+ @echo "Built $@ successfully"
+ @echo
+
+bl31.elf: $(OBJS) $(BL31_OBJS) bl31.ld
+ @echo " LD $@"
+ $(Q)$(LD) -o $@ $(LDFLAGS) $(BL31_LDFLAGS) $(OBJS) $(BL31_OBJS)
+ @echo "Built $@ successfully"
+ @echo
+
+%.bin: %.elf
+ $(OC) -O binary $< $@
diff --git a/arch/aarch64/cpu/cpu_helpers.S b/arch/aarch64/cpu/cpu_helpers.S
new file mode 100644
index 0000000..600b72f
--- /dev/null
+++ b/arch/aarch64/cpu/cpu_helpers.S
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+
+ .weak cpu_reset_handler
+
+
+ .section aarch64_code, "ax"; .align 3
+
+cpu_reset_handler:; .type cpu_reset_handler, %function
+ mov x19, x30 // lr
+
+ /* ---------------------------------------------
+ * As a bare minimal enable the SMP bit and the
+ * I$ for all aarch64 processors. Also set the
+ * exception vector to something sane.
+ * ---------------------------------------------
+ */
+ adr x0, early_exceptions
+ bl write_vbar
+
+ bl read_midr
+ lsr x0, x0, #MIDR_PN_SHIFT
+ and x0, x0, #MIDR_PN_MASK
+ cmp x0, #MIDR_PN_A57
+ b.eq smp_setup_begin
+ cmp x0, #MIDR_PN_A53
+ b.ne smp_setup_end
+smp_setup_begin:
+ bl read_cpuectlr
+ orr x0, x0, #CPUECTLR_SMP_BIT
+ bl write_cpuectlr
+smp_setup_end:
+ bl read_sctlr
+ orr x0, x0, #SCTLR_I_BIT
+ bl write_sctlr
+
+ ret x19
diff --git a/arch/system/gic/aarch64/gic_v3_sysregs.S b/arch/system/gic/aarch64/gic_v3_sysregs.S
new file mode 100644
index 0000000..3a2fb6e
--- /dev/null
+++ b/arch/system/gic/aarch64/gic_v3_sysregs.S
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+ .globl read_icc_sre_el1
+ .globl read_icc_sre_el2
+ .globl read_icc_sre_el3
+ .globl write_icc_sre_el1
+ .globl write_icc_sre_el2
+ .globl write_icc_sre_el3
+ .globl write_icc_pmr_el1
+
+
+/*
+ * Register definitions used by GCC for GICv3 access.
+ * These are defined by ARMCC, so keep them in the GCC specific code for now.
+ */
+#define ICC_SRE_EL1 S3_0_C12_C12_5
+#define ICC_SRE_EL2 S3_4_C12_C9_5
+#define ICC_SRE_EL3 S3_6_C12_C12_5
+#define ICC_CTLR_EL1 S3_0_C12_C12_4
+#define ICC_CTLR_EL3 S3_6_C12_C12_4
+#define ICC_PMR_EL1 S3_0_C4_C6_0
+
+ .section platform_code, "ax"; .align 3
+
+read_icc_sre_el1:; .type read_icc_sre_el1, %function
+ mrs x0, ICC_SRE_EL1
+ ret
+
+
+read_icc_sre_el2:; .type read_icc_sre_el2, %function
+ mrs x0, ICC_SRE_EL2
+ ret
+
+
+read_icc_sre_el3:; .type read_icc_sre_el3, %function
+ mrs x0, ICC_SRE_EL3
+ ret
+
+
+write_icc_sre_el1:; .type write_icc_sre_el1, %function
+ msr ICC_SRE_EL1, x0
+ isb
+ ret
+
+
+write_icc_sre_el2:; .type write_icc_sre_el2, %function
+ msr ICC_SRE_EL2, x0
+ isb
+ ret
+
+
+write_icc_sre_el3:; .type write_icc_sre_el3, %function
+ msr ICC_SRE_EL3, x0
+ isb
+ ret
+
+
+write_icc_pmr_el1:; .type write_icc_pmr_el1, %function
+ msr ICC_PMR_EL1, x0
+ isb
+ ret
diff --git a/arch/system/gic/gic.h b/arch/system/gic/gic.h
new file mode 100644
index 0000000..91ada03
--- /dev/null
+++ b/arch/system/gic/gic.h
@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __GIC_H__
+#define __GIC_H__
+
+#define MAX_SPIS 480
+#define MAX_PPIS 14
+#define MAX_SGIS 16
+
+#define GRP0 0
+#define GRP1 1
+#define MAX_PRI_VAL 0xff
+
+#define ENABLE_GRP0 (1 << 0)
+#define ENABLE_GRP1 (1 << 1)
+
+/* Distributor interface definitions */
+#define GICD_CTLR 0x0
+#define GICD_TYPER 0x4
+#define GICD_IGROUPR 0x80
+#define GICD_ISENABLER 0x100
+#define GICD_ICENABLER 0x180
+#define GICD_ISPENDR 0x200
+#define GICD_ICPENDR 0x280
+#define GICD_ISACTIVER 0x300
+#define GICD_ICACTIVER 0x380
+#define GICD_IPRIORITYR 0x400
+#define GICD_ITARGETSR 0x800
+#define GICD_ICFGR 0xC00
+#define GICD_SGIR 0xF00
+#define GICD_CPENDSGIR 0xF10
+#define GICD_SPENDSGIR 0xF20
+
+#define IGROUPR_SHIFT 5
+#define ISENABLER_SHIFT 5
+#define ICENABLER_SHIFT ISENABLER_SHIFT
+#define ISPENDR_SHIFT 5
+#define ICPENDR_SHIFT ISPENDR_SHIFT
+#define ISACTIVER_SHIFT 5
+#define ICACTIVER_SHIFT ISACTIVER_SHIFT
+#define IPRIORITYR_SHIFT 2
+#define ITARGETSR_SHIFT 2
+#define ICFGR_SHIFT 4
+#define CPENDSGIR_SHIFT 2
+#define SPENDSGIR_SHIFT CPENDSGIR_SHIFT
+
+/* GICD_TYPER bit definitions */
+#define IT_LINES_NO_MASK 0x1f
+
+/* Physical CPU Interface registers */
+#define GICC_CTLR 0x0
+#define GICC_PMR 0x4
+#define GICC_BPR 0x8
+#define GICC_IAR 0xC
+#define GICC_EOIR 0x10
+#define GICC_RPR 0x14
+#define GICC_HPPIR 0x18
+#define GICC_IIDR 0xFC
+#define GICC_DIR 0x1000
+#define GICC_PRIODROP GICC_EOIR
+
+/* GICC_CTLR bit definitions */
+#define EOI_MODE_NS (1 << 10)
+#define EOI_MODE_S (1 << 9)
+#define IRQ_BYP_DIS_GRP1 (1 << 8)
+#define FIQ_BYP_DIS_GRP1 (1 << 7)
+#define IRQ_BYP_DIS_GRP0 (1 << 6)
+#define FIQ_BYP_DIS_GRP0 (1 << 5)
+#define CBPR (1 << 4)
+#define FIQ_EN (1 << 3)
+#define ACK_CTL (1 << 2)
+
+/* GICC_IIDR bit masks and shifts */
+#define GICC_IIDR_PID_SHIFT 20
+#define GICC_IIDR_ARCH_SHIFT 16
+#define GICC_IIDR_REV_SHIFT 12
+#define GICC_IIDR_IMP_SHIFT 0
+
+#define GICC_IIDR_PID_MASK 0xfff
+#define GICC_IIDR_ARCH_MASK 0xf
+#define GICC_IIDR_REV_MASK 0xf
+#define GICC_IIDR_IMP_MASK 0xfff
+
+/* HYP view virtual CPU Interface registers */
+#define GICH_CTL 0x0
+#define GICH_VTR 0x4
+#define GICH_ELRSR0 0x30
+#define GICH_ELRSR1 0x34
+#define GICH_APR0 0xF0
+#define GICH_LR_BASE 0x100
+
+/* Virtual CPU Interface registers */
+#define GICV_CTL 0x0
+#define GICV_PRIMASK 0x4
+#define GICV_BP 0x8
+#define GICV_INTACK 0xC
+#define GICV_EOI 0x10
+#define GICV_RUNNINGPRI 0x14
+#define GICV_HIGHESTPEND 0x18
+#define GICV_DEACTIVATE 0x1000
+
+/* GICv3 Re-distributor interface registers & shifts */
+#define GICR_PCPUBASE_SHIFT 0x11
+#define GICR_WAKER 0x14
+
+/* GICR_WAKER bit definitions */
+#define WAKER_CA (1UL << 2)
+#define WAKER_PS (1UL << 1)
+
+/* GICv3 ICC_SRE register bit definitions*/
+#define ICC_SRE_EN (1UL << 3)
+#define ICC_SRE_SRE (1UL << 0)
+
+#ifndef __ASSEMBLY__
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+extern inline unsigned int gicd_read_typer(unsigned int);
+extern inline unsigned int gicd_read_ctlr(unsigned int);
+extern unsigned int gicd_read_igroupr(unsigned int, unsigned int);
+extern unsigned int gicd_read_isenabler(unsigned int, unsigned int);
+extern unsigned int gicd_read_icenabler(unsigned int, unsigned int);
+extern unsigned int gicd_read_ispendr(unsigned int, unsigned int);
+extern unsigned int gicd_read_icpendr(unsigned int, unsigned int);
+extern unsigned int gicd_read_isactiver(unsigned int, unsigned int);
+extern unsigned int gicd_read_icactiver(unsigned int, unsigned int);
+extern unsigned int gicd_read_ipriorityr(unsigned int, unsigned int);
+extern unsigned int gicd_read_itargetsr(unsigned int, unsigned int);
+extern unsigned int gicd_read_icfgr(unsigned int, unsigned int);
+extern unsigned int gicd_read_sgir(unsigned int);
+extern unsigned int gicd_read_cpendsgir(unsigned int, unsigned int);
+extern unsigned int gicd_read_spendsgir(unsigned int, unsigned int);
+extern inline void gicd_write_ctlr(unsigned int, unsigned int);
+extern void gicd_write_igroupr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_isenabler(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_icenabler(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_ispendr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_icpendr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_isactiver(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_icactiver(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_ipriorityr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_itargetsr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_icfgr(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_sgir(unsigned int, unsigned int);
+extern void gicd_write_cpendsgir(unsigned int, unsigned int, unsigned int);
+extern void gicd_write_spendsgir(unsigned int, unsigned int, unsigned int);
+extern unsigned int gicd_get_igroupr(unsigned int, unsigned int);
+extern void gicd_set_igroupr(unsigned int, unsigned int);
+extern void gicd_clr_igroupr(unsigned int, unsigned int);
+extern void gicd_set_isenabler(unsigned int, unsigned int);
+extern void gicd_set_icenabler(unsigned int, unsigned int);
+extern void gicd_set_ispendr(unsigned int, unsigned int);
+extern void gicd_set_icpendr(unsigned int, unsigned int);
+extern void gicd_set_isactiver(unsigned int, unsigned int);
+extern void gicd_set_icactiver(unsigned int, unsigned int);
+extern void gicd_set_ipriorityr(unsigned int, unsigned int, unsigned int);
+extern void gicd_set_itargetsr(unsigned int, unsigned int, unsigned int);
+extern inline unsigned int gicc_read_ctlr(unsigned int);
+extern inline unsigned int gicc_read_pmr(unsigned int);
+extern inline unsigned int gicc_read_BPR(unsigned int);
+extern inline unsigned int gicc_read_IAR(unsigned int);
+extern inline unsigned int gicc_read_EOIR(unsigned int);
+extern inline unsigned int gicc_read_hppir(unsigned int);
+extern inline unsigned int gicc_read_iidr(unsigned int);
+extern inline unsigned int gicc_read_dir(unsigned int);
+extern inline void gicc_write_ctlr(unsigned int, unsigned int);
+extern inline void gicc_write_pmr(unsigned int, unsigned int);
+extern inline void gicc_write_BPR(unsigned int, unsigned int);
+extern inline void gicc_write_IAR(unsigned int, unsigned int);
+extern inline void gicc_write_EOIR(unsigned int, unsigned int);
+extern inline void gicc_write_hppir(unsigned int, unsigned int);
+extern inline void gicc_write_dir(unsigned int, unsigned int);
+
+/* GICv3 functions */
+extern inline unsigned int gicr_read_waker(unsigned int);
+extern inline void gicr_write_waker(unsigned int, unsigned int);
+extern unsigned int read_icc_sre_el1(void);
+extern unsigned int read_icc_sre_el2(void);
+extern unsigned int read_icc_sre_el3(void);
+extern void write_icc_sre_el1(unsigned int);
+extern void write_icc_sre_el2(unsigned int);
+extern void write_icc_sre_el3(unsigned int);
+extern void write_icc_pmr_el1(unsigned int);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __GIC_H__ */
+
diff --git a/arch/system/gic/gic_v2.c b/arch/system/gic/gic_v2.c
new file mode 100644
index 0000000..4b3d0c5
--- /dev/null
+++ b/arch/system/gic/gic_v2.c
@@ -0,0 +1,426 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <gic.h>
+#include <mmio.h>
+
+/*******************************************************************************
+ * GIC Distributor interface accessesors for reading entire registers
+ ******************************************************************************/
+inline unsigned int gicd_read_ctlr(unsigned int base)
+{
+ return mmio_read_32(base + GICD_CTLR);
+}
+
+inline unsigned int gicd_read_typer(unsigned int base)
+{
+ return mmio_read_32(base + GICD_TYPER);
+}
+
+unsigned int gicd_read_igroupr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> IGROUPR_SHIFT;
+ return mmio_read_32(base + GICD_IGROUPR + (n << 2));
+}
+
+unsigned int gicd_read_isenabler(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ISENABLER_SHIFT;
+ return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+unsigned int gicd_read_icenabler(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ICENABLER_SHIFT;
+ return mmio_read_32(base + GICD_ICENABLER + (n << 2));
+}
+
+unsigned int gicd_read_ispendr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ISPENDR_SHIFT;
+ return mmio_read_32(base + GICD_ISPENDR + (n << 2));
+}
+
+unsigned int gicd_read_icpendr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ICPENDR_SHIFT;
+ return mmio_read_32(base + GICD_ICPENDR + (n << 2));
+}
+
+unsigned int gicd_read_isactiver(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ISACTIVER_SHIFT;
+ return mmio_read_32(base + GICD_ISACTIVER + (n << 2));
+}
+
+unsigned int gicd_read_icactiver(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ICACTIVER_SHIFT;
+ return mmio_read_32(base + GICD_ICACTIVER + (n << 2));
+}
+
+unsigned int gicd_read_ipriorityr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> IPRIORITYR_SHIFT;
+ return mmio_read_32(base + GICD_IPRIORITYR + (n << 2));
+}
+
+unsigned int gicd_read_itargetsr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ITARGETSR_SHIFT;
+ return mmio_read_32(base + GICD_ITARGETSR + (n << 2));
+}
+
+unsigned int gicd_read_icfgr(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> ICFGR_SHIFT;
+ return mmio_read_32(base + GICD_ICFGR + (n << 2));
+}
+
+unsigned int gicd_read_sgir(unsigned int base)
+{
+ return mmio_read_32(base + GICD_SGIR);
+}
+
+unsigned int gicd_read_cpendsgir(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> CPENDSGIR_SHIFT;
+ return mmio_read_32(base + GICD_CPENDSGIR + (n << 2));
+}
+
+unsigned int gicd_read_spendsgir(unsigned int base, unsigned int id)
+{
+ unsigned n = id >> SPENDSGIR_SHIFT;
+ return mmio_read_32(base + GICD_SPENDSGIR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessesors for writing entire registers
+ ******************************************************************************/
+inline void gicd_write_ctlr(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICD_CTLR, val);
+ return;
+}
+
+void gicd_write_igroupr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> IGROUPR_SHIFT;
+ mmio_write_32(base + GICD_IGROUPR + (n << 2), val);
+ return;
+}
+
+void gicd_write_isenabler(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ISENABLER_SHIFT;
+ mmio_write_32(base + GICD_ISENABLER + (n << 2), val);
+ return;
+}
+
+void gicd_write_icenabler(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ICENABLER_SHIFT;
+ mmio_write_32(base + GICD_ICENABLER + (n << 2), val);
+ return;
+}
+
+void gicd_write_ispendr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ISPENDR_SHIFT;
+ mmio_write_32(base + GICD_ISPENDR + (n << 2), val);
+ return;
+}
+
+void gicd_write_icpendr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ICPENDR_SHIFT;
+ mmio_write_32(base + GICD_ICPENDR + (n << 2), val);
+ return;
+}
+
+void gicd_write_isactiver(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ISACTIVER_SHIFT;
+ mmio_write_32(base + GICD_ISACTIVER + (n << 2), val);
+ return;
+}
+
+void gicd_write_icactiver(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ICACTIVER_SHIFT;
+ mmio_write_32(base + GICD_ICACTIVER + (n << 2), val);
+ return;
+}
+
+void gicd_write_ipriorityr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> IPRIORITYR_SHIFT;
+ mmio_write_32(base + GICD_IPRIORITYR + (n << 2), val);
+ return;
+}
+
+void gicd_write_itargetsr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ITARGETSR_SHIFT;
+ mmio_write_32(base + GICD_ITARGETSR + (n << 2), val);
+ return;
+}
+
+void gicd_write_icfgr(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> ICFGR_SHIFT;
+ mmio_write_32(base + GICD_ICFGR + (n << 2), val);
+ return;
+}
+
+void gicd_write_sgir(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICD_SGIR, val);
+ return;
+}
+
+void gicd_write_cpendsgir(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> CPENDSGIR_SHIFT;
+ mmio_write_32(base + GICD_CPENDSGIR + (n << 2), val);
+ return;
+}
+
+void gicd_write_spendsgir(unsigned int base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> SPENDSGIR_SHIFT;
+ mmio_write_32(base + GICD_SPENDSGIR + (n << 2), val);
+ return;
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessesors for individual interrupt manipulation
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << IGROUPR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_igroupr(base, id);
+
+ return (reg_val >> bit_num) & 0x1;
+}
+
+void gicd_set_igroupr(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << IGROUPR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_igroupr(base, id);
+
+ gicd_write_igroupr(base, id, reg_val | (1 << bit_num));
+ return;
+}
+
+void gicd_clr_igroupr(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << IGROUPR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_igroupr(base, id);
+
+ gicd_write_igroupr(base, id, reg_val & ~(1 << bit_num));
+ return;
+}
+
+void gicd_set_isenabler(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ISENABLER_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_isenabler(base, id);
+
+ gicd_write_isenabler(base, id, reg_val | (1 << bit_num));
+ return;
+}
+
+void gicd_set_icenabler(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ICENABLER_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_icenabler(base, id);
+
+ gicd_write_icenabler(base, id, reg_val & ~(1 << bit_num));
+ return;
+}
+
+void gicd_set_ispendr(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ISPENDR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_ispendr(base, id);
+
+ gicd_write_ispendr(base, id, reg_val | (1 << bit_num));
+ return;
+}
+
+void gicd_set_icpendr(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ICPENDR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_icpendr(base, id);
+
+ gicd_write_icpendr(base, id, reg_val & ~(1 << bit_num));
+ return;
+}
+
+void gicd_set_isactiver(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ISACTIVER_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_isactiver(base, id);
+
+ gicd_write_isactiver(base, id, reg_val | (1 << bit_num));
+ return;
+}
+
+void gicd_set_icactiver(unsigned int base, unsigned int id)
+{
+ unsigned bit_num = id & ((1 << ICACTIVER_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_icactiver(base, id);
+
+ gicd_write_icactiver(base, id, reg_val & ~(1 << bit_num));
+ return;
+}
+
+/*
+ * Make sure that the interrupt's group is set before expecting
+ * this function to do its job correctly.
+ */
+void gicd_set_ipriorityr(unsigned int base, unsigned int id, unsigned int pri)
+{
+ unsigned byte_off = id & ((1 << ICACTIVER_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_icactiver(base, id);
+
+ /*
+ * Enforce ARM recommendation to manage priority values such
+ * that group1 interrupts always have a lower priority than
+ * group0 interrupts
+ */
+ if (gicd_get_igroupr(base, id) == GRP1)
+ pri |= 1 << 7;
+ else
+ pri &= ~(1 << 7);
+
+ gicd_write_icactiver(base, id, reg_val & ~(pri << (byte_off << 3)));
+ return;
+}
+
+void gicd_set_itargetsr(unsigned int base, unsigned int id, unsigned int iface)
+{
+ unsigned byte_off = id & ((1 << ITARGETSR_SHIFT) - 1);
+ unsigned int reg_val = gicd_read_itargetsr(base, id);
+
+ gicd_write_itargetsr(base, id, reg_val |
+ (1 << iface) << (byte_off << 3));
+ return;
+}
+
+/*******************************************************************************
+ * GIC CPU interface accessesors for reading entire registers
+ ******************************************************************************/
+inline unsigned int gicc_read_ctlr(unsigned int base)
+{
+ return mmio_read_32(base + GICC_CTLR);
+}
+
+inline unsigned int gicc_read_pmr(unsigned int base)
+{
+ return mmio_read_32(base + GICC_PMR);
+}
+
+inline unsigned int gicc_read_BPR(unsigned int base)
+{
+ return mmio_read_32(base + GICC_BPR);
+}
+
+inline unsigned int gicc_read_IAR(unsigned int base)
+{
+ return mmio_read_32(base + GICC_IAR);
+}
+
+inline unsigned int gicc_read_EOIR(unsigned int base)
+{
+ return mmio_read_32(base + GICC_EOIR);
+}
+
+inline unsigned int gicc_read_hppir(unsigned int base)
+{
+ return mmio_read_32(base + GICC_HPPIR);
+}
+
+inline unsigned int gicc_read_dir(unsigned int base)
+{
+ return mmio_read_32(base + GICC_DIR);
+}
+
+inline unsigned int gicc_read_iidr(unsigned int base)
+{
+ return mmio_read_32(base + GICC_IIDR);
+}
+
+/*******************************************************************************
+ * GIC CPU interface accessesors for writing entire registers
+ ******************************************************************************/
+inline void gicc_write_ctlr(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_CTLR, val);
+ return;
+}
+
+inline void gicc_write_pmr(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_PMR, val);
+ return;
+}
+
+inline void gicc_write_BPR(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_BPR, val);
+ return;
+}
+
+inline void gicc_write_IAR(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_IAR, val);
+ return;
+}
+
+inline void gicc_write_EOIR(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_EOIR, val);
+ return;
+}
+
+inline void gicc_write_hppir(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_HPPIR, val);
+ return;
+}
+
+inline void gicc_write_dir(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICC_DIR, val);
+ return;
+}
+
diff --git a/arch/system/gic/gic_v3.c b/arch/system/gic/gic_v3.c
new file mode 100644
index 0000000..7806a0d
--- /dev/null
+++ b/arch/system/gic/gic_v3.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <gic.h>
+#include <mmio.h>
+
+/*******************************************************************************
+ * GIC Redistributor interface accessesors
+ ******************************************************************************/
+inline unsigned int gicr_read_waker(unsigned int base)
+{
+ return mmio_read_32(base + GICR_WAKER);
+}
+
+inline void gicr_write_waker(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICR_WAKER, val);
+ return;
+}
diff --git a/bl1/aarch64/bl1_arch_setup.c b/bl1/aarch64/bl1_arch_setup.c
new file mode 100644
index 0000000..d4be9d6
--- /dev/null
+++ b/bl1/aarch64/bl1_arch_setup.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+#include <platform.h>
+#include <assert.h>
+
+/*******************************************************************************
+ * Function that does the first bit of architectural setup that affects
+ * execution in the non-secure address space.
+ ******************************************************************************/
+void bl1_arch_setup(void)
+{
+ unsigned long tmp_reg = 0;
+ unsigned int counter_base_frequency;
+
+ /* Enable alignment checks and set the exception endianess to LE */
+ tmp_reg = read_sctlr();
+ tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
+ tmp_reg &= ~SCTLR_EE_BIT;
+ write_sctlr(tmp_reg);
+
+ /*
+ * Enable HVCs, route FIQs to EL3, set the next EL to be aarch64
+ */
+ tmp_reg = SCR_RES1_BITS | SCR_RW_BIT | SCR_HCE_BIT | SCR_FIQ_BIT;
+ write_scr(tmp_reg);
+
+ /* Do not trap coprocessor accesses from lower ELs to EL3 */
+ write_cptr_el3(0);
+
+ /* Read the frequency from Frequency modes table */
+ counter_base_frequency = mmio_read_32(SYS_CNTCTL_BASE + CNTFID_OFF);
+ /* The first entry of the frequency modes table must not be 0 */
+ assert(counter_base_frequency != 0);
+
+ /* Program the counter frequency */
+ write_cntfrq_el0(counter_base_frequency);
+ return;
+}
+
+/*******************************************************************************
+ * Set the Secure EL1 required architectural state
+ ******************************************************************************/
+void bl1_arch_next_el_setup(void) {
+ unsigned long current_sctlr, next_sctlr;
+
+ /* Use the same endianness than the current BL */
+ current_sctlr = read_sctlr();
+ next_sctlr = (current_sctlr & SCTLR_EE_BIT);
+
+ /* Set SCTLR Secure EL1 */
+ next_sctlr |= SCTLR_EL1_RES1;
+
+ write_sctlr_el1(next_sctlr);
+}
diff --git a/bl1/aarch64/bl1_entrypoint.S b/bl1/aarch64/bl1_entrypoint.S
new file mode 100644
index 0000000..f5ccc65
--- /dev/null
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+
+ .globl reset_handler
+
+
+ .section reset_code, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * reset_handler() is the entry point into the trusted
+ * firmware code when a cpu is released from warm or
+ * cold reset.
+ * -----------------------------------------------------
+ */
+
+reset_handler:; .type reset_handler, %function
+ /* ---------------------------------------------
+ * Perform any processor specific actions upon
+ * reset e.g. cache, tlb invalidations etc.
+ * ---------------------------------------------
+ */
+ bl cpu_reset_handler
+
+_wait_for_entrypoint:
+ /* ---------------------------------------------
+ * Find the type of reset and jump to handler
+ * if present. If the handler is null then it is
+ * a cold boot. The primary cpu will set up the
+ * platform while the secondaries wait for
+ * their turn to be woken up
+ * ---------------------------------------------
+ */
+ bl read_mpidr
+ bl platform_get_entrypoint
+ cbnz x0, _do_warm_boot
+ bl read_mpidr
+ bl platform_is_primary_cpu
+ cbnz x0, _do_cold_boot
+
+ /* ---------------------------------------------
+ * Perform any platform specific secondary cpu
+ * actions
+ * ---------------------------------------------
+ */
+ bl plat_secondary_cold_boot_setup
+ b _wait_for_entrypoint
+
+_do_cold_boot:
+ /* ---------------------------------------------
+ * Initialize platform and jump to our c-entry
+ * point for this type of reset
+ * ---------------------------------------------
+ */
+ adr x0, bl1_main
+ bl platform_cold_boot_init
+ b _panic
+
+_do_warm_boot:
+ /* ---------------------------------------------
+ * Jump to BL31 for all warm boot init.
+ * ---------------------------------------------
+ */
+ blr x0
+_panic:
+ b _panic
diff --git a/bl1/aarch64/early_exceptions.S b/bl1/aarch64/early_exceptions.S
new file mode 100644
index 0000000..08a1122
--- /dev/null
+++ b/bl1/aarch64/early_exceptions.S
@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <bl_common.h>
+#include <bl1.h>
+#include <platform.h>
+#include <runtime_svc.h>
+
+ .globl early_exceptions
+
+
+ .section .text, "ax"; .align 11
+
+ /* -----------------------------------------------------
+ * Very simple exception handlers used by BL1 and BL2.
+ * Apart from one SMC exception all other traps loop
+ * endlessly.
+ * -----------------------------------------------------
+ */
+ .align 7
+early_exceptions:
+ /* -----------------------------------------------------
+ * Current EL with SP0 : 0x0 - 0x180
+ * -----------------------------------------------------
+ */
+SynchronousExceptionSP0:
+ mov x0, #SYNC_EXCEPTION_SP_EL0
+ bl plat_report_exception
+ b SynchronousExceptionSP0
+
+ .align 7
+IrqSP0:
+ mov x0, #IRQ_SP_EL0
+ bl plat_report_exception
+ b IrqSP0
+
+ .align 7
+FiqSP0:
+ mov x0, #FIQ_SP_EL0
+ bl plat_report_exception
+ b FiqSP0
+
+ .align 7
+SErrorSP0:
+ mov x0, #SERROR_SP_EL0
+ bl plat_report_exception
+ b SErrorSP0
+
+ /* -----------------------------------------------------
+ * Current EL with SPx: 0x200 - 0x380
+ * -----------------------------------------------------
+ */
+ .align 7
+SynchronousExceptionSPx:
+ mov x0, #SYNC_EXCEPTION_SP_ELX
+ bl plat_report_exception
+ b SynchronousExceptionSPx
+
+ .align 7
+IrqSPx:
+ mov x0, #IRQ_SP_ELX
+ bl plat_report_exception
+ b IrqSPx
+
+ .align 7
+FiqSPx:
+ mov x0, #FIQ_SP_ELX
+ bl plat_report_exception
+ b FiqSPx
+
+ .align 7
+SErrorSPx:
+ mov x0, #SERROR_SP_ELX
+ bl plat_report_exception
+ b SErrorSPx
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch64 : 0x400 - 0x580
+ * -----------------------------------------------------
+ */
+ .align 7
+SynchronousExceptionA64:
+ /* ---------------------------------------------
+ * Only a single SMC exception from BL2 to ask
+ * BL1 to pass EL3 control to BL31 is expected
+ * here.
+ * ---------------------------------------------
+ */
+ sub sp, sp, #0x40
+ stp x0, x1, [sp, #0x0]
+ stp x2, x3, [sp, #0x10]
+ stp x4, x5, [sp, #0x20]
+ stp x6, x7, [sp, #0x30]
+ mov x19, x0
+ mov x20, x1
+ mov x21, x2
+
+ mov x0, #SYNC_EXCEPTION_AARCH64
+ bl plat_report_exception
+
+ bl read_esr
+ ubfx x1, x0, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+ cmp x1, #EC_AARCH64_SMC
+ b.ne panic
+ mov x1, #RUN_IMAGE
+ cmp x19, x1
+ b.ne panic
+ mov x0, x20
+ mov x1, x21
+ mov x2, x3
+ mov x3, x4
+ bl display_boot_progress
+ mov x0, x20
+ bl write_elr
+ mov x0, x21
+ bl write_spsr
+ ubfx x0, x21, #MODE_EL_SHIFT, #2
+ cmp x0, #MODE_EL3
+ b.ne skip_mmu_teardown
+ /* ---------------------------------------------
+ * If BL31 is to be executed in EL3 as well
+ * then turn off the MMU so that it can perform
+ * its own setup. TODO: Assuming flat mapped
+ * translations here. Also all should go into a
+ * separate MMU teardown function
+ * ---------------------------------------------
+ */
+ mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
+ bl read_sctlr
+ bic x0, x0, x1
+ bl write_sctlr
+ mov x0, #DCCISW
+ bl dcsw_op_all
+ bl tlbialle3
+skip_mmu_teardown:
+ ldp x6, x7, [sp, #0x30]
+ ldp x4, x5, [sp, #0x20]
+ ldp x2, x3, [sp, #0x10]
+ ldp x0, x1, [sp, #0x0]
+ add sp, sp, #0x40
+ eret
+panic:
+ b panic
+ .align 7
+IrqA64:
+ mov x0, #IRQ_AARCH64
+ bl plat_report_exception
+ b IrqA64
+
+ .align 7
+FiqA64:
+ mov x0, #FIQ_AARCH64
+ bl plat_report_exception
+ b FiqA64
+
+ .align 7
+SErrorA64:
+ mov x0, #SERROR_AARCH64
+ bl plat_report_exception
+ b SErrorA64
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch32 : 0x0 - 0x180
+ * -----------------------------------------------------
+ */
+ .align 7
+SynchronousExceptionA32:
+ mov x0, #SYNC_EXCEPTION_AARCH32
+ bl plat_report_exception
+ b SynchronousExceptionA32
+
+ .align 7
+IrqA32:
+ mov x0, #IRQ_AARCH32
+ bl plat_report_exception
+ b IrqA32
+
+ .align 7
+FiqA32:
+ mov x0, #FIQ_AARCH32
+ bl plat_report_exception
+ b FiqA32
+
+ .align 7
+SErrorA32:
+ mov x0, #SERROR_AARCH32
+ bl plat_report_exception
+ b SErrorA32
diff --git a/bl1/bl1.ld.S b/bl1/bl1.ld.S
new file mode 100644
index 0000000..5327715
--- /dev/null
+++ b/bl1/bl1.ld.S
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <platform.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+
+MEMORY {
+ /* ROM is read-only and executable */
+ ROM (rx): ORIGIN = TZROM_BASE, LENGTH = TZROM_SIZE
+ /* RAM is read/write and Initialised */
+ RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+}
+
+SECTIONS
+{
+ FIRMWARE_ROM : {
+ *(reset_code)
+ *(.text)
+ *(.rodata)
+ } >ROM
+
+ .bss : {
+ __BSS_RAM_START__ = .;
+ *(.bss)
+ *(COMMON)
+ __BSS_RAM_STOP__ = .;
+ } >RAM AT>ROM
+
+ .data : {
+ __DATA_RAM_START__ = .;
+ *(.data)
+ __DATA_RAM_STOP__ = .;
+ } >RAM AT>ROM
+
+ FIRMWARE_RAM_STACKS ALIGN (PLATFORM_CACHE_LINE_SIZE) : {
+ . += 0x1000;
+ *(tzfw_normal_stacks)
+ . = ALIGN(4096);
+ } >RAM AT>ROM
+
+ FIRMWARE_RAM_COHERENT ALIGN (4096): {
+ *(tzfw_coherent_mem)
+/* . += 0x1000;*/
+/* Do we need to make sure this is at least 4k? */
+ . = ALIGN(4096);
+ } >RAM
+
+ __FIRMWARE_ROM_START__ = LOADADDR(FIRMWARE_ROM);
+ __FIRMWARE_ROM_SIZE__ = SIZEOF(FIRMWARE_ROM);
+
+ __FIRMWARE_DATA_START__ = LOADADDR(.data);
+ __FIRMWARE_DATA_SIZE__ = SIZEOF(.data);
+
+ __FIRMWARE_BSS_START__ = LOADADDR(.bss);
+ __FIRMWARE_BSS_SIZE__ = SIZEOF(.bss);
+
+ __FIRMWARE_RAM_STACKS_START__ = LOADADDR(FIRMWARE_RAM_STACKS);
+ __FIRMWARE_RAM_STACKS_SIZE__ = SIZEOF(FIRMWARE_RAM_STACKS);
+ __FIRMWARE_RAM_COHERENT_START__ = LOADADDR(FIRMWARE_RAM_COHERENT);
+ __FIRMWARE_RAM_COHERENT_SIZE__ = SIZEOF(FIRMWARE_RAM_COHERENT);
+}
diff --git a/bl1/bl1.mk b/bl1/bl1.mk
new file mode 100644
index 0000000..b159fd9
--- /dev/null
+++ b/bl1/bl1.mk
@@ -0,0 +1,46 @@
+#
+# Copyright (c) 2013, ARM Limited. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# 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.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+#
+
+vpath %.c drivers/arm/interconnect/cci-400/ plat/fvp \
+ plat/fvp/${ARCH} drivers/arm/peripherals/pl011 common/ lib/ \
+ lib/semihosting arch/aarch64/ lib/non-semihosting
+
+vpath %.S arch/${ARCH}/cpu plat/common/aarch64 \
+ plat/fvp/${ARCH} lib/semihosting/aarch64 \
+ include/ lib/arch/aarch64
+
+BL1_ASM_OBJS := bl1_entrypoint.o bl1_plat_helpers.o cpu_helpers.o
+BL1_C_OBJS := bl1_main.o cci400.o bl1_plat_setup.o bl1_arch_setup.o \
+ fvp_common.o fvp_helpers.o early_exceptions.o
+BL1_ENTRY_POINT := reset_handler
+BL1_MAPFILE := bl1.map
+BL1_LINKERFILE := bl1.ld
+
+BL1_OBJS := $(BL1_C_OBJS) $(BL1_ASM_OBJS)
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
new file mode 100644
index 0000000..badda64
--- /dev/null
+++ b/bl1/bl1_main.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl1.h>
+
+void bl1_arch_next_el_setup(void);
+
+/*******************************************************************************
+ * Function to perform late architectural and platform specific initialization.
+ * It also locates and loads the BL2 raw binary image in the trusted DRAM. Only
+ * called by the primary cpu after a cold boot.
+ * TODO: Add support for alternative image load mechanism e.g using virtio/elf
+ * loader etc.
+ ******************************************************************************/
+void bl1_main(void)
+{
+ unsigned long sctlr_el3 = read_sctlr();
+ unsigned long bl2_base;
+ unsigned int load_type = TOP_LOAD, spsr;
+ meminfo bl1_tzram_layout, *bl2_tzram_layout = 0x0;
+
+ /*
+ * Ensure that MMU/Caches and coherency are turned on
+ */
+ assert(sctlr_el3 | SCTLR_M_BIT);
+ assert(sctlr_el3 | SCTLR_C_BIT);
+ assert(sctlr_el3 | SCTLR_I_BIT);
+
+ /* Perform remaining generic architectural setup from EL3 */
+ bl1_arch_setup();
+
+ /* Perform platform setup in BL1. */
+ bl1_platform_setup();
+
+ /* Announce our arrival */
+ printf(FIRMWARE_WELCOME_STR);
+ printf("Built : %s, %s\n\r", __TIME__, __DATE__);
+
+ /*
+ * Find out how much free trusted ram remains after BL1 load
+ * & load the BL2 image at its top
+ */
+ bl1_tzram_layout = bl1_get_sec_mem_layout();
+ bl2_base = load_image(&bl1_tzram_layout,
+ (const char *) BL2_IMAGE_NAME,
+ load_type, BL2_BASE);
+
+ /*
+ * Create a new layout of memory for BL2 as seen by BL1 i.e.
+ * tell it the amount of total and free memory available.
+ * This layout is created at the first free address visible
+ * to BL2. BL2 will read the memory layout before using its
+ * memory for other purposes.
+ */
+ bl2_tzram_layout = (meminfo *) bl1_tzram_layout.free_base;
+ init_bl2_mem_layout(&bl1_tzram_layout,
+ bl2_tzram_layout,
+ load_type,
+ bl2_base);
+
+ if (bl2_base) {
+ bl1_arch_next_el_setup();
+ spsr = make_spsr(MODE_EL1, MODE_SP_ELX, MODE_RW_64);
+ printf("Booting trusted firmware boot loader stage 2\n\r");
+#if DEBUG
+ printf("BL2 address = 0x%llx \n\r", (unsigned long long) bl2_base);
+ printf("BL2 cpsr = 0x%x \n\r", spsr);
+ printf("BL2 memory layout address = 0x%llx \n\r",
+ (unsigned long long) bl2_tzram_layout);
+#endif
+ run_image(bl2_base, spsr, SECURE, bl2_tzram_layout, 0);
+ }
+
+ /*
+ * TODO: print failure to load BL2 but also add a tzwdog timer
+ * which will reset the system eventually.
+ */
+ printf("Failed to load boot loader stage 2 (BL2) firmware.\n\r");
+ return;
+}
+
+/*******************************************************************************
+ * Temporary function to print the fact that BL2 has done its job and BL31 is
+ * about to be loaded. This is needed as long as printfs cannot be used
+ ******************************************************************************/
+void display_boot_progress(unsigned long entrypoint,
+ unsigned long spsr,
+ unsigned long mem_layout,
+ unsigned long ns_image_info)
+{
+ printf("Booting trusted firmware boot loader stage 3\n\r");
+#if DEBUG
+ printf("BL31 address = 0x%llx \n\r", (unsigned long long) entrypoint);
+ printf("BL31 cpsr = 0x%llx \n\r", (unsigned long long)spsr);
+ printf("BL31 memory layout address = 0x%llx \n\r", (unsigned long long)mem_layout);
+ printf("BL31 non-trusted image info address = 0x%llx\n\r", (unsigned long long)ns_image_info);
+#endif
+ return;
+}
diff --git a/bl2/aarch64/bl2_arch_setup.c b/bl2/aarch64/bl2_arch_setup.c
new file mode 100644
index 0000000..ed457ee
--- /dev/null
+++ b/bl2/aarch64/bl2_arch_setup.c
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+#include <platform.h>
+
+/*******************************************************************************
+ * Place holder function to perform any S-EL1 specific architectural setup. At
+ * the moment there is nothing to do.
+ ******************************************************************************/
+void bl2_arch_setup(void)
+{
+ /* Give access to FP/SIMD registers */
+ write_cpacr(CPACR_EL1_FPEN(CPACR_EL1_FP_TRAP_NONE));
+}
diff --git a/bl2/aarch64/bl2_entrypoint.S b/bl2/aarch64/bl2_entrypoint.S
new file mode 100644
index 0000000..bade099
--- /dev/null
+++ b/bl2/aarch64/bl2_entrypoint.S
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <bl_common.h>
+
+
+ .globl bl2_entrypoint
+
+
+ .section entry_code, "ax"; .align 3
+
+
+bl2_entrypoint:; .type bl2_entrypoint, %function
+ /*---------------------------------------------
+ * Store the extents of the tzram available to
+ * BL2 for future use. Use the opcode param to
+ * allow implement other functions if needed.
+ * ---------------------------------------------
+ */
+ mov x20, x0
+ mov x21, x1
+ mov x22, x2
+
+ /* ---------------------------------------------
+ * This is BL2 which is expected to be executed
+ * only by the primary cpu (at least for now).
+ * So, make sure no secondary has lost its way.
+ * ---------------------------------------------
+ */
+ bl read_mpidr
+ mov x19, x0
+ bl platform_is_primary_cpu
+ cbz x0, _panic
+
+ /* --------------------------------------------
+ * Give ourselves a small coherent stack to
+ * ease the pain of initializing the MMU
+ * --------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Perform early platform setup & platform
+ * specific early arch. setup e.g. mmu setup
+ * ---------------------------------------------
+ */
+ mov x0, x21
+ mov x1, x22
+ bl bl2_early_platform_setup
+ bl bl2_plat_arch_setup
+
+ /* ---------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* ---------------------------------------------
+ * Jump to main function.
+ * ---------------------------------------------
+ */
+ bl bl2_main
+_panic:
+ b _panic
diff --git a/bl2/bl2.ld.S b/bl2/bl2.ld.S
new file mode 100644
index 0000000..8a8ed35
--- /dev/null
+++ b/bl2/bl2.ld.S
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <platform.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+
+MEMORY {
+ /* RAM is read/write and Initialised */
+ RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+}
+
+
+SECTIONS
+{
+ . = BL2_BASE;
+
+ BL2_RO NEXT (4096): {
+ *(entry_code)
+ *(.text .rodata)
+ } >RAM
+
+ BL2_STACKS NEXT (4096): {
+ *(tzfw_normal_stacks)
+ } >RAM
+
+ BL2_COHERENT_RAM NEXT (4096): {
+ *(tzfw_coherent_mem)
+ /* . += 0x1000;*/
+ /* Do we need to ensure at least 4k here? */
+ . = NEXT(4096);
+ } >RAM
+
+ __BL2_DATA_START__ = .;
+ .bss NEXT (4096): {
+ *(SORT_BY_ALIGNMENT(.bss))
+ *(COMMON)
+ } >RAM
+
+ .data : {
+ *(.data)
+ } >RAM
+ __BL2_DATA_STOP__ = .;
+
+
+ __BL2_RO_BASE__ = LOADADDR(BL2_RO);
+ __BL2_RO_SIZE__ = SIZEOF(BL2_RO);
+
+ __BL2_STACKS_BASE__ = LOADADDR(BL2_STACKS);
+ __BL2_STACKS_SIZE__ = SIZEOF(BL2_STACKS);
+
+ __BL2_COHERENT_RAM_BASE__ = LOADADDR(BL2_COHERENT_RAM);
+ __BL2_COHERENT_RAM_SIZE__ = SIZEOF(BL2_COHERENT_RAM);
+
+ __BL2_RW_BASE__ = __BL2_DATA_START__;
+ __BL2_RW_SIZE__ = __BL2_DATA_STOP__ - __BL2_DATA_START__;
+}
diff --git a/bl2/bl2.mk b/bl2/bl2.mk
new file mode 100644
index 0000000..212aa92
--- /dev/null
+++ b/bl2/bl2.mk
@@ -0,0 +1,48 @@
+#
+# Copyright (c) 2013, ARM Limited. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# 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.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+#
+
+vpath %.c common/ drivers/arm/interconnect/cci-400/ \
+ drivers/arm/peripherals/pl011 common/ lib/ \
+ plat/fvp plat/fvp/${ARCH} lib/semihosting arch/aarch64/ \
+ lib/non-semihosting
+
+vpath %.S lib/arch/aarch64 \
+ lib/semihosting/aarch64 \
+ include lib/sync/locks/exclusive
+
+BL2_ASM_OBJS := bl2_entrypoint.o spinlock.o
+BL2_C_OBJS := bl2_main.o bl2_plat_setup.o bl2_arch_setup.o fvp_common.o \
+ early_exceptions.o
+BL2_ENTRY_POINT := bl2_entrypoint
+BL2_MAPFILE := bl2.map
+BL2_LINKERFILE := bl2.ld
+
+BL2_OBJS := $(BL2_C_OBJS) $(BL2_ASM_OBJS)
+CFLAGS += $(DEFINES)
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
new file mode 100644
index 0000000..aae67b4
--- /dev/null
+++ b/bl2/bl2_main.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl_common.h>
+#include <bl2.h>
+
+/*******************************************************************************
+ * The only thing to do in BL2 is to load further images and pass control to
+ * BL31. The memory occupied by BL2 will be reclaimed by BL3_x stages. BL2 runs
+ * entirely in S-EL1. Since arm standard c libraries are not PIC, printf et al
+ * are not available. We rely on assertions to signal error conditions
+ ******************************************************************************/
+void bl2_main(void)
+{
+ meminfo bl2_tzram_layout, *bl31_tzram_layout;
+ el_change_info *ns_image_info;
+ unsigned long bl31_base, el_status;
+ unsigned int bl2_load, bl31_load, mode;
+
+ /* Perform remaining generic architectural setup in S-El1 */
+ bl2_arch_setup();
+
+ /* Perform platform setup in BL1 */
+ bl2_platform_setup();
+
+#if defined (__GNUC__)
+ printf("BL2 Built : %s, %s\n\r", __TIME__, __DATE__);
+#endif
+
+ /* Find out how much free trusted ram remains after BL2 load */
+ bl2_tzram_layout = bl2_get_sec_mem_layout();
+
+ /*
+ * Load BL31. BL1 tells BL2 whether it has been TOP or BOTTOM loaded.
+ * To avoid fragmentation of trusted SRAM memory, BL31 is always
+ * loaded opposite to BL2. This allows BL31 to reclaim BL2 memory
+ * while maintaining its free space in one contiguous chunk.
+ */
+ bl2_load = bl2_tzram_layout.attr & LOAD_MASK;
+ assert((bl2_load == TOP_LOAD) || (bl2_load == BOT_LOAD));
+ bl31_load = (bl2_load == TOP_LOAD) ? BOT_LOAD : TOP_LOAD;
+ bl31_base = load_image(&bl2_tzram_layout, BL31_IMAGE_NAME,
+ bl31_load, BL31_BASE);
+
+ /* Assert if it has not been possible to load BL31 */
+ assert(bl31_base != 0);
+
+ /*
+ * Create a new layout of memory for BL31 as seen by BL2. This
+ * will gobble up all the BL2 memory.
+ */
+ bl31_tzram_layout = (meminfo *) get_el_change_mem_ptr();
+ init_bl31_mem_layout(&bl2_tzram_layout, bl31_tzram_layout, bl31_load);
+
+ /*
+ * BL2 also needs to tell BL31 where the non-trusted software image
+ * has been loaded. Place this info right after the BL31 memory layout
+ */
+ ns_image_info = (el_change_info *) ((unsigned char *) bl31_tzram_layout
+ + sizeof(meminfo));
+
+ /*
+ * Assume that the non-secure bootloader has already been
+ * loaded to its platform-specific location.
+ */
+ ns_image_info->entrypoint = plat_get_ns_image_entrypoint();
+
+ /* Figure out what mode we enter the non-secure world in */
+ el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+ el_status &= ID_AA64PFR0_ELX_MASK;
+
+ if (el_status)
+ mode = MODE_EL2;
+ else
+ mode = MODE_EL1;
+
+ ns_image_info->spsr = make_spsr(mode, MODE_SP_ELX, MODE_RW_64);
+ ns_image_info->security_state = NON_SECURE;
+ flush_dcache_range((unsigned long) ns_image_info,
+ sizeof(el_change_info));
+
+ /*
+ * Run BL31 via an SMC to BL1. Information on how to pass control to
+ * the non-trusted software image will be passed to BL31 in x2.
+ */
+ if (bl31_base)
+ run_image(bl31_base,
+ make_spsr(MODE_EL3, MODE_SP_ELX, MODE_RW_64),
+ SECURE,
+ bl31_tzram_layout,
+ (void *) ns_image_info);
+
+ /* There is no valid reason for run_image() to return */
+ assert(0);
+}
+
+/*******************************************************************************
+ * BL1 has this function to print the fact that BL2 has done its job and BL31 is
+ * about to be loaded. Since BL2 re-uses BL1's exception table, it needs to
+ * define this function as well.
+ * TODO: Remove this function from BL2.
+ ******************************************************************************/
+void display_boot_progress(unsigned long entrypoint,
+ unsigned long spsr,
+ unsigned long mem_layout,
+ unsigned long ns_image_info)
+{
+ return;
+}
diff --git a/bl31/aarch64/bl31_arch_setup.c b/bl31/aarch64/bl31_arch_setup.c
new file mode 100644
index 0000000..f6fa088
--- /dev/null
+++ b/bl31/aarch64/bl31_arch_setup.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+#include <platform.h>
+#include <assert.h>
+
+/*******************************************************************************
+ * This duplicates what the primary cpu did after a cold boot in BL1. The same
+ * needs to be done when a cpu is hotplugged in. This function could also over-
+ * ride any EL3 setup done by BL1 as this code resides in rw memory.
+ ******************************************************************************/
+void bl31_arch_setup(void)
+{
+ unsigned long tmp_reg = 0;
+ unsigned int counter_base_frequency;
+
+ /* Enable alignment checks and set the exception endianness to LE */
+ tmp_reg = read_sctlr();
+ tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
+ tmp_reg &= ~SCTLR_EE_BIT;
+ write_sctlr(tmp_reg);
+
+ /*
+ * Enable HVCs, allow NS to mask CPSR.A, route FIQs to EL3, set the
+ * next EL to be aarch64
+ */
+ tmp_reg = SCR_RES1_BITS | SCR_RW_BIT | SCR_HCE_BIT | SCR_FIQ_BIT;
+ write_scr(tmp_reg);
+
+ /* Do not trap coprocessor accesses from lower ELs to EL3 */
+ write_cptr_el3(0);
+
+ /* Read the frequency from Frequency modes table */
+ counter_base_frequency = mmio_read_32(SYS_CNTCTL_BASE + CNTFID_OFF);
+ /* The first entry of the frequency modes table must not be 0 */
+ assert(counter_base_frequency != 0);
+
+ /* Program the counter frequency */
+ write_cntfrq_el0(counter_base_frequency);
+ return;
+}
+
+/*******************************************************************************
+ * Detect what is the next Non-Secure EL and setup the required architectural
+ * state
+ ******************************************************************************/
+void bl31_arch_next_el_setup(void) {
+ unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1();
+ unsigned long current_sctlr, next_sctlr;
+ unsigned long el_status;
+ unsigned long scr = read_scr();
+
+ /* Use the same endianness than the current BL */
+ current_sctlr = read_sctlr();
+ next_sctlr = (current_sctlr & SCTLR_EE_BIT);
+
+ /* Find out which EL we are going to */
+ el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) & ID_AA64PFR0_ELX_MASK;
+
+ /* Check what if EL2 is supported */
+ if (el_status && (scr & SCR_HCE_BIT)) {
+ /* Set SCTLR EL2 */
+ next_sctlr |= SCTLR_EL2_RES1;
+
+ write_sctlr_el2(next_sctlr);
+ } else {
+ /* Set SCTLR Non-Secure EL1 */
+ next_sctlr |= SCTLR_EL1_RES1;
+
+ write_sctlr_el1(next_sctlr);
+ }
+}
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
new file mode 100644
index 0000000..3a850e6
--- /dev/null
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <bl1.h>
+#include <bl_common.h>
+#include <platform.h>
+
+
+ .globl bl31_entrypoint
+
+
+ .section entry_code, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * bl31_entrypoint() is the cold boot entrypoint,
+ * executed only by the primary cpu.
+ * -----------------------------------------------------
+ */
+
+bl31_entrypoint:; .type bl31_entrypoint, %function
+ /* ---------------------------------------------
+ * BL2 has populated x0,x3,x4 with the opcode
+ * indicating BL31 should be run, memory layout
+ * of the trusted SRAM available to BL31 and
+ * information about running the non-trusted
+ * software already loaded by BL2. Check the
+ * opcode out of paranoia.
+ * ---------------------------------------------
+ */
+ mov x19, #RUN_IMAGE
+ cmp x0, x19
+ b.ne _panic
+ mov x20, x3
+ mov x21, x4
+
+ /* ---------------------------------------------
+ * This is BL31 which is expected to be executed
+ * only by the primary cpu (at least for now).
+ * So, make sure no secondary has lost its way.
+ * ---------------------------------------------
+ */
+ bl read_mpidr
+ mov x19, x0
+ bl platform_is_primary_cpu
+ cbz x0, _panic
+
+ /* --------------------------------------------
+ * Give ourselves a small coherent stack to
+ * ease the pain of initializing the MMU
+ * --------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Perform platform specific early arch. setup
+ * ---------------------------------------------
+ */
+ mov x0, x20
+ mov x1, x21
+ mov x2, x19
+ bl bl31_early_platform_setup
+ bl bl31_plat_arch_setup
+
+ /* ---------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* ---------------------------------------------
+ * Use SP_EL0 to initialize BL31. It allows us
+ * to jump to the next image without having to
+ * come back here to ensure all of the stack's
+ * been popped out. run_image() is not nice
+ * enough to reset the stack pointer before
+ * handing control to the next stage.
+ * ---------------------------------------------
+ */
+ mov x0, sp
+ msr sp_el0, x0
+ msr spsel, #0
+ isb
+
+ /* ---------------------------------------------
+ * Jump to main function.
+ * ---------------------------------------------
+ */
+ bl bl31_main
+
+_panic:
+ b _panic
diff --git a/bl31/aarch64/exception_handlers.c b/bl31/aarch64/exception_handlers.c
new file mode 100644
index 0000000..860d8eb
--- /dev/null
+++ b/bl31/aarch64/exception_handlers.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+#include <platform.h>
+#include <bl_common.h>
+#include <bl31.h>
+#include <psci.h>
+#include <assert.h>
+#include <runtime_svc.h>
+
+/*******************************************************************************
+ * This function checks whether this is a valid smc e.g.
+ * the function id is correct, top word of args are zeroed
+ * when aarch64 makes an aarch32 call etc.
+ ******************************************************************************/
+int validate_smc(gp_regs *regs)
+{
+ unsigned int rw = GET_RW(regs->spsr);
+ unsigned int cc = GET_SMC_CC(regs->x0);
+
+ /* Check if there is a difference in the caller RW and SMC CC */
+ if (rw == cc) {
+
+ /* Check whether the caller has chosen the right func. id */
+ if (cc == SMC_64) {
+ regs->x0 = SMC_UNK;
+ return SMC_UNK;
+ }
+
+ /*
+ * Paranoid check to zero the top word of passed args
+ * irrespective of caller's register width.
+ *
+ * TODO: Check if this needed if the caller is aarch32
+ */
+ regs->x0 &= (unsigned int) 0xFFFFFFFF;
+ regs->x1 &= (unsigned int) 0xFFFFFFFF;
+ regs->x2 &= (unsigned int) 0xFFFFFFFF;
+ regs->x3 &= (unsigned int) 0xFFFFFFFF;
+ regs->x4 &= (unsigned int) 0xFFFFFFFF;
+ regs->x5 &= (unsigned int) 0xFFFFFFFF;
+ regs->x6 &= (unsigned int) 0xFFFFFFFF;
+ }
+
+ return 0;
+}
+
+/* TODO: Break down the SMC handler into fast and standard SMC handlers. */
+void smc_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ /* Check if the SMC has been correctly called */
+ if (validate_smc(regs) != 0)
+ return;
+
+ switch (regs->x0) {
+ case PSCI_VERSION:
+ regs->x0 = psci_version();
+ break;
+
+ case PSCI_CPU_OFF:
+ regs->x0 = __psci_cpu_off();
+ break;
+
+ case PSCI_CPU_SUSPEND_AARCH64:
+ case PSCI_CPU_SUSPEND_AARCH32:
+ regs->x0 = __psci_cpu_suspend(regs->x1, regs->x2, regs->x3);
+ break;
+
+ case PSCI_CPU_ON_AARCH64:
+ case PSCI_CPU_ON_AARCH32:
+ regs->x0 = psci_cpu_on(regs->x1, regs->x2, regs->x3);
+ break;
+
+ case PSCI_AFFINITY_INFO_AARCH32:
+ case PSCI_AFFINITY_INFO_AARCH64:
+ regs->x0 = psci_affinity_info(regs->x1, regs->x2);
+ break;
+
+ default:
+ regs->x0 = SMC_UNK;
+ }
+
+ return;
+}
+
+void irq_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void fiq_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void serror_handler(unsigned type, unsigned long esr, gp_regs *regs)
+{
+ plat_report_exception(type);
+ assert(0);
+}
+
+void sync_exception_handler(unsigned type, gp_regs *regs)
+{
+ unsigned long esr = read_esr();
+ unsigned int ec = EC_BITS(esr);
+
+ switch (ec) {
+
+ case EC_AARCH32_SMC:
+ case EC_AARCH64_SMC:
+ smc_handler(type, esr, regs);
+ break;
+
+ default:
+ plat_report_exception(type);
+ assert(0);
+ }
+ return;
+}
+
+void async_exception_handler(unsigned type, gp_regs *regs)
+{
+ unsigned long esr = read_esr();
+
+ switch (type) {
+
+ case IRQ_SP_EL0:
+ case IRQ_SP_ELX:
+ case IRQ_AARCH64:
+ case IRQ_AARCH32:
+ irq_handler(type, esr, regs);
+ break;
+
+ case FIQ_SP_EL0:
+ case FIQ_SP_ELX:
+ case FIQ_AARCH64:
+ case FIQ_AARCH32:
+ fiq_handler(type, esr, regs);
+ break;
+
+ case SERROR_SP_EL0:
+ case SERROR_SP_ELX:
+ case SERROR_AARCH64:
+ case SERROR_AARCH32:
+ serror_handler(type, esr, regs);
+ break;
+
+ default:
+ plat_report_exception(type);
+ assert(0);
+ }
+
+ return;
+}
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
new file mode 100644
index 0000000..21976ad
--- /dev/null
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <runtime_svc.h>
+
+ .globl runtime_exceptions
+
+
+#include <asm_macros.S>
+
+
+ .section aarch64_code, "ax"; .align 11
+
+ .align 7
+runtime_exceptions:
+ /* -----------------------------------------------------
+ * Current EL with _sp_el0 : 0x0 - 0x180
+ * -----------------------------------------------------
+ */
+sync_exception_sp_el0:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_SP_EL0
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_sp_el0:
+ exception_entry save_regs
+ mov x0, #IRQ_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_sp_el0:
+ exception_entry save_regs
+ mov x0, #FIQ_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_sp_el0:
+ exception_entry save_regs
+ mov x0, #SERROR_SP_EL0
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Current EL with SPx: 0x200 - 0x380
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_sp_elx:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_SP_ELX
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_sp_elx:
+ exception_entry save_regs
+ mov x0, #IRQ_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_sp_elx:
+ exception_entry save_regs
+ mov x0, #FIQ_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_sp_elx:
+ exception_entry save_regs
+ mov x0, #SERROR_SP_ELX
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch64 : 0x400 - 0x580
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_aarch64:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_AARCH64
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_aarch64:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH64
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_aarch64:
+ exception_entry save_regs
+ mov x0, #FIQ_AARCH64
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_aarch64:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ /* -----------------------------------------------------
+ * Lower EL using AArch32 : 0x600 - 0x780
+ * -----------------------------------------------------
+ */
+ .align 7
+sync_exception_aarch32:
+ exception_entry save_regs
+ mov x0, #SYNC_EXCEPTION_AARCH32
+ mov x1, sp
+ bl sync_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+irq_aarch32:
+ exception_entry save_regs
+ mov x0, #IRQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+fiq_aarch32:
+ exception_entry save_regs
+ mov x0, #FIQ_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+serror_aarch32:
+ exception_entry save_regs
+ mov x0, #SERROR_AARCH32
+ mov x1, sp
+ bl async_exception_handler
+ exception_exit restore_regs
+ eret
+
+ .align 7
+
+save_regs:; .type save_regs, %function
+ sub sp, sp, #0x100
+ stp x0, x1, [sp, #0x0]
+ stp x2, x3, [sp, #0x10]
+ stp x4, x5, [sp, #0x20]
+ stp x6, x7, [sp, #0x30]
+ stp x8, x9, [sp, #0x40]
+ stp x10, x11, [sp, #0x50]
+ stp x12, x13, [sp, #0x60]
+ stp x14, x15, [sp, #0x70]
+ stp x16, x17, [sp, #0x80]
+ stp x18, x19, [sp, #0x90]
+ stp x20, x21, [sp, #0xa0]
+ stp x22, x23, [sp, #0xb0]
+ stp x24, x25, [sp, #0xc0]
+ stp x26, x27, [sp, #0xd0]
+ mrs x0, sp_el0
+ stp x28, x0, [sp, #0xe0]
+ mrs x0, spsr_el3
+ str x0, [sp, #0xf0]
+ ret
+
+
+restore_regs:; .type restore_regs, %function
+ ldr x9, [sp, #0xf0]
+ msr spsr_el3, x9
+ ldp x28, x9, [sp, #0xe0]
+ msr sp_el0, x9
+ ldp x26, x27, [sp, #0xd0]
+ ldp x24, x25, [sp, #0xc0]
+ ldp x22, x23, [sp, #0xb0]
+ ldp x20, x21, [sp, #0xa0]
+ ldp x18, x19, [sp, #0x90]
+ ldp x16, x17, [sp, #0x80]
+ ldp x14, x15, [sp, #0x70]
+ ldp x12, x13, [sp, #0x60]
+ ldp x10, x11, [sp, #0x50]
+ ldp x8, x9, [sp, #0x40]
+ ldp x6, x7, [sp, #0x30]
+ ldp x4, x5, [sp, #0x20]
+ ldp x2, x3, [sp, #0x10]
+ ldp x0, x1, [sp, #0x0]
+ add sp, sp, #0x100
+ ret
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
new file mode 100644
index 0000000..5ad8648
--- /dev/null
+++ b/bl31/bl31.ld.S
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <platform.h>
+
+OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
+OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
+
+
+MEMORY {
+ /* RAM is read/write and Initialised */
+ RAM (rwx): ORIGIN = TZRAM_BASE, LENGTH = TZRAM_SIZE
+}
+
+
+SECTIONS
+{
+ . = BL31_BASE;
+
+ BL31_RO ALIGN (4096): {
+ *(entry_code)
+ *(.text)
+ *(.rodata)
+ } >RAM
+
+ BL31_STACKS ALIGN (4096): {
+ . += 0x1000;
+ *(tzfw_normal_stacks)
+ } >RAM
+
+ BL31_COHERENT_RAM ALIGN (4096): {
+ *(tzfw_coherent_mem)
+ /* . += 0x1000;*/
+ /* Do we need to ensure at least 4k here? */
+ . = ALIGN(4096);
+ } >RAM
+
+ __BL31_DATA_START__ = .;
+ .bss ALIGN (4096): {
+ *(.bss)
+ *(COMMON)
+ } >RAM
+
+ .data : {
+ *(.data)
+ } >RAM
+ __BL31_DATA_STOP__ = .;
+
+
+ __BL31_RO_BASE__ = LOADADDR(BL31_RO);
+ __BL31_RO_SIZE__ = SIZEOF(BL31_RO);
+
+ __BL31_STACKS_BASE__ = LOADADDR(BL31_STACKS);
+ __BL31_STACKS_SIZE__ = SIZEOF(BL31_STACKS);
+
+ __BL31_COHERENT_RAM_BASE__ = LOADADDR(BL31_COHERENT_RAM);
+ __BL31_COHERENT_RAM_SIZE__ = SIZEOF(BL31_COHERENT_RAM);
+
+ __BL31_RW_BASE__ = __BL31_DATA_START__;
+ __BL31_RW_SIZE__ = __BL31_DATA_STOP__ - __BL31_DATA_START__;
+}
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
new file mode 100644
index 0000000..dcf78bc
--- /dev/null
+++ b/bl31/bl31.mk
@@ -0,0 +1,55 @@
+#
+# Copyright (c) 2013, ARM Limited. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# 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.
+#
+# Neither the name of ARM nor the names of its contributors may be used
+# to endorse or promote products derived from this software without specific
+# prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+#
+
+vpath %.c drivers/arm/interconnect/cci-400/ common/ lib/ \
+ drivers/arm/peripherals/pl011 plat/fvp common/psci \
+ lib/semihosting arch/aarch64/ lib/non-semihosting \
+ lib/sync/locks/bakery/ drivers/power/ arch/system/gic/ \
+ plat/fvp/aarch64/
+
+vpath %.S lib/arch/aarch64 common/psci \
+ lib/semihosting/aarch64 include/ plat/fvp/${ARCH} \
+ lib/sync/locks/exclusive plat/common/aarch64/ \
+ arch/system/gic/${ARCH}
+
+BL31_ASM_OBJS := bl31_entrypoint.o runtime_exceptions.o psci_entry.o \
+ spinlock.o gic_v3_sysregs.o fvp_helpers.o
+BL31_C_OBJS := bl31_main.o bl31_plat_setup.o bl31_arch_setup.o \
+ exception_handlers.o bakery_lock.o cci400.o \
+ fvp_common.o fvp_pm.o fvp_pwrc.o fvp_topology.o \
+ runtime_svc.o gic_v3.o gic_v2.o psci_setup.o \
+ psci_common.o psci_afflvl_on.o psci_main.o \
+ psci_afflvl_off.o psci_afflvl_suspend.o
+
+BL31_ENTRY_POINT := bl31_entrypoint
+BL31_MAPFILE := bl31.map
+BL31_LINKERFILE := bl31.ld
+
+BL31_OBJS := $(BL31_C_OBJS) $(BL31_ASM_OBJS)
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
new file mode 100644
index 0000000..e8fa2f8
--- /dev/null
+++ b/bl31/bl31_main.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl_common.h>
+#include <bl31.h>
+#include <runtime_svc.h>
+
+void bl31_arch_next_el_setup(void);
+
+/*******************************************************************************
+ * BL31 is responsible for setting up the runtime services for the primary cpu
+ * before passing control to the bootloader (UEFI) or Linux.
+ ******************************************************************************/
+void bl31_main(void)
+{
+ el_change_info *image_info;
+ unsigned long mpidr = read_mpidr();
+
+ /* Perform remaining generic architectural setup from EL3 */
+ bl31_arch_setup();
+
+ /* Perform platform setup in BL1 */
+ bl31_platform_setup();
+
+#if defined (__GNUC__)
+ printf("BL31 Built : %s, %s\n\r", __TIME__, __DATE__);
+#endif
+
+
+ /* Initialize the runtime services e.g. psci */
+ runtime_svc_init(mpidr);
+
+ /* Clean caches before re-entering normal world */
+ dcsw_op_all(DCCSW);
+
+ image_info = bl31_get_next_image_info(mpidr);
+ bl31_arch_next_el_setup();
+ change_el(image_info);
+
+ /* There is no valid reason for change_el() to return */
+ assert(0);
+}
diff --git a/common/bl_common.c b/common/bl_common.c
new file mode 100644
index 0000000..d125786
--- /dev/null
+++ b/common/bl_common.c
@@ -0,0 +1,516 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl_common.h>
+#include <bl1.h>
+
+/***********************************************************
+ * Memory for sharing data while changing exception levels.
+ * Only used by the primary core.
+ **********************************************************/
+unsigned char bl2_el_change_mem_ptr[EL_CHANGE_MEM_SIZE];
+
+unsigned long *get_el_change_mem_ptr(void)
+{
+ return (unsigned long *) bl2_el_change_mem_ptr;
+}
+
+unsigned long page_align(unsigned long value, unsigned dir)
+{
+ unsigned long page_size = 1 << FOUR_KB_SHIFT;
+
+ /* Round up the limit to the next page boundary */
+ if (value & (page_size - 1)) {
+ value &= ~(page_size - 1);
+ if (dir == UP)
+ value += page_size;
+ }
+
+ return value;
+}
+
+static inline unsigned int is_page_aligned (unsigned long addr) {
+ const unsigned long page_size = 1 << FOUR_KB_SHIFT;
+
+ return (addr & (page_size - 1)) == 0;
+}
+
+void change_security_state(unsigned int target_security_state)
+{
+ unsigned long scr = read_scr();
+
+ if (target_security_state == SECURE)
+ scr &= ~SCR_NS_BIT;
+ else if (target_security_state == NON_SECURE)
+ scr |= SCR_NS_BIT;
+ else
+ assert(0);
+
+ write_scr(scr);
+}
+
+int drop_el(aapcs64_params *args,
+ unsigned long spsr,
+ unsigned long entrypoint)
+{
+ write_spsr(spsr);
+ write_elr(entrypoint);
+ eret(args->arg0,
+ args->arg1,
+ args->arg2,
+ args->arg3,
+ args->arg4,
+ args->arg5,
+ args->arg6,
+ args->arg7);
+ return -EINVAL;
+}
+
+long raise_el(aapcs64_params *args)
+{
+ return smc(args->arg0,
+ args->arg1,
+ args->arg2,
+ args->arg3,
+ args->arg4,
+ args->arg5,
+ args->arg6,
+ args->arg7);
+}
+
+/*
+ * TODO: If we are not EL3 then currently we only issue an SMC.
+ * Add support for dropping into EL0 etc. Consider adding support
+ * for switching from S-EL1 to S-EL0/1 etc.
+ */
+long change_el(el_change_info *info)
+{
+ unsigned long current_el = read_current_el();
+
+ if (GET_EL(current_el) == MODE_EL3) {
+ /*
+ * We can go anywhere from EL3. So find where.
+ * TODO: Lots to do if we are going non-secure.
+ * Flip the NS bit. Restore NS registers etc.
+ * Just doing the bare minimal for now.
+ */
+
+ if (info->security_state == NON_SECURE)
+ change_security_state(info->security_state);
+
+ return drop_el(&info->args, info->spsr, info->entrypoint);
+ } else
+ return raise_el(&info->args);
+}
+
+/* TODO: add a parameter for DAIF. not needed right now */
+unsigned long make_spsr(unsigned long target_el,
+ unsigned long target_sp,
+ unsigned long target_rw)
+{
+ unsigned long spsr;
+
+ /* Disable all exceptions & setup the EL */
+ spsr = (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT)
+ << PSR_DAIF_SHIFT;
+ spsr |= PSR_MODE(target_rw, target_el, target_sp);
+
+ return spsr;
+}
+
+/*******************************************************************************
+ * The next two functions are the weak definitions. Platform specific
+ * code can override them if it wishes to.
+ ******************************************************************************/
+
+/*******************************************************************************
+ * Function that takes a memory layout into which BL31 has been either top or
+ * bottom loaded. Using this information, it populates bl31_mem_layout to tell
+ * BL31 how much memory it has access to and how much is available for use. It
+ * does not need the address where BL31 has been loaded as BL31 will reclaim
+ * all the memory used by BL2.
+ * TODO: Revisit if this and init_bl2_mem_layout can be replaced by a single
+ * routine.
+ ******************************************************************************/
+void init_bl31_mem_layout(const meminfo *bl2_mem_layout,
+ meminfo *bl31_mem_layout,
+ unsigned int load_type)
+{
+ if (load_type == BOT_LOAD) {
+ /*
+ * ------------ ^
+ * | BL2 | |
+ * |----------| ^ | BL2
+ * | | | BL2 free | total
+ * | | | size | size
+ * |----------| BL2 free base v |
+ * | BL31 | |
+ * ------------ BL2 total base v
+ */
+ unsigned long bl31_size;
+
+ bl31_mem_layout->free_base = bl2_mem_layout->free_base;
+
+ bl31_size = bl2_mem_layout->free_base - bl2_mem_layout->total_base;
+ bl31_mem_layout->free_size = bl2_mem_layout->total_size - bl31_size;
+ } else {
+ /*
+ * ------------ ^
+ * | BL31 | |
+ * |----------| ^ | BL2
+ * | | | BL2 free | total
+ * | | | size | size
+ * |----------| BL2 free base v |
+ * | BL2 | |
+ * ------------ BL2 total base v
+ */
+ unsigned long bl2_size;
+
+ bl31_mem_layout->free_base = bl2_mem_layout->total_base;
+
+ bl2_size = bl2_mem_layout->free_base - bl2_mem_layout->total_base;
+ bl31_mem_layout->free_size = bl2_mem_layout->free_size + bl2_size;
+ }
+
+ bl31_mem_layout->total_base = bl2_mem_layout->total_base;
+ bl31_mem_layout->total_size = bl2_mem_layout->total_size;
+ bl31_mem_layout->attr = load_type;
+
+ flush_dcache_range((unsigned long) bl31_mem_layout, sizeof(meminfo));
+ return;
+}
+
+/*******************************************************************************
+ * Function that takes a memory layout into which BL2 has been either top or
+ * bottom loaded along with the address where BL2 has been loaded in it. Using
+ * this information, it populates bl2_mem_layout to tell BL2 how much memory
+ * it has access to and how much is available for use.
+ ******************************************************************************/
+void init_bl2_mem_layout(meminfo *bl1_mem_layout,
+ meminfo *bl2_mem_layout,
+ unsigned int load_type,
+ unsigned long bl2_base)
+{
+ unsigned tmp;
+
+ if (load_type == BOT_LOAD) {
+ bl2_mem_layout->total_base = bl2_base;
+ tmp = bl1_mem_layout->free_base - bl2_base;
+ bl2_mem_layout->total_size = bl1_mem_layout->free_size + tmp;
+
+ } else {
+ bl2_mem_layout->total_base = bl1_mem_layout->free_base;
+ tmp = bl1_mem_layout->total_base + bl1_mem_layout->total_size;
+ bl2_mem_layout->total_size = tmp - bl1_mem_layout->free_base;
+ }
+
+ bl2_mem_layout->free_base = bl1_mem_layout->free_base;
+ bl2_mem_layout->free_size = bl1_mem_layout->free_size;
+ bl2_mem_layout->attr = load_type;
+
+ flush_dcache_range((unsigned long) bl2_mem_layout, sizeof(meminfo));
+ return;
+}
+
+static void dump_load_info(unsigned long image_load_addr,
+ unsigned long image_size,
+ const meminfo *mem_layout)
+{
+#if DEBUG
+ printf("Trying to load image at address 0x%lx, size = 0x%lx\r\n",
+ image_load_addr, image_size);
+ printf("Current memory layout:\r\n");
+ printf(" total region = [0x%lx, 0x%lx]\r\n", mem_layout->total_base,
+ mem_layout->total_base + mem_layout->total_size);
+ printf(" free region = [0x%lx, 0x%lx]\r\n", mem_layout->free_base,
+ mem_layout->free_base + mem_layout->free_size);
+#endif
+}
+
+/*******************************************************************************
+ * Generic function to load an image into the trusted RAM using semihosting
+ * given a name, extents of free memory & whether the image should be loaded at
+ * the bottom or top of the free memory. It updates the memory layout if the
+ * load is successful.
+ ******************************************************************************/
+unsigned long load_image(meminfo *mem_layout,
+ const char *image_name,
+ unsigned int load_type,
+ unsigned long fixed_addr)
+{
+ unsigned long temp_image_base, image_base;
+ long offset;
+ int image_flen;
+
+ /* Find the size of the image */
+ image_flen = semihosting_get_flen(image_name);
+ if (image_flen < 0) {
+ printf("ERROR: Cannot access '%s' file (%i).\r\n",
+ image_name, image_flen);
+ return 0;
+ }
+
+ /* See if we have enough space */
+ if (image_flen > mem_layout->free_size) {
+ printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+ image_name);
+ dump_load_info(0, image_flen, mem_layout);
+ return 0;
+ }
+
+ switch (load_type) {
+
+ case TOP_LOAD:
+
+ /* Load the image in the top of free memory */
+ temp_image_base = mem_layout->free_base + mem_layout->free_size;
+ temp_image_base -= image_flen;
+
+ /* Page align base address and check whether the image still fits */
+ image_base = page_align(temp_image_base, DOWN);
+ assert(image_base <= temp_image_base);
+
+ if (image_base < mem_layout->free_base) {
+ printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+ image_name);
+ dump_load_info(image_base, image_flen, mem_layout);
+ return 0;
+ }
+
+ /* Calculate the amount of extra memory used due to alignment */
+ offset = temp_image_base - image_base;
+
+ break;
+
+ case BOT_LOAD:
+
+ /* Load the BL2 image in the bottom of free memory */
+ temp_image_base = mem_layout->free_base;
+ image_base = page_align(temp_image_base, UP);
+ assert(image_base >= temp_image_base);
+
+ /* Page align base address and check whether the image still fits */
+ if (image_base + image_flen >
+ mem_layout->free_base + mem_layout->free_size) {
+ printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+ image_name);
+ dump_load_info(image_base, image_flen, mem_layout);
+ return 0;
+ }
+
+ /* Calculate the amount of extra memory used due to alignment */
+ offset = image_base - temp_image_base;
+
+ break;
+
+ default:
+ assert(0);
+
+ }
+
+ /*
+ * Some images must be loaded at a fixed address, not a dynamic one.
+ *
+ * This has been implemented as a hack on top of the existing dynamic
+ * loading mechanism, for the time being. If the 'fixed_addr' function
+ * argument is different from zero, then it will force the load address.
+ * So we still have this principle of top/bottom loading but the code
+ * determining the load address is bypassed and the load address is
+ * forced to the fixed one.
+ *
+ * This can result in quite a lot of wasted space because we still use
+ * 1 sole meminfo structure to represent the extents of free memory,
+ * where we should use some sort of linked list.
+ *
+ * E.g. we want to load BL2 at address 0x04020000, the resulting memory
+ * layout should look as follows:
+ * ------------ 0x04040000
+ * | | <- Free space (1)
+ * |----------|
+ * | BL2 |
+ * |----------| 0x04020000
+ * | | <- Free space (2)
+ * |----------|
+ * | BL1 |
+ * ------------ 0x04000000
+ *
+ * But in the current hacky implementation, we'll need to specify
+ * whether BL2 is loaded at the top or bottom of the free memory.
+ * E.g. if BL2 is considered as top-loaded, the meminfo structure
+ * will give the following view of the memory, hiding the chunk of
+ * free memory above BL2:
+ * ------------ 0x04040000
+ * | |
+ * | |
+ * | BL2 |
+ * |----------| 0x04020000
+ * | | <- Free space (2)
+ * |----------|
+ * | BL1 |
+ * ------------ 0x04000000
+ */
+ if (fixed_addr != 0) {
+ /* Load the image at the given address. */
+ image_base = fixed_addr;
+
+ /* Check whether the image fits. */
+ if ((image_base < mem_layout->free_base) ||
+ (image_base + image_flen >
+ mem_layout->free_base + mem_layout->free_size)) {
+ printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+ image_name);
+ dump_load_info(image_base, image_flen, mem_layout);
+ return 0;
+ }
+
+ /* Check whether the fixed load address is page-aligned. */
+ if (!is_page_aligned(image_base)) {
+ printf("ERROR: Cannot load '%s' file at unaligned address 0x%lx.\r\n",
+ image_name, fixed_addr);
+ return 0;
+ }
+
+ /*
+ * Calculate the amount of extra memory used due to fixed
+ * loading.
+ */
+ if (load_type == TOP_LOAD) {
+ unsigned long max_addr, space_used;
+ /*
+ * ------------ max_addr
+ * | /wasted/ | | offset
+ * |..........|..............................
+ * | image | | image_flen
+ * |----------| fixed_addr
+ * | |
+ * | |
+ * ------------ total_base
+ */
+ max_addr = mem_layout->total_base + mem_layout->total_size;
+ /*
+ * Compute the amount of memory used by the image.
+ * Corresponds to all space above the image load
+ * address.
+ */
+ space_used = max_addr - fixed_addr;
+ /*
+ * Calculate the amount of wasted memory within the
+ * amount of memory used by the image.
+ */
+ offset = space_used - image_flen;
+ } else /* BOT_LOAD */
+ /*
+ * ------------
+ * | |
+ * | |
+ * |----------|
+ * | image |
+ * |..........| fixed_addr
+ * | /wasted/ | | offset
+ * ------------ total_base
+ */
+ offset = fixed_addr - mem_layout->total_base;
+ }
+
+ /* We have enough space so load the image now */
+ image_flen = semihosting_download_file(image_name,
+ image_flen,
+ (void *) image_base);
+ if (image_flen <= 0) {
+ printf("ERROR: Failed to load '%s' file from semihosting (%i).\r\n",
+ image_name, image_flen);
+ return 0;
+ }
+
+ /*
+ * File has been successfully loaded. Update the free memory
+ * data structure & flush the contents of the TZRAM so that
+ * the next EL can see it.
+ */
+ /* Update the memory contents */
+ flush_dcache_range(image_base, image_flen);
+
+ mem_layout->free_size -= image_flen + offset;
+
+ /* Update the base of free memory since its moved up */
+ if (load_type == BOT_LOAD)
+ mem_layout->free_base += offset + image_flen;
+
+ return image_base;
+}
+
+/*******************************************************************************
+ * Run a loaded image from the given entry point. This could result in either
+ * dropping into a lower exception level or jumping to a higher exception level.
+ * The only way of doing the latter is through an SMC. In either case, setup the
+ * parameters for the EL change request correctly.
+ ******************************************************************************/
+int run_image(unsigned long entrypoint,
+ unsigned long spsr,
+ unsigned long target_security_state,
+ meminfo *mem_layout,
+ void *data)
+{
+ el_change_info run_image_info;
+ unsigned long current_el = read_current_el();
+
+ /* Tell next EL what we want done */
+ run_image_info.args.arg0 = RUN_IMAGE;
+ run_image_info.entrypoint = entrypoint;
+ run_image_info.spsr = spsr;
+ run_image_info.security_state = target_security_state;
+ run_image_info.next = 0;
+
+ /*
+ * If we are EL3 then only an eret can take us to the desired
+ * exception level. Else for the time being assume that we have
+ * to jump to a higher EL and issue an SMC. Contents of argY
+ * will go into the general purpose register xY e.g. arg0->x0
+ */
+ if (GET_EL(current_el) == MODE_EL3) {
+ run_image_info.args.arg1 = (unsigned long) mem_layout;
+ run_image_info.args.arg2 = (unsigned long) data;
+ } else {
+ run_image_info.args.arg1 = entrypoint;
+ run_image_info.args.arg2 = spsr;
+ run_image_info.args.arg3 = (unsigned long) mem_layout;
+ run_image_info.args.arg4 = (unsigned long) data;
+ }
+
+ return change_el(&run_image_info);
+}
diff --git a/common/psci/psci_afflvl_off.c b/common/psci/psci_afflvl_off.c
new file mode 100644
index 0000000..937ba9d
--- /dev/null
+++ b/common/psci/psci_afflvl_off.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+
+typedef int (*afflvl_off_handler)(unsigned long, aff_map_node *);
+
+/*******************************************************************************
+ * The next three functions implement a handler for each supported affinity
+ * level which is called when that affinity level is turned off.
+ ******************************************************************************/
+static int psci_afflvl0_off(unsigned long mpidr, aff_map_node *cpu_node)
+{
+ unsigned int index, plat_state;
+ int rc = PSCI_E_SUCCESS;
+ unsigned long sctlr = read_sctlr();
+
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /*
+ * Generic management: Get the index for clearing any
+ * lingering re-entry information
+ */
+ index = cpu_node->data;
+ memset(&psci_ns_entry_info[index], 0, sizeof(psci_ns_entry_info[index]));
+
+ /*
+ * Arch. management. Perform the necessary steps to flush all
+ * cpu caches.
+ *
+ * TODO: This power down sequence varies across cpus so it needs to be
+ * abstracted out on the basis of the MIDR like in cpu_reset_handler().
+ * Do the bare minimal for the time being. Fix this before porting to
+ * Cortex models.
+ */
+ sctlr &= ~SCTLR_C_BIT;
+ write_sctlr(sctlr);
+
+ /*
+ * CAUTION: This flush to the level of unification makes an assumption
+ * about the cache hierarchy at affinity level 0 (cpu) in the platform.
+ * Ideally the platform should tell psci which levels to flush to exit
+ * coherency.
+ */
+ dcsw_op_louis(DCCISW);
+
+ /*
+ * Plat. management: Perform platform specific actions to turn this
+ * cpu off e.g. exit cpu coherency, program the power controller etc.
+ */
+ if (psci_plat_pm_ops->affinst_off) {
+
+ /* Get the current physical state of this cpu */
+ plat_state = psci_get_aff_phys_state(cpu_node);
+ rc = psci_plat_pm_ops->affinst_off(mpidr,
+ cpu_node->level,
+ plat_state);
+ }
+
+ /*
+ * The only error cpu_off can return is E_DENIED. So check if that's
+ * indeed the case. The caller will simply 'eret' in case of an error.
+ */
+ if (rc != PSCI_E_SUCCESS)
+ assert(rc == PSCI_E_DENIED);
+
+ return rc;
+}
+
+static int psci_afflvl1_off(unsigned long mpidr, aff_map_node *cluster_node)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+
+ /* Sanity check the cluster level */
+ assert(cluster_node->level == MPIDR_AFFLVL1);
+
+ /*
+ * Keep the physical state of this cluster handy to decide
+ * what action needs to be taken
+ */
+ plat_state = psci_get_aff_phys_state(cluster_node);
+
+ /*
+ * Arch. Management. Flush all levels of caches to PoC if
+ * the cluster is to be shutdown
+ */
+ if (plat_state == PSCI_STATE_OFF)
+ dcsw_op_all(DCCISW);
+
+ /*
+ * Plat. Management. Allow the platform to do it's cluster
+ * specific bookeeping e.g. turn off interconnect coherency,
+ * program the power controller etc.
+ */
+ if (psci_plat_pm_ops->affinst_off)
+ rc = psci_plat_pm_ops->affinst_off(mpidr,
+ cluster_node->level,
+ plat_state);
+
+ return rc;
+}
+
+static int psci_afflvl2_off(unsigned long mpidr, aff_map_node *system_node)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+
+ /* Cannot go beyond this level */
+ assert(system_node->level == MPIDR_AFFLVL2);
+
+ /*
+ * Keep the physical state of the system handy to decide what
+ * action needs to be taken
+ */
+ plat_state = psci_get_aff_phys_state(system_node);
+
+ /* No arch. and generic bookeeping to do here currently */
+
+ /*
+ * Plat. Management : Allow the platform to do it's bookeeping
+ * at this affinity level
+ */
+ if (psci_plat_pm_ops->affinst_off)
+ rc = psci_plat_pm_ops->affinst_off(mpidr,
+ system_node->level,
+ plat_state);
+ return rc;
+}
+
+static const afflvl_off_handler psci_afflvl_off_handlers[] = {
+ psci_afflvl0_off,
+ psci_afflvl1_off,
+ psci_afflvl2_off,
+};
+
+/*******************************************************************************
+ * This function implements the core of the processing required to turn a cpu
+ * off. It's assumed that along with turning the cpu off, higher affinity levels
+ * will be turned off as far as possible. We first need to determine the new
+ * state off all the affinity instances in the mpidr corresponding to the target
+ * cpu. Action will be taken on the basis of this new state. To do the state
+ * change we first need to acquire the locks for all the implemented affinity
+ * level to be able to snapshot the system state. Then we need to start turning
+ * affinity levels off from the lowest to the highest (e.g. a cpu needs to be
+ * off before a cluster can be turned off). To achieve this flow, we start
+ * acquiring the locks from the highest to the lowest affinity level. Once we
+ * reach affinity level 0, we do the state change followed by the actions
+ * corresponding to the new state for affinity level 0. Actions as per the
+ * updated state for higher affinity levels are performed as we unwind back to
+ * highest affinity level.
+ ******************************************************************************/
+int psci_afflvl_off(unsigned long mpidr,
+ int cur_afflvl,
+ int tgt_afflvl)
+{
+ int rc = PSCI_E_SUCCESS, level;
+ unsigned int next_state, prev_state;
+ aff_map_node *aff_node;
+
+ mpidr &= MPIDR_AFFINITY_MASK;;
+
+ /*
+ * Some affinity instances at levels between the current and
+ * target levels could be absent in the mpidr. Skip them and
+ * start from the first present instance.
+ */
+ level = psci_get_first_present_afflvl(mpidr,
+ cur_afflvl,
+ tgt_afflvl,
+ &aff_node);
+ /*
+ * Return if there are no more affinity instances beyond this
+ * level to process. Else ensure that the returned affinity
+ * node makes sense.
+ */
+ if (aff_node == NULL)
+ return rc;
+
+ assert(level == aff_node->level);
+
+ /*
+ * This function acquires the lock corresponding to each
+ * affinity level so that state management can be done safely.
+ */
+ bakery_lock_get(mpidr, &aff_node->lock);
+
+ /* Keep the old state and the next one handy */
+ prev_state = psci_get_state(aff_node->state);
+ next_state = PSCI_STATE_OFF;
+
+ /*
+ * We start from the highest affinity level and work our way
+ * downwards to the lowest i.e. MPIDR_AFFLVL0.
+ */
+ if (aff_node->level == tgt_afflvl) {
+ psci_change_state(mpidr,
+ tgt_afflvl,
+ get_max_afflvl(),
+ next_state);
+ } else {
+ rc = psci_afflvl_off(mpidr, level - 1, tgt_afflvl);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+ }
+
+ /*
+ * Perform generic, architecture and platform specific
+ * handling
+ */
+ rc = psci_afflvl_off_handlers[level](mpidr, aff_node);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+
+ /*
+ * If all has gone as per plan then this cpu should be
+ * marked as OFF
+ */
+ if (level == MPIDR_AFFLVL0) {
+ next_state = psci_get_state(aff_node->state);
+ assert(next_state == PSCI_STATE_OFF);
+ }
+
+exit:
+ bakery_lock_release(mpidr, &aff_node->lock);
+ return rc;
+}
diff --git a/common/psci/psci_afflvl_on.c b/common/psci/psci_afflvl_on.c
new file mode 100644
index 0000000..b0de063
--- /dev/null
+++ b/common/psci/psci_afflvl_on.c
@@ -0,0 +1,416 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+
+typedef int (*afflvl_on_handler)(unsigned long,
+ aff_map_node *,
+ unsigned long,
+ unsigned long);
+
+/*******************************************************************************
+ * This function checks whether a cpu which has been requested to be turned on
+ * is OFF to begin with.
+ ******************************************************************************/
+static int cpu_on_validate_state(unsigned int state)
+{
+ unsigned int psci_state;
+
+ /* Get the raw psci state */
+ psci_state = psci_get_state(state);
+
+ if (psci_state == PSCI_STATE_ON || psci_state == PSCI_STATE_SUSPEND)
+ return PSCI_E_ALREADY_ON;
+
+ if (psci_state == PSCI_STATE_ON_PENDING)
+ return PSCI_E_ON_PENDING;
+
+ assert(psci_state == PSCI_STATE_OFF);
+ return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * Handler routine to turn a cpu on. It takes care of any generic, architectural
+ * or platform specific setup required.
+ * TODO: Split this code across separate handlers for each type of setup?
+ ******************************************************************************/
+static int psci_afflvl0_on(unsigned long target_cpu,
+ aff_map_node *cpu_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id)
+{
+ unsigned int index, plat_state;
+ unsigned long psci_entrypoint;
+ int rc;
+
+ /* Sanity check to safeguard against data corruption */
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /*
+ * Generic management: Ensure that the cpu is off to be
+ * turned on
+ */
+ rc = cpu_on_validate_state(cpu_node->state);
+ if (rc != PSCI_E_SUCCESS)
+ return rc;
+
+ /*
+ * Arch. management: Derive the re-entry information for
+ * the non-secure world from the non-secure state from
+ * where this call originated.
+ */
+ index = cpu_node->data;
+ rc = psci_set_ns_entry_info(index, ns_entrypoint, context_id);
+ if (rc != PSCI_E_SUCCESS)
+ return rc;
+
+ /* Set the secure world (EL3) re-entry point after BL1 */
+ psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
+
+ /*
+ * Plat. management: Give the platform the current state
+ * of the target cpu to allow it to perform the necessary
+ * steps to power on.
+ */
+ if (psci_plat_pm_ops->affinst_on) {
+
+ /* Get the current physical state of this cpu */
+ plat_state = psci_get_aff_phys_state(cpu_node);
+ rc = psci_plat_pm_ops->affinst_on(target_cpu,
+ psci_entrypoint,
+ ns_entrypoint,
+ cpu_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * Handler routine to turn a cluster on. It takes care or any generic, arch.
+ * or platform specific setup required.
+ * TODO: Split this code across separate handlers for each type of setup?
+ ******************************************************************************/
+static int psci_afflvl1_on(unsigned long target_cpu,
+ aff_map_node *cluster_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+ unsigned long psci_entrypoint;
+
+ assert(cluster_node->level == MPIDR_AFFLVL1);
+
+ /*
+ * There is no generic and arch. specific cluster
+ * management required
+ */
+
+ /*
+ * Plat. management: Give the platform the current state
+ * of the target cpu to allow it to perform the necessary
+ * steps to power on.
+ */
+ if (psci_plat_pm_ops->affinst_on) {
+ plat_state = psci_get_aff_phys_state(cluster_node);
+ psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
+ rc = psci_plat_pm_ops->affinst_on(target_cpu,
+ psci_entrypoint,
+ ns_entrypoint,
+ cluster_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * Handler routine to turn a cluster of clusters on. It takes care or any
+ * generic, arch. or platform specific setup required.
+ * TODO: Split this code across separate handlers for each type of setup?
+ ******************************************************************************/
+static int psci_afflvl2_on(unsigned long target_cpu,
+ aff_map_node *system_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+ unsigned long psci_entrypoint;
+
+ /* Cannot go beyond affinity level 2 in this psci imp. */
+ assert(system_node->level == MPIDR_AFFLVL2);
+
+ /*
+ * There is no generic and arch. specific system management
+ * required
+ */
+
+ /*
+ * Plat. management: Give the platform the current state
+ * of the target cpu to allow it to perform the necessary
+ * steps to power on.
+ */
+ if (psci_plat_pm_ops->affinst_on) {
+ plat_state = psci_get_aff_phys_state(system_node);
+ psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
+ rc = psci_plat_pm_ops->affinst_on(target_cpu,
+ psci_entrypoint,
+ ns_entrypoint,
+ system_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+/* Private data structure to make this handlers accessible through indexing */
+static const afflvl_on_handler psci_afflvl_on_handlers[] = {
+ psci_afflvl0_on,
+ psci_afflvl1_on,
+ psci_afflvl2_on,
+};
+
+/*******************************************************************************
+ * This function implements the core of the processing required to turn a cpu
+ * on. It avoids recursion to traverse from the lowest to the highest affinity
+ * level unlike the off/suspend/pon_finisher functions. It does ensure that the
+ * locks are picked in the same order as the order routines to avoid deadlocks.
+ * The flow is: Take all the locks until the highest affinity level, Call the
+ * handlers for turning an affinity level on & finally change the state of the
+ * affinity level.
+ ******************************************************************************/
+int psci_afflvl_on(unsigned long target_cpu,
+ unsigned long entrypoint,
+ unsigned long context_id,
+ int current_afflvl,
+ int target_afflvl)
+{
+ unsigned int prev_state, next_state;
+ int rc = PSCI_E_SUCCESS, level;
+ aff_map_node *aff_node;
+ unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK;
+
+ /*
+ * This loop acquires the lock corresponding to each
+ * affinity level so that by the time we hit the lowest
+ * affinity level, the system topology is snapshot and
+ * state management can be done safely.
+ */
+ for (level = current_afflvl; level >= target_afflvl; level--) {
+ aff_node = psci_get_aff_map_node(target_cpu, level);
+ if (aff_node)
+ bakery_lock_get(mpidr, &aff_node->lock);
+ }
+
+ /*
+ * Perform generic, architecture and platform specific
+ * handling
+ */
+ for (level = current_afflvl; level >= target_afflvl; level--) {
+
+ /* Grab the node for each affinity level once again */
+ aff_node = psci_get_aff_map_node(target_cpu, level);
+ if (aff_node) {
+
+ /* Keep the old state and the next one handy */
+ prev_state = psci_get_state(aff_node->state);
+ rc = psci_afflvl_on_handlers[level](target_cpu,
+ aff_node,
+ entrypoint,
+ context_id);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+ }
+ }
+
+ /*
+ * State management: Update the states since this is the
+ * target affinity level requested.
+ */
+ psci_change_state(target_cpu,
+ target_afflvl,
+ get_max_afflvl(),
+ PSCI_STATE_ON_PENDING);
+
+exit:
+ /*
+ * This loop releases the lock corresponding to each affinity level
+ * in the reverse order. It also checks the final state of the cpu.
+ */
+ for (level = target_afflvl; level <= current_afflvl; level++) {
+ aff_node = psci_get_aff_map_node(target_cpu, level);
+ if (aff_node) {
+ if (level == MPIDR_AFFLVL0) {
+ next_state = psci_get_state(aff_node->state);
+ assert(next_state == PSCI_STATE_ON_PENDING);
+ }
+ bakery_lock_release(mpidr, &aff_node->lock);
+ }
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * The following functions finish an earlier affinity power on request. They
+ * are called by the common finisher routine in psci_common.c.
+ ******************************************************************************/
+static unsigned int psci_afflvl0_on_finish(unsigned long mpidr,
+ aff_map_node *cpu_node,
+ unsigned int prev_state)
+{
+ unsigned int index, plat_state, rc = PSCI_E_SUCCESS;
+
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * for this cpu e.g. enabling the gic or zeroing the mailbox
+ * register. The actual state of this cpu has already been
+ * changed.
+ */
+ if (psci_plat_pm_ops->affinst_on_finish) {
+
+ /* Get the previous physical state of this cpu */
+ plat_state = psci_get_phys_state(prev_state);
+ rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+ cpu_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ /*
+ * Arch. management: Turn on mmu & restore architectural state
+ */
+ write_vbar((unsigned long) runtime_exceptions);
+ enable_mmu();
+
+ /*
+ * All the platform specific actions for turning this cpu
+ * on have completed. Perform enough arch.initialization
+ * to run in the non-secure address space.
+ */
+ bl31_arch_setup();
+
+ /*
+ * Generic management: Now we just need to retrieve the
+ * information that we had stashed away during the cpu_on
+ * call to set this cpu on it's way. First get the index
+ * for restoring the re-entry info
+ */
+ index = cpu_node->data;
+ rc = psci_get_ns_entry_info(index);
+
+ /* Clean caches before re-entering normal world */
+ dcsw_op_louis(DCCSW);
+
+ return rc;
+}
+
+static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
+ aff_map_node *cluster_node,
+ unsigned int prev_state)
+{
+ unsigned int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+
+ assert(cluster_node->level == MPIDR_AFFLVL1);
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * as per the old state of the cluster e.g. enabling
+ * coherency at the interconnect depends upon the state with
+ * which this cluster was powered up. If anything goes wrong
+ * then assert as there is no way to recover from this
+ * situation.
+ */
+ if (psci_plat_pm_ops->affinst_on_finish) {
+ plat_state = psci_get_phys_state(prev_state);
+ rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+ cluster_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ return rc;
+}
+
+
+static unsigned int psci_afflvl2_on_finish(unsigned long mpidr,
+ aff_map_node *system_node,
+ unsigned int prev_state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+
+ /* Cannot go beyond this affinity level */
+ assert(system_node->level == MPIDR_AFFLVL2);
+
+ /*
+ * Currently, there are no architectural actions to perform
+ * at the system level.
+ */
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * as per the old state of the cluster e.g. enabling
+ * coherency at the interconnect depends upon the state with
+ * which this cluster was powered up. If anything goes wrong
+ * then assert as there is no way to recover from this
+ * situation.
+ */
+ if (psci_plat_pm_ops->affinst_on_finish) {
+ plat_state = psci_get_phys_state(system_node->state);
+ rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
+ system_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ return rc;
+}
+
+const afflvl_power_on_finisher psci_afflvl_on_finishers[] = {
+ psci_afflvl0_on_finish,
+ psci_afflvl1_on_finish,
+ psci_afflvl2_on_finish,
+};
+
diff --git a/common/psci/psci_afflvl_suspend.c b/common/psci/psci_afflvl_suspend.c
new file mode 100644
index 0000000..030f15d
--- /dev/null
+++ b/common/psci/psci_afflvl_suspend.c
@@ -0,0 +1,465 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+
+typedef int (*afflvl_suspend_handler)(unsigned long,
+ aff_map_node *,
+ unsigned long,
+ unsigned long,
+ unsigned int);
+
+/*******************************************************************************
+ * The next three functions implement a handler for each supported affinity
+ * level which is called when that affinity level is about to be suspended.
+ ******************************************************************************/
+static int psci_afflvl0_suspend(unsigned long mpidr,
+ aff_map_node *cpu_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id,
+ unsigned int power_state)
+{
+ unsigned int index, plat_state;
+ unsigned long psci_entrypoint, sctlr = read_sctlr();
+ int rc = PSCI_E_SUCCESS;
+
+ /* Sanity check to safeguard against data corruption */
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /*
+ * Generic management: Store the re-entry information for the
+ * non-secure world
+ */
+ index = cpu_node->data;
+ rc = psci_set_ns_entry_info(index, ns_entrypoint, context_id);
+ if (rc != PSCI_E_SUCCESS)
+ return rc;
+
+ /*
+ * Arch. management: Save the secure context, flush the
+ * L1 caches and exit intra-cluster coherency et al
+ */
+ psci_secure_context[index].sctlr = read_sctlr();
+ psci_secure_context[index].scr = read_scr();
+ psci_secure_context[index].cptr = read_cptr();
+ psci_secure_context[index].cpacr = read_cpacr();
+ psci_secure_context[index].cntfrq = read_cntfrq_el0();
+ psci_secure_context[index].mair = read_mair();
+ psci_secure_context[index].tcr = read_tcr();
+ psci_secure_context[index].ttbr = read_ttbr0();
+ psci_secure_context[index].vbar = read_vbar();
+
+ /* Set the secure world (EL3) re-entry point after BL1 */
+ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
+
+ /*
+ * Arch. management. Perform the necessary steps to flush all
+ * cpu caches.
+ *
+ * TODO: This power down sequence varies across cpus so it needs to be
+ * abstracted out on the basis of the MIDR like in cpu_reset_handler().
+ * Do the bare minimal for the time being. Fix this before porting to
+ * Cortex models.
+ */
+ sctlr &= ~SCTLR_C_BIT;
+ write_sctlr(sctlr);
+
+ /*
+ * CAUTION: This flush to the level of unification makes an assumption
+ * about the cache hierarchy at affinity level 0 (cpu) in the platform.
+ * Ideally the platform should tell psci which levels to flush to exit
+ * coherency.
+ */
+ dcsw_op_louis(DCCISW);
+
+ /*
+ * Plat. management: Allow the platform to perform the
+ * necessary actions to turn off this cpu e.g. set the
+ * platform defined mailbox with the psci entrypoint,
+ * program the power controller etc.
+ */
+ if (psci_plat_pm_ops->affinst_suspend) {
+ plat_state = psci_get_aff_phys_state(cpu_node);
+ rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+ psci_entrypoint,
+ ns_entrypoint,
+ cpu_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+static int psci_afflvl1_suspend(unsigned long mpidr,
+ aff_map_node *cluster_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id,
+ unsigned int power_state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+ unsigned long psci_entrypoint;
+
+ /* Sanity check the cluster level */
+ assert(cluster_node->level == MPIDR_AFFLVL1);
+
+ /*
+ * Keep the physical state of this cluster handy to decide
+ * what action needs to be taken
+ */
+ plat_state = psci_get_aff_phys_state(cluster_node);
+
+ /*
+ * Arch. management: Flush all levels of caches to PoC if the
+ * cluster is to be shutdown
+ */
+ if (plat_state == PSCI_STATE_OFF)
+ dcsw_op_all(DCCISW);
+
+ /*
+ * Plat. Management. Allow the platform to do it's cluster
+ * specific bookeeping e.g. turn off interconnect coherency,
+ * program the power controller etc.
+ */
+ if (psci_plat_pm_ops->affinst_suspend) {
+
+ /*
+ * Sending the psci entrypoint is currently redundant
+ * beyond affinity level 0 but one never knows what a
+ * platform might do. Also it allows us to keep the
+ * platform handler prototype the same.
+ */
+ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
+
+ rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+ psci_entrypoint,
+ ns_entrypoint,
+ cluster_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+
+static int psci_afflvl2_suspend(unsigned long mpidr,
+ aff_map_node *system_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id,
+ unsigned int power_state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+ unsigned long psci_entrypoint;
+
+ /* Cannot go beyond this */
+ assert(system_node->level == MPIDR_AFFLVL2);
+
+ /*
+ * Keep the physical state of the system handy to decide what
+ * action needs to be taken
+ */
+ plat_state = psci_get_aff_phys_state(system_node);
+
+ /*
+ * Plat. Management : Allow the platform to do it's bookeeping
+ * at this affinity level
+ */
+ if (psci_plat_pm_ops->affinst_suspend) {
+
+ /*
+ * Sending the psci entrypoint is currently redundant
+ * beyond affinity level 0 but one never knows what a
+ * platform might do. Also it allows us to keep the
+ * platform handler prototype the same.
+ */
+ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry;
+
+ rc = psci_plat_pm_ops->affinst_suspend(mpidr,
+ psci_entrypoint,
+ ns_entrypoint,
+ system_node->level,
+ plat_state);
+ }
+
+ return rc;
+}
+
+static const afflvl_suspend_handler psci_afflvl_suspend_handlers[] = {
+ psci_afflvl0_suspend,
+ psci_afflvl1_suspend,
+ psci_afflvl2_suspend,
+};
+
+/*******************************************************************************
+ * This function implements the core of the processing required to suspend a cpu
+ * It'S assumed that along with suspending the cpu, higher affinity levels will
+ * be suspended as far as possible. Suspending a cpu is equivalent to physically
+ * powering it down, but the cpu is still available to the OS for scheduling.
+ * We first need to determine the new state off all the affinity instances in
+ * the mpidr corresponding to the target cpu. Action will be taken on the basis
+ * of this new state. To do the state change we first need to acquire the locks
+ * for all the implemented affinity level to be able to snapshot the system
+ * state. Then we need to start suspending affinity levels from the lowest to
+ * the highest (e.g. a cpu needs to be suspended before a cluster can be). To
+ * achieve this flow, we start acquiring the locks from the highest to the
+ * lowest affinity level. Once we reach affinity level 0, we do the state change
+ * followed by the actions corresponding to the new state for affinity level 0.
+ * Actions as per the updated state for higher affinity levels are performed as
+ * we unwind back to highest affinity level.
+ ******************************************************************************/
+int psci_afflvl_suspend(unsigned long mpidr,
+ unsigned long entrypoint,
+ unsigned long context_id,
+ unsigned int power_state,
+ int cur_afflvl,
+ int tgt_afflvl)
+{
+ int rc = PSCI_E_SUCCESS, level;
+ unsigned int prev_state, next_state;
+ aff_map_node *aff_node;
+
+ mpidr &= MPIDR_AFFINITY_MASK;
+
+ /*
+ * Some affinity instances at levels between the current and
+ * target levels could be absent in the mpidr. Skip them and
+ * start from the first present instance.
+ */
+ level = psci_get_first_present_afflvl(mpidr,
+ cur_afflvl,
+ tgt_afflvl,
+ &aff_node);
+
+ /*
+ * Return if there are no more affinity instances beyond this
+ * level to process. Else ensure that the returned affinity
+ * node makes sense.
+ */
+ if (aff_node == NULL)
+ return rc;
+
+ assert(level == aff_node->level);
+
+ /*
+ * This function acquires the lock corresponding to each
+ * affinity level so that state management can be done safely.
+ */
+ bakery_lock_get(mpidr, &aff_node->lock);
+
+ /* Keep the old state and the next one handy */
+ prev_state = psci_get_state(aff_node->state);
+ next_state = PSCI_STATE_SUSPEND;
+
+ /*
+ * We start from the highest affinity level and work our way
+ * downwards to the lowest i.e. MPIDR_AFFLVL0.
+ */
+ if (aff_node->level == tgt_afflvl) {
+ psci_change_state(mpidr,
+ tgt_afflvl,
+ get_max_afflvl(),
+ next_state);
+ } else {
+ rc = psci_afflvl_suspend(mpidr,
+ entrypoint,
+ context_id,
+ power_state,
+ level - 1,
+ tgt_afflvl);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+ }
+
+ /*
+ * Perform generic, architecture and platform specific
+ * handling
+ */
+ rc = psci_afflvl_suspend_handlers[level](mpidr,
+ aff_node,
+ entrypoint,
+ context_id,
+ power_state);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+
+ /*
+ * If all has gone as per plan then this cpu should be
+ * marked as OFF
+ */
+ if (level == MPIDR_AFFLVL0) {
+ next_state = psci_get_state(aff_node->state);
+ assert(next_state == PSCI_STATE_SUSPEND);
+ }
+
+exit:
+ bakery_lock_release(mpidr, &aff_node->lock);
+ return rc;
+}
+
+/*******************************************************************************
+ * The following functions finish an earlier affinity suspend request. They
+ * are called by the common finisher routine in psci_common.c.
+ ******************************************************************************/
+static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
+ aff_map_node *cpu_node,
+ unsigned int prev_state)
+{
+ unsigned int index, plat_state, rc = 0;
+
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * before we change the state of the cpu e.g. enabling the
+ * gic or zeroing the mailbox register. If anything goes
+ * wrong then assert as there is no way to recover from this
+ * situation.
+ */
+ if (psci_plat_pm_ops->affinst_suspend_finish) {
+ plat_state = psci_get_phys_state(prev_state);
+ rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+ cpu_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ /* Get the index for restoring the re-entry information */
+ index = cpu_node->data;
+
+ /*
+ * Arch. management: Restore the stashed secure architectural
+ * context in the right order.
+ */
+ write_vbar(psci_secure_context[index].vbar);
+ write_mair(psci_secure_context[index].mair);
+ write_tcr(psci_secure_context[index].tcr);
+ write_ttbr0(psci_secure_context[index].ttbr);
+ write_sctlr(psci_secure_context[index].sctlr);
+
+ /* MMU and coherency should be enabled by now */
+ write_scr(psci_secure_context[index].scr);
+ write_cptr(psci_secure_context[index].cptr);
+ write_cpacr(psci_secure_context[index].cpacr);
+ write_cntfrq_el0(psci_secure_context[index].cntfrq);
+
+ /*
+ * Generic management: Now we just need to retrieve the
+ * information that we had stashed away during the suspend
+ * call to set this cpu on it's way.
+ */
+ rc = psci_get_ns_entry_info(index);
+
+ /* Clean caches before re-entering normal world */
+ dcsw_op_louis(DCCSW);
+
+ return rc;
+}
+
+static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
+ aff_map_node *cluster_node,
+ unsigned int prev_state)
+{
+ unsigned int rc = 0;
+ unsigned int plat_state;
+
+ assert(cluster_node->level == MPIDR_AFFLVL1);
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * as per the old state of the cluster e.g. enabling
+ * coherency at the interconnect depends upon the state with
+ * which this cluster was powered up. If anything goes wrong
+ * then assert as there is no way to recover from this
+ * situation.
+ */
+ if (psci_plat_pm_ops->affinst_suspend_finish) {
+ plat_state = psci_get_phys_state(prev_state);
+ rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+ cluster_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ return rc;
+}
+
+
+static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr,
+ aff_map_node *system_node,
+ unsigned int target_afflvl)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int plat_state;
+
+ /* Cannot go beyond this affinity level */
+ assert(system_node->level == MPIDR_AFFLVL2);
+
+ /*
+ * Currently, there are no architectural actions to perform
+ * at the system level.
+ */
+
+ /*
+ * Plat. management: Perform the platform specific actions
+ * as per the old state of the cluster e.g. enabling
+ * coherency at the interconnect depends upon the state with
+ * which this cluster was powered up. If anything goes wrong
+ * then assert as there is no way to recover from this
+ * situation.
+ */
+ if (psci_plat_pm_ops->affinst_suspend_finish) {
+ plat_state = psci_get_phys_state(system_node->state);
+ rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
+ system_node->level,
+ plat_state);
+ assert(rc == PSCI_E_SUCCESS);
+ }
+
+ return rc;
+}
+
+const afflvl_power_on_finisher psci_afflvl_suspend_finishers[] = {
+ psci_afflvl0_suspend_finish,
+ psci_afflvl1_suspend_finish,
+ psci_afflvl2_suspend_finish,
+};
+
diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c
new file mode 100644
index 0000000..6b07c53
--- /dev/null
+++ b/common/psci/psci_common.c
@@ -0,0 +1,520 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+
+/*******************************************************************************
+ * Arrays that contains information needs to resume a cpu's execution when woken
+ * out of suspend or off states. 'psci_ns_einfo_idx' keeps track of the next
+ * free index in the 'psci_ns_entry_info' & 'psci_secure_context' arrays. Each
+ * cpu is allocated a single entry in each array during startup.
+ ******************************************************************************/
+secure_context psci_secure_context[PSCI_NUM_AFFS];
+ns_entry_info psci_ns_entry_info[PSCI_NUM_AFFS];
+unsigned int psci_ns_einfo_idx;
+
+/*******************************************************************************
+ * Grand array that holds the platform's topology information for state
+ * management of affinity instances. Each node (aff_map_node) in the array
+ * corresponds to an affinity instance e.g. cluster, cpu within an mpidr
+ ******************************************************************************/
+aff_map_node psci_aff_map[PSCI_NUM_AFFS]
+__attribute__ ((section("tzfw_coherent_mem")));
+
+/*******************************************************************************
+ * In a system, a certain number of affinity instances are present at an
+ * affinity level. The cumulative number of instances across all levels are
+ * stored in 'psci_aff_map'. The topology tree has been flattenned into this
+ * array. To retrieve nodes, information about the extents of each affinity
+ * level i.e. start index and end index needs to be present. 'psci_aff_limits'
+ * stores this information.
+ ******************************************************************************/
+aff_limits_node psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+
+/*******************************************************************************
+ * Pointer to functions exported by the platform to complete power mgmt. ops
+ ******************************************************************************/
+plat_pm_ops *psci_plat_pm_ops;
+
+/*******************************************************************************
+ * Simple routine to retrieve the maximum affinity level supported by the
+ * platform and check that it makes sense.
+ ******************************************************************************/
+int get_max_afflvl()
+{
+ int aff_lvl;
+
+ aff_lvl = plat_get_max_afflvl();
+ assert(aff_lvl <= MPIDR_MAX_AFFLVL && aff_lvl >= MPIDR_AFFLVL0);
+
+ return aff_lvl;
+}
+
+/*******************************************************************************
+ * Simple routine to set the id of an affinity instance at a given level in the
+ * mpidr.
+ ******************************************************************************/
+unsigned long mpidr_set_aff_inst(unsigned long mpidr,
+ unsigned char aff_inst,
+ int aff_lvl)
+{
+ unsigned long aff_shift;
+
+ assert(aff_lvl <= MPIDR_AFFLVL3);
+
+ /*
+ * Decide the number of bits to shift by depending upon
+ * the affinity level
+ */
+ aff_shift = get_afflvl_shift(aff_lvl);
+
+ /* Clear the existing affinity instance & set the new one*/
+ mpidr &= ~(MPIDR_AFFLVL_MASK << aff_shift);
+ mpidr |= aff_inst << aff_shift;
+
+ return mpidr;
+}
+
+/*******************************************************************************
+ * Simple routine to determine whether an affinity instance at a given level
+ * in an mpidr exists or not.
+ ******************************************************************************/
+int psci_validate_mpidr(unsigned long mpidr, int level)
+{
+ aff_map_node *node;
+
+ node = psci_get_aff_map_node(mpidr, level);
+ if (node && (node->state & PSCI_AFF_PRESENT))
+ return PSCI_E_SUCCESS;
+ else
+ return PSCI_E_INVALID_PARAMS;
+}
+
+/*******************************************************************************
+ * Simple routine to determine the first affinity level instance that is present
+ * between the start and end affinity levels. This helps to skip handling of
+ * absent affinity levels while performing psci operations.
+ * The start level can be > or <= to the end level depending upon whether this
+ * routine is expected to search top down or bottom up.
+ ******************************************************************************/
+int psci_get_first_present_afflvl(unsigned long mpidr,
+ int start_afflvl,
+ int end_afflvl,
+ aff_map_node **node)
+{
+ int level;
+
+ /* Check whether we have to search up or down */
+ if (start_afflvl <= end_afflvl) {
+ for (level = start_afflvl; level <= end_afflvl; level++) {
+ *node = psci_get_aff_map_node(mpidr, level);
+ if (*node && ((*node)->state & PSCI_AFF_PRESENT))
+ break;
+ }
+ } else {
+ for (level = start_afflvl; level >= end_afflvl; level--) {
+ *node = psci_get_aff_map_node(mpidr, level);
+ if (*node && ((*node)->state & PSCI_AFF_PRESENT))
+ break;
+ }
+ }
+
+ return level;
+}
+
+/*******************************************************************************
+ * Recursively change the affinity state between the current and target affinity
+ * levels. The target state matters only if we are starting from affinity level
+ * 0 i.e. a cpu otherwise the state depends upon the state of the lower affinity
+ * levels.
+ ******************************************************************************/
+int psci_change_state(unsigned long mpidr,
+ int cur_afflvl,
+ int tgt_afflvl,
+ unsigned int tgt_state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int state;
+ aff_map_node *aff_node;
+
+ /* Sanity check the affinity levels */
+ assert(tgt_afflvl >= cur_afflvl);
+
+ aff_node = psci_get_aff_map_node(mpidr, cur_afflvl);
+ assert(aff_node);
+
+ /* TODO: Check whether the affinity level is present or absent*/
+
+ if (cur_afflvl == MPIDR_AFFLVL0) {
+ psci_set_state(aff_node->state, tgt_state);
+ } else {
+ state = psci_calculate_affinity_state(aff_node);
+ psci_set_state(aff_node->state, state);
+ }
+
+ if (cur_afflvl != tgt_afflvl)
+ psci_change_state(mpidr, cur_afflvl + 1, tgt_afflvl, tgt_state);
+
+ return rc;
+}
+
+/*******************************************************************************
+ * This routine does the heavy lifting for psci_change_state(). It examines the
+ * state of each affinity instance at the next lower affinity level and decides
+ * it's final state accordingly. If a lower affinity instance is ON then the
+ * higher affinity instance is ON. If all the lower affinity instances are OFF
+ * then the higher affinity instance is OFF. If atleast one lower affinity
+ * instance is SUSPENDED then the higher affinity instance is SUSPENDED. If only
+ * a single lower affinity instance is ON_PENDING then the higher affinity
+ * instance in ON_PENDING as well.
+ ******************************************************************************/
+unsigned int psci_calculate_affinity_state(aff_map_node *aff_node)
+{
+ int ctr;
+ unsigned int aff_count, hi_aff_state;
+ unsigned long tempidr;
+ aff_map_node *lo_aff_node;
+
+ /* Cannot calculate lowest affinity state. It's simply assigned */
+ assert(aff_node->level > MPIDR_AFFLVL0);
+
+ /*
+ * Find the number of affinity instances at level X-1 e.g. number of
+ * cpus in a cluster. The level X state depends upon the state of each
+ * instance at level X-1
+ */
+ hi_aff_state = PSCI_STATE_OFF;
+ aff_count = plat_get_aff_count(aff_node->level - 1, aff_node->mpidr);
+ for (ctr = 0; ctr < aff_count; ctr++) {
+
+ /*
+ * Create a mpidr for each lower affinity level (X-1). Use their
+ * states to influence the higher affinity state (X).
+ */
+ tempidr = mpidr_set_aff_inst(aff_node->mpidr,
+ ctr,
+ aff_node->level - 1);
+ lo_aff_node = psci_get_aff_map_node(tempidr,
+ aff_node->level - 1);
+ assert(lo_aff_node);
+
+ /* Continue only if the cpu exists within the cluster */
+ if (!(lo_aff_node->state & PSCI_AFF_PRESENT))
+ continue;
+
+ switch (psci_get_state(lo_aff_node->state)) {
+
+ /*
+ * If any lower affinity is on within the cluster, then
+ * the higher affinity is on.
+ */
+ case PSCI_STATE_ON:
+ return PSCI_STATE_ON;
+
+ /*
+ * At least one X-1 needs to be suspended for X to be suspended
+ * but it's effectively on for the affinity_info call.
+ * SUSPEND > ON_PENDING > OFF.
+ */
+ case PSCI_STATE_SUSPEND:
+ hi_aff_state = PSCI_STATE_SUSPEND;
+ continue;
+
+ /*
+ * Atleast one X-1 needs to be on_pending & the rest off for X
+ * to be on_pending. ON_PENDING > OFF.
+ */
+ case PSCI_STATE_ON_PENDING:
+ if (hi_aff_state != PSCI_STATE_SUSPEND)
+ hi_aff_state = PSCI_STATE_ON_PENDING;
+ continue;
+
+ /* Higher affinity is off if all lower affinities are off. */
+ case PSCI_STATE_OFF:
+ continue;
+
+ default:
+ assert(0);
+ }
+ }
+
+ return hi_aff_state;
+}
+
+/*******************************************************************************
+ * This function retrieves all the stashed information needed to correctly
+ * resume a cpu's execution in the non-secure state after it has been physically
+ * powered on i.e. turned ON or resumed from SUSPEND
+ ******************************************************************************/
+unsigned int psci_get_ns_entry_info(unsigned int index)
+{
+ unsigned long sctlr = 0, scr, el_status, id_aa64pfr0;
+
+ scr = read_scr();
+
+ /* Switch to the non-secure view of the registers */
+ write_scr(scr | SCR_NS_BIT);
+
+ /* Find out which EL we are going to */
+ id_aa64pfr0 = read_id_aa64pfr0_el1();
+ el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) &
+ ID_AA64PFR0_ELX_MASK;
+
+ /* Restore endianess */
+ if (psci_ns_entry_info[index].sctlr & SCTLR_EE_BIT)
+ sctlr |= SCTLR_EE_BIT;
+ else
+ sctlr &= ~SCTLR_EE_BIT;
+
+ /* Turn off MMU and Caching */
+ sctlr &= ~(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_M_BIT);
+
+ /* Set the register width */
+ if (psci_ns_entry_info[index].scr & SCR_RW_BIT)
+ scr |= SCR_RW_BIT;
+ else
+ scr &= ~SCR_RW_BIT;
+
+ scr |= SCR_NS_BIT;
+
+ if (el_status)
+ write_sctlr_el2(sctlr);
+ else
+ write_sctlr_el1(sctlr);
+
+ /* Fulfill the cpu_on entry reqs. as per the psci spec */
+ write_scr(scr);
+ write_spsr(psci_ns_entry_info[index].eret_info.spsr);
+ write_elr(psci_ns_entry_info[index].eret_info.entrypoint);
+
+ return psci_ns_entry_info[index].context_id;
+}
+
+/*******************************************************************************
+ * This function retrieves and stashes all the information needed to correctly
+ * resume a cpu's execution in the non-secure state after it has been physically
+ * powered on i.e. turned ON or resumed from SUSPEND. This is done prior to
+ * turning it on or before suspending it.
+ ******************************************************************************/
+int psci_set_ns_entry_info(unsigned int index,
+ unsigned long entrypoint,
+ unsigned long context_id)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int rw, mode, ee, spsr = 0;
+ unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1(), scr = read_scr();
+ unsigned long el_status;
+
+ /* Figure out what mode do we enter the non-secure world in */
+ el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) &
+ ID_AA64PFR0_ELX_MASK;
+
+ /*
+ * Figure out whether the cpu enters the non-secure address space
+ * in aarch32 or aarch64
+ */
+ rw = scr & SCR_RW_BIT;
+ if (rw) {
+
+ /*
+ * Check whether a Thumb entry point has been provided for an
+ * aarch64 EL
+ */
+ if (entrypoint & 0x1)
+ return PSCI_E_INVALID_PARAMS;
+
+ if (el_status && (scr & SCR_HCE_BIT)) {
+ mode = MODE_EL2;
+ ee = read_sctlr_el2() & SCTLR_EE_BIT;
+ } else {
+ mode = MODE_EL1;
+ ee = read_sctlr_el1() & SCTLR_EE_BIT;
+ }
+
+ spsr = DAIF_DBG_BIT | DAIF_ABT_BIT;
+ spsr |= DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+ spsr <<= PSR_DAIF_SHIFT;
+ spsr |= make_spsr(mode, MODE_SP_ELX, !rw);
+
+ psci_ns_entry_info[index].sctlr |= ee;
+ psci_ns_entry_info[index].scr |= SCR_RW_BIT;
+ } else {
+
+ /* Check whether aarch32 has to be entered in Thumb mode */
+ if (entrypoint & 0x1)
+ spsr = SPSR32_T_BIT;
+
+ if (el_status && (scr & SCR_HCE_BIT)) {
+ mode = AARCH32_MODE_HYP;
+ ee = read_sctlr_el2() & SCTLR_EE_BIT;
+ } else {
+ mode = AARCH32_MODE_SVC;
+ ee = read_sctlr_el1() & SCTLR_EE_BIT;
+ }
+
+ /*
+ * TODO: Choose async. exception bits if HYP mode is not
+ * implemented according to the values of SCR.{AW, FW} bits
+ */
+ spsr |= DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+ spsr <<= PSR_DAIF_SHIFT;
+ if(ee)
+ spsr |= SPSR32_EE_BIT;
+ spsr |= mode;
+
+ /* Ensure that the CSPR.E and SCTLR.EE bits match */
+ psci_ns_entry_info[index].sctlr |= ee;
+ psci_ns_entry_info[index].scr &= ~SCR_RW_BIT;
+ }
+
+ psci_ns_entry_info[index].eret_info.entrypoint = entrypoint;
+ psci_ns_entry_info[index].eret_info.spsr = spsr;
+ psci_ns_entry_info[index].context_id = context_id;
+
+ return rc;
+}
+
+/*******************************************************************************
+ * An affinity level could be on, on_pending, suspended or off. These are the
+ * logical states it can be in. Physically either it's off or on. When it's in
+ * the state on_pending then it's about to be turned on. It's not possible to
+ * tell whether that's actually happenned or not. So we err on the side of
+ * caution & treat the affinity level as being turned off.
+ ******************************************************************************/
+inline unsigned int psci_get_phys_state(unsigned int aff_state)
+{
+ return (aff_state != PSCI_STATE_ON ? PSCI_STATE_OFF : PSCI_STATE_ON);
+}
+
+unsigned int psci_get_aff_phys_state(aff_map_node *aff_node)
+{
+ unsigned int aff_state;
+
+ aff_state = psci_get_state(aff_node->state);
+ return psci_get_phys_state(aff_state);
+}
+
+/*******************************************************************************
+ * Generic handler which is called when a cpu is physically powered on. It
+ * recurses through all the affinity levels performing generic, architectural,
+ * platform setup and state management e.g. for a cluster that's been powered
+ * on, it will call the platform specific code which will enable coherency at
+ * the interconnect level. For a cpu it could mean turning on the MMU etc.
+ *
+ * This function traverses from the lowest to the highest affinity level
+ * implemented by the platform. Since it's recursive, for each call the
+ * 'cur_afflvl' & 'tgt_afflvl' parameters keep track of which level we are at
+ * and which level we need to get to respectively. Locks are picked up along the
+ * way so that when the lowest affinity level is hit, state management can be
+ * safely done. Prior to this, each affinity level does it's bookeeping as per
+ * the state out of reset.
+ *
+ * CAUTION: This function is called with coherent stacks so that coherency and
+ * the mmu can be turned on safely.
+ ******************************************************************************/
+unsigned int psci_afflvl_power_on_finish(unsigned long mpidr,
+ int cur_afflvl,
+ int tgt_afflvl,
+ afflvl_power_on_finisher *pon_handlers)
+{
+ unsigned int prev_state, next_state, rc = PSCI_E_SUCCESS;
+ aff_map_node *aff_node;
+ int level;
+
+ mpidr &= MPIDR_AFFINITY_MASK;;
+
+ /*
+ * Some affinity instances at levels between the current and
+ * target levels could be absent in the mpidr. Skip them and
+ * start from the first present instance.
+ */
+ level = psci_get_first_present_afflvl(mpidr,
+ cur_afflvl,
+ tgt_afflvl,
+ &aff_node);
+ /*
+ * Return if there are no more affinity instances beyond this
+ * level to process. Else ensure that the returned affinity
+ * node makes sense.
+ */
+ if (aff_node == NULL)
+ return rc;
+
+ assert(level == aff_node->level);
+
+ /*
+ * This function acquires the lock corresponding to each
+ * affinity level so that by the time we hit the highest
+ * affinity level, the system topology is snapshot and state
+ * management can be done safely.
+ */
+ bakery_lock_get(mpidr, &aff_node->lock);
+
+ /* Keep the old and new state handy */
+ prev_state = psci_get_state(aff_node->state);
+ next_state = PSCI_STATE_ON;
+
+ /* Perform generic, architecture and platform specific handling */
+ rc = pon_handlers[level](mpidr, aff_node, prev_state);
+ if (rc != PSCI_E_SUCCESS) {
+ psci_set_state(aff_node->state, prev_state);
+ goto exit;
+ }
+
+ /*
+ * State management: Update the states if this is the highest
+ * affinity level requested else pass the job to the next level.
+ */
+ if (aff_node->level != tgt_afflvl) {
+ rc = psci_afflvl_power_on_finish(mpidr,
+ level + 1,
+ tgt_afflvl,
+ pon_handlers);
+ } else {
+ psci_change_state(mpidr, MPIDR_AFFLVL0, tgt_afflvl, next_state);
+ }
+
+ /* If all has gone as per plan then this cpu should be marked as ON */
+ if (level == MPIDR_AFFLVL0) {
+ next_state = psci_get_state(aff_node->state);
+ assert(next_state == PSCI_STATE_ON);
+ }
+
+exit:
+ bakery_lock_release(mpidr, &aff_node->lock);
+ return rc;
+}
diff --git a/common/psci/psci_entry.S b/common/psci/psci_entry.S
new file mode 100644
index 0000000..4ea74c5
--- /dev/null
+++ b/common/psci/psci_entry.S
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+#include <asm_macros.S>
+
+ .globl psci_aff_on_finish_entry
+ .globl psci_aff_suspend_finish_entry
+ .globl __psci_cpu_off
+ .globl __psci_cpu_suspend
+
+ .section platform_code, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * This cpu has been physically powered up. Depending
+ * upon whether it was resumed from suspend or simply
+ * turned on, call the common power on finisher with
+ * the handlers (chosen depending upon original state).
+ * For ease, the finisher is called with coherent
+ * stacks. This allows the cluster/cpu finishers to
+ * enter coherency and enable the mmu without running
+ * into issues. We switch back to normal stacks once
+ * all this is done.
+ * -----------------------------------------------------
+ */
+psci_aff_on_finish_entry:
+ adr x23, psci_afflvl_on_finishers
+ b psci_aff_common_finish_entry
+
+psci_aff_suspend_finish_entry:
+ adr x23, psci_afflvl_suspend_finishers
+
+psci_aff_common_finish_entry:
+ adr x22, psci_afflvl_power_on_finish
+ bl read_mpidr
+ mov x19, x0
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Call the finishers starting from affinity
+ * level 0.
+ * ---------------------------------------------
+ */
+ bl get_max_afflvl
+ mov x3, x23
+ mov x2, x0
+ mov x0, x19
+ mov x1, #MPIDR_AFFLVL0
+ blr x22
+ mov x21, x0
+
+ /* --------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * --------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* --------------------------------------------
+ * Restore the context id. value
+ * --------------------------------------------
+ */
+ mov x0, x21
+
+ /* --------------------------------------------
+ * Jump back to the non-secure world assuming
+ * that the elr and spsr setup has been done
+ * by the finishers
+ * --------------------------------------------
+ */
+ eret
+_panic:
+ b _panic
+
+ /* -----------------------------------------------------
+ * The following two stubs give the calling cpu a
+ * coherent stack to allow flushing of caches without
+ * suffering from stack coherency issues
+ * -----------------------------------------------------
+ */
+__psci_cpu_off:
+ func_prologue
+ sub sp, sp, #0x10
+ stp x19, x20, [sp, #0]
+ mov x19, sp
+ bl read_mpidr
+ bl platform_set_coherent_stack
+ bl psci_cpu_off
+ mov x1, #PSCI_E_SUCCESS
+ cmp x0, x1
+ b.eq final_wfi
+ mov sp, x19
+ ldp x19, x20, [sp,#0]
+ add sp, sp, #0x10
+ func_epilogue
+ ret
+
+__psci_cpu_suspend:
+ func_prologue
+ sub sp, sp, #0x20
+ stp x19, x20, [sp, #0]
+ stp x21, x22, [sp, #0x10]
+ mov x19, sp
+ mov x20, x0
+ mov x21, x1
+ mov x22, x2
+ bl read_mpidr
+ bl platform_set_coherent_stack
+ mov x0, x20
+ mov x1, x21
+ mov x2, x22
+ bl psci_cpu_suspend
+ mov x1, #PSCI_E_SUCCESS
+ cmp x0, x1
+ b.eq final_wfi
+ mov sp, x19
+ ldp x21, x22, [sp,#0x10]
+ ldp x19, x20, [sp,#0]
+ add sp, sp, #0x20
+ func_epilogue
+ ret
+
+final_wfi:
+ dsb sy
+ wfi
+wfi_spill:
+ b wfi_spill
+
diff --git a/common/psci/psci_main.c b/common/psci/psci_main.c
new file mode 100644
index 0000000..eca2dec
--- /dev/null
+++ b/common/psci/psci_main.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci_private.h>
+
+/*******************************************************************************
+ * PSCI frontend api for servicing SMCs. Described in the PSCI spec.
+ ******************************************************************************/
+int psci_cpu_on(unsigned long target_cpu,
+ unsigned long entrypoint,
+ unsigned long context_id)
+
+{
+ int rc;
+ unsigned int start_afflvl, target_afflvl;
+
+ /* Determine if the cpu exists of not */
+ rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
+ if (rc != PSCI_E_SUCCESS) {
+ goto exit;
+ }
+
+ start_afflvl = get_max_afflvl();
+ target_afflvl = MPIDR_AFFLVL0;
+ rc = psci_afflvl_on(target_cpu,
+ entrypoint,
+ context_id,
+ start_afflvl,
+ target_afflvl);
+
+exit:
+ return rc;
+}
+
+unsigned int psci_version(void)
+{
+ return PSCI_MAJOR_VER | PSCI_MINOR_VER;
+}
+
+int psci_cpu_suspend(unsigned int power_state,
+ unsigned long entrypoint,
+ unsigned long context_id)
+{
+ int rc;
+ unsigned long mpidr;
+ unsigned int tgt_afflvl, pstate_type;
+
+ /* TODO: Standby states are not supported at the moment */
+ pstate_type = psci_get_pstate_type(power_state);
+ if (pstate_type == 0) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ /* Sanity check the requested state */
+ tgt_afflvl = psci_get_pstate_afflvl(power_state);
+ if (tgt_afflvl > MPIDR_MAX_AFFLVL) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ mpidr = read_mpidr();
+ rc = psci_afflvl_suspend(mpidr,
+ entrypoint,
+ context_id,
+ power_state,
+ tgt_afflvl,
+ MPIDR_AFFLVL0);
+
+exit:
+ if (rc != PSCI_E_SUCCESS)
+ assert(rc == PSCI_E_INVALID_PARAMS);
+ return rc;
+}
+
+int psci_cpu_off(void)
+{
+ int rc;
+ unsigned long mpidr;
+ int target_afflvl = get_max_afflvl();
+
+ mpidr = read_mpidr();
+
+ /*
+ * Traverse from the highest to the lowest affinity level. When the
+ * lowest affinity level is hit, all the locks are acquired. State
+ * management is done immediately followed by cpu, cluster ...
+ * ..target_afflvl specific actions as this function unwinds back.
+ */
+ rc = psci_afflvl_off(mpidr, target_afflvl, MPIDR_AFFLVL0);
+
+ if (rc != PSCI_E_SUCCESS) {
+ assert(rc == PSCI_E_DENIED);
+ }
+
+ return rc;
+}
+
+int psci_affinity_info(unsigned long target_affinity,
+ unsigned int lowest_affinity_level)
+{
+ int rc = PSCI_E_INVALID_PARAMS;
+ unsigned int aff_state;
+ aff_map_node *node;
+
+ if (lowest_affinity_level > get_max_afflvl()) {
+ goto exit;
+ }
+
+ node = psci_get_aff_map_node(target_affinity, lowest_affinity_level);
+ if (node && (node->state & PSCI_AFF_PRESENT)) {
+ aff_state = psci_get_state(node->state);
+
+ /* A suspended cpu is available & on for the OS */
+ if (aff_state == PSCI_STATE_SUSPEND) {
+ aff_state = PSCI_STATE_ON;
+ }
+
+ rc = aff_state;
+ }
+exit:
+ return rc;
+}
+
+/* Unimplemented */
+int psci_migrate(unsigned int target_cpu)
+{
+ return PSCI_E_NOT_SUPPORTED;
+}
+
+/* Unimplemented */
+unsigned int psci_migrate_info_type(void)
+{
+ return PSCI_TOS_NOT_PRESENT;
+}
+
+unsigned long psci_migrate_info_up_cpu(void)
+{
+ /*
+ * Return value of this currently unsupported call depends upon
+ * what psci_migrate_info_type() returns.
+ */
+ return PSCI_E_SUCCESS;
+}
+
+/* Unimplemented */
+void psci_system_off(void)
+{
+ assert(0);
+}
+
+/* Unimplemented */
+void psci_system_reset(void)
+{
+ assert(0);
+}
+
diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h
new file mode 100644
index 0000000..48d40d0
--- /dev/null
+++ b/common/psci/psci_private.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __PSCI_PRIVATE_H__
+#define __PSCI_PRIVATE_H__
+
+#include <bakery_lock.h>
+
+#ifndef __ASSEMBLY__
+/*******************************************************************************
+ * The following two data structures hold the generic information to bringup
+ * a suspended/hotplugged out cpu
+ ******************************************************************************/
+typedef struct {
+ unsigned long entrypoint;
+ unsigned long spsr;
+} eret_params;
+
+typedef struct {
+ eret_params eret_info;
+ unsigned long context_id;
+ unsigned int scr;
+ unsigned int sctlr;
+} ns_entry_info;
+
+/*******************************************************************************
+ *
+ *
+ ******************************************************************************/
+typedef struct {
+ unsigned long sctlr;
+ unsigned long scr;
+ unsigned long cptr;
+ unsigned long cpacr;
+ unsigned long cntfrq;
+ unsigned long mair;
+ unsigned long tcr;
+ unsigned long ttbr;
+ unsigned long vbar;
+} secure_context;
+
+/*******************************************************************************
+ * The following two data structures hold the topology tree which in turn tracks
+ * the state of the all the affinity instances supported by the platform.
+ ******************************************************************************/
+typedef struct {
+ unsigned long mpidr;
+ unsigned char state;
+ char level;
+ unsigned int data;
+ bakery_lock lock;
+} aff_map_node;
+
+typedef struct {
+ int min;
+ int max;
+} aff_limits_node;
+
+typedef unsigned int (*afflvl_power_on_finisher)(unsigned long,
+ aff_map_node *,
+ unsigned int);
+
+/*******************************************************************************
+ * Data prototypes
+ ******************************************************************************/
+extern secure_context psci_secure_context[PSCI_NUM_AFFS];
+extern ns_entry_info psci_ns_entry_info[PSCI_NUM_AFFS];
+extern unsigned int psci_ns_einfo_idx;
+extern aff_limits_node psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+extern plat_pm_ops *psci_plat_pm_ops;
+extern aff_map_node psci_aff_map[PSCI_NUM_AFFS];
+extern afflvl_power_on_finisher psci_afflvl_off_finish_handlers[];
+extern afflvl_power_on_finisher psci_afflvl_sus_finish_handlers[];
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+/* Private exported functions from psci_common.c */
+extern int get_max_afflvl(void);
+extern unsigned int psci_get_phys_state(unsigned int);
+extern unsigned int psci_get_aff_phys_state(aff_map_node *);
+extern unsigned int psci_calculate_affinity_state(aff_map_node *);
+extern unsigned int psci_get_ns_entry_info(unsigned int index);
+extern unsigned long mpidr_set_aff_inst(unsigned long,unsigned char, int);
+extern int psci_change_state(unsigned long, int, int, unsigned int);
+extern int psci_validate_mpidr(unsigned long, int);
+extern unsigned int psci_afflvl_power_on_finish(unsigned long,
+ int,
+ int,
+ afflvl_power_on_finisher *);
+extern int psci_set_ns_entry_info(unsigned int index,
+ unsigned long entrypoint,
+ unsigned long context_id);
+extern int psci_get_first_present_afflvl(unsigned long,
+ int, int,
+ aff_map_node **);
+/* Private exported functions from psci_setup.c */
+extern aff_map_node *psci_get_aff_map_node(unsigned long, int);
+
+/* Private exported functions from psci_affinity_on.c */
+extern int psci_afflvl_on(unsigned long,
+ unsigned long,
+ unsigned long,
+ int,
+ int);
+
+/* Private exported functions from psci_affinity_off.c */
+extern int psci_afflvl_off(unsigned long, int, int);
+
+/* Private exported functions from psci_affinity_suspend.c */
+extern int psci_afflvl_suspend(unsigned long,
+ unsigned long,
+ unsigned long,
+ unsigned int,
+ int,
+ int);
+extern unsigned int psci_afflvl_suspend_finish(unsigned long, int, int);
+#endif /*__ASSEMBLY__*/
+
+#endif /* __PSCI_PRIVATE_H__ */
diff --git a/common/psci/psci_setup.c b/common/psci/psci_setup.c
new file mode 100644
index 0000000..9095e75
--- /dev/null
+++ b/common/psci/psci_setup.c
@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci_private.h>
+
+/*******************************************************************************
+ * Routines for retrieving the node corresponding to an affinity level instance
+ * in the mpidr. The first one uses binary search to find the node corresponding
+ * to the mpidr (key) at a particular affinity level. The second routine decides
+ * extents of the binary search at each affinity level.
+ ******************************************************************************/
+static int psci_aff_map_get_idx(unsigned long key,
+ int min_idx,
+ int max_idx)
+{
+ int mid;
+
+ /*
+ * Terminating condition: If the max and min indices have crossed paths
+ * during the binary search then the key has not been found.
+ */
+ if (max_idx < min_idx)
+ return PSCI_E_INVALID_PARAMS;
+
+ /*
+ * Bisect the array around 'mid' and then recurse into the array chunk
+ * where the key is likely to be found. The mpidrs in each node in the
+ * 'psci_aff_map' for a given affinity level are stored in an ascending
+ * order which makes the binary search possible.
+ */
+ mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */
+ if (psci_aff_map[mid].mpidr > key)
+ return psci_aff_map_get_idx(key, min_idx, mid - 1);
+ else if (psci_aff_map[mid].mpidr < key)
+ return psci_aff_map_get_idx(key, mid + 1, max_idx);
+ else
+ return mid;
+}
+
+aff_map_node *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl)
+{
+ int rc;
+
+ /* Right shift the mpidr to the required affinity level */
+ mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl);
+
+ rc = psci_aff_map_get_idx(mpidr,
+ psci_aff_limits[aff_lvl].min,
+ psci_aff_limits[aff_lvl].max);
+ if (rc >= 0)
+ return &psci_aff_map[rc];
+ else
+ return NULL;
+}
+
+/*******************************************************************************
+ * Function which initializes the 'aff_map_node' corresponding to an affinity
+ * level instance. Each node has a unique mpidr, level and bakery lock. The data
+ * field is opaque and holds affinity level specific data e.g. for affinity
+ * level 0 it contains the index into arrays that hold the secure/non-secure
+ * state for a cpu that's been turned on/off
+ ******************************************************************************/
+static void psci_init_aff_map_node(unsigned long mpidr,
+ int level,
+ unsigned int idx)
+{
+ unsigned char state;
+ psci_aff_map[idx].mpidr = mpidr;
+ psci_aff_map[idx].level = level;
+ bakery_lock_init(&psci_aff_map[idx].lock);
+
+ /*
+ * If an affinity instance is present then mark it as OFF to begin with.
+ */
+ state = plat_get_aff_state(level, mpidr);
+ psci_aff_map[idx].state = state;
+ if (state & PSCI_AFF_PRESENT) {
+ psci_set_state(psci_aff_map[idx].state, PSCI_STATE_OFF);
+ }
+
+ if (level == MPIDR_AFFLVL0) {
+ /* Ensure that we have not overflowed the psci_ns_einfo array */
+ assert(psci_ns_einfo_idx < PSCI_NUM_AFFS);
+
+ psci_aff_map[idx].data = psci_ns_einfo_idx;
+ psci_ns_einfo_idx++;
+ }
+
+ return;
+}
+
+/*******************************************************************************
+ * Core routine used by the Breadth-First-Search algorithm to populate the
+ * affinity tree. Each level in the tree corresponds to an affinity level. This
+ * routine's aim is to traverse to the target affinity level and populate nodes
+ * in the 'psci_aff_map' for all the siblings at that level. It uses the current
+ * affinity level to keep track of how many levels from the root of the tree
+ * have been traversed. If the current affinity level != target affinity level,
+ * then the platform is asked to return the number of children that each
+ * affinity instance has at the current affinity level. Traversal is then done
+ * for each child at the next lower level i.e. current affinity level - 1.
+ *
+ * CAUTION: This routine assumes that affinity instance ids are allocated in a
+ * monotonically increasing manner at each affinity level in a mpidr starting
+ * from 0. If the platform breaks this assumption then this code will have to
+ * be reworked accordingly.
+ ******************************************************************************/
+static unsigned int psci_init_aff_map(unsigned long mpidr,
+ unsigned int affmap_idx,
+ int cur_afflvl,
+ int tgt_afflvl)
+{
+ unsigned int ctr, aff_count;
+
+ assert(cur_afflvl >= tgt_afflvl);
+
+ /*
+ * Find the number of siblings at the current affinity level &
+ * assert if there are none 'cause then we have been invoked with
+ * an invalid mpidr.
+ */
+ aff_count = plat_get_aff_count(cur_afflvl, mpidr);
+ assert(aff_count);
+
+ if (tgt_afflvl < cur_afflvl) {
+ for (ctr = 0; ctr < aff_count; ctr++) {
+ mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl);
+ affmap_idx = psci_init_aff_map(mpidr,
+ affmap_idx,
+ cur_afflvl - 1,
+ tgt_afflvl);
+ }
+ } else {
+ for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) {
+ mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl);
+ psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx);
+ }
+
+ /* affmap_idx is 1 greater than the max index of cur_afflvl */
+ psci_aff_limits[cur_afflvl].max = affmap_idx - 1;
+ }
+
+ return affmap_idx;
+}
+
+/*******************************************************************************
+ * This function initializes the topology tree by querying the platform. To do
+ * so, it's helper routines implement a Breadth-First-Search. At each affinity
+ * level the platform conveys the number of affinity instances that exist i.e.
+ * the affinity count. The algorithm populates the psci_aff_map recursively
+ * using this information. On a platform that implements two clusters of 4 cpus
+ * each, the populated aff_map_array would look like this:
+ *
+ * <- cpus cluster0 -><- cpus cluster1 ->
+ * ---------------------------------------------------
+ * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 |
+ * ---------------------------------------------------
+ * ^ ^
+ * cluster __| cpu __|
+ * limit limit
+ *
+ * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus
+ * within cluster 0. The last 4 entries are of cpus within cluster 1.
+ * The 'psci_aff_limits' array contains the max & min index of each affinity
+ * level within the 'psci_aff_map' array. This allows restricting search of a
+ * node at an affinity level between the indices in the limits array.
+ ******************************************************************************/
+void psci_setup(unsigned long mpidr)
+{
+ int afflvl, affmap_idx, rc, max_afflvl;
+ aff_map_node *node;
+
+ /* Initialize psci's internal state */
+ memset(psci_aff_map, 0, sizeof(psci_aff_map));
+ memset(psci_aff_limits, 0, sizeof(psci_aff_limits));
+ memset(psci_ns_entry_info, 0, sizeof(psci_ns_entry_info));
+ psci_ns_einfo_idx = 0;
+ psci_plat_pm_ops = NULL;
+
+ /* Find out the maximum affinity level that the platform implements */
+ max_afflvl = get_max_afflvl();
+ assert(max_afflvl <= MPIDR_MAX_AFFLVL);
+
+ /*
+ * This call traverses the topology tree with help from the platform and
+ * populates the affinity map using a breadth-first-search recursively.
+ * We assume that the platform allocates affinity instance ids from 0
+ * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0
+ */
+ affmap_idx = 0;
+ for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) {
+ affmap_idx = psci_init_aff_map(FIRST_MPIDR,
+ affmap_idx,
+ max_afflvl,
+ afflvl);
+ }
+
+ /*
+ * Set the bounds for the affinity counts of each level in the map. Also
+ * flush out the entire array so that it's visible to subsequent power
+ * management operations. The 'psci_aff_map' array is allocated in
+ * coherent memory so does not need flushing. The 'psci_aff_limits'
+ * array is allocated in normal memory. It will be accessed when the mmu
+ * is off e.g. after reset. Hence it needs to be flushed.
+ */
+ for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) {
+ psci_aff_limits[afflvl].min =
+ psci_aff_limits[afflvl + 1].max + 1;
+ }
+
+ flush_dcache_range((unsigned long) psci_aff_limits,
+ sizeof(psci_aff_limits));
+
+ /*
+ * Mark the affinity instances in our mpidr as ON. No need to lock as
+ * this is the primary cpu.
+ */
+ mpidr &= MPIDR_AFFINITY_MASK;
+ for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) {
+
+ node = psci_get_aff_map_node(mpidr, afflvl);
+ assert(node);
+
+ /* Mark each present node as ON. */
+ if (node->state & PSCI_AFF_PRESENT) {
+ psci_set_state(node->state, PSCI_STATE_ON);
+ }
+ }
+
+ rc = platform_setup_pm(&psci_plat_pm_ops);
+ assert(rc == 0);
+ assert(psci_plat_pm_ops);
+
+ return;
+}
diff --git a/common/runtime_svc.c b/common/runtime_svc.c
new file mode 100644
index 0000000..ed1225f
--- /dev/null
+++ b/common/runtime_svc.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <semihosting.h>
+#include <bl_common.h>
+#include <psci.h>
+
+/*******************************************************************************
+ * Perform initialization of runtime services possibly across exception levels
+ * in the secure address space e.g. psci & interrupt handling.
+ ******************************************************************************/
+void runtime_svc_init(unsigned long mpidr)
+{
+ psci_setup(mpidr);
+ return;
+}
diff --git a/docs/change-log.md b/docs/change-log.md
new file mode 100644
index 0000000..3a9e5cd
--- /dev/null
+++ b/docs/change-log.md
@@ -0,0 +1,76 @@
+ARM Trusted Firmware - version 0.2
+==================================
+
+New features
+------------
+
+* First source release.
+
+* Code for the PSCI suspend feature is supplied, although this is not enabled
+ by default since there are known issues (see below).
+
+
+Issues resolved since last release
+----------------------------------
+
+* The "psci" nodes in the FDTs provided in this release now fully comply
+ with the recommendations made in the PSCI specification.
+
+
+Known issues
+------------
+
+The following is a list of issues which are expected to be fixed in the future
+releases of the ARM Trusted Firmware.
+
+* The TrustZone Address Space Controller (TZC-400) is not being programmed
+ yet. Use of model parameter `-C bp.secure_memory=1` is not supported.
+
+* No support yet for secure world interrupt handling or for switching context
+ between secure and normal worlds in EL3.
+
+* GICv3 support is experimental. The Linux kernel patches to support this are
+ not widely available. There are known issues with GICv3 initialization in
+ the ARM Trusted Firmware.
+
+* Dynamic image loading is not available yet. The current image loader
+ implementation (used to load BL2 and all subsequent images) has some
+ limitations. Changing BL2 or BL3-1 load addresses in certain ways can lead
+ to loading errors, even if the images should theoretically fit in memory.
+
+* Although support for PSCI `CPU_SUSPEND` is present, it is not yet stable
+ and ready for use.
+
+* PSCI api calls `AFFINITY_INFO` & `PSCI_VERSION` are implemented but have not
+ been tested.
+
+* The ARM Trusted Firmware make files result in all build artifacts being
+ placed in the root of the project. These should be placed in appropriate
+ sub-directories.
+
+* The compilation of ARM Trusted Firmware is not free from compilation
+ warnings. Some of these warnings have not been investigated yet so they
+ could mask real bugs.
+
+* The ARM Trusted Firmware currently uses toolchain/system include files like
+ stdio.h. It should provide versions of these within the project to maintain
+ compatibility between toolchains/systems.
+
+* The PSCI code takes some locks in an incorrect sequence. This may cause
+ problems with suspend and hotplug in certain conditions.
+
+* The Linux kernel used in this release is based on version 3.12-rc4. Using
+ this kernel with the ARM Trusted Firmware fails to start the file-system as
+ a RAM-disk. It fails to execute user-space `init` from the RAM-disk. As an
+ alternative, the VirtioBlock mechanism can be used to provide a file-system
+ to the kernel.
+
+
+Detailed changes since last release
+-----------------------------------
+
+First source release – not applicable.
+
+- - - - - - - - - - - - - - - - - - - - - - - - - -
+
+_Copyright (c) 2013 ARM Ltd. All rights reserved._
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
new file mode 100644
index 0000000..ae77c55
--- /dev/null
+++ b/docs/porting-guide.md
@@ -0,0 +1,939 @@
+ARM Trusted Firmware Porting Guide
+==================================
+
+Contents
+--------
+
+1. Introduction
+2. Common Modifications
+ * Common mandatory modifications
+ * Common optional modifications
+3. Boot Loader stage specific modifications
+ * Boot Loader stage 1 (BL1)
+ * Boot Loader stage 2 (BL2)
+ * Boot Loader stage 3-1 (BL3-1)
+ * PSCI implementation (in BL3-1)
+
+- - - - - - - - - - - - - - - - - -
+
+1. Introduction
+----------------
+
+Porting the ARM Trusted Firmware to a new platform involves making some
+mandatory and optional modifications for both the cold and warm boot paths.
+Modifications consist of:
+
+* Implementing a platform-specific function or variable,
+* Setting up the execution context in a certain way, or
+* Defining certain constants (for example #defines).
+
+The firmware provides a default implementation of variables and functions to
+fulfill the optional requirements. These implementations are all weakly defined;
+they are provided to ease the porting effort. Each platform port can override
+them with its own implementation if the default implementation is inadequate.
+
+Some modifications are common to all Boot Loader (BL) stages. Section 2
+discusses these in detail. The subsequent sections discuss the remaining
+modifications for each BL stage in detail.
+
+This document should be read in conjunction with the ARM Trusted Firmware
+[User Guide].
+
+
+2. Common modifications
+------------------------
+
+This section covers the modifications that should be made by the platform for
+each BL stage to correctly port the firmware stack. They are categorized as
+either mandatory or optional.
+
+
+2.1 Common mandatory modifications
+----------------------------------
+A platform port must enable the Memory Management Unit (MMU) with identity
+mapped page tables, and enable both the instruction and data caches for each BL
+stage. In the ARM FVP port, each BL stage configures the MMU in its platform-
+specific architecture setup function, for example `blX_plat_arch_setup()`.
+
+Each platform must allocate a block of identity mapped secure memory with
+Device-nGnRE attributes aligned to page boundary (4K) for each BL stage. This
+memory is identified by the section name `tzfw_coherent_mem` so that its
+possible for the firmware to place variables in it using the following C code
+directive:
+
+ __attribute__ ((section("tzfw_coherent_mem")))
+
+Or alternatively the following assembler code directive:
+
+ .section tzfw_coherent_mem
+
+The `tzfw_coherent_mem` section is used to allocate any data structures that are
+accessed both when a CPU is executing with its MMU and caches enabled, and when
+it's running with its MMU and caches disabled. Examples are given below.
+
+The following variables, functions and constants must be defined by the platform
+for the firmware to work correctly.
+
+
+### File : platform.h [mandatory]
+
+Each platform must export a header file of this name with the following
+constants defined. In the ARM FVP port, this file is found in
+[../plat/fvp/platform.h].
+
+* ** #define : PLATFORM_LINKER_FORMAT **
+
+ Defines the linker format used by the platform, for example
+ `elf64-littleaarch64` used by the FVP.
+
+* ** #define : PLATFORM_LINKER_ARCH **
+
+ Defines the processor architecture for the linker by the platform, for
+ example `aarch64` used by the FVP.
+
+* ** #define : PLATFORM_STACK_SIZE **
+
+ Defines the normal stack memory available to each CPU. This constant is used
+ by `platform_set_stack()`.
+
+* ** #define : FIRMWARE_WELCOME_STR **
+
+ Defines the character string printed by BL1 upon entry into the `bl1_main()`
+ function.
+
+* ** #define : BL2_IMAGE_NAME **
+
+ Name of the BL2 binary image on the host file-system. This name is used by
+ BL1 to load BL2 into secure memory using semi-hosting.
+
+* ** #define : PLATFORM_CACHE_LINE_SIZE **
+
+ Defines the size (in bytes) of the largest cache line across all the cache
+ levels in the platform.
+
+* ** #define : PLATFORM_CLUSTER_COUNT **
+
+ Defines the total number of clusters implemented by the platform in the
+ system.
+
+* ** #define : PLATFORM_CORE_COUNT **
+
+ Defines the total number of CPUs implemented by the platform across all
+ clusters in the system.
+
+* ** #define : PLATFORM_MAX_CPUS_PER_CLUSTER **
+
+ Defines the maximum number of CPUs that can be implemented within a cluster
+ on the platform.
+
+* ** #define : PRIMARY_CPU **
+
+ Defines the `MPIDR` of the primary CPU on the platform. This value is used
+ after a cold boot to distinguish between primary and secondary CPUs.
+
+* ** #define : TZROM_BASE **
+
+ Defines the base address of secure ROM on the platform, where the BL1 binary
+ is loaded. This constant is used by the linker scripts to ensure that the
+ BL1 image fits into the available memory.
+
+* ** #define : TZROM_SIZE **
+
+ Defines the size of secure ROM on the platform. This constant is used by the
+ linker scripts to ensure that the BL1 image fits into the available memory.
+
+* ** #define : TZRAM_BASE **
+
+ Defines the base address of the secure RAM on platform, where the data
+ section of the BL1 binary is loaded. The BL2 and BL3-1 images are also
+ loaded in this secure RAM region. This constant is used by the linker
+ scripts to ensure that the BL1 data section and BL2/BL3-1 binary images fit
+ into the available memory.
+
+* ** #define : TZRAM_SIZE **
+
+ Defines the size of the secure RAM on the platform. This constant is used by
+ the linker scripts to ensure that the BL1 data section and BL2/BL3-1 binary
+ images fit into the available memory.
+
+* ** #define : SYS_CNTCTL_BASE **
+
+ Defines the base address of the `CNTCTLBase` frame of the memory mapped
+ counter and timer in the system level implementation of the generic timer.
+
+* ** #define : BL2_BASE **
+
+ Defines the base address in secure RAM where BL1 loads the BL2 binary image.
+
+* ** #define : BL31_BASE **
+
+ Defines the base address in secure RAM where BL2 loads the BL3-1 binary
+ image.
+
+
+### Other mandatory modifications
+
+The following following mandatory modifications may be implemented in any file
+the implementer chooses. In the ARM FVP port, they are implemented in
+[../plat/fvp/aarch64/fvp_common.c].
+
+* ** Variable : unsigned char platform_normal_stacks[X][Y] **
+
+ where X = PLATFORM_STACK_SIZE
+ and Y = PLATFORM_CORE_COUNT
+
+ Each platform must allocate a block of memory with Normal Cacheable, Write
+ back, Write allocate and Inner Shareable attributes aligned to the size (in
+ bytes) of the largest cache line amongst all caches implemented in the
+ system. A pointer to this memory should be exported with the name
+ `platform_normal_stacks`. This pointer is used by the common platform helper
+ function `platform_set_stack()` to allocate a stack to each CPU in the
+ platform (see [../plat/common/aarch64/platform_helpers.S]).
+
+
+2.2 Common optional modifications
+---------------------------------
+
+The following are helper functions implemented by the firmware that perform
+common platform-specific tasks. A platform may choose to override these
+definitions.
+
+
+### Function : platform_get_core_pos()
+
+ Argument : unsigned long
+ Return : int
+
+A platform may need to convert the `MPIDR` of a CPU to an absolute number, which
+can be used as a CPU-specific linear index into blocks of memory (for example
+while allocating per-CPU stacks). This routine contains a simple mechanism
+to perform this conversion, using the assumption that each cluster contains a
+maximum of 4 CPUs:
+
+ linear index = cpu_id + (cluster_id * 4)
+
+ cpu_id = 8-bit value in MPIDR at affinity level 0
+ cluster_id = 8-bit value in MPIDR at affinity level 1
+
+
+### Function : platform_set_coherent_stack()
+
+ Argument : unsigned long
+ Return : void
+
+A platform may need stack memory that is coherent with main memory to perform
+certain operations like:
+
+* Turning the MMU on, or
+* Flushing caches prior to powering down a CPU or cluster.
+
+Each BL stage allocates this coherent stack memory for each CPU in the
+`tzfw_coherent_mem` section. A pointer to this memory (`pcpu_dv_mem_stack`) is
+used by this function to allocate a coherent stack for each CPU. A CPU is
+identified by its `MPIDR`, which is passed as an argument to this function.
+
+The size of the stack allocated to each CPU is specified by the constant
+`PCPU_DV_MEM_STACK_SIZE`.
+
+
+### Function : platform_is_primary_cpu()
+
+ Argument : unsigned long
+ Return : unsigned int
+
+This function identifies a CPU by its `MPIDR`, which is passed as the argument,
+to determine whether this CPU is the primary CPU or a secondary CPU. A return
+value of zero indicates that the CPU is not the primary CPU, while a non-zero
+return value indicates that the CPU is the primary CPU.
+
+
+### Function : platform_set_stack()
+
+ Argument : unsigned long
+ Return : void
+
+This function uses the `platform_normal_stacks` pointer variable to allocate
+stacks to each CPU. Further details are given in the description of the
+`platform_normal_stacks` variable below. A CPU is identified by its `MPIDR`,
+which is passed as the argument.
+
+The size of the stack allocated to each CPU is specified by the platform defined
+constant `PLATFORM_STACK_SIZE`.
+
+
+### Function : plat_report_exception()
+
+ Argument : unsigned int
+ Return : void
+
+A platform may need to report various information about its status when an
+exception is taken, for example the current exception level, the CPU security
+state (secure/non-secure), the exception type, and so on. This function is
+called in the following circumstances:
+
+* In BL1, whenever an exception is taken.
+* In BL2, whenever an exception is taken.
+* In BL3-1, whenever an asynchronous exception or a synchronous exception
+ other than an SMC32/SMC64 exception is taken.
+
+The default implementation doesn't do anything, to avoid making assumptions
+about the way the platform displays its status information.
+
+This function receives the exception type as its argument. Possible values for
+exceptions types are listed in the [../include/runtime_svc.h] header file. Note
+that these constants are not related to any architectural exception code; they
+are just an ARM Trusted Firmware convention.
+
+
+3. Modifications specific to a Boot Loader stage
+-------------------------------------------------
+
+3.1 Boot Loader Stage 1 (BL1)
+-----------------------------
+
+BL1 implements the reset vector where execution starts from after a cold or
+warm boot. For each CPU, BL1 is responsible for the following tasks:
+
+1. Distinguishing between a cold boot and a warm boot.
+
+2. In the case of a cold boot and the CPU being the primary CPU, ensuring that
+ only this CPU executes the remaining BL1 code, including loading and passing
+ control to the BL2 stage.
+
+3. In the case of a cold boot and the CPU being a secondary CPU, ensuring that
+ the CPU is placed in a platform-specific state until the primary CPU
+ performs the necessary steps to remove it from this state.
+
+4. In the case of a warm boot, ensuring that the CPU jumps to a platform-
+ specific address in the BL3-1 image in the same processor mode as it was
+ when released from reset.
+
+5. Loading the BL2 image in secure memory using semi-hosting at the
+ address specified by the platform defined constant `BL2_BASE`.
+
+6. Populating a `meminfo` structure with the following information in memory,
+ accessible by BL2 immediately upon entry.
+
+ meminfo.total_base = Base address of secure RAM visible to BL2
+ meminfo.total_size = Size of secure RAM visible to BL2
+ meminfo.free_base = Base address of secure RAM available for
+ allocation to BL2
+ meminfo.free_size = Size of secure RAM available for allocation to BL2
+
+ BL1 places this `meminfo` structure at the beginning of the free memory
+ available for its use. Since BL1 cannot allocate memory dynamically at the
+ moment, its free memory will be available for BL2's use as-is. However, this
+ means that BL2 must read the `meminfo` structure before it starts using its
+ free memory (this is discussed in Section 3.2).
+
+ In future releases of the ARM Trusted Firmware it will be possible for
+ the platform to decide where it wants to place the `meminfo` structure for
+ BL2.
+
+ BL1 implements the `init_bl2_mem_layout()` function to populate the
+ BL2 `meminfo` structure. The platform may override this implementation, for
+ example if the platform wants to restrict the amount of memory visible to
+ BL2. Details of how to do this are given below.
+
+The following functions need to be implemented by the platform port to enable
+BL1 to perform the above tasks.
+
+
+### Function : platform_get_entrypoint() [mandatory]
+
+ Argument : unsigned long
+ Return : unsigned int
+
+This function is called with the `SCTLR.M` and `SCTLR.C` bits disabled. The CPU
+is identified by its `MPIDR`, which is passed as the argument. The function is
+responsible for distinguishing between a warm and cold reset using platform-
+specific means. If it's a warm reset then it returns the entrypoint into the
+BL3-1 image that the CPU must jump to. If it's a cold reset then this function
+must return zero.
+
+This function is also responsible for implementing a platform-specific mechanism
+to handle the condition where the CPU has been warm reset but there is no
+entrypoint to jump to.
+
+This function does not follow the Procedure Call Standard used by the
+Application Binary Interface for the ARM 64-bit architecture. The caller should
+not assume that callee saved registers are preserved across a call to this
+function.
+
+This function fulfills requirement 1 listed above.
+
+
+### Function : plat_secondary_cold_boot_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function is called with the MMU and data caches disabled. It is responsible
+for placing the executing secondary CPU in a platform-specific state until the
+primary CPU performs the necessary actions to bring it out of that state and
+allow entry into the OS.
+
+In the ARM FVP port, each secondary CPU powers itself off. The primary CPU is
+responsible for powering up the secondary CPU when normal world software
+requires them.
+
+This function fulfills requirement 3 above.
+
+
+### Function : platform_cold_boot_init() [mandatory]
+
+ Argument : unsigned long
+ Return : unsigned int
+
+This function executes with the MMU and data caches disabled. It is only called
+by the primary CPU. The argument to this function is the address of the
+`bl1_main()` routine where the generic BL1-specific actions are performed.
+This function performs any platform-specific and architectural setup that the
+platform requires to make execution of `bl1_main()` possible.
+
+The platform must enable the MMU with identity mapped page tables and enable
+caches by setting the `SCTLR.I` and `SCTLR.C` bits.
+
+Platform-specific setup might include configuration of memory controllers,
+configuration of the interconnect to allow the cluster to service cache snoop
+requests from another cluster, zeroing of the ZI section, and so on.
+
+In the ARM FVP port, this function enables CCI snoops into the cluster that the
+primary CPU is part of. It also enables the MMU and initializes the ZI section
+in the BL1 image through the use of linker defined symbols.
+
+This function helps fulfill requirement 2 above.
+
+
+### Function : bl1_platform_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function executes with the MMU and data caches enabled. It is responsible
+for performing any remaining platform-specific setup that can occur after the
+MMU and data cache have been enabled.
+
+In the ARM FVP port, it zeros out the ZI section, enables the system level
+implementation of the generic timer counter and initializes the console.
+
+This function helps fulfill requirement 5 above.
+
+
+### Function : bl1_get_sec_mem_layout() [mandatory]
+
+ Argument : void
+ Return : meminfo
+
+This function executes with the MMU and data caches enabled. The `meminfo`
+structure returned by this function must contain the extents and availability of
+secure RAM for the BL1 stage.
+
+ meminfo.total_base = Base address of secure RAM visible to BL1
+ meminfo.total_size = Size of secure RAM visible to BL1
+ meminfo.free_base = Base address of secure RAM available for allocation
+ to BL1
+ meminfo.free_size = Size of secure RAM available for allocation to BL1
+
+This information is used by BL1 to load the BL2 image in secure RAM. BL1 also
+populates a similar structure to tell BL2 the extents of memory available for
+its own use.
+
+This function helps fulfill requirement 5 above.
+
+
+### Function : init_bl2_mem_layout() [optional]
+
+ Argument : meminfo *, meminfo *, unsigned int, unsigned long
+ Return : void
+
+Each BL stage needs to tell the next stage the amount of secure RAM available
+for it to use. For example, as part of handing control to BL2, BL1 informs BL2
+of the extents of secure RAM available for BL2 to use. BL2 must do the same when
+passing control to BL3-1. This information is populated in a `meminfo`
+structure.
+
+Depending upon where BL2 has been loaded in secure RAM (determined by
+`BL2_BASE`), BL1 calculates the amount of free memory available for BL2 to use.
+BL1 also ensures that its data sections resident in secure RAM are not visible
+to BL2. An illustration of how this is done in the ARM FVP port is given in the
+[User Guide], in the Section "Memory layout on Base FVP".
+
+
+3.2 Boot Loader Stage 2 (BL2)
+-----------------------------
+
+The BL2 stage is executed only by the primary CPU, which is determined in BL1
+using the `platform_is_primary_cpu()` function. BL1 passed control to BL2 at
+`BL2_BASE`. BL2 executes in Secure EL1 and is responsible for:
+
+1. Loading the BL3-1 binary image in secure RAM using semi-hosting. To load the
+ BL3-1 image, BL2 makes use of the `meminfo` structure passed to it by BL1.
+ This structure allows BL2 to calculate how much secure RAM is available for
+ its use. The platform also defines the address in secure RAM where BL3-1 is
+ loaded through the constant `BL31_BASE`. BL2 uses this information to
+ determine if there is enough memory to load the BL3-1 image.
+
+2. Arranging to pass control to a normal world BL image that has been
+ pre-loaded at a platform-specific address. This address is determined using
+ the `plat_get_ns_image_entrypoint()` function described below.
+
+ BL2 populates an `el_change_info` structure in memory provided by the
+ platform with information about how BL3-1 should pass control to the normal
+ world BL image.
+
+3. Populating a `meminfo` structure with the following information in
+ memory that is accessible by BL3-1 immediately upon entry.
+
+ meminfo.total_base = Base address of secure RAM visible to BL3-1
+ meminfo.total_size = Size of secure RAM visible to BL3-1
+ meminfo.free_base = Base address of secure RAM available for allocation
+ to BL3-1
+ meminfo.free_size = Size of secure RAM available for allocation to
+ BL3-1
+
+ BL2 places this `meminfo` structure in memory provided by the
+ platform (`bl2_el_change_mem_ptr`). BL2 implements the
+ `init_bl31_mem_layout()` function to populate the BL3-1 meminfo structure
+ described above. The platform may override this implementation, for example
+ if the platform wants to restrict the amount of memory visible to BL3-1.
+ Details of this function are given below.
+
+The following functions must be implemented by the platform port to enable BL2
+to perform the above tasks.
+
+
+### Function : bl2_early_platform_setup() [mandatory]
+
+ Argument : meminfo *, void *
+ Return : void
+
+This function executes with the MMU and data caches disabled. It is only called
+by the primary CPU. The arguments to this function are:
+
+* The address of the `meminfo` structure populated by BL1
+* An opaque pointer that the platform may use as needed.
+
+The platform must copy the contents of the `meminfo` structure into a private
+variable as the original memory may be subsequently overwritten by BL2. The
+copied structure is made available to all BL2 code through the
+`bl2_get_sec_mem_layout()` function.
+
+
+### Function : bl2_plat_arch_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function executes with the MMU and data caches disabled. It is only called
+by the primary CPU.
+
+The purpose of this function is to perform any architectural initialization
+that varies across platforms, for example enabling the MMU (since the memory
+map differs across platforms).
+
+
+### Function : bl2_platform_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initialization in `bl2_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+The purpose of this function is to perform any platform initialization specific
+to BL2. This function must initialize a pointer to memory
+(`bl2_el_change_mem_ptr`), which can then be used to populate an
+`el_change_info` structure. The underlying requirement is that the platform must
+initialize this pointer before the `get_el_change_mem_ptr()` function
+accesses it in `bl2_main()`.
+
+The ARM FVP port initializes this pointer to the base address of Secure DRAM
+(`0x06000000`).
+
+
+### Variable : unsigned char bl2_el_change_mem_ptr[EL_CHANGE_MEM_SIZE] [mandatory]
+
+As mentioned in the description of `bl2_platform_setup()`, this pointer is
+initialized by the platform to point to memory where an `el_change_info`
+structure can be populated.
+
+
+### Function : bl2_get_sec_mem_layout() [mandatory]
+
+ Argument : void
+ Return : meminfo
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initialization in `bl2_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+The purpose of this function is to return a `meminfo` structure populated with
+the extents of secure RAM available for BL2 to use. See
+`bl2_early_platform_setup()` above.
+
+
+### Function : init_bl31_mem_layout() [optional]
+
+ Argument : meminfo *, meminfo *, unsigned int
+ Return : void
+
+Each BL stage needs to tell the next stage the amount of secure RAM that is
+available for it to use. For example, as part of handing control to BL2, BL1
+must inform BL2 about the extents of secure RAM that is available for BL2 to
+use. BL2 must do the same when passing control to BL3-1. This information is
+populated in a `meminfo` structure.
+
+Depending upon where BL3-1 has been loaded in secure RAM (determined by
+`BL31_BASE`), BL2 calculates the amount of free memory available for BL3-1 to
+use. BL2 also ensures that BL3-1 is able reclaim memory occupied by BL2. This
+is done because BL2 never executes again after passing control to BL3-1.
+An illustration of how this is done in the ARM FVP port is given in the
+[User Guide], in the section "Memory layout on Base FVP".
+
+
+### Function : plat_get_ns_image_entrypoint() [mandatory]
+
+ Argument : void
+ Return : unsigned long
+
+As previously described, BL2 is responsible for arranging for control to be
+passed to a normal world BL image through BL3-1. This function returns the
+entrypoint of that image, which BL3-1 uses to jump to it.
+
+The ARM FVP port assumes that flash memory has been pre-loaded with the UEFI
+image, and so returns the base address of flash memory.
+
+
+3.2 Boot Loader Stage 3-1 (BL3-1)
+---------------------------------
+
+During cold boot, the BL3-1 stage is executed only by the primary CPU. This is
+determined in BL1 using the `platform_is_primary_cpu()` function. BL1 passes
+control to BL3-1 at `BL31_BASE`. During warm boot, BL3-1 is executed by all
+CPUs. BL3-1 executes at EL3 and is responsible for:
+
+1. Re-initializing all architectural and platform state. Although BL1 performs
+ some of this initialization, BL3-1 remains resident in EL3 and must ensure
+ that EL3 architectural and platform state is completely initialized. It
+ should make no assumptions about the system state when it receives control.
+
+2. Passing control to a normal world BL image, pre-loaded at a platform-
+ specific address by BL2. BL3-1 uses the `el_change_info` structure that BL2
+ populated in memory to do this.
+
+3. Providing runtime firmware services. Currently, BL3-1 only implements a
+ subset of the Power State Coordination Interface (PSCI) API as a runtime
+ service. See Section 3.3 below for details of porting the PSCI
+ implementation.
+
+The following functions must be implemented by the platform port to enable BL3-1
+to perform the above tasks.
+
+
+### Function : bl31_early_platform_setup() [mandatory]
+
+ Argument : meminfo *, void *, unsigned long
+ Return : void
+
+This function executes with the MMU and data caches disabled. It is only called
+by the primary CPU. The arguments to this function are:
+
+* The address of the `meminfo` structure populated by BL2.
+* An opaque pointer that the platform may use as needed.
+* The `MPIDR` of the primary CPU.
+
+The platform must copy the contents of the `meminfo` structure into a private
+variable as the original memory may be subsequently overwritten by BL3-1. The
+copied structure is made available to all BL3-1 code through the
+`bl31_get_sec_mem_layout()` function.
+
+
+### Function : bl31_plat_arch_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function executes with the MMU and data caches disabled. It is only called
+by the primary CPU.
+
+The purpose of this function is to perform any architectural initialization
+that varies across platforms, for example enabling the MMU (since the memory
+map differs across platforms).
+
+
+### Function : bl31_platform_setup() [mandatory]
+
+ Argument : void
+ Return : void
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initialization in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+The purpose of this function is to complete platform initialization so that both
+BL3-1 runtime services and normal world software can function correctly.
+
+The ARM FVP port does the following:
+* Initializes the generic interrupt controller.
+* Configures the CLCD controller.
+* Grants access to the system counter timer module
+* Initializes the FVP power controller device
+* Detects the system topology.
+
+
+### Function : bl31_get_next_image_info() [mandatory]
+
+ Argument : unsigned long
+ Return : el_change_info *
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`.
+
+This function is called by `bl31_main()` to retrieve information provided by
+BL2, so that BL3-1 can pass control to the normal world software image. This
+function must return a pointer to the `el_change_info` structure (that was
+copied during `bl31_early_platform_setup()`).
+
+
+### Function : bl31_get_sec_mem_layout() [mandatory]
+
+ Argument : void
+ Return : meminfo
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+The purpose of this function is to return a `meminfo` structure populated with
+the extents of secure RAM available for BL3-1 to use. See
+`bl31_early_platform_setup()` above.
+
+
+3.3 Power State Coordination Interface (in BL3-1)
+------------------------------------------------
+
+The ARM Trusted Firmware's implementation of the PSCI API is based around the
+concept of an _affinity instance_. Each _affinity instance_ can be uniquely
+identified in a system by a CPU ID (the processor `MPIDR` is used in the PSCI
+interface) and an _affinity level_. A processing element (for example, a
+CPU) is at level 0. If the CPUs in the system are described in a tree where the
+node above a CPU is a logical grouping of CPUs that share some state, then
+affinity level 1 is that group of CPUs (for example, a cluster), and affinity
+level 2 is a group of clusters (for example, the system). The implementation
+assumes that the affinity level 1 ID can be computed from the affinity level 0
+ID (for example, a unique cluster ID can be computed from the CPU ID). The
+current implementation computes this on the basis of the recommended use of
+`MPIDR` affinity fields in the ARM Architecture Reference Manual.
+
+BL3-1's platform initialization code exports a pointer to the platform-specific
+power management operations required for the PSCI implementation to function
+correctly. This information is populated in the `plat_pm_ops` structure. The
+PSCI implementation calls members of the `plat_pm_ops` structure for performing
+power management operations for each affinity instance. For example, the target
+CPU is specified by its `MPIDR` in a PSCI `CPU_ON` call. The `affinst_on()`
+handler (if present) is called for each affinity instance as the PSCI
+implementation powers up each affinity level implemented in the `MPIDR` (for
+example, CPU, cluster and system).
+
+The following functions must be implemented to initialize PSCI functionality in
+the ARM Trusted Firmware.
+
+
+### Function : plat_get_aff_count() [mandatory]
+
+ Argument : unsigned int, unsigned long
+ Return : unsigned int
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+This function is called by the PSCI initialization code to detect the system
+topology. Its purpose is to return the number of affinity instances implemented
+at a given `affinity level` (specified by the first argument) and a given
+`MPIDR` (specified by the second argument). For example, on a dual-cluster
+system where first cluster implements 2 CPUs and the second cluster implements 4
+CPUs, a call to this function with an `MPIDR` corresponding to the first cluster
+(`0x0`) and affinity level 0, would return 2. A call to this function with an
+`MPIDR` corresponding to the second cluster (`0x100`) and affinity level 0,
+would return 4.
+
+
+### Function : plat_get_aff_state() [mandatory]
+
+ Argument : unsigned int, unsigned long
+ Return : unsigned int
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+This function is called by the PSCI initialization code. Its purpose is to
+return the state of an affinity instance. The affinity instance is determined by
+the affinity ID at a given `affinity level` (specified by the first argument)
+and an `MPIDR` (specified by the second argument). The state can be one of
+`PSCI_AFF_PRESENT` or `PSCI_AFF_ABSENT`. The latter state is used to cater for
+system topologies where certain affinity instances are unimplemented. For
+example, consider a platform that implements a single cluster with 4 CPUs and
+another CPU implemented directly on the interconnect with the cluster. The
+`MPIDR`s of the cluster would range from `0x0-0x3`. The `MPIDR` of the single
+CPU would be 0x100 to indicate that it does not belong to cluster 0. Cluster 1
+is missing but needs to be accounted for to reach this single CPU in the
+topology tree. Hence it is marked as `PSCI_AFF_ABSENT`.
+
+
+### Function : plat_get_max_afflvl() [mandatory]
+
+ Argument : void
+ Return : int
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+This function is called by the PSCI implementation both during cold and warm
+boot, to determine the maximum affinity level that the power management
+operations should apply to. ARMv8 has support for 4 affinity levels. It is
+likely that hardware will implement fewer affinity levels. This function allows
+the PSCI implementation to consider only those affinity levels in the system
+that the platform implements. For example, the Base AEM FVP implements two
+clusters with a configurable number of CPUs. It reports the maximum affinity
+level as 1, resulting in PSCI power control up to the cluster level.
+
+
+### Function : platform_setup_pm() [mandatory]
+
+ Argument : plat_pm_ops **
+ Return : int
+
+This function may execute with the MMU and data caches enabled if the platform
+port does the necessary initializations in `bl31_plat_arch_setup()`. It is only
+called by the primary CPU.
+
+This function is called by PSCI initialization code. Its purpose is to export
+handler routines for platform-specific power management actions by populating
+the passed pointer with a pointer to BL3-1's private `plat_pm_ops` structure.
+
+A description of each member of this structure is given below. Please refer to
+the ARM FVP specific implementation of these handlers in [../plat/fvp/fvp_pm.c]
+as an example. A platform port may choose not implement some of the power
+management operations. For example, the ARM FVP port does not implement the
+`affinst_standby()` function.
+
+#### plat_pm_ops.affinst_standby()
+
+Perform the platform-specific setup to enter the standby state indicated by the
+passed argument.
+
+#### plat_pm_ops.affinst_on()
+
+Perform the platform specific setup to power on an affinity instance, specified
+by the `MPIDR` (first argument) and `affinity level` (fourth argument). The
+`state` (fifth argument) contains the current state of that affinity instance
+(ON or OFF). This is useful to determine whether any action must be taken. For
+example, while powering on a CPU, the cluster that contains this CPU might
+already be in the ON state. The platform decides what actions must be taken to
+transition from the current state to the target state (indicated by the power
+management operation).
+
+#### plat_pm_ops.affinst_off()
+
+Perform the platform specific setup to power off an affinity instance in the
+`MPIDR` of the calling CPU. It is called by the PSCI `CPU_OFF` API
+implementation.
+
+The `MPIDR` (first argument), `affinity level` (second argument) and `state`
+(third argument) have a similar meaning as described in the `affinst_on()`
+operation. They are used to identify the affinity instance on which the call
+is made and its current state. This gives the platform port an indication of the
+state transition it must make to perform the requested action. For example, if
+the calling CPU is the last powered on CPU in the cluster, after powering down
+affinity level 0 (CPU), the platform port should power down affinity level 1
+(the cluster) as well.
+
+This function is called with coherent stacks. This allows the PSCI
+implementation to flush caches at a given affinity level without running into
+stale stack state after turning off the caches. On ARMv8 cache hits do not occur
+after the cache has been turned off.
+
+#### plat_pm_ops.affinst_suspend()
+
+Perform the platform specific setup to power off an affinity instance in the
+`MPIDR` of the calling CPU. It is called by the PSCI `CPU_SUSPEND` API
+implementation.
+
+The `MPIDR` (first argument), `affinity level` (third argument) and `state`
+(fifth argument) have a similar meaning as described in the `affinst_on()`
+operation. They are used to identify the affinity instance on which the call
+is made and its current state. This gives the platform port an indication of the
+state transition it must make to perform the requested action. For example, if
+the calling CPU is the last powered on CPU in the cluster, after powering down
+affinity level 0 (CPU), the platform port should power down affinity level 1
+(the cluster) as well.
+
+The difference between turning an affinity instance off versus suspending it
+is that in the former case, the affinity instance is expected to re-initialize
+its state when its next powered on (see `affinst_on_finish()`). In the latter
+case, the affinity instance is expected to save enough state so that it can
+resume execution by restoring this state when its powered on (see
+`affinst_suspend_finish()`).
+
+This function is called with coherent stacks. This allows the PSCI
+implementation to flush caches at a given affinity level without running into
+stale stack state after turning off the caches. On ARMv8 cache hits do not occur
+after the cache has been turned off.
+
+#### plat_pm_ops.affinst_on_finish()
+
+This function is called by the PSCI implementation after the calling CPU is
+powered on and released from reset in response to an earlier PSCI `CPU_ON` call.
+It performs the platform-specific setup required to initialize enough state for
+this CPU to enter the normal world and also provide secure runtime firmware
+services.
+
+The `MPIDR` (first argument), `affinity level` (second argument) and `state`
+(third argument) have a similar meaning as described in the previous operations.
+
+This function is called with coherent stacks. This allows the PSCI
+implementation to flush caches at a given affinity level without running into
+stale stack state after turning off the caches. On ARMv8 cache hits do not occur
+after the cache has been turned off.
+
+#### plat_pm_ops.affinst_on_suspend()
+
+This function is called by the PSCI implementation after the calling CPU is
+powered on and released from reset in response to an asynchronous wakeup
+event, for example a timer interrupt that was programmed by the CPU during the
+`CPU_SUSPEND` call. It performs the platform-specific setup required to
+restore the saved state for this CPU to resume execution in the normal world
+and also provide secure runtime firmware services.
+
+The `MPIDR` (first argument), `affinity level` (second argument) and `state`
+(third argument) have a similar meaning as described in the previous operations.
+
+This function is called with coherent stacks. This allows the PSCI
+implementation to flush caches at a given affinity level without running into
+stale stack state after turning off the caches. On ARMv8 cache hits do not occur
+after the cache has been turned off.
+
+BL3-1 platform initialization code must also detect the system topology and
+the state of each affinity instance in the topology. This information is
+critical for the PSCI runtime service to function correctly. More details are
+provided in the description of the `plat_get_aff_count()` and
+`plat_get_aff_state()` functions above.
+
+
+- - - - - - - - - - - - - - - - - - - - - - - - - -
+
+_Copyright (c) 2013 ARM Ltd. All rights reserved._
+
+
+[User Guide]: user-guide.md
+
+[../plat/common/aarch64/platform_helpers.S]: ../plat/common/aarch64/platform_helpers.S
+[../plat/fvp/platform.h]: ../plat/fvp/platform.h
+[../plat/fvp/aarch64/fvp_common.c]: ../plat/fvp/aarch64/fvp_common.c
+[../plat/fvp/fvp_pm.c]: ../plat/fvp/fvp_pm.c
+[../include/runtime_svc.h]: ../include/runtime_svc.h
diff --git a/docs/user-guide.md b/docs/user-guide.md
new file mode 100644
index 0000000..20483e4
--- /dev/null
+++ b/docs/user-guide.md
@@ -0,0 +1,961 @@
+ARM Trusted Firmware User Guide
+===============================
+
+Contents :
+
+1. Introduction
+2. Using the Software
+3. Firmware Design
+4. References
+
+
+1. Introduction
+----------------
+
+The ARM Trusted Firmware implements a subset of the Trusted Board Boot
+Requirements (TBBR) Platform Design Document (PDD) [1] for ARM reference
+platforms. The TBB sequence starts when the platform is powered on and runs up
+to the stage where it hands-off control to firmware running in the normal
+world in DRAM. This is the cold boot path.
+
+The ARM Trusted Firmware also implements the Power State Coordination Interface
+([PSCI]) PDD [2] as a runtime service. PSCI is the interface from normal world
+software to firmware implementing power management use-cases (for example,
+secondary CPU boot, hotplug and idle). Normal world software can access ARM
+Trusted Firmware runtime services via the ARM SMC (Secure Monitor Call)
+instruction. The SMC instruction must be used as mandated by the [SMC Calling
+Convention PDD][SMCCC] [3].
+
+
+2. Using the Software
+----------------------
+
+### Host machine requirements
+
+The minimum recommended machine specification is an Intel Core2Duo clocking at
+2.6GHz or above, and 12GB RAM. For best performance, use a machine with Intel
+Core i7 (SandyBridge) and 16GB of RAM.
+
+
+### Tools
+
+The following tools are required to use the ARM Trusted Firmware:
+
+* Ubuntu desktop OS. The software has been tested on Ubuntu 12.04.02 (64-bit).
+ The following packages are also needed:
+
+* `ia32-libs` package.
+
+* `make` and `uuid-dev` packages for building UEFI.
+
+* `bc` and `ncurses-dev` packages for building Linux.
+
+* Baremetal GNU GCC tools. Verified packages can be downloaded from [Linaro]
+ [Linaro Toolchain]. The rest of this document assumes that the
+ `gcc-linaro-aarch64-none-elf-4.8-2013.09-01_linux.tar.xz` tools are used.
+
+ wget http://releases.linaro.org/13.09/components/toolchain/binaries/gcc-linaro-aarch64-none-elf-4.8-2013.09-01_linux.tar.xz
+ tar -xf gcc-linaro-aarch64-none-elf-4.8-2013.09-01_linux.tar.xz
+
+* The Device Tree Compiler (DTC) included with Linux kernel 3.12-rc4 is used
+ to build the Flattened Device Tree (FDT) source files (`.dts` files)
+ provided with this release.
+
+* (Optional) For debugging, ARM [Development Studio 5 (DS-5)][DS-5] v5.16.
+
+
+### Building the Trusted Firmware
+
+To build the software for the Base FVPs, follow these steps:
+
+1. Clone the ARM Trusted Firmware repository from Github:
+
+ git clone https://github.com/ARM-software/arm-trusted-firmware.git
+
+2. Change to the trusted firmware directory:
+
+ cd arm-trusted-firmware
+
+3. Set the compiler path and build:
+
+ CROSS_COMPILE=<path/to>/aarch64-none-elf- make
+
+ By default this produces a release version of the build. To produce a debug
+ version instead, refer to the "Debugging options" section below.
+
+ The build creates ELF and raw binary files in the current directory. It
+ generates the following boot loader binary files from the ELF files:
+
+ * `bl1.bin`
+ * `bl2.bin`
+ * `bl31.bin`
+
+4. Copy the above 3 boot loader binary files to the directory where the FVPs
+ are launched from. Symbolic links of the same names may be created instead.
+
+5. (Optional) To clean the build directory use
+
+ make distclean
+
+
+#### Debugging options
+
+To compile a debug version and make the build more verbose use
+
+ CROSS_COMPILE=<path/to>/aarch64-none-elf- make DEBUG=1 V=1
+
+AArch64 GCC uses DWARF version 4 debugging symbols by default. Some tools (for
+example DS-5) might not support this and may need an older version of DWARF
+symbols to be emitted by GCC. This can be achieved by using the
+`-gdwarf-<version>` flag, with the version being set to 2 or 3. Setting the
+version to 2 is recommended for DS-5 versions older than 5.16.
+
+When debugging logic problems it might also be useful to disable all compiler
+optimizations by using `-O0`.
+
+NOTE: Using `-O0` could cause output images to be larger and base addresses
+might need to be recalculated (see the later memory layout section).
+
+Extra debug options can be passed to the build system by setting `CFLAGS`:
+
+ CFLAGS='-O0 -gdwarf-2' CROSS_COMPILE=<path/to>/aarch64-none-elf- make DEBUG=1 V=1
+
+
+### Obtaining the normal world software
+
+#### Obtaining UEFI
+
+Download an archive of the [EDK2 (EFI Development Kit 2) source code][EDK2]
+supporting the Base FVPs. EDK2 is an open source implementation of the UEFI
+specification:
+
+ wget http://sourceforge.net/projects/edk2/files/ARM/aarch64-uefi-rev14582.tgz/download -O aarch64-uefi-rev14582.tgz
+ tar -xf aarch64-uefi-rev14582.tgz
+
+To build the software for the Base FVPs, follow these steps:
+
+1. Change into the unpacked EDK2 source directory
+
+ cd uefi
+
+2. Copy build config templates to local workspace
+
+ export EDK_TOOLS_PATH=$(pwd)/BaseTools
+ . edksetup.sh $(pwd)/BaseTools/
+
+3. Rebuild EDK2 host tools
+
+ make -C "$EDK_TOOLS_PATH" clean
+ make -C "$EDK_TOOLS_PATH"
+
+4. Build the software
+
+ AARCH64GCC_TOOLS_PATH=<full-path-to-aarch64-gcc>/bin/ \
+ build -v -d3 -a AARCH64 -t ARMGCC \
+ -p ArmPlatformPkg/ArmVExpressPkg/ArmVExpress-FVP-AArch64.dsc
+
+ The EDK2 binary for use with the ARM Trusted Firmware can then be found
+ here:
+
+ Build/ArmVExpress-FVP-AArch64/DEBUG_ARMGCC/FV/FVP_AARCH64_EFI.fd
+
+This will build EDK2 for the default settings as used by the FVPs.
+
+To boot Linux using a VirtioBlock file-system, the command line passed from EDK2
+to the Linux kernel must be modified as described in the "Obtaining a
+File-system" section below.
+
+If legacy GICv2 locations are used, the EDK2 platform description must be
+updated. This is required as EDK2 does not support probing for the GIC location.
+To do this, open the `ArmPlatformPkg/ArmVExpressPkg/ArmVExpress-FVP-AArch64.dsc`
+file for editing and make the modifications as below. Rebuild EDK2 after doing a
+`clean`.
+
+ gArmTokenSpaceGuid.PcdGicDistributorBase|0x2C001000
+ gArmTokenSpaceGuid.PcdGicInterruptInterfaceBase|0x2C002000
+
+The EDK2 binary `FVP_AARCH64_EFI.fd` should be loaded into FVP FLASH0 via model
+parameters as described in the "Running the Software" section below.
+
+#### Obtaining a Linux kernel
+
+The software has been verified using Linux kernel version 3.12-rc4. Patches
+have been applied to the kernel in order to enable CPU hotplug.
+
+Preparing a Linux kernel for use on the FVPs with hotplug support can
+be done as follows (GICv2 support only):
+
+1. Clone Linux:
+
+ git clone git://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
+
+ The CPU hotplug features are not yet included in the mainline kernel. To use
+ these, add the patches from Mark Rutland's kernel, based on Linux 3.12-rc4:
+
+ cd linux
+ git remote add -f --tags markr git://linux-arm.org/linux-mr.git
+ git checkout -b hotplug arm64-cpu-hotplug-20131023
+
+2. Build with the Linaro GCC tools.
+
+ # in linux/
+ make mrproper
+ make ARCH=arm64 defconfig
+
+ # Enable Hotplug
+ make ARCH=arm64 menuconfig
+ # Kernel Features ---> [*] Support for hot-pluggable CPUs
+
+ CROSS_COMPILE=/path/to/aarch64-none-elf- make -j6 ARCH=arm64
+
+3. Copy the Linux image `arch/arm64/boot/Image` to the working directory from
+ where the FVP is launched. A symbolic link may also be created instead.
+
+#### Obtaining the Flattened Device Trees
+
+Depending on the FVP configuration and Linux configuration used, different
+FDT files are required. FDTs for the Base FVP can be found in the Trusted
+Firmware source directory under `fdts`.
+
+* `fvp-base-gicv2-psci.dtb`
+
+ (Default) For use with both AEMv8 and Cortex-A57-A53 Base FVPs with
+ default memory map configuration.
+
+* `fvp-base-gicv2legacy-psci.dtb`
+
+ For use with both AEMv8 and Cortex-A57-A53 Base FVPs with legacy GICv2
+ memory map configuration.
+
+* `fvp-base-gicv3-psci.dtb`
+
+ For use with AEMv8 Base FVP with default memory map configuration and
+ Linux GICv3 support.
+
+Copy the chosen FDT blob as `fdt.dtb` to the directory from which the FVP
+is launched. A symbolic link may also be created instead.
+
+#### Obtaining a File-system
+
+To prepare a Linaro LAMP based Open Embedded file-system, the following
+instructions can be used as a guide. The file-system can be provided to Linux
+via VirtioBlock or as a RAM-disk. Both methods are described below.
+
+##### Prepare VirtioBlock
+
+To prepare a VirtioBlock file-system, do the following:
+
+1. Download and unpack the disk image.
+
+ NOTE: The unpacked disk image grows to 2 GiB in size.
+
+ wget http://releases.linaro.org/13.09/openembedded/aarch64/vexpress64-openembedded_lamp-armv8_20130927-7.img.gz
+ gunzip vexpress64-openembedded_lamp-armv8_20130927-7.img.gz
+
+2. Make sure the Linux kernel has Virtio support enabled using
+ `make ARCH=arm64 menuconfig`.
+
+ Device Drivers ---> Virtio drivers ---> <*> Platform bus driver for memory mapped virtio devices
+ Device Drivers ---> [*] Block devices ---> <*> Virtio block driver
+ File systems ---> <*> The Extended 4 (ext4) filesystem
+
+ If some of these configurations are missing, enable them, save the kernel
+ configuration, then rebuild the kernel image using the instructions provided
+ in the section "Obtaining a Linux kernel".
+
+3. Change the Kernel command line to include `root=/dev/vda2`. This can either
+ be done in the EDK2 boot menu or in the platform file. Editing the platform
+ file and rebuilding EDK2 will make the change persist. To do this:
+
+ 1. In EDK, edit the following file:
+
+ ArmPlatformPkg/ArmVExpressPkg/ArmVExpress-FVP-AArch64.dsc
+
+ 2. Add `root=/dev/vda2` to:
+
+ gArmPlatformTokenSpaceGuid.PcdDefaultBootArgument|"<Other default options>"
+
+ 3. Remove the entry:
+
+ gArmPlatformTokenSpaceGuid.PcdDefaultBootInitrdPath|""
+
+ 4. Rebuild EDK2 (see "Obtaining UEFI" section above).
+
+4. The file-system image file should be provided to the model environment by
+ passing it the correct command line option. In the Base FVP the following
+ option should be provided in addition to the ones described in the
+ "Running the software" section below.
+
+ NOTE: A symbolic link to this file cannot be used with the FVP; the path
+ to the real file must be provided.
+
+ -C bp.virtioblockdevice.image_path="<path/to/>vexpress64-openembedded_lamp-armv8_20130927-7.img"
+
+5. Ensure that the FVP doesn't output any error messages. If the following
+ error message is displayed:
+
+ ERROR: BlockDevice: Failed to open "vexpress64-openembedded_lamp-armv8_20130927-7.img"!
+
+ then make sure the path to the file-system image in the model parameter is
+ correct and that read permission is correctly set on the file-system image
+ file.
+
+##### Prepare RAM-disk
+
+NOTE: The RAM-disk option does not currently work with the Linux kernel version
+described above; use the VirtioBlock method instead. For further information
+please see the "Known issues" section in the [Change Log].
+
+To Prepare a RAM-disk file-system, do the following:
+
+1. Download the file-system image:
+
+ wget http://releases.linaro.org/13.09/openembedded/aarch64/linaro-image-lamp-genericarmv8-20130912-487.rootfs.tar.gz
+
+2. Modify the Linaro image:
+
+ # Prepare for use as RAM-disk. Normally use MMC, NFS or VirtioBlock.
+ # Be careful, otherwise you could damage your host file-system.
+ mkdir tmp; cd tmp
+ sudo sh -c "zcat ../linaro-image-lamp-genericarmv8-20130912-487.rootfs.tar.gz | cpio -id"
+ sudo ln -s sbin/init .
+ sudo ln -s S35mountall.sh etc/rcS.d/S03mountall.sh
+ sudo sh -c "echo 'devtmpfs /dev devtmpfs mode=0755,nosuid 0 0' >> etc/fstab"
+ sudo sh -c "find . | cpio --quiet -H newc -o | gzip -3 -n > ../filesystem.cpio.gz"
+ cd ..
+
+3. Copy the resultant `filesystem.cpio.gz` to the directory where the FVP is
+ launched from. A symbolic link may also be created instead.
+
+
+### Running the software
+
+This release of the ARM Trusted Firmware has been tested on the following ARM
+FVPs (64-bit versions only).
+
+* `FVP_Base_AEMv8A-AEMv8A` (Version 5.1 build 8)
+* `FVP_Base_Cortex-A57x4-A53x4` (Version 5.1 build 8)
+
+Please refer to the FVP documentation for a detailed description of the model
+parameter options. A brief description of the important ones that affect the
+ARM Trusted Firmware and normal world software behavior is provided below.
+
+#### Running on the AEMv8 Base FVP
+
+The following `FVP_Base_AEMv8A-AEMv8A` parameters should be used to boot Linux
+with 8 CPUs using the ARM Trusted Firmware.
+
+NOTE: Using `cache_state_modelled=1` makes booting very slow. The software will
+still work (and run much faster) without this option but this will hide any
+cache maintenance defects in the software.
+
+NOTE: Using the `-C bp.virtioblockdevice.image_path` parameter is not necessary
+if a Linux RAM-disk file-system is used (see the "Obtaining a File-system"
+section above).
+
+ FVP_Base_AEMv8A-AEMv8A \
+ -C pctl.startup=0.0.0.0 \
+ -C bp.secure_memory=0 \
+ -C cluster0.NUM_CORES=4 \
+ -C cluster1.NUM_CORES=4 \
+ -C cache_state_modelled=1 \
+ -C bp.pl011_uart0.untimed_fifos=1 \
+ -C bp.secureflashloader.fname=<path to bl1.bin> \
+ -C bp.flashloader0.fname=<path to UEFI binary> \
+ -C bp.virtioblockdevice.image_path="<path/to/>vexpress64-openembedded_lamp-armv8_20130927-7.img"
+
+#### Running on the Cortex-A57-A53 Base FVP
+
+The following `FVP_Base_Cortex-A57x4-A53x4` model parameters should be used to
+boot Linux with 8 CPUs using the ARM Trusted Firmware.
+
+NOTE: Using `cache_state_modelled=1` makes booting very slow. The software will
+still work (and run much faster) without this option but this will hide any
+cache maintenance defects in the software.
+
+NOTE: Using the `-C bp.virtioblockdevice.image_path` parameter is not necessary
+if a Linux RAM-disk file-system is used (see the "Obtaining a File-system"
+section above).
+
+ FVP_Base_Cortex-A57x4-A53x4 \
+ -C pctl.startup=0.0.0.0 \
+ -C bp.secure_memory=0 \
+ -C cache_state_modelled=1 \
+ -C bp.pl011_uart0.untimed_fifos=1 \
+ -C bp.secureflashloader.fname=<path to bl1.bin> \
+ -C bp.flashloader0.fname=<path to UEFI binary> \
+ -C bp.virtioblockdevice.image_path="<path/to/>vexpress64-openembedded_lamp-armv8_20130927-7.img"
+
+### Configuring the GICv2 memory map
+
+The Base FVP models support GICv2 with the default model parameters at the
+following addresses.
+
+ GICv2 Distributor Interface 0x2f000000
+ GICv2 CPU Interface 0x2c000000
+ GICv2 Virtual CPU Interface 0x2c010000
+ GICv2 Hypervisor Interface 0x2c02f000
+
+The models can be configured to support GICv2 at addresses corresponding to the
+legacy (Versatile Express) memory map as follows.
+
+ GICv2 Distributor Interface 0x2c001000
+ GICv2 CPU Interface 0x2c002000
+ GICv2 Virtual CPU Interface 0x2c004000
+ GICv2 Hypervisor Interface 0x2c006000
+
+The choice of memory map is reflected in the build field (bits[15:12]) in the
+`SYS_ID` register (Offset `0x0`) in the Versatile Express System registers
+memory map (`0x1c010000`).
+
+* `SYS_ID.Build[15:12]`
+
+ `0x1` corresponds to the presence of the default GICv2 memory map. This is
+ the default value.
+
+* `SYS_ID.Build[15:12]`
+
+ `0x0` corresponds to the presence of the Legacy VE GICv2 memory map. This
+ value can be configured as described in the next section.
+
+NOTE: If the legacy VE GICv2 memory map is used, then the corresponding FDT and
+UEFI images should be used.
+
+#### Configuring AEMv8 Base FVP for legacy VE memory map
+
+The following parameters configure the GICv2 memory map in legacy VE mode:
+
+NOTE: Using the `-C bp.virtioblockdevice.image_path` parameter is not necessary
+if a Linux RAM-disk file-system is used (see the "Obtaining a File-system"
+section above).
+
+ FVP_Base_AEMv8A-AEMv8A \
+ -C cluster0.gic.GICD-offset=0x1000 \
+ -C cluster0.gic.GICC-offset=0x2000 \
+ -C cluster0.gic.GICH-offset=0x4000 \
+ -C cluster0.gic.GICH-other-CPU-offset=0x5000 \
+ -C cluster0.gic.GICV-offset=0x6000 \
+ -C cluster0.gic.PERIPH-size=0x8000 \
+ -C cluster1.gic.GICD-offset=0x1000 \
+ -C cluster1.gic.GICC-offset=0x2000 \
+ -C cluster1.gic.GICH-offset=0x4000 \
+ -C cluster1.gic.GICH-other-CPU-offset=0x5000 \
+ -C cluster1.gic.GICV-offset=0x6000 \
+ -C cluster1.gic.PERIPH-size=0x8000 \
+ -C gic_distributor.GICD-alias=0x2c001000 \
+ -C bp.variant=0x0 \
+ -C bp.virtioblockdevice.image_path="<path/to/>vexpress64-openembedded_lamp-armv8_20130927-7.img"
+
+The last parameter sets the build variant field of the `SYS_ID` register to
+`0x0`. This allows the ARM Trusted Firmware to detect the legacy VE memory map
+while configuring the GIC.
+
+#### Configuring Cortex-A57-A53 Base FVP for legacy VE memory map
+
+Configuration of the GICv2 as per the legacy VE memory map is controlled by
+the following parameter. In this case, separate configuration of the `SYS_ID`
+register is not required.
+
+NOTE: Using the `-C bp.virtioblockdevice.image_path` parameter is not necessary
+if a Linux RAM-disk file-system is used (see the "Obtaining a File-system"
+section above).
+
+ FVP_Base_Cortex-A57x4-A53x4 \
+ -C legacy_gicv2_map=1 \
+ -C bp.virtioblockdevice.image_path="<path/to/>vexpress64-openembedded_lamp-armv8_20130927-7.img"
+
+3. Firmware Design
+-------------------
+
+The cold boot path starts when the platform is physically turned on. One of
+the CPUs released from reset is chosen as the primary CPU, and the remaining
+CPUs are considered secondary CPUs. The primary CPU is chosen through
+platform-specific means. The cold boot path is mainly executed by the primary
+CPU, other than essential CPU initialization executed by all CPUs. The
+secondary CPUs are kept in a safe platform-specific state until the primary
+CPU has performed enough initialization to boot them.
+
+The cold boot path in this implementation of the ARM Trusted Firmware is divided
+into three stages (in order of execution):
+
+* Boot Loader stage 1 (BL1)
+* Boot Loader stage 2 (BL2)
+* Boot Loader stage 3 (BL3-1). The '1' distinguishes this from other 3rd level
+ boot loader stages.
+
+The ARM Fixed Virtual Platforms (FVPs) provide trusted ROM, trusted SRAM and
+trusted DRAM regions. Each boot loader stage uses one or more of these
+memories for its code and data.
+
+
+### BL1
+
+This stage begins execution from the platform's reset vector in trusted ROM at
+EL3. BL1 code starts at `0x00000000` (trusted ROM) in the FVP memory map. The
+BL1 data section is placed at the start of trusted SRAM, `0x04000000`. The
+functionality implemented by this stage is as follows.
+
+#### Determination of boot path
+
+Whenever a CPU is released from reset, BL1 needs to distinguish between a warm
+boot and a cold boot. This is done using a platform-specific mechanism. The
+ARM FVPs implement a simple power controller at `0x1c100000`. The `PSYS`
+register (`0x10`) is used to distinguish between a cold and warm boot. This
+information is contained in the `PSYS.WK[25:24]` field. Additionally, a
+per-CPU mailbox is maintained in trusted DRAM (`0x00600000`), to which BL1
+writes an entrypoint. Each CPU jumps to this entrypoint upon warm boot. During
+cold boot, BL1 places the secondary CPUs in a safe platform-specific state while
+the primary CPU executes the remaining cold boot path as described in the
+following sections.
+
+#### Architectural initialization
+
+BL1 performs minimal architectural initialization as follows.
+
+* Exception vectors
+
+ BL1 sets up simple exception vectors for both synchronous and asynchronous
+ exceptions. The default behavior upon receiving an exception is to set a
+ status code. In the case of the FVP this code is written to the Versatile
+ Express System LED register in the following format:
+
+ SYS_LED[0] - Security state (Secure=0/Non-Secure=1)
+ SYS_LED[2:1] - Exception Level (EL3=0x3, EL2=0x2, EL1=0x1, EL0=0x0)
+ SYS_LED[7:3] - Exception Class (Sync/Async & origin). The values for
+ each exception class are:
+
+ 0x0 : Synchronous exception from Current EL with SP_EL0
+ 0x1 : IRQ exception from Current EL with SP_EL0
+ 0x2 : FIQ exception from Current EL with SP_EL0
+ 0x3 : System Error exception from Current EL with SP_EL0
+ 0x4 : Synchronous exception from Current EL with SP_ELx
+ 0x5 : IRQ exception from Current EL with SP_ELx
+ 0x6 : FIQ exception from Current EL with SP_ELx
+ 0x7 : System Error exception from Current EL with SP_ELx
+ 0x8 : Synchronous exception from Lower EL using aarch64
+ 0x9 : IRQ exception from Lower EL using aarch64
+ 0xa : FIQ exception from Lower EL using aarch64
+ 0xb : System Error exception from Lower EL using aarch64
+ 0xc : Synchronous exception from Lower EL using aarch32
+ 0xd : IRQ exception from Lower EL using aarch32
+ 0xe : FIQ exception from Lower EL using aarch32
+ 0xf : System Error exception from Lower EL using aarch32
+
+ A write to the LED register reflects in the System LEDs (S6LED0..7) in the
+ CLCD window of the FVP. This behavior is because this boot loader stage
+ does not expect to receive any exceptions other than the SMC exception.
+ For the latter, BL1 installs a simple stub. The stub expects to receive
+ only a single type of SMC (determined by its function ID in the general
+ purpose register `X0`). This SMC is raised by BL2 to make BL1 pass control
+ to BL3-1 (loaded by BL2) at EL3. Any other SMC leads to an assertion
+ failure.
+
+* MMU setup
+
+ BL1 sets up EL3 memory translation by creating page tables to cover the
+ first 4GB of physical address space. This covers all the memories and
+ peripherals needed by BL1.
+
+* Control register setup
+ - `SCTLR_EL3`. Instruction cache is enabled by setting the `SCTLR_EL3.I`
+ bit. Alignment and stack alignment checking is enabled by setting the
+ `SCTLR_EL3.A` and `SCTLR_EL3.SA` bits. Exception endianness is set to
+ little-endian by clearing the `SCTLR_EL3.EE` bit.
+
+ - `CPUECTLR`. When the FVP includes a model of a specific ARM processor
+ implementation (for example A57 or A53), then intra-cluster coherency is
+ enabled by setting the `CPUECTLR.SMPEN` bit. The AEMv8 Base FVP is
+ inherently coherent so does not implement `CPUECTLR`.
+
+ - `SCR`. Use of the HVC instruction from EL1 is enabled by setting the
+ `SCR.HCE` bit. FIQ exceptions are configured to be taken in EL3 by
+ setting the `SCR.FIQ` bit. The register width of the next lower
+ exception level is set to AArch64 by setting the `SCR.RW` bit.
+
+ - `CPTR_EL3`. Accesses to the `CPACR` from EL1 or EL2, or the `CPTR_EL2`
+ from EL2 are configured to not trap to EL3 by clearing the
+ `CPTR_EL3.TCPAC` bit. Instructions that access the registers associated
+ with Floating Point and Advanced SIMD execution are configured to not
+ trap to EL3 by clearing the `CPTR_EL3.TFP` bit.
+
+ - `CNTFRQ_EL0`. The `CNTFRQ_EL0` register is programmed with the base
+ frequency of the system counter, which is retrieved from the first entry
+ in the frequency modes table.
+
+ - Generic Timer. The system level implementation of the generic timer is
+ enabled through the memory mapped interface.
+
+#### Platform initialization
+
+BL1 enables issuing of snoop and DVM (Distributed Virtual Memory) requests from
+the CCI-400 slave interface corresponding to the cluster that includes the
+primary CPU. BL1 also initializes UART0 (PL011 console), which enables access to
+the `printf` family of functions.
+
+#### BL2 image load and execution
+
+BL1 execution continues as follows:
+
+1. BL1 determines the amount of free trusted SRAM memory available by
+ calculating the extent of its own data section, which also resides in
+ trusted SRAM. BL1 loads a BL2 raw binary image through semi-hosting, at a
+ platform-specific base address. The filename of the BL2 raw binary image on
+ the host file system must be `bl2.bin`. If the BL2 image file is not present
+ or if there is not enough free trusted SRAM the following error message
+ is printed:
+
+ "Failed to load boot loader stage 2 (BL2) firmware."
+
+ If the load is successful, BL1 updates the limits of the remaining free
+ trusted SRAM. It also populates information about the amount of trusted
+ SRAM used by the BL2 image. The exact load location of the image is
+ provided as a base address in the platform header. Further description of
+ the memory layout can be found later in this document.
+
+2. BL1 prints the following string from the primary CPU to indicate successful
+ execution of the BL1 stage:
+
+ "Booting trusted firmware boot loader stage 1"
+
+3. BL1 passes control to the BL2 image at Secure EL1, starting from its load
+ address.
+
+4. BL1 also passes information about the amount of trusted SRAM used and
+ available for use. This information is populated at a platform-specific
+ memory address.
+
+
+### BL2
+
+BL1 loads and passes control to BL2 at Secure EL1. BL2 is linked against and
+loaded at a platform-specific base address (more information can found later
+in this document). The functionality implemented by BL2 is as follows.
+
+#### Architectural initialization
+
+BL2 performs minimal architectural initialization required for subsequent
+stages of the ARM Trusted Firmware and normal world software. It sets up
+Secure EL1 memory translation by creating page tables to address the first 4GB
+of the physical address space in a similar way to BL1. EL1 and EL0 are given
+access to Floating Point & Advanced SIMD registers by clearing the `CPACR.FPEN`
+bits.
+
+#### Platform initialization
+
+BL2 does not perform any platform initialization that affects subsequent
+stages of the ARM Trusted Firmware or normal world software. It copies the
+information regarding the trusted SRAM populated by BL1 using a
+platform-specific mechanism. It also calculates the limits of DRAM (main memory)
+to determine whether there is enough space to load the normal world software
+images. A platform defined base address is used to specify the load address for
+the BL3-1 image.
+
+#### Normal world image load
+
+BL2 loads a rich boot firmware image (UEFI). The image executes in the normal
+world. BL2 relies on BL3-1 to pass control to the normal world software image it
+loads. Hence, BL2 populates a platform-specific area of memory with the
+entrypoint and Current Program Status Register (`CPSR`) of the normal world
+software image. The entrypoint is the load address of the normal world software
+image. The `CPSR` is determined as specified in Section 5.13 of the [PSCI PDD]
+[PSCI]. This information is passed to BL3-1.
+
+##### UEFI firmware load
+
+By default, BL2 assumes the UEFI image is present at the base of NOR flash0
+(`0x08000000`), and arranges for BL3-1 to pass control to that location. As
+mentioned earlier, BL2 populates platform-specific memory with the entrypoint
+and `CPSR` of the UEFI image.
+
+#### BL3-1 image load and execution
+
+BL2 execution continues as follows:
+
+1. BL2 loads the BL3-1 image into a platform-specific address in trusted SRAM.
+ This is done using semi-hosting. The image is identified by the file
+ `bl31.bin` on the host file-system. If there is not enough memory to load
+ the image or the image is missing it leads to an assertion failure. If the
+ BL3-1 image loads successfully, BL1 updates the amount of trusted SRAM used
+ and available for use by BL3-1. This information is populated at a
+ platform-specific memory address.
+
+2. BL2 passes control back to BL1 by raising an SMC, providing BL1 with the
+ BL3-1 entrypoint. The exception is handled by the SMC exception handler
+ installed by BL1.
+
+3. BL1 turns off the MMU and flushes the caches. It clears the
+ `SCTLR_EL3.M/I/C` bits, flushes the data cache to the point of coherency
+ and invalidates the TLBs.
+
+4. BL1 passes control to BL3-1 at the specified entrypoint at EL3.
+
+
+### BL3-1
+
+The image for this stage is loaded by BL2 and BL1 passes control to BL3-1 at
+EL3. BL3-1 executes solely in trusted SRAM. BL3-1 is linked against and
+loaded at a platform-specific base address (more information can found later
+in this document). The functionality implemented by BL3-1 is as follows.
+
+#### Architectural initialization
+
+Currently, BL3-1 performs a similar architectural initialization to BL1 as
+far as system register settings are concerned. Since BL1 code resides in ROM,
+architectural initialization in BL3-1 allows override of any previous
+initialization done by BL1. BL3-1 creates page tables to address the first
+4GB of physical address space and initializes the MMU accordingly. It replaces
+the exception vectors populated by BL1 with its own. BL3-1 exception vectors
+signal error conditions in the same way as BL1 does if an unexpected
+exception is raised. They implement more elaborate support for handling SMCs
+since this is the only mechanism to access the runtime services implemented by
+BL3-1 (PSCI for example). BL3-1 checks each SMC for validity as specified by
+the [SMC calling convention PDD][SMCCC] before passing control to the required
+SMC handler routine.
+
+#### Platform initialization
+
+BL3-1 performs detailed platform initialization, which enables normal world
+software to function correctly. It also retrieves entrypoint information for
+the normal world software image loaded by BL2 from the platform defined
+memory address populated by BL2.
+
+* GICv2 initialization:
+
+ - Enable group0 interrupts in the GIC CPU interface.
+ - Configure group0 interrupts to be asserted as FIQs.
+ - Disable the legacy interrupt bypass mechanism.
+ - Configure the priority mask register to allow interrupts of all
+ priorities to be signaled to the CPU interface.
+ - Mark SGIs 8-15, the secure physical timer interrupt (#29) and the
+ trusted watchdog interrupt (#56) as group0 (secure).
+ - Target the trusted watchdog interrupt to CPU0.
+ - Enable these group0 interrupts in the GIC distributor.
+ - Configure all other interrupts as group1 (non-secure).
+ - Enable signaling of group0 interrupts in the GIC distributor.
+
+* GICv3 initialization:
+
+ If a GICv3 implementation is available in the platform, BL3-1 initializes
+ the GICv3 in GICv2 emulation mode with settings as described for GICv2
+ above.
+
+* Power management initialization:
+
+ BL3-1 implements a state machine to track CPU and cluster state. The state
+ can be one of `OFF`, `ON_PENDING`, `SUSPEND` or `ON`. All secondary CPUs are
+ initially in the `OFF` state. The cluster that the primary CPU belongs to is
+ `ON`; any other cluster is `OFF`. BL3-1 initializes the data structures that
+ implement the state machine, including the locks that protect them. BL3-1
+ accesses the state of a CPU or cluster immediately after reset and before
+ the MMU is enabled in the warm boot path. It is not currently possible to
+ use 'exclusive' based spinlocks, therefore BL3-1 uses locks based on
+ Lamport's Bakery algorithm instead. BL3-1 allocates these locks in device
+ memory. They are accessible irrespective of MMU state.
+
+* Runtime services initialization:
+
+ The only runtime service implemented by BL3-1 is PSCI. The complete PSCI API
+ is not yet implemented. The following functions are currently implemented:
+
+ - `PSCI_VERSION`
+ - `CPU_OFF`
+ - `CPU_ON`
+ - `AFFINITY_INFO`
+
+ The `CPU_ON` and `CPU_OFF` functions implement the warm boot path in ARM
+ Trusted Firmware. These are the only functions which have been tested.
+ `AFFINITY_INFO` & `PSCI_VERSION` are present but completely untested in
+ this release.
+
+ Unsupported PSCI functions that can return, return the `NOT_SUPPORTED`
+ (`-1`) error code. Other unsupported PSCI functions that don't return,
+ signal an assertion failure.
+
+ BL3-1 returns the error code `-1` if an SMC is raised for any other runtime
+ service. This behavior is mandated by the [SMC calling convention PDD]
+ [SMCCC].
+
+
+### Normal world software execution
+
+BL3-1 uses the entrypoint information provided by BL2 to jump to the normal
+world software image at the highest available Exception Level (EL2 if
+available, otherwise EL1).
+
+
+### Memory layout on Base FVP ###
+
+The current implementation of the image loader has some limitations. It is
+designed to load images dynamically, at a load address chosen to minimize memory
+fragmentation. The chosen image location can be either at the top or the bottom
+of free memory. However, until this feature is fully functional, the code also
+contains support for loading images at a link-time fixed address. The code that
+dynamically calculates the load address is bypassed and the load address is
+specified statically by the platform.
+
+BL1 is always loaded at address `0x0`. BL2 and BL3-1 are loaded at specified
+locations in Trusted SRAM. The lack of dynamic image loader support means these
+load addresses must currently be adjusted as the code grows. The individual
+images must be linked against their ultimate runtime locations.
+
+BL2 is loaded near the top of the Trusted SRAM. BL3-1 is loaded between BL1
+and BL2. As a general rule, the following constraints must always be enforced:
+
+1. `BL2_MAX_ADDR <= (<Top of Trusted SRAM>)`
+2. `BL31_BASE >= BL1_MAX_ADDR`
+3. `BL2_BASE >= BL31_MAX_ADDR`
+
+Constraint 1 is enforced by BL2's linker script. If it is violated then the
+linker will report an error while building BL2 to indicate that it doesn't
+fit. For example:
+
+ aarch64-none-elf-ld: address 0x40400c8 of bl2.elf section `.bss' is not
+ within region `RAM'
+
+This error means that the BL2 base address needs to be moved down. Be sure that
+the new BL2 load address still obeys constraint 3.
+
+Constraints 2 & 3 must currently be checked by hand. To ensure they are
+enforced, first determine the maximum addresses used by BL1 and BL3-1. This can
+be deduced from the link map files of the different images.
+
+The BL1 link map file (`bl1.map`) gives these 2 values:
+
+* `FIRMWARE_RAM_COHERENT_START`
+* `FIRMWARE_RAM_COHERENT_SIZE`
+
+The maximum address used by BL1 can then be easily determined:
+
+ BL1_MAX_ADDR = FIRMWARE_RAM_COHERENT_START + FIRMWARE_RAM_COHERENT_SIZE
+
+The BL3-1 link map file (`bl31.map`) gives the following value:
+
+* `BL31_DATA_STOP`. This is the the maximum address used by BL3-1.
+
+The current implementation can result in wasted space because a simplified
+`meminfo` structure represents the extents of free memory. For example, to load
+BL2 at address `0x04020000`, the resulting memory layout should be as follows:
+
+ ------------ 0x04040000
+ | | <- Free space (1)
+ |----------|
+ | BL2 |
+ |----------| BL2_BASE (0x0402D000)
+ | | <- Free space (2)
+ |----------|
+ | BL1 |
+ ------------ 0x04000000
+
+In the current implementation, we need to specify whether BL2 is loaded at the
+top or bottom of the free memory. BL2 is top-loaded so in the example above,
+the free space (1) above BL2 is hidden, resulting in the following view of
+memory:
+
+ ------------ 0x04040000
+ | |
+ | |
+ | BL2 |
+ |----------| BL2_BASE (0x0402D000)
+ | | <- Free space (2)
+ |----------|
+ | BL1 |
+ ------------ 0x04000000
+
+BL3-1 is bottom-loaded above BL1. For example, if BL3-1 is bottom-loaded at
+`0x0400E000`, the memory layout should look like this:
+
+ ------------ 0x04040000
+ | |
+ | |
+ | BL2 |
+ |----------| BL2_BASE (0x0402D000)
+ | | <- Free space (2)
+ | |
+ |----------|
+ | |
+ | BL31 |
+ |----------| BL31_BASE (0x0400E000)
+ | | <- Free space (3)
+ |----------|
+ | BL1 |
+ ------------ 0x04000000
+
+But the free space (3) between BL1 and BL3-1 is wasted, resulting in the
+following view:
+
+ ------------ 0x04040000
+ | |
+ | |
+ | BL2 |
+ |----------| BL2_BASE (0x0402D000)
+ | | <- Free space (2)
+ | |
+ |----------|
+ | |
+ | |
+ | BL31 | BL31_BASE (0x0400E000)
+ | |
+ |----------|
+ | BL1 |
+ ------------ 0x04000000
+
+
+### Code Structure ###
+
+Trusted Firmware code is logically divided between the three boot loader
+stages mentioned in the previous sections. The code is also divided into the
+following categories (present as directories in the source code):
+
+* **Architecture specific.** This could be AArch32 or AArch64.
+* **Platform specific.** Choice of architecture specific code depends upon
+ the platform.
+* **Common code.** This is platform and architecture agnostic code.
+* **Library code.** This code comprises of functionality commonly used by all
+ other code.
+* **Stage specific.** Code specific to a boot stage.
+* **Drivers.**
+
+Each boot loader stage uses code from one or more of the above mentioned
+categories. Based upon the above, the code layout looks like this:
+
+ Directory Used by BL1? Used by BL2? Used by BL3?
+ bl1 Yes No No
+ bl2 No Yes No
+ bl31 No No Yes
+ arch Yes Yes Yes
+ plat Yes Yes Yes
+ drivers Yes No Yes
+ common Yes Yes Yes
+ lib Yes Yes Yes
+
+All assembler files have the `.S` extension. The linker files for each boot
+stage has the `.ld.S` extension. These are processed by GCC to create the
+resultant `.ld` files used for linking.
+
+FDTs provide a description of the hardware platform and is used by the Linux
+kernel at boot time. These can be found in the `fdts` directory.
+
+
+4. References
+--------------
+
+1. Trusted Board Boot Requirements CLIENT PDD (ARM DEN 0006B-5). Available
+ under NDA through your ARM account representative.
+
+2. [Power State Coordination Interface PDD (ARM DEN 0022B.b)][PSCI].
+
+3. [SMC Calling Convention PDD (ARM DEN 0028A)][SMCCC].
+
+
+- - - - - - - - - - - - - - - - - - - - - - - - - -
+
+_Copyright (c) 2013 ARM Ltd. All rights reserved._
+
+
+[Change Log]: change-log.md
+
+[Linaro Toolchain]: http://releases.linaro.org/13.09/components/toolchain/binaries/
+[EDK2]: http://sourceforge.net/projects/edk2/files/ARM/aarch64-uefi-rev14582.tgz/download
+[DS-5]: http://www.arm.com/products/tools/software-tools/ds-5/index.php
+[PSCI]: http://infocenter.arm.com/help/topic/com.arm.doc.den0022b/index.html "Power State Coordination Interface PDD (ARM DEN 0022B.b)"
+[SMCCC]: http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html "SMC Calling Convention PDD (ARM DEN 0028A)"
diff --git a/drivers/arm/interconnect/cci-400/cci400.c b/drivers/arm/interconnect/cci-400/cci400.c
new file mode 100644
index 0000000..60586ab
--- /dev/null
+++ b/drivers/arm/interconnect/cci-400/cci400.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <assert.h>
+#include <platform.h>
+#include <cci400.h>
+
+static inline unsigned long get_slave_iface_base(unsigned long mpidr)
+{
+ return CCI400_BASE + SLAVE_IFACE_OFFSET(CCI400_SL_IFACE_INDEX(mpidr));
+}
+
+void cci_enable_coherency(unsigned long mpidr)
+{
+ /* Enable Snoops and DVM messages */
+ mmio_write_32(get_slave_iface_base(mpidr) + SNOOP_CTRL_REG,
+ DVM_EN_BIT | SNOOP_EN_BIT);
+
+ /* Wait for the dust to settle down */
+ while (mmio_read_32(CCI400_BASE + STATUS_REG) & CHANGE_PENDING_BIT);
+}
+
+void cci_disable_coherency(unsigned long mpidr)
+{
+ /* Disable Snoops and DVM messages */
+ mmio_write_32(get_slave_iface_base(mpidr) + SNOOP_CTRL_REG,
+ ~(DVM_EN_BIT | SNOOP_EN_BIT));
+
+ /* Wait for the dust to settle down */
+ while (mmio_read_32(CCI400_BASE + STATUS_REG) & CHANGE_PENDING_BIT);
+}
+
diff --git a/drivers/arm/interconnect/cci-400/cci400.h b/drivers/arm/interconnect/cci-400/cci400.h
new file mode 100644
index 0000000..62e2fbb
--- /dev/null
+++ b/drivers/arm/interconnect/cci-400/cci400.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __CCI_400_H__
+#define __CCI_400_H__
+
+/* Slave interface offsets from PERIPHBASE */
+#define SLAVE_IFACE4_OFFSET 0x5000
+#define SLAVE_IFACE3_OFFSET 0x4000
+#define SLAVE_IFACE2_OFFSET 0x3000
+#define SLAVE_IFACE1_OFFSET 0x2000
+#define SLAVE_IFACE0_OFFSET 0x1000
+#define SLAVE_IFACE_OFFSET(index) SLAVE_IFACE0_OFFSET + (0x1000 * index)
+
+/* Control and ID register offsets */
+#define CTRL_OVERRIDE_REG 0x0
+#define SPEC_CTRL_REG 0x4
+#define SECURE_ACCESS_REG 0x8
+#define STATUS_REG 0xc
+#define IMPRECISE_ERR_REG 0x10
+#define PERFMON_CTRL_REG 0x100
+
+/* Slave interface register offsets */
+#define SNOOP_CTRL_REG 0x0
+#define SH_OVERRIDE_REG 0x4
+#define READ_CHNL_QOS_VAL_OVERRIDE_REG 0x100
+#define WRITE_CHNL_QOS_VAL_OVERRIDE_REG 0x104
+#define QOS_CTRL_REG 0x10c
+#define MAX_OT_REG 0x110
+#define TARGET_LATENCY_REG 0x130
+#define LATENCY_REGULATION_REG 0x134
+#define QOS_RANGE_REG 0x138
+
+/* Snoop Control register bit definitions */
+#define DVM_EN_BIT (1 << 1)
+#define SNOOP_EN_BIT (1 << 0)
+
+/* Status register bit definitions */
+#define CHANGE_PENDING_BIT (1 << 0)
+
+/* Function declarations */
+extern void cci_enable_coherency(unsigned long mpidr);
+extern void cci_disable_coherency(unsigned long mpidr);
+
+#endif /* __CCI_400_H__ */
diff --git a/drivers/arm/peripherals/pl011/console.h b/drivers/arm/peripherals/pl011/console.h
new file mode 100644
index 0000000..b98db61
--- /dev/null
+++ b/drivers/arm/peripherals/pl011/console.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __CONSOLE_H__
+#define __CONSOLE_H__
+
+void console_init(void);
+int console_putc(int c);
+int console_getc(void);
+
+#endif /* __CONSOLE_H__ */
+
diff --git a/drivers/arm/peripherals/pl011/pl011.c b/drivers/arm/peripherals/pl011/pl011.c
new file mode 100644
index 0000000..2f6f5ea
--- /dev/null
+++ b/drivers/arm/peripherals/pl011/pl011.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <console.h>
+#include <platform.h>
+#include <pl011.h>
+
+/*
+ * TODO: Console init functions shoule be in a console.c. This file should
+ * only contain the pl011 accessors.
+ */
+void console_init(void)
+{
+ unsigned int divisor;
+
+ /* Baud Rate */
+
+#if defined(PL011_INTEGER) && defined(PL011_FRACTIONAL)
+ mmio_write_32(PL011_BASE + UARTIBRD, PL011_INTEGER);
+ mmio_write_32(PL011_BASE + UARTFBRD, PL011_FRACTIONAL);
+#else
+ divisor = (PL011_CLK_IN_HZ * 4) / PL011_BAUDRATE;
+ mmio_write_32(PL011_BASE + UARTIBRD, divisor >> 6);
+ mmio_write_32(PL011_BASE + UARTFBRD, divisor & 0x3F);
+#endif
+
+
+ mmio_write_32(PL011_BASE + UARTLCR_H, PL011_LINE_CONTROL);
+
+ /* Clear any pending errors */
+ mmio_write_32(PL011_BASE + UARTECR, 0);
+
+ /* Enable tx, rx, and uart overall */
+ mmio_write_32(PL011_BASE + UARTCR,
+ PL011_UARTCR_RXE | PL011_UARTCR_TXE |
+ PL011_UARTCR_UARTEN);
+}
+
+int console_putc(int c)
+{
+ if (c == '\n') {
+ console_putc('\r');
+ }
+ while ((mmio_read_32(PL011_BASE + UARTFR) & PL011_UARTFR_TXFE)
+ == 0) ;
+ mmio_write_32(PL011_BASE + UARTDR, c);
+ return c;
+}
+
+int console_getc(void)
+{
+ while ((mmio_read_32(PL011_BASE + UARTFR) & PL011_UARTFR_RXFE)
+ != 0) ;
+ return mmio_read_32(PL011_BASE + UARTDR);
+}
diff --git a/drivers/arm/peripherals/pl011/pl011.h b/drivers/arm/peripherals/pl011/pl011.h
new file mode 100644
index 0000000..53b4dab
--- /dev/null
+++ b/drivers/arm/peripherals/pl011/pl011.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __PL011_H__
+#define __PL011_H__
+
+/* PL011 Registers */
+#define UARTDR 0x000
+#define UARTRSR 0x004
+#define UARTECR 0x004
+#define UARTFR 0x018
+#define UARTILPR 0x020
+#define UARTIBRD 0x024
+#define UARTFBRD 0x028
+#define UARTLCR_H 0x02C
+#define UARTCR 0x030
+#define UARTIFLS 0x034
+#define UARTIMSC 0x038
+#define UARTRIS 0x03C
+#define UARTMIS 0x040
+#define UARTICR 0x044
+#define UARTDMACR 0x048
+
+/* Data status bits */
+#define UART_DATA_ERROR_MASK 0x0F00
+
+/* Status reg bits */
+#define UART_STATUS_ERROR_MASK 0x0F
+
+/* Flag reg bits */
+#define PL011_UARTFR_RI (1 << 8) /* Ring indicator */
+#define PL011_UARTFR_TXFE (1 << 7) /* Transmit FIFO empty */
+#define PL011_UARTFR_RXFF (1 << 6) /* Receive FIFO full */
+#define PL011_UARTFR_TXFF (1 << 5) /* Transmit FIFO full */
+#define PL011_UARTFR_RXFE (1 << 4) /* Receive FIFO empty */
+#define PL011_UARTFR_BUSY (1 << 3) /* UART busy */
+#define PL011_UARTFR_DCD (1 << 2) /* Data carrier detect */
+#define PL011_UARTFR_DSR (1 << 1) /* Data set ready */
+#define PL011_UARTFR_CTS (1 << 0) /* Clear to send */
+
+/* Control reg bits */
+#define PL011_UARTCR_CTSEN (1 << 15) /* CTS hardware flow control enable */
+#define PL011_UARTCR_RTSEN (1 << 14) /* RTS hardware flow control enable */
+#define PL011_UARTCR_RTS (1 << 11) /* Request to send */
+#define PL011_UARTCR_DTR (1 << 10) /* Data transmit ready. */
+#define PL011_UARTCR_RXE (1 << 9) /* Receive enable */
+#define PL011_UARTCR_TXE (1 << 8) /* Transmit enable */
+#define PL011_UARTCR_LBE (1 << 7) /* Loopback enable */
+#define PL011_UARTCR_UARTEN (1 << 0) /* UART Enable */
+
+#if !defined(PL011_BASE)
+#error "The PL011_BASE macro must be defined."
+#endif
+
+#if !defined(PL011_BAUDRATE)
+#define PL011_BAUDRATE 115200
+#endif
+
+#if !defined(PL011_CLK_IN_HZ)
+#define PL011_CLK_IN_HZ 24000000
+#endif
+
+#if !defined(PL011_LINE_CONTROL)
+/* FIFO Enabled / No Parity / 8 Data bit / One Stop Bit */
+#define PL011_LINE_CONTROL (PL011_UARTLCR_H_FEN | PL011_UARTLCR_H_WLEN_8)
+#endif
+
+/* Line Control Register Bits */
+#define PL011_UARTLCR_H_SPS (1 << 7) /* Stick parity select */
+#define PL011_UARTLCR_H_WLEN_8 (3 << 5)
+#define PL011_UARTLCR_H_WLEN_7 (2 << 5)
+#define PL011_UARTLCR_H_WLEN_6 (1 << 5)
+#define PL011_UARTLCR_H_WLEN_5 (0 << 5)
+#define PL011_UARTLCR_H_FEN (1 << 4) /* FIFOs Enable */
+#define PL011_UARTLCR_H_STP2 (1 << 3) /* Two stop bits select */
+#define PL011_UARTLCR_H_EPS (1 << 2) /* Even parity select */
+#define PL011_UARTLCR_H_PEN (1 << 1) /* Parity Enable */
+#define PL011_UARTLCR_H_BRK (1 << 0) /* Send break */
+
+#endif /* __PL011_H__ */
diff --git a/drivers/power/fvp_pwrc.c b/drivers/power/fvp_pwrc.c
new file mode 100644
index 0000000..c7db33b
--- /dev/null
+++ b/drivers/power/fvp_pwrc.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <fvp_pwrc.h>
+#include <bakery_lock.h>
+
+/*
+ * TODO: Someday there will be a generic power controller api. At the moment
+ * each platform has its own pwrc so just exporting functions is fine.
+ */
+static bakery_lock pwrc_lock __attribute__ ((section("tzfw_coherent_mem")));
+
+unsigned int fvp_pwrc_get_cpu_wkr(unsigned long mpidr)
+{
+ unsigned int rc = 0;
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PSYSR_OFF, (unsigned int) mpidr);
+ rc = PSYSR_WK(mmio_read_32(PWRC_BASE + PSYSR_OFF));
+ bakery_lock_release(mpidr, &pwrc_lock);
+ return rc;
+}
+
+unsigned int fvp_pwrc_read_psysr(unsigned long mpidr)
+{
+ unsigned int rc = 0;
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PSYSR_OFF, (unsigned int) mpidr);
+ rc = mmio_read_32(PWRC_BASE + PSYSR_OFF);
+ bakery_lock_release(mpidr, &pwrc_lock);
+ return rc;
+}
+
+void fvp_pwrc_write_pponr(unsigned long mpidr)
+{
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PPONR_OFF, (unsigned int) mpidr);
+ bakery_lock_release(mpidr, &pwrc_lock);
+}
+
+void fvp_pwrc_write_ppoffr(unsigned long mpidr)
+{
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PPOFFR_OFF, (unsigned int) mpidr);
+ bakery_lock_release(mpidr, &pwrc_lock);
+}
+
+void fvp_pwrc_write_pwkupr(unsigned long mpidr)
+{
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PWKUPR_OFF,
+ (unsigned int) (PWKUPR_WEN | mpidr));
+ bakery_lock_release(mpidr, &pwrc_lock);
+}
+
+void fvp_pwrc_write_pcoffr(unsigned long mpidr)
+{
+ bakery_lock_get(mpidr, &pwrc_lock);
+ mmio_write_32(PWRC_BASE + PCOFFR_OFF, (unsigned int) mpidr);
+ bakery_lock_release(mpidr, &pwrc_lock);
+}
+
+/* Nothing else to do here apart from initializing the lock */
+int fvp_pwrc_setup(void)
+{
+ bakery_lock_init(&pwrc_lock);
+ return 0;
+}
+
+
+
diff --git a/drivers/power/fvp_pwrc.h b/drivers/power/fvp_pwrc.h
new file mode 100644
index 0000000..a2efcc5
--- /dev/null
+++ b/drivers/power/fvp_pwrc.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __FVP_PWRC_H__
+#define __FVP_PWRC_H__
+
+/* FVP Power controller register offset etc */
+#define PPOFFR_OFF 0x0
+#define PPONR_OFF 0x4
+#define PCOFFR_OFF 0x8
+#define PWKUPR_OFF 0xc
+#define PSYSR_OFF 0x10
+
+#define PWKUPR_WEN (1ull << 31)
+
+#define PSYSR_AFF_L2 (1 << 31)
+#define PSYSR_AFF_L1 (1 << 30)
+#define PSYSR_AFF_L0 (1 << 29)
+#define PSYSR_WEN (1 << 28)
+#define PSYSR_PC (1 << 27)
+#define PSYSR_PP (1 << 26)
+
+#define PSYSR_WK_SHIFT 24
+#define PSYSR_WK_MASK 0x3
+#define PSYSR_WK(x) (x >> PSYSR_WK_SHIFT) & PSYSR_WK_MASK
+
+#define WKUP_COLD 0x0
+#define WKUP_RESET 0x1
+#define WKUP_PPONR 0x2
+#define WKUP_GICREQ 0x3
+
+#define PSYSR_INVALID 0xffffffff
+
+#ifndef __ASSEMBLY__
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+extern int fvp_pwrc_setup(void);
+extern void fvp_pwrc_write_pcoffr(unsigned long);
+extern void fvp_pwrc_write_ppoffr(unsigned long);
+extern void fvp_pwrc_write_pponr(unsigned long);
+extern void fvp_pwrc_write_pwkupr(unsigned long);
+extern unsigned int fvp_pwrc_read_psysr(unsigned long);
+extern unsigned int fvp_pwrc_get_cpu_wkr(unsigned long);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __FVP_PWRC_H__ */
diff --git a/fdts/fvp-base-gicv2-psci.dtb b/fdts/fvp-base-gicv2-psci.dtb
new file mode 100644
index 0000000..bfb2710
--- /dev/null
+++ b/fdts/fvp-base-gicv2-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv2-psci.dts b/fdts/fvp-base-gicv2-psci.dts
new file mode 100644
index 0000000..7aa18a5
--- /dev/null
+++ b/fdts/fvp-base-gicv2-psci.dts
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/ {
+};
+
+/ {
+ model = "FVP Base";
+ compatible = "arm,vfp-base", "arm,vexpress";
+ interrupt-parent = <&gic>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+
+ chosen { };
+
+ aliases {
+ serial0 = &v2m_serial0;
+ serial1 = &v2m_serial1;
+ serial2 = &v2m_serial2;
+ serial3 = &v2m_serial3;
+ };
+
+ psci {
+ compatible = "arm,psci";
+ method = "smc";
+ cpu_suspend = <0xc4000001>;
+ cpu_off = <0x84000002>;
+ cpu_on = <0xc4000003>;
+ };
+
+ cpus {
+ #address-cells = <2>;
+ #size-cells = <0>;
+
+ cpu@0 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x0>;
+ enable-method = "psci";
+ };
+ cpu@1 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x1>;
+ enable-method = "psci";
+ };
+ cpu@2 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x2>;
+ enable-method = "psci";
+ };
+ cpu@3 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x3>;
+ enable-method = "psci";
+ };
+ cpu@100 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x100>;
+ enable-method = "psci";
+ };
+ cpu@101 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x101>;
+ enable-method = "psci";
+ };
+ cpu@102 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x102>;
+ enable-method = "psci";
+ };
+ cpu@103 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x103>;
+ enable-method = "psci";
+ };
+ };
+
+ memory@80000000 {
+ device_type = "memory";
+ reg = <0x00000000 0x80000000 0 0x80000000>;
+ /*
+ <0x00000008 0x80000000 0 0x80000000>;
+ */
+ };
+
+ gic: interrupt-controller@2f000000 {
+ compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+ #interrupt-cells = <3>;
+ #address-cells = <0>;
+ interrupt-controller;
+ reg = <0x0 0x2f000000 0 0x10000>,
+ <0x0 0x2c000000 0 0x2000>,
+ <0x0 0x2c010000 0 0x2000>,
+ <0x0 0x2c02F000 0 0x2000>;
+ interrupts = <1 9 0xf04>;
+ };
+
+ timer {
+ compatible = "arm,armv8-timer";
+ interrupts = <1 13 0xff01>,
+ <1 14 0xff01>,
+ <1 11 0xff01>,
+ <1 10 0xff01>;
+ clock-frequency = <100000000>;
+ };
+
+ timer@2a810000 {
+ compatible = "arm,armv7-timer-mem";
+ reg = <0x0 0x2a810000 0x0 0x10000>;
+ clock-frequency = <100000000>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+ ranges;
+ frame@2a820000 {
+ frame-number = <0>;
+ interrupts = <0 25 4>;
+ reg = <0x0 0x2a820000 0x0 0x10000>;
+ };
+ };
+
+ pmu {
+ compatible = "arm,armv8-pmuv3";
+ interrupts = <0 60 4>,
+ <0 61 4>,
+ <0 62 4>,
+ <0 63 4>;
+ };
+
+ smb {
+ compatible = "simple-bus";
+
+ #address-cells = <2>;
+ #size-cells = <1>;
+ ranges = <0 0 0 0x08000000 0x04000000>,
+ <1 0 0 0x14000000 0x04000000>,
+ <2 0 0 0x18000000 0x04000000>,
+ <3 0 0 0x1c000000 0x04000000>,
+ <4 0 0 0x0c000000 0x04000000>,
+ <5 0 0 0x10000000 0x04000000>;
+
+ #interrupt-cells = <1>;
+ interrupt-map-mask = <0 0 63>;
+ interrupt-map = <0 0 0 &gic 0 0 4>,
+ <0 0 1 &gic 0 1 4>,
+ <0 0 2 &gic 0 2 4>,
+ <0 0 3 &gic 0 3 4>,
+ <0 0 4 &gic 0 4 4>,
+ <0 0 5 &gic 0 5 4>,
+ <0 0 6 &gic 0 6 4>,
+ <0 0 7 &gic 0 7 4>,
+ <0 0 8 &gic 0 8 4>,
+ <0 0 9 &gic 0 9 4>,
+ <0 0 10 &gic 0 10 4>,
+ <0 0 11 &gic 0 11 4>,
+ <0 0 12 &gic 0 12 4>,
+ <0 0 13 &gic 0 13 4>,
+ <0 0 14 &gic 0 14 4>,
+ <0 0 15 &gic 0 15 4>,
+ <0 0 16 &gic 0 16 4>,
+ <0 0 17 &gic 0 17 4>,
+ <0 0 18 &gic 0 18 4>,
+ <0 0 19 &gic 0 19 4>,
+ <0 0 20 &gic 0 20 4>,
+ <0 0 21 &gic 0 21 4>,
+ <0 0 22 &gic 0 22 4>,
+ <0 0 23 &gic 0 23 4>,
+ <0 0 24 &gic 0 24 4>,
+ <0 0 25 &gic 0 25 4>,
+ <0 0 26 &gic 0 26 4>,
+ <0 0 27 &gic 0 27 4>,
+ <0 0 28 &gic 0 28 4>,
+ <0 0 29 &gic 0 29 4>,
+ <0 0 30 &gic 0 30 4>,
+ <0 0 31 &gic 0 31 4>,
+ <0 0 32 &gic 0 32 4>,
+ <0 0 33 &gic 0 33 4>,
+ <0 0 34 &gic 0 34 4>,
+ <0 0 35 &gic 0 35 4>,
+ <0 0 36 &gic 0 36 4>,
+ <0 0 37 &gic 0 37 4>,
+ <0 0 38 &gic 0 38 4>,
+ <0 0 39 &gic 0 39 4>,
+ <0 0 40 &gic 0 40 4>,
+ <0 0 41 &gic 0 41 4>,
+ <0 0 42 &gic 0 42 4>;
+
+ /include/ "rtsm_ve-motherboard.dtsi"
+ };
+
+ panels {
+ panel@0 {
+ compatible = "panel";
+ mode = "XVGA";
+ refresh = <60>;
+ xres = <1024>;
+ yres = <768>;
+ pixclock = <15748>;
+ left_margin = <152>;
+ right_margin = <48>;
+ upper_margin = <23>;
+ lower_margin = <3>;
+ hsync_len = <104>;
+ vsync_len = <4>;
+ sync = <0>;
+ vmode = "FB_VMODE_NONINTERLACED";
+ tim2 = "TIM2_BCD", "TIM2_IPC";
+ cntl = "CNTL_LCDTFT", "CNTL_BGR", "CNTL_LCDVCOMP(1)";
+ caps = "CLCD_CAP_5551", "CLCD_CAP_565", "CLCD_CAP_888";
+ bpp = <16>;
+ };
+ };
+};
diff --git a/fdts/fvp-base-gicv2legacy-psci.dtb b/fdts/fvp-base-gicv2legacy-psci.dtb
new file mode 100644
index 0000000..227c161
--- /dev/null
+++ b/fdts/fvp-base-gicv2legacy-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv2legacy-psci.dts b/fdts/fvp-base-gicv2legacy-psci.dts
new file mode 100644
index 0000000..340ae50
--- /dev/null
+++ b/fdts/fvp-base-gicv2legacy-psci.dts
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/ {
+};
+
+/ {
+ model = "FVP Base";
+ compatible = "arm,vfp-base", "arm,vexpress";
+ interrupt-parent = <&gic>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+
+ chosen { };
+
+ aliases {
+ serial0 = &v2m_serial0;
+ serial1 = &v2m_serial1;
+ serial2 = &v2m_serial2;
+ serial3 = &v2m_serial3;
+ };
+
+ psci {
+ compatible = "arm,psci";
+ method = "smc";
+ cpu_suspend = <0xc4000001>;
+ cpu_off = <0x84000002>;
+ cpu_on = <0xc4000003>;
+ };
+
+ cpus {
+ #address-cells = <2>;
+ #size-cells = <0>;
+
+ cpu@0 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x0>;
+ enable-method = "psci";
+ };
+ cpu@1 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x1>;
+ enable-method = "psci";
+ };
+ cpu@2 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x2>;
+ enable-method = "psci";
+ };
+ cpu@3 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x3>;
+ enable-method = "psci";
+ };
+ cpu@100 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x100>;
+ enable-method = "psci";
+ };
+ cpu@101 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x101>;
+ enable-method = "psci";
+ };
+ cpu@102 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x102>;
+ enable-method = "psci";
+ };
+ cpu@103 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x103>;
+ enable-method = "psci";
+ };
+ };
+
+ memory@80000000 {
+ device_type = "memory";
+ reg = <0x00000000 0x80000000 0 0x80000000>;
+ /*
+ <0x00000008 0x80000000 0 0x80000000>;
+ */
+ };
+
+ gic: interrupt-controller@2c001000 {
+ compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+ #interrupt-cells = <3>;
+ #address-cells = <0>;
+ interrupt-controller;
+ reg = <0x0 0x2c001000 0 0x1000>,
+ <0x0 0x2c002000 0 0x1000>,
+ <0x0 0x2c004000 0 0x2000>,
+ <0x0 0x2c006000 0 0x2000>;
+ interrupts = <1 9 0xf04>;
+ };
+
+ timer {
+ compatible = "arm,armv8-timer";
+ interrupts = <1 13 0xff01>,
+ <1 14 0xff01>,
+ <1 11 0xff01>,
+ <1 10 0xff01>;
+ clock-frequency = <100000000>;
+ };
+
+ timer@2a810000 {
+ compatible = "arm,armv7-timer-mem";
+ reg = <0x0 0x2a810000 0x0 0x10000>;
+ clock-frequency = <100000000>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+ ranges;
+ frame@2a820000 {
+ frame-number = <0>;
+ interrupts = <0 25 4>;
+ reg = <0x0 0x2a820000 0x0 0x10000>;
+ };
+ };
+
+ pmu {
+ compatible = "arm,armv8-pmuv3";
+ interrupts = <0 60 4>,
+ <0 61 4>,
+ <0 62 4>,
+ <0 63 4>;
+ };
+
+ smb {
+ compatible = "simple-bus";
+
+ #address-cells = <2>;
+ #size-cells = <1>;
+ ranges = <0 0 0 0x08000000 0x04000000>,
+ <1 0 0 0x14000000 0x04000000>,
+ <2 0 0 0x18000000 0x04000000>,
+ <3 0 0 0x1c000000 0x04000000>,
+ <4 0 0 0x0c000000 0x04000000>,
+ <5 0 0 0x10000000 0x04000000>;
+
+ #interrupt-cells = <1>;
+ interrupt-map-mask = <0 0 63>;
+ interrupt-map = <0 0 0 &gic 0 0 4>,
+ <0 0 1 &gic 0 1 4>,
+ <0 0 2 &gic 0 2 4>,
+ <0 0 3 &gic 0 3 4>,
+ <0 0 4 &gic 0 4 4>,
+ <0 0 5 &gic 0 5 4>,
+ <0 0 6 &gic 0 6 4>,
+ <0 0 7 &gic 0 7 4>,
+ <0 0 8 &gic 0 8 4>,
+ <0 0 9 &gic 0 9 4>,
+ <0 0 10 &gic 0 10 4>,
+ <0 0 11 &gic 0 11 4>,
+ <0 0 12 &gic 0 12 4>,
+ <0 0 13 &gic 0 13 4>,
+ <0 0 14 &gic 0 14 4>,
+ <0 0 15 &gic 0 15 4>,
+ <0 0 16 &gic 0 16 4>,
+ <0 0 17 &gic 0 17 4>,
+ <0 0 18 &gic 0 18 4>,
+ <0 0 19 &gic 0 19 4>,
+ <0 0 20 &gic 0 20 4>,
+ <0 0 21 &gic 0 21 4>,
+ <0 0 22 &gic 0 22 4>,
+ <0 0 23 &gic 0 23 4>,
+ <0 0 24 &gic 0 24 4>,
+ <0 0 25 &gic 0 25 4>,
+ <0 0 26 &gic 0 26 4>,
+ <0 0 27 &gic 0 27 4>,
+ <0 0 28 &gic 0 28 4>,
+ <0 0 29 &gic 0 29 4>,
+ <0 0 30 &gic 0 30 4>,
+ <0 0 31 &gic 0 31 4>,
+ <0 0 32 &gic 0 32 4>,
+ <0 0 33 &gic 0 33 4>,
+ <0 0 34 &gic 0 34 4>,
+ <0 0 35 &gic 0 35 4>,
+ <0 0 36 &gic 0 36 4>,
+ <0 0 37 &gic 0 37 4>,
+ <0 0 38 &gic 0 38 4>,
+ <0 0 39 &gic 0 39 4>,
+ <0 0 40 &gic 0 40 4>,
+ <0 0 41 &gic 0 41 4>,
+ <0 0 42 &gic 0 42 4>;
+
+ /include/ "rtsm_ve-motherboard.dtsi"
+ };
+
+ panels {
+ panel@0 {
+ compatible = "panel";
+ mode = "XVGA";
+ refresh = <60>;
+ xres = <1024>;
+ yres = <768>;
+ pixclock = <15748>;
+ left_margin = <152>;
+ right_margin = <48>;
+ upper_margin = <23>;
+ lower_margin = <3>;
+ hsync_len = <104>;
+ vsync_len = <4>;
+ sync = <0>;
+ vmode = "FB_VMODE_NONINTERLACED";
+ tim2 = "TIM2_BCD", "TIM2_IPC";
+ cntl = "CNTL_LCDTFT", "CNTL_BGR", "CNTL_LCDVCOMP(1)";
+ caps = "CLCD_CAP_5551", "CLCD_CAP_565", "CLCD_CAP_888";
+ bpp = <16>;
+ };
+ };
+};
diff --git a/fdts/fvp-base-gicv3-psci.dtb b/fdts/fvp-base-gicv3-psci.dtb
new file mode 100644
index 0000000..678b45b
--- /dev/null
+++ b/fdts/fvp-base-gicv3-psci.dtb
Binary files differ
diff --git a/fdts/fvp-base-gicv3-psci.dts b/fdts/fvp-base-gicv3-psci.dts
new file mode 100644
index 0000000..98c7487
--- /dev/null
+++ b/fdts/fvp-base-gicv3-psci.dts
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/ {
+};
+
+/ {
+ model = "FVP Base";
+ compatible = "arm,vfp-base", "arm,vexpress";
+ interrupt-parent = <&gic>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+
+ chosen { };
+
+ aliases {
+ serial0 = &v2m_serial0;
+ serial1 = &v2m_serial1;
+ serial2 = &v2m_serial2;
+ serial3 = &v2m_serial3;
+ };
+
+ psci {
+ compatible = "arm,psci";
+ method = "smc";
+ cpu_suspend = <0xc4000001>;
+ cpu_off = <0x84000002>;
+ cpu_on = <0xc4000003>;
+ };
+
+ cpus {
+ #address-cells = <2>;
+ #size-cells = <0>;
+
+ cpu@0 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x0>;
+ enable-method = "psci";
+ };
+ cpu@1 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x1>;
+ enable-method = "psci";
+ };
+ cpu@2 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x2>;
+ enable-method = "psci";
+ };
+ cpu@3 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x3>;
+ enable-method = "psci";
+ };
+ cpu@100 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x100>;
+ enable-method = "psci";
+ };
+ cpu@101 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x101>;
+ enable-method = "psci";
+ };
+ cpu@102 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x102>;
+ enable-method = "psci";
+ };
+ cpu@103 {
+ device_type = "cpu";
+ compatible = "arm,armv8";
+ reg = <0x0 0x103>;
+ enable-method = "psci";
+ };
+ };
+
+ memory@80000000 {
+ device_type = "memory";
+ reg = <0x00000000 0x80000000 0 0x80000000>;
+ /*
+ <0x00000008 0x80000000 0 0x80000000>;
+ */
+ };
+
+ gic: interrupt-controller@2cf00000 {
+ compatible = "arm,gic-v3";
+ #interrupt-cells = <3>;
+ interrupt-controller;
+ reg = <0x0 0x2f000000 0 0x10000>, // GICD
+ <0x0 0x2f100000 0 0x200000>, // GICR
+ <0x0 0x2c000000 0 0x2000>, // GICC
+ <0x0 0x2c010000 0 0x2000>, // GICH
+ <0x0 0x2c02F000 0 0x2000>; // GICV
+ interrupts = <1 9 4>;
+ };
+
+ timer {
+ compatible = "arm,armv8-timer";
+ interrupts = <1 13 0xff01>,
+ <1 14 0xff01>,
+ <1 11 0xff01>,
+ <1 10 0xff01>;
+ clock-frequency = <100000000>;
+ };
+
+ timer@2a810000 {
+ compatible = "arm,armv7-timer-mem";
+ reg = <0x0 0x2a810000 0x0 0x10000>;
+ clock-frequency = <100000000>;
+ #address-cells = <2>;
+ #size-cells = <2>;
+ ranges;
+ frame@2a820000 {
+ frame-number = <0>;
+ interrupts = <0 25 4>;
+ reg = <0x0 0x2a820000 0x0 0x10000>;
+ };
+ };
+
+ pmu {
+ compatible = "arm,armv8-pmuv3";
+ interrupts = <0 60 4>,
+ <0 61 4>,
+ <0 62 4>,
+ <0 63 4>;
+ };
+
+ smb {
+ compatible = "simple-bus";
+
+ #address-cells = <2>;
+ #size-cells = <1>;
+ ranges = <0 0 0 0x08000000 0x04000000>,
+ <1 0 0 0x14000000 0x04000000>,
+ <2 0 0 0x18000000 0x04000000>,
+ <3 0 0 0x1c000000 0x04000000>,
+ <4 0 0 0x0c000000 0x04000000>,
+ <5 0 0 0x10000000 0x04000000>;
+
+ #interrupt-cells = <1>;
+ interrupt-map-mask = <0 0 63>;
+ interrupt-map = <0 0 0 &gic 0 0 4>,
+ <0 0 1 &gic 0 1 4>,
+ <0 0 2 &gic 0 2 4>,
+ <0 0 3 &gic 0 3 4>,
+ <0 0 4 &gic 0 4 4>,
+ <0 0 5 &gic 0 5 4>,
+ <0 0 6 &gic 0 6 4>,
+ <0 0 7 &gic 0 7 4>,
+ <0 0 8 &gic 0 8 4>,
+ <0 0 9 &gic 0 9 4>,
+ <0 0 10 &gic 0 10 4>,
+ <0 0 11 &gic 0 11 4>,
+ <0 0 12 &gic 0 12 4>,
+ <0 0 13 &gic 0 13 4>,
+ <0 0 14 &gic 0 14 4>,
+ <0 0 15 &gic 0 15 4>,
+ <0 0 16 &gic 0 16 4>,
+ <0 0 17 &gic 0 17 4>,
+ <0 0 18 &gic 0 18 4>,
+ <0 0 19 &gic 0 19 4>,
+ <0 0 20 &gic 0 20 4>,
+ <0 0 21 &gic 0 21 4>,
+ <0 0 22 &gic 0 22 4>,
+ <0 0 23 &gic 0 23 4>,
+ <0 0 24 &gic 0 24 4>,
+ <0 0 25 &gic 0 25 4>,
+ <0 0 26 &gic 0 26 4>,
+ <0 0 27 &gic 0 27 4>,
+ <0 0 28 &gic 0 28 4>,
+ <0 0 29 &gic 0 29 4>,
+ <0 0 30 &gic 0 30 4>,
+ <0 0 31 &gic 0 31 4>,
+ <0 0 32 &gic 0 32 4>,
+ <0 0 33 &gic 0 33 4>,
+ <0 0 34 &gic 0 34 4>,
+ <0 0 35 &gic 0 35 4>,
+ <0 0 36 &gic 0 36 4>,
+ <0 0 37 &gic 0 37 4>,
+ <0 0 38 &gic 0 38 4>,
+ <0 0 39 &gic 0 39 4>,
+ <0 0 40 &gic 0 40 4>,
+ <0 0 41 &gic 0 41 4>,
+ <0 0 42 &gic 0 42 4>;
+
+ /include/ "rtsm_ve-motherboard.dtsi"
+ };
+
+ panels {
+ panel@0 {
+ compatible = "panel";
+ mode = "XVGA";
+ refresh = <60>;
+ xres = <1024>;
+ yres = <768>;
+ pixclock = <15748>;
+ left_margin = <152>;
+ right_margin = <48>;
+ upper_margin = <23>;
+ lower_margin = <3>;
+ hsync_len = <104>;
+ vsync_len = <4>;
+ sync = <0>;
+ vmode = "FB_VMODE_NONINTERLACED";
+ tim2 = "TIM2_BCD", "TIM2_IPC";
+ cntl = "CNTL_LCDTFT", "CNTL_BGR", "CNTL_LCDVCOMP(1)";
+ caps = "CLCD_CAP_5551", "CLCD_CAP_565", "CLCD_CAP_888";
+ bpp = <16>;
+ };
+ };
+};
diff --git a/fdts/rtsm_ve-motherboard.dtsi b/fdts/rtsm_ve-motherboard.dtsi
new file mode 100644
index 0000000..00e92c5
--- /dev/null
+++ b/fdts/rtsm_ve-motherboard.dtsi
@@ -0,0 +1,264 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+ motherboard {
+ arm,v2m-memory-map = "rs1";
+ compatible = "arm,vexpress,v2m-p1", "simple-bus";
+ #address-cells = <2>; /* SMB chipselect number and offset */
+ #size-cells = <1>;
+ #interrupt-cells = <1>;
+ ranges;
+
+ flash@0,00000000 {
+ compatible = "arm,vexpress-flash", "cfi-flash";
+ reg = <0 0x00000000 0x04000000>,
+ <4 0x00000000 0x04000000>;
+ bank-width = <4>;
+ };
+
+ vram@2,00000000 {
+ compatible = "arm,vexpress-vram";
+ reg = <2 0x00000000 0x00800000>;
+ };
+
+ ethernet@2,02000000 {
+ compatible = "smsc,lan91c111";
+ reg = <2 0x02000000 0x10000>;
+ interrupts = <15>;
+ };
+
+ v2m_clk24mhz: clk24mhz {
+ compatible = "fixed-clock";
+ #clock-cells = <0>;
+ clock-frequency = <24000000>;
+ clock-output-names = "v2m:clk24mhz";
+ };
+
+ v2m_refclk1mhz: refclk1mhz {
+ compatible = "fixed-clock";
+ #clock-cells = <0>;
+ clock-frequency = <1000000>;
+ clock-output-names = "v2m:refclk1mhz";
+ };
+
+ v2m_refclk32khz: refclk32khz {
+ compatible = "fixed-clock";
+ #clock-cells = <0>;
+ clock-frequency = <32768>;
+ clock-output-names = "v2m:refclk32khz";
+ };
+
+ iofpga@3,00000000 {
+ compatible = "arm,amba-bus", "simple-bus";
+ #address-cells = <1>;
+ #size-cells = <1>;
+ ranges = <0 3 0 0x200000>;
+
+ v2m_sysreg: sysreg@010000 {
+ compatible = "arm,vexpress-sysreg";
+ reg = <0x010000 0x1000>;
+ gpio-controller;
+ #gpio-cells = <2>;
+ };
+
+ v2m_sysctl: sysctl@020000 {
+ compatible = "arm,sp810", "arm,primecell";
+ reg = <0x020000 0x1000>;
+ clocks = <&v2m_refclk32khz>, <&v2m_refclk1mhz>, <&v2m_clk24mhz>;
+ clock-names = "refclk", "timclk", "apb_pclk";
+ #clock-cells = <1>;
+ clock-output-names = "timerclken0", "timerclken1", "timerclken2", "timerclken3";
+ };
+
+ aaci@040000 {
+ compatible = "arm,pl041", "arm,primecell";
+ reg = <0x040000 0x1000>;
+ interrupts = <11>;
+ clocks = <&v2m_clk24mhz>;
+ clock-names = "apb_pclk";
+ };
+
+ mmci@050000 {
+ compatible = "arm,pl180", "arm,primecell";
+ reg = <0x050000 0x1000>;
+ interrupts = <9 10>;
+ cd-gpios = <&v2m_sysreg 0 0>;
+ wp-gpios = <&v2m_sysreg 1 0>;
+ max-frequency = <12000000>;
+ vmmc-supply = <&v2m_fixed_3v3>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "mclk", "apb_pclk";
+ };
+
+ kmi@060000 {
+ compatible = "arm,pl050", "arm,primecell";
+ reg = <0x060000 0x1000>;
+ interrupts = <12>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "KMIREFCLK", "apb_pclk";
+ };
+
+ kmi@070000 {
+ compatible = "arm,pl050", "arm,primecell";
+ reg = <0x070000 0x1000>;
+ interrupts = <13>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "KMIREFCLK", "apb_pclk";
+ };
+
+ v2m_serial0: uart@090000 {
+ compatible = "arm,pl011", "arm,primecell";
+ reg = <0x090000 0x1000>;
+ interrupts = <5>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "uartclk", "apb_pclk";
+ };
+
+ v2m_serial1: uart@0a0000 {
+ compatible = "arm,pl011", "arm,primecell";
+ reg = <0x0a0000 0x1000>;
+ interrupts = <6>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "uartclk", "apb_pclk";
+ };
+
+ v2m_serial2: uart@0b0000 {
+ compatible = "arm,pl011", "arm,primecell";
+ reg = <0x0b0000 0x1000>;
+ interrupts = <7>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "uartclk", "apb_pclk";
+ };
+
+ v2m_serial3: uart@0c0000 {
+ compatible = "arm,pl011", "arm,primecell";
+ reg = <0x0c0000 0x1000>;
+ interrupts = <8>;
+ clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+ clock-names = "uartclk", "apb_pclk";
+ };
+
+ wdt@0f0000 {
+ compatible = "arm,sp805", "arm,primecell";
+ reg = <0x0f0000 0x1000>;
+ interrupts = <0>;
+ clocks = <&v2m_refclk32khz>, <&v2m_clk24mhz>;
+ clock-names = "wdogclk", "apb_pclk";
+ };
+
+ v2m_timer01: timer@110000 {
+ compatible = "arm,sp804", "arm,primecell";
+ reg = <0x110000 0x1000>;
+ interrupts = <2>;
+ clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_clk24mhz>;
+ clock-names = "timclken1", "timclken2", "apb_pclk";
+ };
+
+ v2m_timer23: timer@120000 {
+ compatible = "arm,sp804", "arm,primecell";
+ reg = <0x120000 0x1000>;
+ interrupts = <3>;
+ clocks = <&v2m_sysctl 2>, <&v2m_sysctl 3>, <&v2m_clk24mhz>;
+ clock-names = "timclken1", "timclken2", "apb_pclk";
+ };
+
+ rtc@170000 {
+ compatible = "arm,pl031", "arm,primecell";
+ reg = <0x170000 0x1000>;
+ interrupts = <4>;
+ clocks = <&v2m_clk24mhz>;
+ clock-names = "apb_pclk";
+ };
+
+ clcd@1f0000 {
+ compatible = "arm,pl111", "arm,primecell";
+ reg = <0x1f0000 0x1000>;
+ interrupts = <14>;
+ clocks = <&v2m_oscclk1>, <&v2m_clk24mhz>;
+ clock-names = "clcdclk", "apb_pclk";
+ mode = "XVGA";
+ use_dma = <0>;
+ framebuffer = <0x18000000 0x00180000>;
+ };
+
+ virtio_block@0130000 {
+ compatible = "virtio,mmio";
+ reg = <0x130000 0x1000>;
+ interrupts = <0x2a>;
+ };
+ };
+
+ v2m_fixed_3v3: fixedregulator@0 {
+ compatible = "regulator-fixed";
+ regulator-name = "3V3";
+ regulator-min-microvolt = <3300000>;
+ regulator-max-microvolt = <3300000>;
+ regulator-always-on;
+ };
+
+ mcc {
+ compatible = "arm,vexpress,config-bus", "simple-bus";
+ arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+ v2m_oscclk1: osc@1 {
+ /* CLCD clock */
+ compatible = "arm,vexpress-osc";
+ arm,vexpress-sysreg,func = <1 1>;
+ freq-range = <23750000 63500000>;
+ #clock-cells = <0>;
+ clock-output-names = "v2m:oscclk1";
+ };
+
+ reset@0 {
+ compatible = "arm,vexpress-reset";
+ arm,vexpress-sysreg,func = <5 0>;
+ };
+
+ muxfpga@0 {
+ compatible = "arm,vexpress-muxfpga";
+ arm,vexpress-sysreg,func = <7 0>;
+ };
+
+ shutdown@0 {
+ compatible = "arm,vexpress-shutdown";
+ arm,vexpress-sysreg,func = <8 0>;
+ };
+
+ reboot@0 {
+ compatible = "arm,vexpress-reboot";
+ arm,vexpress-sysreg,func = <9 0>;
+ };
+
+ dvimode@0 {
+ compatible = "arm,vexpress-dvimode";
+ arm,vexpress-sysreg,func = <11 0>;
+ };
+ };
+ };
diff --git a/include/aarch64/arch.h b/include/aarch64/arch.h
new file mode 100644
index 0000000..3a23e4f
--- /dev/null
+++ b/include/aarch64/arch.h
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __ARCH_H__
+#define __ARCH_H__
+
+#include <bl_common.h>
+
+/*******************************************************************************
+ * MIDR bit definitions
+ ******************************************************************************/
+#define MIDR_PN_MASK 0xfff
+#define MIDR_PN_SHIFT 0x4
+#define MIDR_PN_AEM 0xd0f
+#define MIDR_PN_A57 0xd07
+#define MIDR_PN_A53 0xd03
+
+/*******************************************************************************
+ * MPIDR macros
+ ******************************************************************************/
+#define MPIDR_CPU_MASK MPIDR_AFFLVL_MASK
+#define MPIDR_CLUSTER_MASK MPIDR_AFFLVL_MASK << MPIDR_AFFINITY_BITS
+#define MPIDR_AFFINITY_BITS 8
+#define MPIDR_AFFLVL_MASK 0xff
+#define MPIDR_AFF0_SHIFT 0
+#define MPIDR_AFF1_SHIFT 8
+#define MPIDR_AFF2_SHIFT 16
+#define MPIDR_AFF3_SHIFT 32
+#define MPIDR_AFFINITY_MASK 0xff00ffffff
+#define MPIDR_AFFLVL_SHIFT 3
+#define MPIDR_AFFLVL0 0
+#define MPIDR_AFFLVL1 1
+#define MPIDR_AFFLVL2 2
+#define MPIDR_AFFLVL3 3
+/* TODO: Support only the first 3 affinity levels for now */
+#define MPIDR_MAX_AFFLVL 2
+
+/* Constant to highlight the assumption that MPIDR allocation starts from 0 */
+#define FIRST_MPIDR 0
+
+/*******************************************************************************
+ * Implementation defined sysreg encodings
+ ******************************************************************************/
+#define CPUECTLR_EL1 S3_1_C15_C2_1
+
+/*******************************************************************************
+ * System register bit definitions
+ ******************************************************************************/
+/* CLIDR definitions */
+#define LOUIS_SHIFT 21
+#define LOC_SHIFT 24
+#define CLIDR_FIELD_WIDTH 3
+
+/* CSSELR definitions */
+#define LEVEL_SHIFT 1
+
+/* D$ set/way op type defines */
+#define DCISW 0x0
+#define DCCISW 0x1
+#define DCCSW 0x2
+
+/* ID_AA64PFR0_EL1 definitions */
+#define ID_AA64PFR0_EL0_SHIFT 0
+#define ID_AA64PFR0_EL1_SHIFT 4
+#define ID_AA64PFR0_EL2_SHIFT 8
+#define ID_AA64PFR0_EL3_SHIFT 12
+#define ID_AA64PFR0_ELX_MASK 0xf
+
+/* ID_PFR1_EL1 definitions */
+#define ID_PFR1_VIRTEXT_SHIFT 12
+#define ID_PFR1_VIRTEXT_MASK 0xf
+#define GET_VIRT_EXT(id) ((id >> ID_PFR1_VIRTEXT_SHIFT) \
+ & ID_PFR1_VIRTEXT_MASK)
+
+/* SCTLR definitions */
+#define SCTLR_EL2_RES1 ((1 << 29) | (1 << 28) | (1 << 23) | (1 << 22) | \
+ (1 << 18) | (1 << 16) | (1 << 11) | (1 << 5) | \
+ (1 << 4))
+
+#define SCTLR_EL1_RES1 ((1 << 29) | (1 << 28) | (1 << 23) | (1 << 22) | \
+ (1 << 11))
+#define SCTLR_M_BIT (1 << 0)
+#define SCTLR_A_BIT (1 << 1)
+#define SCTLR_C_BIT (1 << 2)
+#define SCTLR_SA_BIT (1 << 3)
+#define SCTLR_B_BIT (1 << 7)
+#define SCTLR_Z_BIT (1 << 11)
+#define SCTLR_I_BIT (1 << 12)
+#define SCTLR_WXN_BIT (1 << 19)
+#define SCTLR_EXCEPTION_BITS (0x3 << 6)
+#define SCTLR_EE_BIT (1 << 25)
+
+/* CPUECTLR definitions */
+#define CPUECTLR_SMP_BIT (1 << 6)
+
+/* CPACR_El1 definitions */
+#define CPACR_EL1_FPEN(x) (x << 20)
+#define CPACR_EL1_FP_TRAP_EL0 0x1
+#define CPACR_EL1_FP_TRAP_ALL 0x2
+#define CPACR_EL1_FP_TRAP_NONE 0x3
+
+/* SCR definitions */
+#define SCR_RES1_BITS ((1 << 4) | (1 << 5))
+#define SCR_TWE_BIT (1 << 13)
+#define SCR_TWI_BIT (1 << 12)
+#define SCR_ST_BIT (1 << 11)
+#define SCR_RW_BIT (1 << 10)
+#define SCR_SIF_BIT (1 << 9)
+#define SCR_HCE_BIT (1 << 8)
+#define SCR_SMD_BIT (1 << 7)
+#define SCR_EA_BIT (1 << 3)
+#define SCR_FIQ_BIT (1 << 2)
+#define SCR_IRQ_BIT (1 << 1)
+#define SCR_NS_BIT (1 << 0)
+
+/* HCR definitions */
+#define HCR_RW_BIT (1ull << 31)
+#define HCR_AMO_BIT (1 << 5)
+#define HCR_IMO_BIT (1 << 4)
+#define HCR_FMO_BIT (1 << 3)
+
+/* CNTHCTL_EL2 definitions */
+#define EL1PCEN_BIT (1 << 1)
+#define EL1PCTEN_BIT (1 << 0)
+
+/* CNTKCTL_EL1 definitions */
+#define EL0PTEN_BIT (1 << 9)
+#define EL0VTEN_BIT (1 << 8)
+#define EL0PCTEN_BIT (1 << 0)
+#define EL0VCTEN_BIT (1 << 1)
+
+/* CPTR_EL3 definitions */
+#define TCPAC_BIT (1ull << 31)
+#define TFP_BIT (1 << 10)
+
+/* CPSR/SPSR definitions */
+#define DAIF_FIQ_BIT (1 << 0)
+#define DAIF_IRQ_BIT (1 << 1)
+#define DAIF_ABT_BIT (1 << 2)
+#define DAIF_DBG_BIT (1 << 3)
+#define PSR_DAIF_SHIFT 0x6
+
+/*
+ * TCR defintions
+ */
+#define TCR_EL3_RES1 ((1UL << 31) | (1UL << 23))
+
+#define TCR_T0SZ_4GB 32
+
+#define TCR_RGN_INNER_NC (0x0 << 8)
+#define TCR_RGN_INNER_WBA (0x1 << 8)
+#define TCR_RGN_INNER_WT (0x2 << 8)
+#define TCR_RGN_INNER_WBNA (0x3 << 8)
+
+#define TCR_RGN_OUTER_NC (0x0 << 10)
+#define TCR_RGN_OUTER_WBA (0x1 << 10)
+#define TCR_RGN_OUTER_WT (0x2 << 10)
+#define TCR_RGN_OUTER_WBNA (0x3 << 10)
+
+#define TCR_SH_NON_SHAREABLE (0x0 << 12)
+#define TCR_SH_OUTER_SHAREABLE (0x2 << 12)
+#define TCR_SH_INNER_SHAREABLE (0x3 << 12)
+
+#define MODE_RW_64 0x0
+#define MODE_RW_32 0x1
+#define MODE_SP_EL0 0x0
+#define MODE_SP_ELX 0x1
+#define MODE_EL3 0x3
+#define MODE_EL2 0x2
+#define MODE_EL1 0x1
+#define MODE_EL0 0x0
+
+#define MODE_RW_SHIFT 0x4
+#define MODE_EL_SHIFT 0x2
+#define MODE_SP_SHIFT 0x0
+
+#define GET_RW(mode) ((mode >> MODE_RW_SHIFT) & 0x1)
+#define GET_EL(mode) ((mode >> MODE_EL_SHIFT) & 0x3)
+#define GET_SP(mode) ((mode >> MODE_SP_SHIFT) & 0x1)
+#define PSR_MODE(rw, el, sp) (rw << MODE_RW_SHIFT | el << MODE_EL_SHIFT \
+ | sp << MODE_SP_SHIFT)
+
+#define SPSR32_EE_BIT (1 << 9)
+#define SPSR32_T_BIT (1 << 5)
+
+#define AARCH32_MODE_SVC 0x13
+#define AARCH32_MODE_HYP 0x1a
+
+/* Miscellaneous MMU related constants */
+#define NUM_2MB_IN_GB (1 << 9)
+#define NUM_4K_IN_2MB (1 << 9)
+
+#define TWO_MB_SHIFT 21
+#define ONE_GB_SHIFT 30
+#define FOUR_KB_SHIFT 12
+
+#define ONE_GB_INDEX(x) ((x) >> ONE_GB_SHIFT)
+#define TWO_MB_INDEX(x) ((x) >> TWO_MB_SHIFT)
+#define FOUR_KB_INDEX(x) ((x) >> FOUR_KB_SHIFT)
+
+#define INVALID_DESC 0x0
+#define BLOCK_DESC 0x1
+#define TABLE_DESC 0x3
+
+#define FIRST_LEVEL_DESC_N ONE_GB_SHIFT
+#define SECOND_LEVEL_DESC_N TWO_MB_SHIFT
+#define THIRD_LEVEL_DESC_N FOUR_KB_SHIFT
+
+#define LEVEL1 1
+#define LEVEL2 2
+#define LEVEL3 3
+
+#define XN (1ull << 2)
+#define PXN (1ull << 1)
+#define CONT_HINT (1ull << 0)
+
+#define UPPER_ATTRS(x) (x & 0x7) << 52
+#define NON_GLOBAL (1 << 9)
+#define ACCESS_FLAG (1 << 8)
+#define NSH (0x0 << 6)
+#define OSH (0x2 << 6)
+#define ISH (0x3 << 6)
+
+/*
+ * AP[1] bit is ignored by hardware and is
+ * treated as if it is One in EL2/EL3
+ */
+#define AP_RO (0x1 << 5)
+#define AP_RW (0x0 << 5)
+
+#define NS (0x1 << 3)
+#define ATTR_SO_INDEX 0x2
+#define ATTR_DEVICE_INDEX 0x1
+#define ATTR_IWBWA_OWBWA_NTR_INDEX 0x0
+#define LOWER_ATTRS(x) (((x) & 0xfff) << 2)
+#define ATTR_SO (0x0)
+#define ATTR_DEVICE (0x4)
+#define ATTR_IWBWA_OWBWA_NTR (0xff)
+#define MAIR_ATTR_SET(attr, index) (attr << (index << 3))
+
+/* Exception Syndrome register bits and bobs */
+#define ESR_EC_SHIFT 26
+#define ESR_EC_MASK 0x3f
+#define ESR_EC_LENGTH 6
+#define EC_UNKNOWN 0x0
+#define EC_WFE_WFI 0x1
+#define EC_AARCH32_CP15_MRC_MCR 0x3
+#define EC_AARCH32_CP15_MRRC_MCRR 0x4
+#define EC_AARCH32_CP14_MRC_MCR 0x5
+#define EC_AARCH32_CP14_LDC_STC 0x6
+#define EC_FP_SIMD 0x7
+#define EC_AARCH32_CP10_MRC 0x8
+#define EC_AARCH32_CP14_MRRC_MCRR 0xc
+#define EC_ILLEGAL 0xe
+#define EC_AARCH32_SVC 0x11
+#define EC_AARCH32_HVC 0x12
+#define EC_AARCH32_SMC 0x13
+#define EC_AARCH64_SVC 0x15
+#define EC_AARCH64_HVC 0x16
+#define EC_AARCH64_SMC 0x17
+#define EC_AARCH64_SYS 0x18
+#define EC_IABORT_LOWER_EL 0x20
+#define EC_IABORT_CUR_EL 0x21
+#define EC_PC_ALIGN 0x22
+#define EC_DABORT_LOWER_EL 0x24
+#define EC_DABORT_CUR_EL 0x25
+#define EC_SP_ALIGN 0x26
+#define EC_AARCH32_FP 0x28
+#define EC_AARCH64_FP 0x2c
+#define EC_SERROR 0x2f
+
+#define EC_BITS(x) (x >> ESR_EC_SHIFT) & ESR_EC_MASK
+
+#ifndef __ASSEMBLY__
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+
+extern void early_exceptions(void);
+extern void runtime_exceptions(void);
+extern void bl1_arch_setup(void);
+extern void bl2_arch_setup(void);
+extern void bl31_arch_setup(void);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __ARCH_H__ */
diff --git a/include/aarch64/arch_helpers.h b/include/aarch64/arch_helpers.h
new file mode 100644
index 0000000..348d545
--- /dev/null
+++ b/include/aarch64/arch_helpers.h
@@ -0,0 +1,295 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __ARCH_HELPERS_H__
+#define __ARCH_HELPERS_H__
+
+#include <arch.h>
+
+/*******************************************************************************
+ * Generic timer memory mapped registers & offsets
+ ******************************************************************************/
+#define CNTCR_OFF 0x000
+#define CNTFID_OFF 0x020
+
+#define CNTCR_EN (1 << 0)
+#define CNTCR_HDBG (1 << 1)
+#define CNTCR_FCREQ(x) (1 << (8 + (x)))
+
+#ifndef __ASSEMBLY__
+
+/*******************************************************************************
+ * TLB maintenance accessor prototypes
+ ******************************************************************************/
+extern void tlbiall(void);
+extern void tlbiallis(void);
+extern void tlbialle1(void);
+extern void tlbialle1is(void);
+extern void tlbialle2(void);
+extern void tlbialle2is(void);
+extern void tlbialle3(void);
+extern void tlbialle3is(void);
+extern void tlbivmalle1(void);
+
+/*******************************************************************************
+ * Cache maintenance accessor prototypes
+ ******************************************************************************/
+extern void dcisw(unsigned long);
+extern void dccisw(unsigned long);
+extern void dccsw(unsigned long);
+extern void dccvac(unsigned long);
+extern void dcivac(unsigned long);
+extern void dccivac(unsigned long);
+extern void dccvau(unsigned long);
+extern void dczva(unsigned long);
+extern void flush_dcache_range(unsigned long, unsigned long);
+extern void inv_dcache_range(unsigned long, unsigned long);
+extern void dcsw_op_louis(unsigned int);
+extern void dcsw_op_all(unsigned int);
+
+/*******************************************************************************
+ * Misc. accessor prototypes
+ ******************************************************************************/
+extern void enable_irq(void);
+extern void enable_fiq(void);
+extern void enable_serror(void);
+
+extern void disable_irq(void);
+extern void disable_fiq(void);
+extern void disable_serror(void);
+
+extern unsigned long read_id_pfr1_el1(void);
+extern unsigned long read_id_aa64pfr0_el1(void);
+extern unsigned long read_current_el(void);
+extern unsigned long read_daif(void);
+extern unsigned long read_spsr(void);
+extern unsigned long read_spsr_el1(void);
+extern unsigned long read_spsr_el2(void);
+extern unsigned long read_spsr_el3(void);
+extern unsigned long read_elr(void);
+extern unsigned long read_elr_el1(void);
+extern unsigned long read_elr_el2(void);
+extern unsigned long read_elr_el3(void);
+
+extern void write_daif(unsigned long);
+extern void write_spsr(unsigned long);
+extern void write_spsr_el1(unsigned long);
+extern void write_spsr_el2(unsigned long);
+extern void write_spsr_el3(unsigned long);
+extern void write_elr(unsigned long);
+extern void write_elr_el1(unsigned long);
+extern void write_elr_el2(unsigned long);
+extern void write_elr_el3(unsigned long);
+
+extern void wfi(void);
+extern void wfe(void);
+extern void rfe(void);
+extern void sev(void);
+extern void dsb(void);
+extern void isb(void);
+
+extern unsigned int get_afflvl_shift(unsigned int);
+extern unsigned int mpidr_mask_lower_afflvls(unsigned long, unsigned int);
+
+extern void eret(unsigned long, unsigned long,
+ unsigned long, unsigned long,
+ unsigned long, unsigned long,
+ unsigned long, unsigned long);
+
+extern unsigned long smc(unsigned long, unsigned long,
+ unsigned long, unsigned long,
+ unsigned long, unsigned long,
+ unsigned long, unsigned long);
+
+/*******************************************************************************
+ * System register accessor prototypes
+ ******************************************************************************/
+extern unsigned long read_midr(void);
+extern unsigned long read_mpidr(void);
+
+extern unsigned long read_scr(void);
+extern unsigned long read_hcr(void);
+
+extern unsigned long read_vbar(void);
+extern unsigned long read_vbar_el1(void);
+extern unsigned long read_vbar_el2(void);
+extern unsigned long read_vbar_el3(void);
+
+extern unsigned long read_sctlr(void);
+extern unsigned long read_sctlr_el1(void);
+extern unsigned long read_sctlr_el2(void);
+extern unsigned long read_sctlr_el3(void);
+
+extern unsigned long read_actlr(void);
+extern unsigned long read_actlr_el1(void);
+extern unsigned long read_actlr_el2(void);
+extern unsigned long read_actlr_el3(void);
+
+extern unsigned long read_esr(void);
+extern unsigned long read_esr_el1(void);
+extern unsigned long read_esr_el2(void);
+extern unsigned long read_esr_el3(void);
+
+extern unsigned long read_afsr0(void);
+extern unsigned long read_afsr0_el1(void);
+extern unsigned long read_afsr0_el2(void);
+extern unsigned long read_afsr0_el3(void);
+
+extern unsigned long read_afsr1(void);
+extern unsigned long read_afsr1_el1(void);
+extern unsigned long read_afsr1_el2(void);
+extern unsigned long read_afsr1_el3(void);
+
+extern unsigned long read_far(void);
+extern unsigned long read_far_el1(void);
+extern unsigned long read_far_el2(void);
+extern unsigned long read_far_el3(void);
+
+extern unsigned long read_mair(void);
+extern unsigned long read_mair_el1(void);
+extern unsigned long read_mair_el2(void);
+extern unsigned long read_mair_el3(void);
+
+extern unsigned long read_amair(void);
+extern unsigned long read_amair_el1(void);
+extern unsigned long read_amair_el2(void);
+extern unsigned long read_amair_el3(void);
+
+extern unsigned long read_rvbar(void);
+extern unsigned long read_rvbar_el1(void);
+extern unsigned long read_rvbar_el2(void);
+extern unsigned long read_rvbar_el3(void);
+
+extern unsigned long read_rmr(void);
+extern unsigned long read_rmr_el1(void);
+extern unsigned long read_rmr_el2(void);
+extern unsigned long read_rmr_el3(void);
+
+extern unsigned long read_tcr(void);
+extern unsigned long read_tcr_el1(void);
+extern unsigned long read_tcr_el2(void);
+extern unsigned long read_tcr_el3(void);
+
+extern unsigned long read_ttbr0(void);
+extern unsigned long read_ttbr0_el1(void);
+extern unsigned long read_ttbr0_el2(void);
+extern unsigned long read_ttbr0_el3(void);
+
+extern unsigned long read_ttbr1(void);
+extern unsigned long read_ttbr1_el1(void);
+extern unsigned long read_ttbr1_el2(void);
+
+extern unsigned long read_cptr(void);
+extern unsigned long read_cptr_el2(void);
+extern unsigned long read_cptr_el3(void);
+
+extern unsigned long read_cpacr(void);
+extern unsigned long read_cpuectlr(void);
+extern unsigned int read_cntfrq_el0(void);
+extern unsigned long read_cnthctl_el2(void);
+
+extern void write_scr(unsigned long);
+extern void write_hcr(unsigned long);
+extern void write_cpacr(unsigned long);
+extern void write_cntfrq_el0(unsigned int);
+extern void write_cnthctl_el2(unsigned long);
+
+extern void write_vbar(unsigned long);
+extern void write_vbar_el1(unsigned long);
+extern void write_vbar_el2(unsigned long);
+extern void write_vbar_el3(unsigned long);
+
+extern void write_sctlr(unsigned long);
+extern void write_sctlr_el1(unsigned long);
+extern void write_sctlr_el2(unsigned long);
+extern void write_sctlr_el3(unsigned long);
+
+extern void write_actlr(unsigned long);
+extern void write_actlr_el1(unsigned long);
+extern void write_actlr_el2(unsigned long);
+extern void write_actlr_el3(unsigned long);
+
+extern void write_esr(unsigned long);
+extern void write_esr_el1(unsigned long);
+extern void write_esr_el2(unsigned long);
+extern void write_esr_el3(unsigned long);
+
+extern void write_afsr0(unsigned long);
+extern void write_afsr0_el1(unsigned long);
+extern void write_afsr0_el2(unsigned long);
+extern void write_afsr0_el3(unsigned long);
+
+extern void write_afsr1(unsigned long);
+extern void write_afsr1_el1(unsigned long);
+extern void write_afsr1_el2(unsigned long);
+extern void write_afsr1_el3(unsigned long);
+
+extern void write_far(unsigned long);
+extern void write_far_el1(unsigned long);
+extern void write_far_el2(unsigned long);
+extern void write_far_el3(unsigned long);
+
+extern void write_mair(unsigned long);
+extern void write_mair_el1(unsigned long);
+extern void write_mair_el2(unsigned long);
+extern void write_mair_el3(unsigned long);
+
+extern void write_amair(unsigned long);
+extern void write_amair_el1(unsigned long);
+extern void write_amair_el2(unsigned long);
+extern void write_amair_el3(unsigned long);
+
+extern void write_rmr(unsigned long);
+extern void write_rmr_el1(unsigned long);
+extern void write_rmr_el2(unsigned long);
+extern void write_rmr_el3(unsigned long);
+
+extern void write_tcr(unsigned long);
+extern void write_tcr_el1(unsigned long);
+extern void write_tcr_el2(unsigned long);
+extern void write_tcr_el3(unsigned long);
+
+extern void write_ttbr0(unsigned long);
+extern void write_ttbr0_el1(unsigned long);
+extern void write_ttbr0_el2(unsigned long);
+extern void write_ttbr0_el3(unsigned long);
+
+extern void write_ttbr1(unsigned long);
+extern void write_ttbr1_el1(unsigned long);
+extern void write_ttbr1_el2(unsigned long);
+
+extern void write_cptr(unsigned long);
+extern void write_cpuectlr(unsigned long);
+extern void write_cptr_el2(unsigned long);
+extern void write_cptr_el3(unsigned long);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __ARCH_HELPERS_H__ */
diff --git a/include/asm_macros.S b/include/asm_macros.S
new file mode 100644
index 0000000..f7afdfc
--- /dev/null
+++ b/include/asm_macros.S
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+ .macro func_prologue
+ stp x29, x30, [sp, #-0x10]!
+ mov x29,sp
+ .endm
+
+ .macro func_epilogue
+ ldp x29, x30, [sp], #0x10
+ .endm
+
+
+ .macro dcache_line_size reg, tmp
+ mrs \tmp, ctr_el0
+ ubfx \tmp, \tmp, #16, #4
+ mov \reg, #4
+ lsl \reg, \reg, \tmp
+ .endm
+
+
+ .macro icache_line_size reg, tmp
+ mrs \tmp, ctr_el0
+ and \tmp, \tmp, #0xf
+ mov \reg, #4
+ lsl \reg, \reg, \tmp
+ .endm
+
+
+ .macro exception_entry func
+ stp x29, x30, [sp, #-0x10]!
+ bl \func
+ .endm
+
+
+ .macro exception_exit func
+ bl \func
+ ldp x29, x30, [sp], #0x10
+ .endm
+
+
+ .macro smc_check label
+ bl read_esr
+ ubfx x0, x0, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+ cmp x0, #EC_AARCH64_SMC
+ b.ne $label
+ .endm
+
+
+ .macro setup_dcsw_op_args start_level, end_level, clidr, shift, fw, ls
+ mrs \clidr, clidr_el1
+ mov \start_level, xzr
+ ubfx \end_level, \clidr, \shift, \fw
+ lsl \end_level, \end_level, \ls
+ .endm
diff --git a/include/bakery_lock.h b/include/bakery_lock.h
new file mode 100644
index 0000000..6c4ab8f
--- /dev/null
+++ b/include/bakery_lock.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __BAKERY_LOCK_H__
+#define __BAKERY_LOCK_H__
+
+#include <platform.h>
+
+#define BAKERY_LOCK_MAX_CPUS PLATFORM_CORE_COUNT
+
+#ifndef __ASSEMBLY__
+typedef struct {
+ volatile int owner;
+ volatile char entering[BAKERY_LOCK_MAX_CPUS];
+ volatile unsigned number[BAKERY_LOCK_MAX_CPUS];
+} bakery_lock;
+
+#define NO_OWNER (-1)
+
+void bakery_lock_init(bakery_lock* bakery);
+/* Check whether a lock is held. Mainly used for debug purpose. */
+int bakery_lock_held(unsigned long mpidr, const bakery_lock * bakery);
+void bakery_lock_get(unsigned long mpidr, bakery_lock* bakery);
+void bakery_lock_release(unsigned long mpidr, bakery_lock* bakery);
+int bakery_lock_try(unsigned long mpidr, bakery_lock* bakery);
+#endif /*__ASSEMBLY__*/
+
+#endif /* __BAKERY_LOCK_H__ */
diff --git a/include/bl1.h b/include/bl1.h
new file mode 100644
index 0000000..868ee4f
--- /dev/null
+++ b/include/bl1.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __BL1_H__
+#define __BL1_H__
+
+#include <bl_common.h>
+
+/******************************************************************************
+ * Function ID of the only SMC that the BL1 exception handlers service.
+ * The chosen value is the first function ID of the ARM SMC64 range.
+ *****************************************************************************/
+#define RUN_IMAGE 0xC0000000
+
+#ifndef __ASSEMBLY__
+
+/******************************************
+ * Function prototypes
+ *****************************************/
+extern void bl1_platform_setup(void);
+extern meminfo bl1_get_sec_mem_layout(void);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __BL1_H__ */
diff --git a/include/bl2.h b/include/bl2.h
new file mode 100644
index 0000000..6fa8721
--- /dev/null
+++ b/include/bl2.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __BL2_H__
+#define __BL2_H__
+
+#include <bl_common.h>
+
+/******************************************
+ * Data declarations
+ *****************************************/
+extern unsigned long long bl2_entrypoint;
+
+/******************************************
+ * Function prototypes
+ *****************************************/
+extern void bl2_platform_setup(void);
+extern meminfo bl2_get_sec_mem_layout(void);
+extern meminfo bl2_get_ns_mem_layout(void);
+
+#endif /* __BL2_H__ */
diff --git a/include/bl31.h b/include/bl31.h
new file mode 100644
index 0000000..0d123a4
--- /dev/null
+++ b/include/bl31.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __BL31_H__
+#define __BL31_H__
+
+#include <bl_common.h>
+
+/*******************************************************************************
+ * Data declarations
+ ******************************************************************************/
+extern unsigned long bl31_entrypoint;
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+extern void bl31_platform_setup(void);
+extern meminfo bl31_get_sec_mem_layout(void);
+extern el_change_info* bl31_get_next_image_info(unsigned long);
+extern void gic_cpuif_deactivate(unsigned int);
+extern void gic_cpuif_setup(unsigned int);
+extern void gic_pcpu_distif_setup(unsigned int);
+extern void gic_setup(void);
+#endif /* __BL31_H__ */
diff --git a/include/bl_common.h b/include/bl_common.h
new file mode 100644
index 0000000..58accdb
--- /dev/null
+++ b/include/bl_common.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __BL_COMMON_H__
+#define __BL_COMMON_H__
+
+#define SECURE 0
+#define NON_SECURE 1
+
+#define UP 1
+#define DOWN 0
+
+/*******************************************************************************
+ * Constants for loading images. When BLx wants to load BLy, it looks at a
+ * meminfo structure to find the extents of free memory. Then depending upon
+ * how it has been configured, it can either load BLy at the top or bottom of
+ * the free memory. These constants indicate the choice.
+ * TODO: Make this configurable while building the trusted firmware.
+ ******************************************************************************/
+#define TOP_LOAD 0x1
+#define BOT_LOAD !TOP_LOAD
+#define LOAD_MASK (1 << 0)
+
+/*******************************************************************************
+ * Size of memory for sharing data while changing exception levels.
+ *
+ * There are 2 cases where this memory buffer is used:
+ *
+ * - when BL1 (running in EL3) passes control to BL2 (running in S-EL1).
+ * BL1 needs to pass the memory layout to BL2, to allow BL2 to find out
+ * how much free trusted ram remains;
+ *
+ * - when BL2 (running in S-EL1) passes control back to BL1 (running in EL3)
+ * to make it run BL31. BL2 needs to pass the memory layout, as well as
+ * information on how to pass control to the non-trusted software image.
+ ******************************************************************************/
+#define EL_CHANGE_MEM_SIZE (sizeof(meminfo) + sizeof(el_change_info))
+
+
+#ifndef __ASSEMBLY__
+/*******************************************************************************
+ * Structure used for telling the next BL how much of a particular type of
+ * memory is available for its use and how much is already used.
+ ******************************************************************************/
+typedef struct {
+ unsigned long total_base;
+ long total_size;
+ unsigned long free_base;
+ long free_size;
+ unsigned long attr;
+ unsigned long next;
+} meminfo;
+
+typedef struct {
+ unsigned long arg0;
+ unsigned long arg1;
+ unsigned long arg2;
+ unsigned long arg3;
+ unsigned long arg4;
+ unsigned long arg5;
+ unsigned long arg6;
+ unsigned long arg7;
+} aapcs64_params;
+
+/*******************************************************************************
+ * This structure represents the superset of information needed while switching
+ * exception levels. The only two mechanisms to do so are ERET & SMC. In case of
+ * SMC all members apart from 'aapcs64_params' will be ignored. The 'next'
+ * member is a placeholder for a complicated case in the distant future when BL2
+ * will load multiple BL3x images as well as a non-secure image. So multiple
+ * such structures will have to be passed to BL31 in S-EL3.
+ ******************************************************************************/
+typedef struct {
+ unsigned long entrypoint;
+ unsigned long spsr;
+ unsigned long security_state;
+ aapcs64_params args;
+ unsigned long next;
+} el_change_info;
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+extern unsigned long page_align(unsigned long, unsigned);
+extern void change_security_state(unsigned int);
+extern int drop_el(aapcs64_params *, unsigned long, unsigned long);
+extern long raise_el(aapcs64_params *);
+extern long change_el(el_change_info *);
+extern unsigned long make_spsr(unsigned long, unsigned long, unsigned long);
+extern void init_bl2_mem_layout(meminfo *,
+ meminfo *,
+ unsigned int,
+ unsigned long) __attribute__((weak));
+extern void init_bl31_mem_layout(const meminfo *,
+ meminfo *,
+ unsigned int) __attribute__((weak));
+extern unsigned long load_image(meminfo *, const char *, unsigned int, unsigned long);
+extern int run_image(unsigned long,
+ unsigned long,
+ unsigned long,
+ meminfo *,
+ void *);
+extern unsigned long *get_el_change_mem_ptr(void);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __BL_COMMON_H__ */
diff --git a/include/mmio.h b/include/mmio.h
new file mode 100644
index 0000000..ecc1f87
--- /dev/null
+++ b/include/mmio.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __MMIO_H__
+#define __MMIO_H__
+
+#ifndef __ASSEMBLY__
+
+#include <stdint.h>
+
+extern void mmio_write_32(uintptr_t addr, uint32_t value);
+extern uint32_t mmio_read_32(uintptr_t addr);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __MMIO_H__ */
diff --git a/include/pm.h b/include/pm.h
new file mode 100644
index 0000000..7a4ef8b
--- /dev/null
+++ b/include/pm.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __PM_H__
+#define __PM_H__
+
+#ifndef __ASSEMBLY__
+
+/*******************************************************************************
+ * Structure populated by platform specific code to export routines which
+ * perform common low level pm functions
+ ******************************************************************************/
+typedef struct {
+ int (*cpu_on)(unsigned long);
+ int (*cpu_off)(unsigned long);
+ int (*cpu_suspend)(unsigned long);
+ int (*affinity_info)(unsigned long, unsigned int);
+} pm_frontend_ops;
+
+/*******************************************************************************
+ * Structure populated by a generic power management api implementation e.g.
+ * psci to perform api specific bits after a cpu has been turned on.
+ ******************************************************************************/
+typedef struct {
+ unsigned long (*cpu_off_finisher)(unsigned long);
+ unsigned long (*cpu_suspend_finisher)(unsigned long);
+} pm_backend_ops;
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+extern pm_frontend_ops *get_pm_frontend_ops(void);
+extern pm_backend_ops *get_pm_backend_ops(void);
+extern void set_pm_frontend_ops(pm_frontend_ops *);
+extern void set_pm_backend_ops(pm_backend_ops *);
+
+#endif /*__ASSEMBLY__*/
+
+#endif /* __PM_H__ */
diff --git a/include/psci.h b/include/psci.h
new file mode 100644
index 0000000..f63e32c
--- /dev/null
+++ b/include/psci.h
@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __PSCI_H__
+#define __PSCI_H__
+
+/*******************************************************************************
+ * Defines for runtime services func ids
+ ******************************************************************************/
+#define PSCI_VERSION 0x84000000
+#define PSCI_CPU_SUSPEND_AARCH32 0x84000001
+#define PSCI_CPU_SUSPEND_AARCH64 0xc4000001
+#define PSCI_CPU_OFF 0x84000002
+#define PSCI_CPU_ON_AARCH32 0x84000003
+#define PSCI_CPU_ON_AARCH64 0xc4000003
+#define PSCI_AFFINITY_INFO_AARCH32 0x84000004
+#define PSCI_AFFINITY_INFO_AARCH64 0xc4000004
+#define PSCI_MIG_AARCH32 0x84000005
+#define PSCI_MIG_AARCH64 0xc4000005
+#define PSCI_MIG_INFO_TYPE 0x84000006
+#define PSCI_MIG_INFO_UP_CPU_AARCH32 0x84000007
+#define PSCI_MIG_INFO_UP_CPU_AARCH64 0xc4000007
+#define PSCI_SYSTEM_OFF 0x84000008
+#define PSCI_SYSTEM_RESET 0x84000009
+
+/*******************************************************************************
+ * PSCI Migrate and friends
+ ******************************************************************************/
+#define PSCI_TOS_UP_MIG_CAP 0
+#define PSCI_TOS_NOT_UP_MIG_CAP 1
+#define PSCI_TOS_NOT_PRESENT 2
+
+/*******************************************************************************
+ * PSCI CPU_SUSPEND 'power_state' parameter specific defines
+ ******************************************************************************/
+#define PSTATE_ID_SHIFT 15
+#define PSTATE_TYPE_SHIFT 16
+#define PSTATE_AFF_LVL_SHIFT 25
+
+#define PSTATE_ID_MASK 0xffff
+#define PSTATE_TYPE_MASK 0x1
+#define PSTATE_AFF_LVL_MASK 0x3
+
+#define psci_get_pstate_id(pstate) (pstate >> PSTATE_ID_SHIFT) & \
+ PSTATE_ID_MASK
+#define psci_get_pstate_type(pstate) (pstate >> PSTATE_TYPE_SHIFT) & \
+ PSTATE_TYPE_MASK
+#define psci_get_pstate_afflvl(pstate) (pstate >> PSTATE_AFF_LVL_SHIFT) & \
+ PSTATE_AFF_LVL_MASK
+
+/*******************************************************************************
+ * PSCI version
+ ******************************************************************************/
+#define PSCI_MAJOR_VER (0 << 16)
+#define PSCI_MINOR_VER 0x2
+
+/*******************************************************************************
+ * PSCI error codes
+ ******************************************************************************/
+#define PSCI_E_SUCCESS 0
+#define PSCI_E_NOT_SUPPORTED -1
+#define PSCI_E_INVALID_PARAMS -2
+#define PSCI_E_DENIED -3
+#define PSCI_E_ALREADY_ON -4
+#define PSCI_E_ON_PENDING -5
+#define PSCI_E_INTERN_FAIL -6
+#define PSCI_E_NOT_PRESENT -7
+#define PSCI_E_DISABLED -8
+
+/*******************************************************************************
+ * PSCI affinity state related constants. An affinity instance could be present
+ * or absent physically to cater for asymmetric topologies. If present then it
+ * could in one of the 4 further defined states.
+ ******************************************************************************/
+#define PSCI_STATE_SHIFT 1
+#define PSCI_STATE_MASK 0x7
+#define psci_get_state(x) (x >> PSCI_STATE_SHIFT) & PSCI_STATE_MASK
+#define psci_set_state(x,y) x &= ~(PSCI_STATE_MASK << PSCI_STATE_SHIFT); \
+ x |= (y & PSCI_STATE_MASK) << PSCI_STATE_SHIFT;
+
+#define PSCI_AFF_ABSENT 0x0
+#define PSCI_AFF_PRESENT 0x1
+#define PSCI_STATE_OFF 0x0
+#define PSCI_STATE_ON_PENDING 0x1
+#define PSCI_STATE_SUSPEND 0x2
+#define PSCI_STATE_ON 0x3
+
+/* Number of affinity instances whose state this psci imp. can track */
+#define PSCI_NUM_AFFS 32ull
+
+#ifndef __ASSEMBLY__
+/*******************************************************************************
+ * Structure populated by platform specific code to export routines which
+ * perform common low level pm functions
+ ******************************************************************************/
+typedef struct {
+ int (*affinst_standby)(unsigned int);
+ int (*affinst_on)(unsigned long,
+ unsigned long,
+ unsigned long,
+ unsigned int,
+ unsigned int);
+ int (*affinst_off)(unsigned long, unsigned int, unsigned int);
+ int (*affinst_suspend)(unsigned long,
+ unsigned long,
+ unsigned long,
+ unsigned int,
+ unsigned int);
+ int (*affinst_on_finish)(unsigned long, unsigned int, unsigned int);
+ int (*affinst_suspend_finish)(unsigned long,
+ unsigned int,
+ unsigned int);
+} plat_pm_ops;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+extern unsigned int psci_version(void);
+extern int psci_cpu_on(unsigned long,
+ unsigned long,
+ unsigned long);
+extern int __psci_cpu_suspend(unsigned int, unsigned long, unsigned long);
+extern int __psci_cpu_off(void);
+extern int psci_affinity_info(unsigned long, unsigned int);
+extern int psci_migrate(unsigned int);
+extern unsigned int psci_migrate_info_type(void);
+extern unsigned long psci_migrate_info_up_cpu(void);
+extern void psci_system_off(void);
+extern void psci_system_reset(void);
+extern int psci_cpu_on(unsigned long,
+ unsigned long,
+ unsigned long);
+extern void psci_aff_on_finish_entry(void);
+extern void psci_aff_suspend_finish_entry(void);
+extern void psci_setup(unsigned long);
+#endif /*__ASSEMBLY__*/
+
+
+#endif /* __PSCI_H__ */
diff --git a/include/runtime_svc.h b/include/runtime_svc.h
new file mode 100644
index 0000000..ea6accb
--- /dev/null
+++ b/include/runtime_svc.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __RUNTIME_SVC_H__
+#define __RUNTIME_SVC_H__
+#include <psci.h>
+
+/*******************************************************************************
+ * Bit definitions inside the function id as per the SMC calling convention
+ ******************************************************************************/
+#define FUNCID_TYPE_SHIFT 31
+#define FUNCID_CC_SHIFT 30
+#define FUNCID_OWNER_SHIFT 24
+#define FUNCID_NUM_SHIFT 0
+
+#define FUNCID_TYPE_MASK 0x1
+#define FUNCID_CC_MASK 0x1
+#define FUNCID_OWNER_MASK 0x3f
+#define FUNCID_NUM_MASK 0xffff
+
+#define GET_SMC_CC(id) ((id >> FUNCID_CC_SHIFT) & \
+ FUNCID_CC_MASK)
+
+#define SMC_64 1
+#define SMC_32 0
+#define SMC_UNK 0xffffffff
+
+/*******************************************************************************
+ * Constants to indicate type of exception to the common exception handler.
+ ******************************************************************************/
+#define SYNC_EXCEPTION_SP_EL0 0x0
+#define IRQ_SP_EL0 0x1
+#define FIQ_SP_EL0 0x2
+#define SERROR_SP_EL0 0x3
+#define SYNC_EXCEPTION_SP_ELX 0x4
+#define IRQ_SP_ELX 0x5
+#define FIQ_SP_ELX 0x6
+#define SERROR_SP_ELX 0x7
+#define SYNC_EXCEPTION_AARCH64 0x8
+#define IRQ_AARCH64 0x9
+#define FIQ_AARCH64 0xa
+#define SERROR_AARCH64 0xb
+#define SYNC_EXCEPTION_AARCH32 0xc
+#define IRQ_AARCH32 0xd
+#define FIQ_AARCH32 0xe
+#define SERROR_AARCH32 0xf
+
+#ifndef __ASSEMBLY__
+
+typedef struct {
+ unsigned long x0;
+ unsigned long x1;
+ unsigned long x2;
+ unsigned long x3;
+ unsigned long x4;
+ unsigned long x5;
+ unsigned long x6;
+ unsigned long x7;
+ unsigned long x8;
+ unsigned long x9;
+ unsigned long x10;
+ unsigned long x11;
+ unsigned long x12;
+ unsigned long x13;
+ unsigned long x14;
+ unsigned long x15;
+ unsigned long x16;
+ unsigned long x17;
+ unsigned long x18;
+ unsigned long x19;
+ unsigned long x20;
+ unsigned long x21;
+ unsigned long x22;
+ unsigned long x23;
+ unsigned long x24;
+ unsigned long x25;
+ unsigned long x26;
+ unsigned long x27;
+ unsigned long x28;
+ unsigned long sp_el0;
+ unsigned long spsr;
+ unsigned long fp;
+ unsigned long lr;
+} gp_regs;
+
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+extern void runtime_svc_init(unsigned long mpidr);
+
+#endif /*__ASSEMBLY__*/
+
+
+#endif /* __RUNTIME_SVC_H__ */
diff --git a/include/semihosting.h b/include/semihosting.h
new file mode 100644
index 0000000..b56ff2f
--- /dev/null
+++ b/include/semihosting.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __SEMIHOSTING_H__
+#define __SEMIHOSTING_H__
+
+#define SEMIHOSTING_SYS_OPEN 0x01
+#define SEMIHOSTING_SYS_CLOSE 0x02
+#define SEMIHOSTING_SYS_WRITE0 0x04
+#define SEMIHOSTING_SYS_WRITEC 0x03
+#define SEMIHOSTING_SYS_WRITE 0x05
+#define SEMIHOSTING_SYS_READ 0x06
+#define SEMIHOSTING_SYS_READC 0x07
+#define SEMIHOSTING_SYS_SEEK 0x0A
+#define SEMIHOSTING_SYS_FLEN 0x0C
+#define SEMIHOSTING_SYS_REMOVE 0x0E
+#define SEMIHOSTING_SYS_SYSTEM 0x12
+#define SEMIHOSTING_SYS_ERRNO 0x13
+
+#define FOPEN_MODE_R 0x0
+#define FOPEN_MODE_RB 0x1
+#define FOPEN_MODE_RPLUS 0x2
+#define FOPEN_MODE_RPLUSB 0x3
+#define FOPEN_MODE_W 0x4
+#define FOPEN_MODE_WB 0x5
+#define FOPEN_MODE_WPLUS 0x6
+#define FOPEN_MODE_WPLUSB 0x7
+#define FOPEN_MODE_A 0x8
+#define FOPEN_MODE_AB 0x9
+#define FOPEN_MODE_APLUS 0xa
+#define FOPEN_MODE_APLUSB 0xb
+
+int semihosting_connection_supported(void);
+int semihosting_file_open(const char *file_name, unsigned int mode);
+int semihosting_file_seek(int file_handle, unsigned int offset);
+int semihosting_file_read(int file_handle, int *length, void *buffer);
+int semihosting_file_write(int file_handle, int *length, void *buffer);
+int semihosting_file_close(int file_handle);
+int semihosting_file_length(int file_handle);
+int semihosting_system(char *command_line);
+int semihosting_get_flen(const char* file_name);
+int semihosting_download_file(const char* file_name, int buf_size, void *buf);
+void semihosting_write_char(char character);
+void semihosting_write_string(char *string);
+char semihosting_read_char(void);
+
+#endif /* __SEMIHOSTING_H__ */
diff --git a/include/spinlock.h b/include/spinlock.h
new file mode 100644
index 0000000..9cc261f
--- /dev/null
+++ b/include/spinlock.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+#ifndef __SPINLOCK_H__
+#define __SPINLOCK_H__
+
+typedef struct {
+ volatile unsigned int lock;
+} spinlock_t;
+
+void spin_lock(spinlock_t *lock);
+void spin_unlock(spinlock_t *lock);
+
+#endif /* __SPINLOCK_H__ */
diff --git a/lib/arch/aarch64/cache_helpers.S b/lib/arch/aarch64/cache_helpers.S
new file mode 100644
index 0000000..b8a5608
--- /dev/null
+++ b/lib/arch/aarch64/cache_helpers.S
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+#include <asm_macros.S>
+
+ .globl dcisw
+ .globl dccisw
+ .globl dccsw
+ .globl dccvac
+ .globl dcivac
+ .globl dccivac
+ .globl dccvau
+ .globl dczva
+ .globl flush_dcache_range
+ .globl inv_dcache_range
+ .globl dcsw_op_louis
+ .globl dcsw_op_all
+
+ .section .text, "ax"; .align 3
+
+dcisw:; .type dcisw, %function
+ dc isw, x0
+ dsb sy
+ isb
+ ret
+
+
+dccisw:; .type dccisw, %function
+ dc cisw, x0
+ dsb sy
+ isb
+ ret
+
+
+dccsw:; .type dccsw, %function
+ dc csw, x0
+ dsb sy
+ isb
+ ret
+
+
+dccvac:; .type dccvac, %function
+ dc cvac, x0
+ dsb sy
+ isb
+ ret
+
+
+dcivac:; .type dcivac, %function
+ dc ivac, x0
+ dsb sy
+ isb
+ ret
+
+
+dccivac:; .type dccivac, %function
+ dc civac, x0
+ dsb sy
+ isb
+ ret
+
+
+dccvau:; .type dccvau, %function
+ dc cvau, x0
+ dsb sy
+ isb
+ ret
+
+
+dczva:; .type dczva, %function
+ dc zva, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* ------------------------------------------
+ * Clean+Invalidate from base address till
+ * size. 'x0' = addr, 'x1' = size
+ * ------------------------------------------
+ */
+flush_dcache_range:; .type flush_dcache_range, %function
+ dcache_line_size x2, x3
+ add x1, x0, x1
+ sub x3, x2, #1
+ bic x0, x0, x3
+flush_loop:
+ dc civac, x0
+ add x0, x0, x2
+ cmp x0, x1
+ b.lo flush_loop
+ dsb sy
+ ret
+
+
+ /* ------------------------------------------
+ * Invalidate from base address till
+ * size. 'x0' = addr, 'x1' = size
+ * ------------------------------------------
+ */
+inv_dcache_range:; .type inv_dcache_range, %function
+ dcache_line_size x2, x3
+ add x1, x0, x1
+ sub x3, x2, #1
+ bic x0, x0, x3
+inv_loop:
+ dc ivac, x0
+ add x0, x0, x2
+ cmp x0, x1
+ b.lo inv_loop
+ dsb sy
+ ret
+
+
+ /* ------------------------------------------
+ * Data cache operations by set/way to the
+ * level specified
+ * ------------------------------------------
+ * ----------------------------------
+ * Call this func with the clidr in
+ * x0, starting cache level in x10,
+ * last cache level in x3 & cm op in
+ * x14
+ * ----------------------------------
+ */
+dcsw_op:; .type dcsw_op, %function
+all_start_at_level:
+ add x2, x10, x10, lsr #1 // work out 3x current cache level
+ lsr x1, x0, x2 // extract cache type bits from clidr
+ and x1, x1, #7 // mask of the bits for current cache only
+ cmp x1, #2 // see what cache we have at this level
+ b.lt skip // skip if no cache, or just i-cache
+ msr csselr_el1, x10 // select current cache level in csselr
+ isb // isb to sych the new cssr&csidr
+ mrs x1, ccsidr_el1 // read the new ccsidr
+ and x2, x1, #7 // extract the length of the cache lines
+ add x2, x2, #4 // add 4 (line length offset)
+ mov x4, #0x3ff
+ and x4, x4, x1, lsr #3 // find maximum number on the way size
+ clz w5, w4 // find bit position of way size increment
+ mov x7, #0x7fff
+ and x7, x7, x1, lsr #13 // extract max number of the index size
+loop2:
+ mov x9, x4 // create working copy of max way size
+loop3:
+ lsl x6, x9, x5
+ orr x11, x10, x6 // factor way and cache number into x11
+ lsl x6, x7, x2
+ orr x11, x11, x6 // factor index number into x11
+ mov x12, x0
+ mov x13, x30 // lr
+ mov x0, x11
+ blr x14
+ mov x0, x12
+ mov x30, x13 // lr
+ subs x9, x9, #1 // decrement the way
+ b.ge loop3
+ subs x7, x7, #1 // decrement the index
+ b.ge loop2
+skip:
+ add x10, x10, #2 // increment cache number
+ cmp x3, x10
+ b.gt all_start_at_level
+finished:
+ mov x10, #0 // swith back to cache level 0
+ msr csselr_el1, x10 // select current cache level in csselr
+ dsb sy
+ isb
+ ret
+
+
+do_dcsw_op:; .type do_dcsw_op, %function
+ cbz x3, exit
+ cmp x0, #DCISW
+ b.eq dc_isw
+ cmp x0, #DCCISW
+ b.eq dc_cisw
+ cmp x0, #DCCSW
+ b.eq dc_csw
+dc_isw:
+ mov x0, x9
+ adr x14, dcisw
+ b dcsw_op
+dc_cisw:
+ mov x0, x9
+ adr x14, dccisw
+ b dcsw_op
+dc_csw:
+ mov x0, x9
+ adr x14, dccsw
+ b dcsw_op
+exit:
+ ret
+
+
+dcsw_op_louis:; .type dcsw_op_louis, %function
+ dsb sy
+ setup_dcsw_op_args x10, x3, x9, #LOUIS_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+ b do_dcsw_op
+
+
+dcsw_op_all:; .type dcsw_op_all, %function
+ dsb sy
+ setup_dcsw_op_args x10, x3, x9, #LOC_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+ b do_dcsw_op
diff --git a/lib/arch/aarch64/misc_helpers.S b/lib/arch/aarch64/misc_helpers.S
new file mode 100644
index 0000000..8c1f740
--- /dev/null
+++ b/lib/arch/aarch64/misc_helpers.S
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+
+ .globl enable_irq
+ .globl disable_irq
+
+ .globl enable_fiq
+ .globl disable_fiq
+
+ .globl enable_serror
+ .globl disable_serror
+
+ .globl read_daif
+ .globl write_daif
+
+ .globl read_spsr
+ .globl read_spsr_el1
+ .globl read_spsr_el2
+ .globl read_spsr_el3
+
+ .globl write_spsr
+ .globl write_spsr_el1
+ .globl write_spsr_el2
+ .globl write_spsr_el3
+
+ .globl read_elr
+ .globl read_elr_el1
+ .globl read_elr_el2
+ .globl read_elr_el3
+
+ .globl write_elr
+ .globl write_elr_el1
+ .globl write_elr_el2
+ .globl write_elr_el3
+
+ .globl get_afflvl_shift
+ .globl mpidr_mask_lower_afflvls
+ .globl dsb
+ .globl isb
+ .globl sev
+ .globl wfe
+ .globl wfi
+ .globl eret
+ .globl smc
+
+
+ .section .text, "ax"
+
+get_afflvl_shift:; .type get_afflvl_shift, %function
+ cmp x0, #3
+ cinc x0, x0, eq
+ mov x1, #MPIDR_AFFLVL_SHIFT
+ lsl x0, x0, x1
+ ret
+
+mpidr_mask_lower_afflvls:; .type mpidr_mask_lower_afflvls, %function
+ cmp x1, #3
+ cinc x1, x1, eq
+ mov x2, #MPIDR_AFFLVL_SHIFT
+ lsl x2, x1, x2
+ lsr x0, x0, x2
+ lsl x0, x0, x2
+ ret
+
+ /* -----------------------------------------------------
+ * Asynchronous exception manipulation accessors
+ * -----------------------------------------------------
+ */
+enable_irq:; .type enable_irq, %function
+ msr daifclr, #DAIF_IRQ_BIT
+ ret
+
+
+enable_fiq:; .type enable_fiq, %function
+ msr daifclr, #DAIF_FIQ_BIT
+ ret
+
+
+enable_serror:; .type enable_serror, %function
+ msr daifclr, #DAIF_ABT_BIT
+ ret
+
+
+disable_irq:; .type disable_irq, %function
+ msr daifset, #DAIF_IRQ_BIT
+ ret
+
+
+disable_fiq:; .type disable_fiq, %function
+ msr daifset, #DAIF_FIQ_BIT
+ ret
+
+
+disable_serror:; .type disable_serror, %function
+ msr daifset, #DAIF_ABT_BIT
+ ret
+
+
+read_daif:; .type read_daif, %function
+ mrs x0, daif
+ ret
+
+
+write_daif:; .type write_daif, %function
+ msr daif, x0
+ ret
+
+
+read_spsr:; .type read_spsr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_spsr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_spsr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_spsr_el3
+
+
+read_spsr_el1:; .type read_spsr_el1, %function
+ mrs x0, spsr_el1
+ ret
+
+
+read_spsr_el2:; .type read_spsr_el2, %function
+ mrs x0, spsr_el2
+ ret
+
+
+read_spsr_el3:; .type read_spsr_el3, %function
+ mrs x0, spsr_el3
+ ret
+
+
+write_spsr:; .type write_spsr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_spsr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_spsr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_spsr_el3
+
+
+write_spsr_el1:; .type write_spsr_el1, %function
+ msr spsr_el1, x0
+ isb
+ ret
+
+
+write_spsr_el2:; .type write_spsr_el2, %function
+ msr spsr_el2, x0
+ isb
+ ret
+
+
+write_spsr_el3:; .type write_spsr_el3, %function
+ msr spsr_el3, x0
+ isb
+ ret
+
+
+read_elr:; .type read_elr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_elr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_elr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_elr_el3
+
+
+read_elr_el1:; .type read_elr_el1, %function
+ mrs x0, elr_el1
+ ret
+
+
+read_elr_el2:; .type read_elr_el2, %function
+ mrs x0, elr_el2
+ ret
+
+
+read_elr_el3:; .type read_elr_el3, %function
+ mrs x0, elr_el3
+ ret
+
+
+write_elr:; .type write_elr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_elr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_elr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_elr_el3
+
+
+write_elr_el1:; .type write_elr_el1, %function
+ msr elr_el1, x0
+ isb
+ ret
+
+
+write_elr_el2:; .type write_elr_el2, %function
+ msr elr_el2, x0
+ isb
+ ret
+
+
+write_elr_el3:; .type write_elr_el3, %function
+ msr elr_el3, x0
+ isb
+ ret
+
+
+dsb:; .type dsb, %function
+ dsb sy
+ ret
+
+
+isb:; .type isb, %function
+ isb
+ ret
+
+
+sev:; .type sev, %function
+ sev
+ ret
+
+
+wfe:; .type wfe, %function
+ wfe
+ ret
+
+
+wfi:; .type wfi, %function
+ wfi
+ ret
+
+
+eret:; .type eret, %function
+ eret
+
+
+smc:; .type smc, %function
+ smc #0
diff --git a/lib/arch/aarch64/sysreg_helpers.S b/lib/arch/aarch64/sysreg_helpers.S
new file mode 100644
index 0000000..e68192f
--- /dev/null
+++ b/lib/arch/aarch64/sysreg_helpers.S
@@ -0,0 +1,1154 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+
+ .globl read_vbar
+ .globl read_vbar_el1
+ .globl read_vbar_el2
+ .globl read_vbar_el3
+ .globl write_vbar
+ .globl write_vbar_el1
+ .globl write_vbar_el2
+ .globl write_vbar_el3
+
+ .globl read_sctlr
+ .globl read_sctlr_el1
+ .globl read_sctlr_el2
+ .globl read_sctlr_el3
+ .globl write_sctlr
+ .globl write_sctlr_el1
+ .globl write_sctlr_el2
+ .globl write_sctlr_el3
+
+ .globl read_actlr
+ .globl read_actlr_el1
+ .globl read_actlr_el2
+ .globl read_actlr_el3
+ .globl write_actlr
+ .globl write_actlr_el1
+ .globl write_actlr_el2
+ .globl write_actlr_el3
+
+ .globl read_esr
+ .globl read_esr_el1
+ .globl read_esr_el2
+ .globl read_esr_el3
+ .globl write_esr
+ .globl write_esr_el1
+ .globl write_esr_el2
+ .globl write_esr_el3
+
+ .globl read_afsr0
+ .globl read_afsr0_el1
+ .globl read_afsr0_el2
+ .globl read_afsr0_el3
+ .globl write_afsr0
+ .globl write_afsr0_el1
+ .globl write_afsr0_el2
+ .globl write_afsr0_el3
+
+ .globl read_afsr1
+ .globl read_afsr1_el1
+ .globl read_afsr1_el2
+ .globl read_afsr1_el3
+ .globl write_afsr1
+ .globl write_afsr1_el1
+ .globl write_afsr1_el2
+ .globl write_afsr1_el3
+
+ .globl read_far
+ .globl read_far_el1
+ .globl read_far_el2
+ .globl read_far_el3
+ .globl write_far
+ .globl write_far_el1
+ .globl write_far_el2
+ .globl write_far_el3
+
+ .globl read_mair
+ .globl read_mair_el1
+ .globl read_mair_el2
+ .globl read_mair_el3
+ .globl write_mair
+ .globl write_mair_el1
+ .globl write_mair_el2
+ .globl write_mair_el3
+
+ .globl read_amair
+ .globl read_amair_el1
+ .globl read_amair_el2
+ .globl read_amair_el3
+ .globl write_amair
+ .globl write_amair_el1
+ .globl write_amair_el2
+ .globl write_amair_el3
+
+ .globl read_rvbar
+ .globl read_rvbar_el1
+ .globl read_rvbar_el2
+ .globl read_rvbar_el3
+
+ .globl read_rmr
+ .globl read_rmr_el1
+ .globl read_rmr_el2
+ .globl read_rmr_el3
+ .globl write_rmr
+ .globl write_rmr_el1
+ .globl write_rmr_el2
+ .globl write_rmr_el3
+
+ .globl read_tcr
+ .globl read_tcr_el1
+ .globl read_tcr_el2
+ .globl read_tcr_el3
+ .globl write_tcr
+ .globl write_tcr_el1
+ .globl write_tcr_el2
+ .globl write_tcr_el3
+
+ .globl read_cptr
+ .globl read_cptr_el2
+ .globl read_cptr_el3
+ .globl write_cptr
+ .globl write_cptr_el2
+ .globl write_cptr_el3
+
+ .globl read_ttbr0
+ .globl read_ttbr0_el1
+ .globl read_ttbr0_el2
+ .globl read_ttbr0_el3
+ .globl write_ttbr0
+ .globl write_ttbr0_el1
+ .globl write_ttbr0_el2
+ .globl write_ttbr0_el3
+
+ .globl read_ttbr1
+ .globl read_ttbr1_el1
+ .globl read_ttbr1_el2
+ .globl write_ttbr1
+ .globl write_ttbr1_el1
+ .globl write_ttbr1_el2
+
+ .globl read_cpacr
+ .globl write_cpacr
+
+ .globl read_cntfrq
+ .globl write_cntfrq
+
+ .globl read_cpuectlr
+ .globl write_cpuectlr
+
+ .globl read_cnthctl_el2
+ .globl write_cnthctl_el2
+
+ .globl read_cntfrq_el0
+ .globl write_cntfrq_el0
+
+ .globl read_scr
+ .globl write_scr
+
+ .globl read_hcr
+ .globl write_hcr
+
+ .globl read_midr
+ .globl read_mpidr
+
+ .globl read_current_el
+ .globl read_id_pfr1_el1
+ .globl read_id_aa64pfr0_el1
+
+#if SUPPORT_VFP
+ .globl enable_vfp
+ .globl read_fpexc
+ .globl write_fpexc
+#endif
+
+
+ .section .text, "ax"
+
+read_current_el:; .type read_current_el, %function
+ mrs x0, CurrentEl
+ ret
+
+
+read_id_pfr1_el1:; .type read_id_pfr1_el1, %function
+ mrs x0, id_pfr1_el1
+ ret
+
+
+read_id_aa64pfr0_el1:; .type read_id_aa64pfr0_el1, %function
+ mrs x0, id_aa64pfr0_el1
+ ret
+
+
+ /* -----------------------------------------------------
+ * VBAR accessors
+ * -----------------------------------------------------
+ */
+read_vbar:; .type read_vbar, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_vbar_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_vbar_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_vbar_el3
+
+
+read_vbar_el1:; .type read_vbar_el1, %function
+ mrs x0, vbar_el1
+ ret
+
+
+read_vbar_el2:; .type read_vbar_el2, %function
+ mrs x0, vbar_el2
+ ret
+
+
+read_vbar_el3:; .type read_vbar_el3, %function
+ mrs x0, vbar_el3
+ ret
+
+
+write_vbar:; .type write_vbar, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_vbar_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_vbar_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_vbar_el3
+
+
+write_vbar_el1:; .type write_vbar_el1, %function
+ msr vbar_el1, x0
+ isb
+ ret
+
+
+write_vbar_el2:; .type write_vbar_el2, %function
+ msr vbar_el2, x0
+ isb
+ ret
+
+
+write_vbar_el3:; .type write_vbar_el3, %function
+ msr vbar_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * AFSR0 accessors
+ * -----------------------------------------------------
+ */
+read_afsr0:; .type read_afsr0, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_afsr0_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_afsr0_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_afsr0_el3
+
+
+read_afsr0_el1:; .type read_afsr0_el1, %function
+ mrs x0, afsr0_el1
+ ret
+
+
+read_afsr0_el2:; .type read_afsr0_el2, %function
+ mrs x0, afsr0_el2
+ ret
+
+
+read_afsr0_el3:; .type read_afsr0_el3, %function
+ mrs x0, afsr0_el3
+ ret
+
+
+write_afsr0:; .type write_afsr0, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_afsr0_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_afsr0_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_afsr0_el3
+
+
+write_afsr0_el1:; .type write_afsr0_el1, %function
+ msr afsr0_el1, x0
+ isb
+ ret
+
+
+write_afsr0_el2:; .type write_afsr0_el2, %function
+ msr afsr0_el2, x0
+ isb
+ ret
+
+
+write_afsr0_el3:; .type write_afsr0_el3, %function
+ msr afsr0_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * FAR accessors
+ * -----------------------------------------------------
+ */
+read_far:; .type read_far, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_far_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_far_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_far_el3
+
+
+read_far_el1:; .type read_far_el1, %function
+ mrs x0, far_el1
+ ret
+
+
+read_far_el2:; .type read_far_el2, %function
+ mrs x0, far_el2
+ ret
+
+
+read_far_el3:; .type read_far_el3, %function
+ mrs x0, far_el3
+ ret
+
+
+write_far:; .type write_far, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_far_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_far_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_far_el3
+
+
+write_far_el1:; .type write_far_el1, %function
+ msr far_el1, x0
+ isb
+ ret
+
+
+write_far_el2:; .type write_far_el2, %function
+ msr far_el2, x0
+ isb
+ ret
+
+
+write_far_el3:; .type write_far_el3, %function
+ msr far_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * MAIR accessors
+ * -----------------------------------------------------
+ */
+read_mair:; .type read_mair, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_mair_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_mair_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_mair_el3
+
+
+read_mair_el1:; .type read_mair_el1, %function
+ mrs x0, mair_el1
+ ret
+
+
+read_mair_el2:; .type read_mair_el2, %function
+ mrs x0, mair_el2
+ ret
+
+
+read_mair_el3:; .type read_mair_el3, %function
+ mrs x0, mair_el3
+ ret
+
+
+write_mair:; .type write_mair, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_mair_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_mair_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_mair_el3
+
+
+write_mair_el1:; .type write_mair_el1, %function
+ msr mair_el1, x0
+ isb
+ ret
+
+
+write_mair_el2:; .type write_mair_el2, %function
+ msr mair_el2, x0
+ isb
+ ret
+
+
+write_mair_el3:; .type write_mair_el3, %function
+ msr mair_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * AMAIR accessors
+ * -----------------------------------------------------
+ */
+read_amair:; .type read_amair, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_amair_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_amair_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_amair_el3
+
+
+read_amair_el1:; .type read_amair_el1, %function
+ mrs x0, amair_el1
+ ret
+
+
+read_amair_el2:; .type read_amair_el2, %function
+ mrs x0, amair_el2
+ ret
+
+
+read_amair_el3:; .type read_amair_el3, %function
+ mrs x0, amair_el3
+ ret
+
+
+write_amair:; .type write_amair, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_amair_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_amair_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_amair_el3
+
+
+write_amair_el1:; .type write_amair_el1, %function
+ msr amair_el1, x0
+ isb
+ ret
+
+
+write_amair_el2:; .type write_amair_el2, %function
+ msr amair_el2, x0
+ isb
+ ret
+
+
+write_amair_el3:; .type write_amair_el3, %function
+ msr amair_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * RVBAR accessors
+ * -----------------------------------------------------
+ */
+read_rvbar:; .type read_rvbar, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_rvbar_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_rvbar_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_rvbar_el3
+
+
+read_rvbar_el1:; .type read_rvbar_el1, %function
+ mrs x0, rvbar_el1
+ ret
+
+
+read_rvbar_el2:; .type read_rvbar_el2, %function
+ mrs x0, rvbar_el2
+ ret
+
+
+read_rvbar_el3:; .type read_rvbar_el3, %function
+ mrs x0, rvbar_el3
+ ret
+
+
+ /* -----------------------------------------------------
+ * RMR accessors
+ * -----------------------------------------------------
+ */
+read_rmr:; .type read_rmr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_rmr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_rmr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_rmr_el3
+
+
+read_rmr_el1:; .type read_rmr_el1, %function
+ mrs x0, rmr_el1
+ ret
+
+
+read_rmr_el2:; .type read_rmr_el2, %function
+ mrs x0, rmr_el2
+ ret
+
+
+read_rmr_el3:; .type read_rmr_el3, %function
+ mrs x0, rmr_el3
+ ret
+
+
+write_rmr:; .type write_rmr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_rmr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_rmr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_rmr_el3
+
+
+write_rmr_el1:; .type write_rmr_el1, %function
+ msr rmr_el1, x0
+ isb
+ ret
+
+
+write_rmr_el2:; .type write_rmr_el2, %function
+ msr rmr_el2, x0
+ isb
+ ret
+
+
+write_rmr_el3:; .type write_rmr_el3, %function
+ msr rmr_el3, x0
+ isb
+ ret
+
+
+read_afsr1:; .type read_afsr1, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_afsr1_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_afsr1_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_afsr1_el3
+
+
+ /* -----------------------------------------------------
+ * AFSR1 accessors
+ * -----------------------------------------------------
+ */
+read_afsr1_el1:; .type read_afsr1_el1, %function
+ mrs x0, afsr1_el1
+ ret
+
+
+read_afsr1_el2:; .type read_afsr1_el2, %function
+ mrs x0, afsr1_el2
+ ret
+
+
+read_afsr1_el3:; .type read_afsr1_el3, %function
+ mrs x0, afsr1_el3
+ ret
+
+
+write_afsr1:; .type write_afsr1, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_afsr1_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_afsr1_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_afsr1_el3
+
+
+write_afsr1_el1:; .type write_afsr1_el1, %function
+ msr afsr1_el1, x0
+ isb
+ ret
+
+
+write_afsr1_el2:; .type write_afsr1_el2, %function
+ msr afsr1_el2, x0
+ isb
+ ret
+
+
+write_afsr1_el3:; .type write_afsr1_el3, %function
+ msr afsr1_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * SCTLR accessors
+ * -----------------------------------------------------
+ */
+read_sctlr:; .type read_sctlr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_sctlr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_sctlr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_sctlr_el3
+
+
+read_sctlr_el1:; .type read_sctlr_el1, %function
+ mrs x0, sctlr_el1
+ ret
+
+
+read_sctlr_el2:; .type read_sctlr_el2, %function
+ mrs x0, sctlr_el2
+ ret
+
+
+read_sctlr_el3:; .type read_sctlr_el3, %function
+ mrs x0, sctlr_el3
+ ret
+
+
+write_sctlr:; .type write_sctlr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_sctlr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_sctlr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_sctlr_el3
+
+
+write_sctlr_el1:; .type write_sctlr_el1, %function
+ msr sctlr_el1, x0
+ dsb sy
+ isb
+ ret
+
+
+write_sctlr_el2:; .type write_sctlr_el2, %function
+ msr sctlr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+write_sctlr_el3:; .type write_sctlr_el3, %function
+ msr sctlr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * ACTLR accessors
+ * -----------------------------------------------------
+ */
+read_actlr:; .type read_actlr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_actlr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_actlr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_actlr_el3
+
+
+read_actlr_el1:; .type read_actlr_el1, %function
+ mrs x0, actlr_el1
+ ret
+
+
+read_actlr_el2:; .type read_actlr_el2, %function
+ mrs x0, actlr_el2
+ ret
+
+
+read_actlr_el3:; .type read_actlr_el3, %function
+ mrs x0, actlr_el3
+ ret
+
+
+write_actlr:; .type write_actlr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_actlr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_actlr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_actlr_el3
+
+
+write_actlr_el1:; .type write_actlr_el1, %function
+ msr actlr_el1, x0
+ dsb sy
+ isb
+ ret
+
+
+write_actlr_el2:; .type write_actlr_el2, %function
+ msr actlr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+write_actlr_el3:; .type write_actlr_el3, %function
+ msr actlr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * ESR accessors
+ * -----------------------------------------------------
+ */
+read_esr:; .type read_esr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_esr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_esr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_esr_el3
+
+
+read_esr_el1:; .type read_esr_el1, %function
+ mrs x0, esr_el1
+ ret
+
+
+read_esr_el2:; .type read_esr_el2, %function
+ mrs x0, esr_el2
+ ret
+
+
+read_esr_el3:; .type read_esr_el3, %function
+ mrs x0, esr_el3
+ ret
+
+
+write_esr:; .type write_esr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_esr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_esr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_esr_el3
+
+
+write_esr_el1:; .type write_esr_el1, %function
+ msr esr_el1, x0
+ dsb sy
+ isb
+ ret
+
+
+write_esr_el2:; .type write_esr_el2, %function
+ msr esr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+write_esr_el3:; .type write_esr_el3, %function
+ msr esr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * TCR accessors
+ * -----------------------------------------------------
+ */
+read_tcr:; .type read_tcr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_tcr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_tcr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_tcr_el3
+
+
+read_tcr_el1:; .type read_tcr_el1, %function
+ mrs x0, tcr_el1
+ ret
+
+
+read_tcr_el2:; .type read_tcr_el2, %function
+ mrs x0, tcr_el2
+ ret
+
+
+read_tcr_el3:; .type read_tcr_el3, %function
+ mrs x0, tcr_el3
+ ret
+
+
+write_tcr:; .type write_tcr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_tcr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_tcr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_tcr_el3
+
+
+write_tcr_el1:; .type write_tcr_el1, %function
+ msr tcr_el1, x0
+ dsb sy
+ isb
+ ret
+
+
+write_tcr_el2:; .type write_tcr_el2, %function
+ msr tcr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+write_tcr_el3:; .type write_tcr_el3, %function
+ msr tcr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * CPTR accessors
+ * -----------------------------------------------------
+ */
+read_cptr:; .type read_cptr, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_cptr_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_cptr_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_cptr_el3
+
+
+read_cptr_el1:; .type read_cptr_el1, %function
+ b read_cptr_el1
+ ret
+
+
+read_cptr_el2:; .type read_cptr_el2, %function
+ mrs x0, cptr_el2
+ ret
+
+
+read_cptr_el3:; .type read_cptr_el3, %function
+ mrs x0, cptr_el3
+ ret
+
+
+write_cptr:; .type write_cptr, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_cptr_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_cptr_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_cptr_el3
+
+
+write_cptr_el1:; .type write_cptr_el1, %function
+ b write_cptr_el1
+
+
+write_cptr_el2:; .type write_cptr_el2, %function
+ msr cptr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+write_cptr_el3:; .type write_cptr_el3, %function
+ msr cptr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * TTBR0 accessors
+ * -----------------------------------------------------
+ */
+read_ttbr0:; .type read_ttbr0, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_ttbr0_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_ttbr0_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_ttbr0_el3
+
+
+read_ttbr0_el1:; .type read_ttbr0_el1, %function
+ mrs x0, ttbr0_el1
+ ret
+
+
+read_ttbr0_el2:; .type read_ttbr0_el2, %function
+ mrs x0, ttbr0_el2
+ ret
+
+
+read_ttbr0_el3:; .type read_ttbr0_el3, %function
+ mrs x0, ttbr0_el3
+ ret
+
+
+write_ttbr0:; .type write_ttbr0, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_ttbr0_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_ttbr0_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_ttbr0_el3
+
+
+write_ttbr0_el1:; .type write_ttbr0_el1, %function
+ msr ttbr0_el1, x0
+ isb
+ ret
+
+
+write_ttbr0_el2:; .type write_ttbr0_el2, %function
+ msr ttbr0_el2, x0
+ isb
+ ret
+
+
+write_ttbr0_el3:; .type write_ttbr0_el3, %function
+ msr ttbr0_el3, x0
+ isb
+ ret
+
+
+ /* -----------------------------------------------------
+ * TTBR1 accessors
+ * -----------------------------------------------------
+ */
+read_ttbr1:; .type read_ttbr1, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq read_ttbr1_el1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq read_ttbr1_el2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq read_ttbr1_el3
+
+
+read_ttbr1_el1:; .type read_ttbr1_el1, %function
+ mrs x0, ttbr1_el1
+ ret
+
+
+read_ttbr1_el2:; .type read_ttbr1_el2, %function
+ b read_ttbr1_el2
+
+
+read_ttbr1_el3:; .type read_ttbr1_el3, %function
+ b read_ttbr1_el3
+
+
+write_ttbr1:; .type write_ttbr1, %function
+ mrs x1, CurrentEl
+ cmp x1, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq write_ttbr1_el1
+ cmp x1, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq write_ttbr1_el2
+ cmp x1, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq write_ttbr1_el3
+
+
+write_ttbr1_el1:; .type write_ttbr1_el1, %function
+ msr ttbr1_el1, x0
+ isb
+ ret
+
+
+write_ttbr1_el2:; .type write_ttbr1_el2, %function
+ b write_ttbr1_el2
+
+
+write_ttbr1_el3:; .type write_ttbr1_el3, %function
+ b write_ttbr1_el3
+
+
+read_hcr:; .type read_hcr, %function
+ mrs x0, hcr_el2
+ ret
+
+
+write_hcr:; .type write_hcr, %function
+ msr hcr_el2, x0
+ dsb sy
+ isb
+ ret
+
+
+read_cpacr:; .type read_cpacr, %function
+ mrs x0, cpacr_el1
+ ret
+
+
+write_cpacr:; .type write_cpacr, %function
+ msr cpacr_el1, x0
+ ret
+
+
+read_cntfrq_el0:; .type read_cntfrq_el0, %function
+ mrs x0, cntfrq_el0
+ ret
+
+
+write_cntfrq_el0:; .type write_cntfrq_el0, %function
+ msr cntfrq_el0, x0
+ ret
+
+
+read_cpuectlr:; .type read_cpuectlr, %function
+ mrs x0, CPUECTLR_EL1
+ ret
+
+
+write_cpuectlr:; .type write_cpuectlr, %function
+ msr CPUECTLR_EL1, x0
+ dsb sy
+ isb
+ ret
+
+
+read_cnthctl_el2:; .type read_cnthctl_el2, %function
+ mrs x0, cnthctl_el2
+ ret
+
+
+write_cnthctl_el2:; .type write_cnthctl_el2, %function
+ msr cnthctl_el2, x0
+ ret
+
+
+read_cntfrq:; .type read_cntfrq, %function
+ mrs x0, cntfrq_el0
+ ret
+
+
+write_cntfrq:; .type write_cntfrq, %function
+ msr cntfrq_el0, x0
+ ret
+
+
+write_scr:; .type write_scr, %function
+ msr scr_el3, x0
+ dsb sy
+ isb
+ ret
+
+
+read_scr:; .type read_scr, %function
+ mrs x0, scr_el3
+ ret
+
+
+read_midr:; .type read_midr, %function
+ mrs x0, midr_el1
+ ret
+
+
+read_mpidr:; .type read_mpidr, %function
+ mrs x0, mpidr_el1
+ ret
+
+
+#if SUPPORT_VFP
+enable_vfp:; .type enable_vfp, %function
+ mrs x0, cpacr_el1
+ orr x0, x0, #CPACR_VFP_BITS
+ msr cpacr_el1, x0
+ mrs x0, cptr_el3
+ mov x1, #AARCH64_CPTR_TFP
+ bic x0, x0, x1
+ msr cptr_el3, x0
+ ret
+
+
+ // int read_fpexc(void)
+read_fpexc:; .type read_fpexc, %function
+ b read_fpexc
+ ret
+
+
+ // void write_fpexc(int fpexc)
+write_fpexc:; .type write_fpexc, %function
+ b write_fpexc
+ ret
+
+#endif
diff --git a/lib/arch/aarch64/tlb_helpers.S b/lib/arch/aarch64/tlb_helpers.S
new file mode 100644
index 0000000..8377f2c
--- /dev/null
+++ b/lib/arch/aarch64/tlb_helpers.S
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch_helpers.h>
+
+ .globl tlbiall
+ .globl tlbiallis
+ .globl tlbialle1
+ .globl tlbialle1is
+ .globl tlbialle2
+ .globl tlbialle2is
+ .globl tlbialle3
+ .globl tlbialle3is
+ .globl tlbivmalle1
+
+
+ .section .text, "ax"
+
+tlbiall:; .type tlbiall, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq tlbialle1
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq tlbialle2
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq tlbialle3
+
+
+tlbiallis:; .type tlbiallis, %function
+ mrs x0, CurrentEl
+ cmp x0, #(MODE_EL1 << MODE_EL_SHIFT)
+ b.eq tlbialle1is
+ cmp x0, #(MODE_EL2 << MODE_EL_SHIFT)
+ b.eq tlbialle2is
+ cmp x0, #(MODE_EL3 << MODE_EL_SHIFT)
+ b.eq tlbialle3is
+
+
+tlbialle1:; .type tlbialle1, %function
+ tlbi alle1
+ dsb sy
+ isb
+ ret
+
+
+tlbialle1is:; .type tlbialle1is, %function
+ tlbi alle1is
+ dsb sy
+ isb
+ ret
+
+
+tlbialle2:; .type tlbialle2, %function
+ tlbi alle2
+ dsb sy
+ isb
+ ret
+
+
+tlbialle2is:; .type tlbialle2is, %function
+ tlbi alle2is
+ dsb sy
+ isb
+ ret
+
+
+tlbialle3:; .type tlbialle3, %function
+ tlbi alle3
+ dsb sy
+ isb
+ ret
+
+
+tlbialle3is:; .type tlbialle3is, %function
+ tlbi alle3is
+ dsb sy
+ isb
+ ret
+
+tlbivmalle1:; .type tlbivmalle1, %function
+ tlbi vmalle1
+ dsb sy
+ isb
+ ret
diff --git a/lib/mmio.c b/lib/mmio.c
new file mode 100644
index 0000000..bf35e36
--- /dev/null
+++ b/lib/mmio.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdint.h>
+
+void mmio_write_32(uintptr_t addr, uint32_t value)
+{
+ *(volatile uint32_t*)addr = value;
+}
+
+unsigned mmio_read_32(uintptr_t addr)
+{
+ return *(volatile uint32_t*)addr;
+}
diff --git a/lib/non-semihosting/ctype.h b/lib/non-semihosting/ctype.h
new file mode 100644
index 0000000..88e7da1
--- /dev/null
+++ b/lib/non-semihosting/ctype.h
@@ -0,0 +1,60 @@
+/*-
+ * Copyright (c) 1982, 1988, 1991, 1993
+ * The Regents of the University of California. All rights reserved.
+ * (c) UNIX System Laboratories, Inc.
+ * All or some portions of this file are derived from material licensed
+ * to the University of California by American Telephone and Telegraph
+ * Co. or Unix System Laboratories, Inc. and are reproduced herein with
+ * the permission of UNIX System Laboratories, Inc.
+ *
+ * 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ *
+ * $FreeBSD$
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: include/lib/ctype.h
+ */
+
+#ifndef _SYS_CTYPE_H_
+#define _SYS_CTYPE_H_
+
+#define isspace(c) ((c) == ' ' || ((c) >= '\t' && (c) <= '\r'))
+#define isascii(c) (((c) & ~0x7f) == 0)
+#define isupper(c) ((c) >= 'A' && (c) <= 'Z')
+#define islower(c) ((c) >= 'a' && (c) <= 'z')
+#define isalpha(c) (isupper(c) || islower(c))
+#define isdigit(c) ((c) >= '0' && (c) <= '9')
+#define isxdigit(c) (isdigit(c) \
+ || ((c) >= 'A' && (c) <= 'F') \
+ || ((c) >= 'a' && (c) <= 'f'))
+#define isprint(c) ((c) >= ' ' && (c) <= '~')
+
+#define toupper(c) ((c) - 0x20 * (((c) >= 'a') && ((c) <= 'z')))
+#define tolower(c) ((c) + 0x20 * (((c) >= 'A') && ((c) <= 'Z')))
+
+#endif /* !_SYS_CTYPE_H_ */
diff --git a/lib/non-semihosting/mem.c b/lib/non-semihosting/mem.c
new file mode 100644
index 0000000..bca9ab5
--- /dev/null
+++ b/lib/non-semihosting/mem.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stddef.h> /* size_t */
+
+/*
+ * Fill @count bytes of memory pointed to by @dst with @val
+ */
+void *memset(void *dst, int val, size_t count)
+{
+ char *ptr = dst;
+
+ while (count--)
+ *ptr++ = val;
+
+ return dst;
+}
+
+/*
+ * Compare @len bytes of @s1 and @s2
+ */
+int memcmp(const void *s1, const void *s2, size_t len)
+{
+ const char *s = s1;
+ const char *d = s2;
+ char dc;
+ char sc;
+
+ while (len--) {
+ sc = *s++;
+ dc = *d++;
+ if (sc - dc)
+ return (sc - dc);
+ }
+
+ return 0;
+}
+
+
+/*
+ * Move @len bytes from @src to @dst
+ */
+void *memmove(void *dst, const void *src, size_t len)
+{
+ const char *s = src;
+ char *d = dst;
+
+ while (len--)
+ *d++ = *s++;
+ return d;
+}
+
+/*
+ * Copy @len bytes from @src to @dst
+ */
+void *memcpy(void *dst, const void *src, size_t len)
+{
+ return memmove(dst, src, len);
+}
+
+
+/*
+ * Scan @len bytes of @src for value @c
+ */
+void *memchr(const void *src, int c, size_t len)
+{
+ const char *s = src;
+
+ while (len--) {
+ if (*s == c)
+ return (void *) s;
+ s++;
+ }
+
+ return NULL;
+}
diff --git a/lib/non-semihosting/std.c b/lib/non-semihosting/std.c
new file mode 100644
index 0000000..ea91d5f
--- /dev/null
+++ b/lib/non-semihosting/std.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <console.h>
+
+#if defined (__GNUC__)
+
+#include <stdio.h>
+#include <stddef.h> /* size_t */
+#include <stdarg.h> /* va_list */
+
+// Code from VTB.
+#include "mem.c"
+
+// Make mem functions that will operate on DEV mem. "memset_io"?
+
+
+//Code from VTB
+#include "strlen.c"
+
+int puts(const char *s)
+{
+ int count = 0;
+ while(*s)
+ {
+ if (console_putc(*s++)) {
+ count++;
+ } else {
+ count = EOF; // -1 in stdio.h
+ break;
+ }
+ }
+ return count;
+}
+
+// From VTB
+#include "ctype.h"
+#include "subr_prf.c"
+
+ // Choose max of 128 chars for now.
+#define PRINT_BUFFER_SIZE 128
+int printf(const char *fmt, ...)
+{
+ va_list args;
+ va_start(args, fmt);
+ char buf[PRINT_BUFFER_SIZE];
+ vsnprintf(buf, sizeof(buf) - 1, fmt, args);
+ buf[PRINT_BUFFER_SIZE - 1] = '\0';
+ return puts(buf);
+}
+
+
+// I just made this up. Probably make it beter.
+void __assert_func (const char *file, int l, const char *func, const char *error)
+{
+ printf("ASSERT: %s <%d> : %s\n\r", func, l, error);
+ while(1);
+}
+
+extern void __assert_fail (const char *assertion, const char *file,
+ unsigned int line, const char *function)
+{
+ printf("ASSERT: %s <%d> : %s\n\r", function, line, assertion);
+ while(1);
+}
+
+
+// I just made this up. Probably make it beter.
+void abort (void)
+{
+ printf("ABORT\n\r");
+ while(1);
+}
+
+
+#else
+#error "No standard library binding defined."
+#endif
diff --git a/lib/non-semihosting/strcmp.c b/lib/non-semihosting/strcmp.c
new file mode 100644
index 0000000..e5921ba
--- /dev/null
+++ b/lib/non-semihosting/strcmp.c
@@ -0,0 +1,49 @@
+/*-
+ * Copyright (c) 1990, 1993
+ * The Regents of the University of California. All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Chris Torek.
+ *
+ * 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strcmp.c
+ */
+
+/*
+ * Compare strings.
+ */
+int
+strcmp(const char *s1, const char *s2)
+{
+ while (*s1 == *s2++)
+ if (*s1++ == '\0')
+ return (0);
+ return (*(const unsigned char *)s1 - *(const unsigned char *)(s2 - 1));
+}
diff --git a/lib/non-semihosting/string.c b/lib/non-semihosting/string.c
new file mode 100644
index 0000000..5bb01a1
--- /dev/null
+++ b/lib/non-semihosting/string.c
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 "ctype.h"
+
+/* Return pointer to the first non-space character */
+const char *skip_spaces(const char *str)
+{
+ while (isspace(*str))
+ ++str;
+ return str;
+}
diff --git a/lib/non-semihosting/strlen.c b/lib/non-semihosting/strlen.c
new file mode 100644
index 0000000..5c1e7a6
--- /dev/null
+++ b/lib/non-semihosting/strlen.c
@@ -0,0 +1,46 @@
+/*-
+ * Copyright (c) 1990, 1993
+ * The Regents of the University of California. 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strlen.c
+ */
+
+#include <stddef.h>
+
+size_t
+strlen(str)
+ const char *str;
+{
+ register const char *s;
+
+ for (s = str; *s; ++s);
+ return(s - str);
+}
diff --git a/lib/non-semihosting/strncmp.c b/lib/non-semihosting/strncmp.c
new file mode 100644
index 0000000..984b7a0
--- /dev/null
+++ b/lib/non-semihosting/strncmp.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 1989, 1993
+ * The Regents of the University of California. 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strncmp.c
+ */
+
+#include "types.h"
+
+int
+strncmp(const char *s1, const char *s2, size_t n)
+{
+
+ if (n == 0)
+ return (0);
+ do {
+ if (*s1 != *s2++)
+ return (*(const unsigned char *)s1 -
+ *(const unsigned char *)(s2 - 1));
+ if (*s1++ == '\0')
+ break;
+ } while (--n != 0);
+ return (0);
+}
diff --git a/lib/non-semihosting/strncpy.c b/lib/non-semihosting/strncpy.c
new file mode 100644
index 0000000..56a8a69
--- /dev/null
+++ b/lib/non-semihosting/strncpy.c
@@ -0,0 +1,62 @@
+/*-
+ * Copyright (c) 1990, 1993
+ * The Regents of the University of California. All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Chris Torek.
+ *
+ * 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strncpy.c
+ */
+
+#include "types.h"
+
+/*
+ * Copy src to dst, truncating or null-padding to always copy n bytes.
+ * Return dst.
+ */
+char *
+strncpy(char *dst, const char *src, size_t n)
+{
+ if (n != 0) {
+ char *d = dst;
+ const char *s = src;
+
+ do {
+ if ((*d++ = *s++) == '\0') {
+ /* NUL pad the remaining n-1 bytes */
+ while (--n != 0)
+ *d++ = '\0';
+ break;
+ }
+ } while (--n != 0);
+ }
+ return (dst);
+}
diff --git a/lib/non-semihosting/strsep.c b/lib/non-semihosting/strsep.c
new file mode 100644
index 0000000..1f80af4
--- /dev/null
+++ b/lib/non-semihosting/strsep.c
@@ -0,0 +1,74 @@
+/*-
+ * Copyright (c) 1990, 1993
+ * The Regents of the University of California. 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strsep.c
+ */
+
+#include "types.h"
+
+/*
+ * Get next token from string *stringp, where tokens are possibly-empty
+ * strings separated by characters from delim.
+ *
+ * Writes NULs into the string at *stringp to end tokens.
+ * delim need not remain constant from call to call.
+ * On return, *stringp points past the last NUL written (if there might
+ * be further tokens), or is NULL (if there are definitely no more tokens).
+ *
+ * If *stringp is NULL, strsep returns NULL.
+ */
+char *
+strsep(char **stringp, const char *delim)
+{
+ char *s;
+ const char *spanp;
+ int c, sc;
+ char *tok;
+
+ if ((s = *stringp) == NULL)
+ return (NULL);
+ for (tok = s;;) {
+ c = *s++;
+ spanp = delim;
+ do {
+ if ((sc = *spanp++) == c) {
+ if (c == 0)
+ s = NULL;
+ else
+ s[-1] = 0;
+ *stringp = s;
+ return (tok);
+ }
+ } while (sc != 0);
+ }
+ /* NOTREACHED */
+}
diff --git a/lib/non-semihosting/strtol.c b/lib/non-semihosting/strtol.c
new file mode 100644
index 0000000..4a5a404
--- /dev/null
+++ b/lib/non-semihosting/strtol.c
@@ -0,0 +1,146 @@
+/*-
+ * Copyright (c) 1990, 1993
+ * The Regents of the University of California. All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Chris Torek.
+ *
+ * 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ *
+ * From: @(#)strtol.c 8.1 (Berkeley) 6/4/93
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strtol.c
+ */
+
+#include "types.h"
+#include "ctype.h"
+#include "limits.h"
+
+/*
+ * Convert a string to a long integer.
+ *
+ * Ignores `locale' stuff. Assumes that the upper and lower case
+ * alphabets and digits are each contiguous.
+ */
+static long
+bsd_strtol(nptr, endptr, base)
+ const char *nptr;
+ char **endptr;
+ int base;
+{
+ const char *s = nptr;
+ unsigned long acc;
+ unsigned char c;
+ unsigned long cutoff;
+ int neg = 0, any, cutlim;
+
+ /*
+ * Skip white space and pick up leading +/- sign if any.
+ * If base is 0, allow 0x for hex and 0 for octal, else
+ * assume decimal; if base is already 16, allow 0x.
+ */
+ do {
+ c = *s++;
+ } while (isspace(c));
+ if (c == '-') {
+ neg = 1;
+ c = *s++;
+ } else if (c == '+')
+ c = *s++;
+ if ((base == 0 || base == 16) &&
+ c == '0' && (*s == 'x' || *s == 'X')) {
+ c = s[1];
+ s += 2;
+ base = 16;
+ }
+ if (base == 0)
+ base = c == '0' ? 8 : 10;
+
+ /*
+ * Compute the cutoff value between legal numbers and illegal
+ * numbers. That is the largest legal value, divided by the
+ * base. An input number that is greater than this value, if
+ * followed by a legal input character, is too big. One that
+ * is equal to this value may be valid or not; the limit
+ * between valid and invalid numbers is then based on the last
+ * digit. For instance, if the range for longs is
+ * [-2147483648..2147483647] and the input base is 10,
+ * cutoff will be set to 214748364 and cutlim to either
+ * 7 (neg==0) or 8 (neg==1), meaning that if we have accumulated
+ * a value > 214748364, or equal but the next digit is > 7 (or 8),
+ * the number is too big, and we will return a range error.
+ *
+ * Set any if any `digits' consumed; make it negative to indicate
+ * overflow.
+ */
+ cutoff = neg ? -(unsigned long)LONG_MIN : LONG_MAX;
+ cutlim = cutoff % (unsigned long)base;
+ cutoff /= (unsigned long)base;
+ for (acc = 0, any = 0;; c = *s++) {
+ if (!isascii(c))
+ break;
+ if (isdigit(c))
+ c -= '0';
+ else if (isalpha(c))
+ c -= isupper(c) ? 'A' - 10 : 'a' - 10;
+ else
+ break;
+ if (c >= base)
+ break;
+ if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+ any = -1;
+ else {
+ any = 1;
+ acc *= base;
+ acc += c;
+ }
+ }
+ if (any < 0) {
+ acc = neg ? LONG_MIN : LONG_MAX;
+ } else if (neg)
+ acc = -acc;
+ if (endptr != 0)
+ *((const char **)endptr) = any ? s - 1 : nptr;
+ return (acc);
+}
+
+int strict_strtol(const char *str, unsigned int base, long *result)
+{
+ if (*str == '-')
+ *result = 0 - bsd_strtol(str + 1, NULL, base);
+ else
+ *result = bsd_strtol(str, NULL, base);
+ return 0;
+}
+
+int strict_strtoul(const char *str, unsigned int base, unsigned long *result)
+{
+ *result = bsd_strtol(str, NULL, base);
+ return 0;
+}
diff --git a/lib/non-semihosting/strtoull.c b/lib/non-semihosting/strtoull.c
new file mode 100644
index 0000000..e46ef4c
--- /dev/null
+++ b/lib/non-semihosting/strtoull.c
@@ -0,0 +1,117 @@
+/*-
+ * Copyright (c) 1992, 1993
+ * The Regents of the University of California. 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/strtoull.c
+ */
+
+#include "types.h"
+#include "ctype.h"
+#include "limits.h"
+
+/*
+ * Convert a string to an unsigned long long integer.
+ *
+ * Assumes that the upper and lower case
+ * alphabets and digits are each contiguous.
+ */
+static unsigned long long
+bsd_strtoull(const char *nptr, char **endptr, int base)
+{
+ const char *s;
+ unsigned long long acc;
+ char c;
+ unsigned long long cutoff;
+ int neg, any, cutlim;
+
+ /*
+ * See strtoq for comments as to the logic used.
+ */
+ s = nptr;
+ do {
+ c = *s++;
+ } while (isspace((unsigned char)c));
+ if (c == '-') {
+ neg = 1;
+ c = *s++;
+ } else {
+ neg = 0;
+ if (c == '+')
+ c = *s++;
+ }
+ if ((base == 0 || base == 16) &&
+ c == '0' && (*s == 'x' || *s == 'X') &&
+ ((s[1] >= '0' && s[1] <= '9') ||
+ (s[1] >= 'A' && s[1] <= 'F') ||
+ (s[1] >= 'a' && s[1] <= 'f'))) {
+ c = s[1];
+ s += 2;
+ base = 16;
+ }
+ if (base == 0)
+ base = c == '0' ? 8 : 10;
+ acc = any = 0;
+
+ cutoff = ULLONG_MAX / base;
+ cutlim = ULLONG_MAX % base;
+ for ( ; ; c = *s++) {
+ if (c >= '0' && c <= '9')
+ c -= '0';
+ else if (c >= 'A' && c <= 'Z')
+ c -= 'A' - 10;
+ else if (c >= 'a' && c <= 'z')
+ c -= 'a' - 10;
+ else
+ break;
+ if (c >= base)
+ break;
+ if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+ any = -1;
+ else {
+ any = 1;
+ acc *= base;
+ acc += c;
+ }
+ }
+ if (any < 0) {
+ acc = ULLONG_MAX;
+ } else if (neg)
+ acc = -acc;
+ if (endptr != NULL)
+ *endptr = (char *)(any ? s - 1 : nptr);
+ return (acc);
+}
+
+int strict_strtoull(const char *str, unsigned int base, long long *result)
+{
+ *result = bsd_strtoull(str, NULL, base);
+ return 0;
+}
diff --git a/lib/non-semihosting/subr_prf.c b/lib/non-semihosting/subr_prf.c
new file mode 100644
index 0000000..6e2a1ac
--- /dev/null
+++ b/lib/non-semihosting/subr_prf.c
@@ -0,0 +1,557 @@
+/*-
+ * Copyright (c) 1986, 1988, 1991, 1993
+ * The Regents of the University of California. All rights reserved.
+ * (c) UNIX System Laboratories, Inc.
+ * All or some portions of this file are derived from material licensed
+ * to the University of California by American Telephone and Telegraph
+ * Co. or Unix System Laboratories, Inc. and are reproduced herein with
+ * the permission of UNIX System Laboratories, Inc.
+ *
+ * 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.
+ * 4. Neither the name of the University nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
+ *
+ * @(#)subr_prf.c 8.3 (Berkeley) 1/21/94
+ */
+
+/*
+ * Portions copyright (c) 2009-2013, ARM Ltd. All rights reserved.
+ * ---------------------------------------------------------------
+ * File: lib/subr_prf.c
+ */
+
+/*
+#include "types.h"
+#include "varargs.h"
+#include "ctype.h"
+#include "string.h"
+*/
+#include <stddef.h>
+#include <sys/types.h> /* For ssize_t */
+#include <stdint.h>
+#include <string.h>
+
+#include "ctype.h"
+
+typedef uint64_t uintmax_t;
+typedef int64_t intmax_t;
+typedef unsigned char u_char;
+typedef unsigned int u_int;
+typedef int64_t quad_t;
+typedef uint64_t u_quad_t;
+typedef unsigned long u_long;
+typedef unsigned short u_short;
+
+static inline int imax(int a, int b) { return (a > b ? a : b); }
+
+/*
+ * Note that stdarg.h and the ANSI style va_start macro is used for both
+ * ANSI and traditional C compilers.
+ */
+
+#define TOCONS 0x01
+#define TOTTY 0x02
+#define TOLOG 0x04
+
+/* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
+#define MAXNBUF (sizeof(intmax_t) * 8 + 1)
+
+struct putchar_arg {
+ int flags;
+ int pri;
+ struct tty *tty;
+ char *p_bufr;
+ size_t n_bufr;
+ char *p_next;
+ size_t remain;
+};
+
+struct snprintf_arg {
+ char *str;
+ size_t remain;
+};
+
+extern int log_open;
+
+static char *ksprintn(char *nbuf, uintmax_t num, int base, int *len, int upper);
+static void snprintf_func(int ch, void *arg);
+static int kvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap);
+
+int vsnprintf(char *str, size_t size, const char *format, va_list ap);
+
+static char const hex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
+#define hex2ascii(hex) (hex2ascii_data[hex])
+
+/*
+ * Scaled down version of sprintf(3).
+ */
+int
+sprintf(char *buf, const char *cfmt, ...)
+{
+ int retval;
+ va_list ap;
+
+ va_start(ap, cfmt);
+ retval = kvprintf(cfmt, NULL, (void *)buf, 10, ap);
+ buf[retval] = '\0';
+ va_end(ap);
+ return (retval);
+}
+
+/*
+ * Scaled down version of vsprintf(3).
+ */
+int
+vsprintf(char *buf, const char *cfmt, va_list ap)
+{
+ int retval;
+
+ retval = kvprintf(cfmt, NULL, (void *)buf, 10, ap);
+ buf[retval] = '\0';
+ return (retval);
+}
+
+/*
+ * Scaled down version of snprintf(3).
+ */
+int
+snprintf(char *str, size_t size, const char *format, ...)
+{
+ int retval;
+ va_list ap;
+
+ va_start(ap, format);
+ retval = vsnprintf(str, size, format, ap);
+ va_end(ap);
+ return(retval);
+}
+
+/*
+ * Scaled down version of vsnprintf(3).
+ */
+int
+vsnprintf(char *str, size_t size, const char *format, va_list ap)
+{
+ struct snprintf_arg info;
+ int retval;
+
+ info.str = str;
+ info.remain = size;
+ retval = kvprintf(format, snprintf_func, &info, 10, ap);
+ if (info.remain >= 1)
+ *info.str++ = '\0';
+ return (retval);
+}
+
+static void
+snprintf_func(int ch, void *arg)
+{
+ struct snprintf_arg *const info = arg;
+
+ if (info->remain >= 2) {
+ *info->str++ = ch;
+ info->remain--;
+ }
+}
+
+
+/*
+ * Kernel version which takes radix argument vsnprintf(3).
+ */
+int
+vsnrprintf(char *str, size_t size, int radix, const char *format, va_list ap)
+{
+ struct snprintf_arg info;
+ int retval;
+
+ info.str = str;
+ info.remain = size;
+ retval = kvprintf(format, snprintf_func, &info, radix, ap);
+ if (info.remain >= 1)
+ *info.str++ = '\0';
+ return (retval);
+}
+
+
+/*
+ * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
+ * order; return an optional length and a pointer to the last character
+ * written in the buffer (i.e., the first character of the string).
+ * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
+ */
+static char *
+ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
+{
+ char *p, c;
+
+ p = nbuf;
+ *p = '\0';
+ do {
+ c = hex2ascii(num % base);
+ *++p = upper ? toupper(c) : c;
+ } while (num /= base);
+ if (lenp)
+ *lenp = p - nbuf;
+ return (p);
+}
+
+/*
+ * Scaled down version of printf(3).
+ *
+ * Two additional formats:
+ *
+ * The format %b is supported to decode error registers.
+ * Its usage is:
+ *
+ * printf("reg=%b\n", regval, "<base><arg>*");
+ *
+ * where <base> is the output base expressed as a control character, e.g.
+ * \10 gives octal; \20 gives hex. Each arg is a sequence of characters,
+ * the first of which gives the bit number to be inspected (origin 1), and
+ * the next characters (up to a control character, i.e. a character <= 32),
+ * give the name of the register. Thus:
+ *
+ * kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
+ *
+ * would produce output:
+ *
+ * reg=3<BITTWO,BITONE>
+ *
+ * XXX: %D -- Hexdump, takes pointer and separator string:
+ * ("%6D", ptr, ":") -> XX:XX:XX:XX:XX:XX
+ * ("%*D", len, ptr, " " -> XX XX XX XX ...
+ */
+int
+kvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
+{
+#define PCHAR(c) {int cc=(c); if (func) (*func)(cc,arg); else *d++ = cc; retval++; }
+ char nbuf[MAXNBUF];
+ char *d;
+ const char *p, *percent, *q;
+ u_char *up;
+ int ch, n;
+ uintmax_t num;
+ int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
+ int cflag, hflag, jflag, tflag, zflag;
+ int dwidth, upper;
+ char padc;
+ int stop = 0, retval = 0;
+
+ num = 0;
+ if (!func)
+ d = (char *) arg;
+ else
+ d = NULL;
+
+ if (fmt == NULL)
+ fmt = "(fmt null)\n";
+
+ if (radix < 2 || radix > 36)
+ radix = 10;
+
+ for (;;) {
+ padc = ' ';
+ width = 0;
+ while ((ch = (u_char)*fmt++) != '%' || stop) {
+ if (ch == '\0')
+ return (retval);
+ PCHAR(ch);
+ }
+ percent = fmt - 1;
+ qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
+ sign = 0; dot = 0; dwidth = 0; upper = 0;
+ cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
+reswitch: switch (ch = (u_char)*fmt++) {
+ case '.':
+ dot = 1;
+ goto reswitch;
+ case '#':
+ sharpflag = 1;
+ goto reswitch;
+ case '+':
+ sign = 1;
+ goto reswitch;
+ case '-':
+ ladjust = 1;
+ goto reswitch;
+ case '%':
+ PCHAR(ch);
+ break;
+ case '*':
+ if (!dot) {
+ width = va_arg(ap, int);
+ if (width < 0) {
+ ladjust = !ladjust;
+ width = -width;
+ }
+ } else {
+ dwidth = va_arg(ap, int);
+ }
+ goto reswitch;
+ case '0':
+ if (!dot) {
+ padc = '0';
+ goto reswitch;
+ }
+ case '1': case '2': case '3': case '4':
+ case '5': case '6': case '7': case '8': case '9':
+ for (n = 0;; ++fmt) {
+ n = n * 10 + ch - '0';
+ ch = *fmt;
+ if (ch < '0' || ch > '9')
+ break;
+ }
+ if (dot)
+ dwidth = n;
+ else
+ width = n;
+ goto reswitch;
+ case 'b':
+ num = (u_int)va_arg(ap, int);
+ p = va_arg(ap, char *);
+ for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;)
+ PCHAR(*q--);
+
+ if (num == 0)
+ break;
+
+ for (tmp = 0; *p;) {
+ n = *p++;
+ if (num & (1 << (n - 1))) {
+ PCHAR(tmp ? ',' : '<');
+ for (; (n = *p) > ' '; ++p)
+ PCHAR(n);
+ tmp = 1;
+ } else
+ for (; *p > ' '; ++p)
+ continue;
+ }
+ if (tmp)
+ PCHAR('>');
+ break;
+ case 'c':
+ PCHAR(va_arg(ap, int));
+ break;
+ case 'D':
+ up = va_arg(ap, u_char *);
+ p = va_arg(ap, char *);
+ if (!width)
+ width = 16;
+ while(width--) {
+ PCHAR(hex2ascii(*up >> 4));
+ PCHAR(hex2ascii(*up & 0x0f));
+ up++;
+ if (width)
+ for (q=p;*q;q++)
+ PCHAR(*q);
+ }
+ break;
+ case 'd':
+ case 'i':
+ base = 10;
+ sign = 1;
+ goto handle_sign;
+ case 'h':
+ if (hflag) {
+ hflag = 0;
+ cflag = 1;
+ } else
+ hflag = 1;
+ goto reswitch;
+ case 'j':
+ jflag = 1;
+ goto reswitch;
+ case 'l':
+ if (lflag) {
+ lflag = 0;
+ qflag = 1;
+ } else
+ lflag = 1;
+ goto reswitch;
+ case 'n':
+ if (jflag)
+ *(va_arg(ap, intmax_t *)) = retval;
+ else if (qflag)
+ *(va_arg(ap, quad_t *)) = retval;
+ else if (lflag)
+ *(va_arg(ap, long *)) = retval;
+ else if (zflag)
+ *(va_arg(ap, size_t *)) = retval;
+ else if (hflag)
+ *(va_arg(ap, short *)) = retval;
+ else if (cflag)
+ *(va_arg(ap, char *)) = retval;
+ else
+ *(va_arg(ap, int *)) = retval;
+ break;
+ case 'o':
+ base = 8;
+ goto handle_nosign;
+ case 'p':
+ base = 16;
+ sharpflag = (width == 0);
+ sign = 0;
+ num = (uintptr_t)va_arg(ap, void *);
+ goto number;
+ case 'q':
+ qflag = 1;
+ goto reswitch;
+ case 'r':
+ base = radix;
+ if (sign)
+ goto handle_sign;
+ goto handle_nosign;
+ case 's':
+ p = va_arg(ap, char *);
+ if (p == NULL)
+ p = "(null)";
+ if (!dot)
+ n = strlen (p);
+ else
+ for (n = 0; n < dwidth && p[n]; n++)
+ continue;
+
+ width -= n;
+
+ if (!ladjust && width > 0)
+ while (width--)
+ PCHAR(padc);
+ while (n--)
+ PCHAR(*p++);
+ if (ladjust && width > 0)
+ while (width--)
+ PCHAR(padc);
+ break;
+ case 't':
+ tflag = 1;
+ goto reswitch;
+ case 'u':
+ base = 10;
+ goto handle_nosign;
+ case 'X':
+ upper = 1;
+ case 'x':
+ base = 16;
+ goto handle_nosign;
+ case 'y':
+ base = 16;
+ sign = 1;
+ goto handle_sign;
+ case 'z':
+ zflag = 1;
+ goto reswitch;
+handle_nosign:
+ sign = 0;
+ if (jflag)
+ num = va_arg(ap, uintmax_t);
+ else if (qflag)
+ num = va_arg(ap, u_quad_t);
+ else if (tflag)
+ num = va_arg(ap, ptrdiff_t);
+ else if (lflag)
+ num = va_arg(ap, u_long);
+ else if (zflag)
+ num = va_arg(ap, size_t);
+ else if (hflag)
+ num = (u_short)va_arg(ap, int);
+ else if (cflag)
+ num = (u_char)va_arg(ap, int);
+ else
+ num = va_arg(ap, u_int);
+ goto number;
+handle_sign:
+ if (jflag)
+ num = va_arg(ap, intmax_t);
+ else if (qflag)
+ num = va_arg(ap, quad_t);
+ else if (tflag)
+ num = va_arg(ap, ptrdiff_t);
+ else if (lflag)
+ num = va_arg(ap, long);
+ else if (zflag)
+ num = va_arg(ap, ssize_t);
+ else if (hflag)
+ num = (short)va_arg(ap, int);
+ else if (cflag)
+ num = (char)va_arg(ap, int);
+ else
+ num = va_arg(ap, int);
+number:
+ if (sign && (intmax_t)num < 0) {
+ neg = 1;
+ num = -(intmax_t)num;
+ }
+ p = ksprintn(nbuf, num, base, &n, upper);
+ tmp = 0;
+ if (sharpflag && num != 0) {
+ if (base == 8)
+ tmp++;
+ else if (base == 16)
+ tmp += 2;
+ }
+ if (neg)
+ tmp++;
+
+ if (!ladjust && padc == '0')
+ dwidth = width - tmp;
+ width -= tmp + imax(dwidth, n);
+ dwidth -= n;
+ if (!ladjust)
+ while (width-- > 0)
+ PCHAR(' ');
+ if (neg)
+ PCHAR('-');
+ if (sharpflag && num != 0) {
+ if (base == 8) {
+ PCHAR('0');
+ } else if (base == 16) {
+ PCHAR('0');
+ PCHAR('x');
+ }
+ }
+ while (dwidth-- > 0)
+ PCHAR('0');
+
+ while (*p)
+ PCHAR(*p--);
+
+ if (ladjust)
+ while (width-- > 0)
+ PCHAR(' ');
+
+ break;
+ default:
+ while (percent < fmt)
+ PCHAR(*percent++);
+ /*
+ * Since we ignore an formatting argument it is no
+ * longer safe to obey the remaining formatting
+ * arguments as the arguments will no longer match
+ * the format specs.
+ */
+ stop = 1;
+ break;
+ }
+ }
+#undef PCHAR
+}
diff --git a/lib/semihosting/aarch64/semihosting_call.S b/lib/semihosting/aarch64/semihosting_call.S
new file mode 100644
index 0000000..cc72ec2
--- /dev/null
+++ b/lib/semihosting/aarch64/semihosting_call.S
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+ .globl semihosting_call
+
+ .section .text, "ax"
+
+semihosting_call:; .type semihosting_call, %function
+ hlt #0xf000
+ ret
diff --git a/lib/semihosting/semihosting.c b/lib/semihosting/semihosting.c
new file mode 100644
index 0000000..558973a
--- /dev/null
+++ b/lib/semihosting/semihosting.c
@@ -0,0 +1,234 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <semihosting.h>
+
+#ifndef SEMIHOSTING_SUPPORTED
+#define SEMIHOSTING_SUPPORTED 1
+#endif
+
+extern int semihosting_call(unsigned int operation,
+ void *system_block_address);
+
+typedef struct {
+ const char *file_name;
+ unsigned int mode;
+ unsigned int name_length;
+} smh_file_open_block;
+
+typedef struct {
+ int handle;
+ void *buffer;
+ unsigned int length;
+} smh_file_read_write_block;
+
+typedef struct {
+ int handle;
+ unsigned int location;
+} smh_file_seek_block;
+
+typedef struct {
+ char *command_line;
+ unsigned int command_length;
+} smh_system_block;
+
+int semihosting_connection_supported(void)
+{
+ return SEMIHOSTING_SUPPORTED;
+}
+
+int semihosting_file_open(const char *file_name, unsigned int mode)
+{
+ smh_file_open_block open_block;
+
+ open_block.file_name = file_name;
+ open_block.mode = mode;
+ open_block.name_length = strlen(file_name);
+
+ return semihosting_call(SEMIHOSTING_SYS_OPEN,
+ (void *) &open_block);
+}
+
+int semihosting_file_seek(int file_handle, unsigned int offset)
+{
+ smh_file_seek_block seek_block;
+ int result;
+
+ seek_block.handle = file_handle;
+ seek_block.location = offset;
+
+ result = semihosting_call(SEMIHOSTING_SYS_SEEK,
+ (void *) &seek_block);
+
+ if (result)
+ result = semihosting_call(SEMIHOSTING_SYS_ERRNO, 0);
+
+ return result;
+}
+
+int semihosting_file_read(int file_handle, int *length, void *buffer)
+{
+ smh_file_read_write_block read_block;
+ int result = -EINVAL;
+
+ if ((length == NULL) || (buffer == NULL))
+ return result;
+
+ read_block.handle = file_handle;
+ read_block.buffer = buffer;
+ read_block.length = *length;
+
+ result = semihosting_call(SEMIHOSTING_SYS_READ,
+ (void *) &read_block);
+
+ if (result == *length) {
+ return -EINVAL;
+ } else if (result < *length) {
+ *length -= result;
+ return 0;
+ } else
+ return result;
+}
+
+int semihosting_file_write(int file_handle, int *length, void *buffer)
+{
+ smh_file_read_write_block write_block;
+
+ if ((length == NULL) || (buffer == NULL))
+ return -EINVAL;
+
+ write_block.handle = file_handle;
+ write_block.buffer = buffer;
+ write_block.length = *length;
+
+ *length = semihosting_call(SEMIHOSTING_SYS_WRITE,
+ (void *) &write_block);
+
+ return *length;
+}
+
+int semihosting_file_close(int file_handle)
+{
+ return semihosting_call(SEMIHOSTING_SYS_CLOSE,
+ (void *) &file_handle);
+}
+
+int semihosting_file_length(int file_handle)
+{
+ return semihosting_call(SEMIHOSTING_SYS_FLEN,
+ (void *) &file_handle);
+}
+
+char semihosting_read_char(void)
+{
+ return semihosting_call(SEMIHOSTING_SYS_READC, NULL);
+}
+
+void semihosting_write_char(char character)
+{
+ semihosting_call(SEMIHOSTING_SYS_WRITEC, (void *) &character);
+}
+
+void semihosting_write_string(char *string)
+{
+ semihosting_call(SEMIHOSTING_SYS_WRITE0, (void *) string);
+}
+
+int semihosting_system(char *command_line)
+{
+ smh_system_block system_block;
+
+ system_block.command_line = command_line;
+ system_block.command_length = strlen(command_line);
+
+ return semihosting_call(SEMIHOSTING_SYS_SYSTEM,
+ (void *) &system_block);
+}
+
+int semihosting_get_flen(const char *file_name)
+{
+ int file_handle, length;
+
+ assert(semihosting_connection_supported());
+
+ file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+ if (file_handle == -1)
+ return file_handle;
+
+ /* Find the length of the file */
+ length = semihosting_file_length(file_handle);
+
+ return semihosting_file_close(file_handle) ? -1 : length;
+}
+
+int semihosting_download_file(const char *file_name,
+ int buf_size,
+ void *buf)
+{
+ int ret = -EINVAL, file_handle, length;
+
+ /* Null pointer check */
+ if (!buf)
+ return ret;
+
+ assert(semihosting_connection_supported());
+
+ file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+ if (file_handle == -1)
+ return ret;
+
+ /* Find the actual length of the file */
+ length = semihosting_file_length(file_handle);
+ if (length == -1)
+ goto semihosting_fail;
+
+ /* Signal error if we do not have enough space for the file */
+ if (length > buf_size)
+ goto semihosting_fail;
+
+ /*
+ * A successful read will return 0 in which case we pass back
+ * the actual number of bytes read. Else we pass a negative
+ * value indicating an error.
+ */
+ ret = semihosting_file_read(file_handle, &length, buf);
+ if (ret)
+ goto semihosting_fail;
+ else
+ ret = length;
+
+semihosting_fail:
+ semihosting_file_close(file_handle);
+ return ret;
+}
diff --git a/lib/sync/locks/bakery/bakery_lock.c b/lib/sync/locks/bakery/bakery_lock.c
new file mode 100644
index 0000000..d3c780c
--- /dev/null
+++ b/lib/sync/locks/bakery/bakery_lock.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <assert.h>
+#include <string.h>
+
+#include <bakery_lock.h>
+
+#define assert_bakery_entry_valid(entry, bakery) do { \
+ assert(bakery); \
+ assert(entry < BAKERY_LOCK_MAX_CPUS); \
+} while(0)
+
+void bakery_lock_init(bakery_lock * bakery)
+{
+ assert(bakery);
+ memset(bakery, 0, sizeof(*bakery));
+ bakery->owner = NO_OWNER;
+}
+
+void bakery_lock_get(unsigned long mpidr, bakery_lock * bakery)
+{
+ unsigned int i, max = 0, my_full_number, his_full_number, entry;
+
+ entry = platform_get_core_pos(mpidr);
+
+ assert_bakery_entry_valid(entry, bakery);
+
+ // Catch recursive attempts to take the lock under the same entry:
+ assert(bakery->owner != entry);
+
+ // Get a ticket
+ bakery->entering[entry] = 1;
+ for (i = 0; i < BAKERY_LOCK_MAX_CPUS; ++i) {
+ if (bakery->number[i] > max) {
+ max = bakery->number[i];
+ }
+ }
+ ++max;
+ bakery->number[entry] = max;
+ bakery->entering[entry] = 0;
+
+ // Wait for our turn
+ my_full_number = (max << 8) + entry;
+ for (i = 0; i < BAKERY_LOCK_MAX_CPUS; ++i) {
+ while (bakery->entering[i]) ; /* Wait */
+ do {
+ his_full_number = bakery->number[i];
+ if (his_full_number) {
+ his_full_number = (his_full_number << 8) + i;
+ }
+ }
+ while (his_full_number && (his_full_number < my_full_number));
+ }
+
+ bakery->owner = entry;
+}
+
+void bakery_lock_release(unsigned long mpidr, bakery_lock * bakery)
+{
+ unsigned int entry = platform_get_core_pos(mpidr);
+
+ assert_bakery_entry_valid(entry, bakery);
+ assert(bakery_lock_held(entry, bakery));
+
+ bakery->owner = NO_OWNER;
+ bakery->number[entry] = 0;
+}
+
+int bakery_lock_held(unsigned long mpidr, const bakery_lock * bakery)
+{
+ unsigned int entry = platform_get_core_pos(mpidr);
+
+ assert_bakery_entry_valid(entry, bakery);
+
+ return bakery->owner == entry;
+}
diff --git a/lib/sync/locks/exclusive/spinlock.S b/lib/sync/locks/exclusive/spinlock.S
new file mode 100644
index 0000000..4269d95
--- /dev/null
+++ b/lib/sync/locks/exclusive/spinlock.S
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+
+ .globl spin_lock
+ .globl spin_unlock
+
+
+ .section .text, "ax";
+
+spin_lock:; .type spin_lock, %function
+ mov w2, #1
+ sevl
+l1: wfe
+l2: ldaxr w1, [x0]
+ cbnz w1, l1
+ stxr w1, w2, [x0]
+ cbnz w1, l2
+ ret
+
+
+spin_unlock:; .type spin_unlock, %function
+ stlr wzr, [x0]
+ ret
diff --git a/license.md b/license.md
new file mode 100644
index 0000000..7652f10
--- /dev/null
+++ b/license.md
@@ -0,0 +1,26 @@
+Copyright (c) 2013, ARM Limited. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+
+* 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.
+
+* Neither the name of ARM nor the names of its contributors may be used to
+ endorse or promote products derived from this software without specific prior
+ written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
diff --git a/plat/common/aarch64/platform_helpers.S b/plat/common/aarch64/platform_helpers.S
new file mode 100644
index 0000000..c574eb9
--- /dev/null
+++ b/plat/common/aarch64/platform_helpers.S
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <platform.h>
+
+
+ .globl pcpu_dv_mem_stack
+ .weak platform_get_core_pos
+ .weak platform_set_stack
+ .weak platform_is_primary_cpu
+ .weak platform_set_coherent_stack
+ .weak platform_check_mpidr
+ .weak plat_report_exception
+
+ /* -----------------------------------------------------
+ * 512 bytes of coherent stack for each cpu
+ * -----------------------------------------------------
+ */
+#define PCPU_DV_MEM_STACK_SIZE 0x200
+
+
+ .section .text, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * unsigned long long platform_set_coherent_stack
+ * (unsigned mpidr);
+ * For a given mpidr, this function returns the stack
+ * pointer allocated in device memory. This stack can
+ * be used by C code which enables/disables the SCTLR.M
+ * SCTLR.C bit e.g. while powering down a cpu
+ * -----------------------------------------------------
+ */
+platform_set_coherent_stack:; .type platform_set_coherent_stack, %function
+ mov x5, x30 // lr
+ bl platform_get_core_pos
+ add x0, x0, #1
+ mov x1, #PCPU_DV_MEM_STACK_SIZE
+ mul x0, x0, x1
+ ldr x1, =pcpu_dv_mem_stack
+ add sp, x1, x0
+ ret x5
+
+
+ /* -----------------------------------------------------
+ * int platform_get_core_pos(int mpidr);
+ * With this function: CorePos = (ClusterId * 4) +
+ * CoreId
+ * -----------------------------------------------------
+ */
+platform_get_core_pos:; .type platform_get_core_pos, %function
+ and x1, x0, #MPIDR_CPU_MASK
+ and x0, x0, #MPIDR_CLUSTER_MASK
+ add x0, x1, x0, LSR #6
+ ret
+
+
+ /* -----------------------------------------------------
+ * void platform_is_primary_cpu (unsigned int mpid);
+ *
+ * Given the mpidr say whether this cpu is the primary
+ * cpu (applicable ony after a cold boot)
+ * -----------------------------------------------------
+ */
+platform_is_primary_cpu:; .type platform_is_primary_cpu, %function
+ and x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
+ cmp x0, #PRIMARY_CPU
+ cset x0, eq
+ ret
+
+
+ /* -----------------------------------------------------
+ * void platform_set_stack (int mpidr)
+ * -----------------------------------------------------
+ */
+platform_set_stack:; .type platform_set_stack, %function
+ mov x9, x30 // lr
+ bl platform_get_core_pos
+ add x0, x0, #1
+ mov x1, #PLATFORM_STACK_SIZE
+ mul x0, x0, x1
+ ldr x1, =platform_normal_stacks
+ add sp, x1, x0
+ ret x9
+
+ /* -----------------------------------------------------
+ * Placeholder function which should be redefined by
+ * each platform.
+ * -----------------------------------------------------
+ */
+platform_check_mpidr:; .type platform_check_mpidr, %function
+ mov x0, xzr
+ ret
+
+ /* -----------------------------------------------------
+ * Placeholder function which should be redefined by
+ * each platform.
+ * -----------------------------------------------------
+ */
+plat_report_exception:
+ ret
+
+ /* -----------------------------------------------------
+ * Per-cpu stacks in device memory.
+ * Used for C code just before power down or right after
+ * power up when the MMU or caches need to be turned on
+ * or off. Each cpu gets a stack of 512 bytes.
+ * -----------------------------------------------------
+ */
+ .section tzfw_coherent_mem, "aw", %nobits; .align 6
+
+pcpu_dv_mem_stack:
+ /* Zero fill */
+ .space (PLATFORM_CORE_COUNT * PCPU_DV_MEM_STACK_SIZE), 0
diff --git a/plat/fvp/aarch64/bl1_plat_helpers.S b/plat/fvp/aarch64/bl1_plat_helpers.S
new file mode 100644
index 0000000..d72dc39
--- /dev/null
+++ b/plat/fvp/aarch64/bl1_plat_helpers.S
@@ -0,0 +1,264 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <platform.h>
+#include <fvp_pwrc.h>
+#include <gic.h>
+
+ .globl platform_get_entrypoint
+ .globl platform_cold_boot_init
+ .globl plat_secondary_cold_boot_setup
+
+
+ .section platform_code, "ax"; .align 3
+
+
+ .macro platform_choose_gicmmap param1, param2, x_tmp, w_tmp, res
+ ldr \x_tmp, =VE_SYSREGS_BASE + V2M_SYS_ID
+ ldr \w_tmp, [\x_tmp]
+ ubfx \w_tmp, \w_tmp, #SYS_ID_BLD_SHIFT, #SYS_ID_BLD_LENGTH
+ cmp \w_tmp, #BLD_GIC_VE_MMAP
+ csel \res, \param1, \param2, eq
+ .endm
+
+ /* -----------------------------------------------------
+ * void plat_secondary_cold_boot_setup (void);
+ *
+ * This function performs any platform specific actions
+ * needed for a secondary cpu after a cold reset e.g
+ * mark the cpu's presence, mechanism to place it in a
+ * holding pen etc.
+ * TODO: Should we read the PSYS register to make sure
+ * that the request has gone through.
+ * -----------------------------------------------------
+ */
+plat_secondary_cold_boot_setup:; .type plat_secondary_cold_boot_setup, %function
+ bl read_mpidr
+ mov x19, x0
+ bl platform_get_core_pos
+ mov x20, x0
+
+ /* ---------------------------------------------
+ * Mark this cpu as being present. This is a
+ * SO write. This array will be read using
+ * normal memory so invalidate any prefetched
+ * stale copies first.
+ * ---------------------------------------------
+ */
+ ldr x1, =TZDRAM_BASE
+ mov x0, #AFFMAP_OFF
+ add x1, x0, x1
+ mov x2, #PLATFORM_CACHE_LINE_SIZE
+ mul x2, x2, x20
+ add x0, x1, x2
+ bl dcivac
+ str x19, [x1, x2]
+
+ /* ---------------------------------------------
+ * Power down this cpu.
+ * TODO: Do we need to worry about powering the
+ * cluster down as well here. That will need
+ * locks which we won't have unless an elf-
+ * loader zeroes out the zi section.
+ * ---------------------------------------------
+ */
+ ldr x1, =PWRC_BASE
+ str w19, [x1, #PPOFFR_OFF]
+
+ /* ---------------------------------------------
+ * Deactivate the gic cpu interface as well
+ * ---------------------------------------------
+ */
+ ldr x0, =VE_GICC_BASE
+ ldr x1, =BASE_GICC_BASE
+ platform_choose_gicmmap x0, x1, x2, w2, x1
+ mov w0, #(IRQ_BYP_DIS_GRP1 | FIQ_BYP_DIS_GRP1)
+ orr w0, w0, #(IRQ_BYP_DIS_GRP0 | FIQ_BYP_DIS_GRP0)
+ str w0, [x1, #GICC_CTLR]
+
+ /* ---------------------------------------------
+ * There is no sane reason to come out of this
+ * wfi so panic if we do. This cpu will be pow-
+ * ered on and reset by the cpu_on pm api
+ * ---------------------------------------------
+ */
+ dsb sy
+ wfi
+cb_panic:
+ b cb_panic
+
+
+ /* -----------------------------------------------------
+ * void platform_get_entrypoint (unsigned int mpid);
+ *
+ * Main job of this routine is to distinguish between
+ * a cold and warm boot.
+ * On a cold boot the secondaries first wait for the
+ * platform to be initialized after which they are
+ * hotplugged in. The primary proceeds to perform the
+ * platform initialization.
+ * On a warm boot, each cpu jumps to the address in its
+ * mailbox.
+ *
+ * TODO: Not a good idea to save lr in a temp reg
+ * TODO: PSYSR is a common register and should be
+ * accessed using locks. Since its not possible
+ * to use locks immediately after a cold reset
+ * we are relying on the fact that after a cold
+ * reset all cpus will read the same WK field
+ * -----------------------------------------------------
+ */
+platform_get_entrypoint:; .type platform_get_entrypoint, %function
+ mov x9, x30 // lr
+ mov x2, x0
+ ldr x1, =PWRC_BASE
+ str w2, [x1, #PSYSR_OFF]
+ ldr w2, [x1, #PSYSR_OFF]
+ ubfx w2, w2, #PSYSR_WK_SHIFT, #PSYSR_WK_MASK
+ cbnz w2, warm_reset
+ mov x0, x2
+ b exit
+warm_reset:
+ /* ---------------------------------------------
+ * A per-cpu mailbox is maintained in the tru-
+ * sted DRAM. Its flushed out of the caches
+ * after every update using normal memory so
+ * its safe to read it here with SO attributes
+ * ---------------------------------------------
+ */
+ ldr x10, =TZDRAM_BASE + MBOX_OFF
+ bl platform_get_core_pos
+ lsl x0, x0, #CACHE_WRITEBACK_SHIFT
+ ldr x0, [x10, x0]
+ cbz x0, _panic
+exit:
+ ret x9
+_panic: b _panic
+
+
+ /* -----------------------------------------------------
+ * void platform_mem_init (void);
+ *
+ * Zero out the mailbox registers in the TZDRAM. The
+ * mmu is turned off right now and only the primary can
+ * ever execute this code. Secondaries will read the
+ * mailboxes using SO accesses. In short, BL31 will
+ * update the mailboxes after mapping the tzdram as
+ * normal memory. It will flush its copy after update.
+ * BL1 will always read the mailboxes with the MMU off
+ * -----------------------------------------------------
+ */
+platform_mem_init:; .type platform_mem_init, %function
+ ldr x0, =TZDRAM_BASE + MBOX_OFF
+ stp xzr, xzr, [x0, #0]
+ stp xzr, xzr, [x0, #0x10]
+ stp xzr, xzr, [x0, #0x20]
+ stp xzr, xzr, [x0, #0x30]
+ ret
+
+
+ /* -----------------------------------------------------
+ * void platform_cold_boot_init (bl1_main function);
+ *
+ * Routine called only by the primary cpu after a cold
+ * boot to perform early platform initialization
+ * -----------------------------------------------------
+ */
+platform_cold_boot_init:; .type platform_cold_boot_init, %function
+ mov x20, x0
+ bl platform_mem_init
+ bl read_mpidr
+ mov x19, x0
+
+ /* ---------------------------------------------
+ * Give ourselves a small coherent stack to
+ * ease the pain of initializing the MMU and
+ * CCI in assembler
+ * ---------------------------------------------
+ */
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Mark this cpu as being present. This is a
+ * SO write. Invalidate any stale copies out of
+ * paranoia as there is no one else around.
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl platform_get_core_pos
+ mov x21, x0
+
+ ldr x1, =TZDRAM_BASE
+ mov x0, #AFFMAP_OFF
+ add x1, x0, x1
+ mov x2, #PLATFORM_CACHE_LINE_SIZE
+ mul x2, x2, x21
+ add x0, x1, x2
+ bl dcivac
+ str x19, [x1, x2]
+
+ /* ---------------------------------------------
+ * Enable CCI-400 for this cluster. No need
+ * for locks as no other cpu is active at the
+ * moment
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl cci_enable_coherency
+
+ /* ---------------------------------------------
+ * Architectural init. can be generic e.g.
+ * enabling stack alignment and platform spec-
+ * ific e.g. MMU & page table setup as per the
+ * platform memory map. Perform the latter here
+ * and the former in bl1_main.
+ * ---------------------------------------------
+ */
+ bl bl1_early_platform_setup
+ bl bl1_plat_arch_setup
+
+ /* ---------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* ---------------------------------------------
+ * Jump to the main function. Returning from it
+ * is a terminal error.
+ * ---------------------------------------------
+ */
+ blr x20
+
+cb_init_panic:
+ b cb_init_panic
diff --git a/plat/fvp/aarch64/fvp_common.c b/plat/fvp/aarch64/fvp_common.c
new file mode 100644
index 0000000..762f542
--- /dev/null
+++ b/plat/fvp/aarch64/fvp_common.c
@@ -0,0 +1,600 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <bl_common.h>
+/* Included only for error codes */
+#include <psci.h>
+
+unsigned char platform_normal_stacks[PLATFORM_STACK_SIZE][PLATFORM_CORE_COUNT]
+__attribute__ ((aligned(PLATFORM_CACHE_LINE_SIZE),
+ section("tzfw_normal_stacks")));
+
+/*******************************************************************************
+ * This array holds the characteristics of the differences between the three
+ * FVP platforms (Base, A53_A57 & Foundation). It will be populated during cold
+ * boot at each boot stage by the primary before enabling the MMU (to allow cci
+ * configuration) & used thereafter. Each BL will have its own copy to allow
+ * independent operation.
+ ******************************************************************************/
+static unsigned long platform_config[CONFIG_LIMIT];
+
+/*******************************************************************************
+ * TODO: Check page table alignment to avoid space wastage
+ ******************************************************************************/
+
+/*******************************************************************************
+ * Level 1 translation tables need 4 entries for the 4GB address space accessib-
+ * le by the secure firmware. Input address space will be restricted using the
+ * T0SZ settings in the TCR.
+ ******************************************************************************/
+static unsigned long l1_xlation_table[ADDR_SPACE_SIZE >> 30]
+__attribute__ ((aligned((ADDR_SPACE_SIZE >> 30) << 3)));
+
+/*******************************************************************************
+ * Level 2 translation tables describe the first & second gb of the address
+ * space needed to address secure peripherals e.g. trusted ROM and RAM.
+ ******************************************************************************/
+static unsigned long l2_xlation_table[NUM_L2_PAGETABLES][NUM_2MB_IN_GB]
+__attribute__ ((aligned(NUM_2MB_IN_GB << 3)));
+
+/*******************************************************************************
+ * Level 3 translation tables (2 sets) describe the trusted & non-trusted RAM
+ * regions at a granularity of 4K.
+ ******************************************************************************/
+static unsigned long l3_xlation_table[NUM_L3_PAGETABLES][NUM_4K_IN_2MB]
+__attribute__ ((aligned(NUM_4K_IN_2MB << 3)));
+
+/*******************************************************************************
+ * Helper to create a level 1/2 table descriptor which points to a level 2/3
+ * table.
+ ******************************************************************************/
+static unsigned long create_table_desc(unsigned long *next_table_ptr)
+{
+ unsigned long desc = (unsigned long) next_table_ptr;
+
+ /* Clear the last 12 bits */
+ desc >>= FOUR_KB_SHIFT;
+ desc <<= FOUR_KB_SHIFT;
+
+ desc |= TABLE_DESC;
+
+ return desc;
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to addr
+ ******************************************************************************/
+static unsigned long create_block_desc(unsigned long desc,
+ unsigned long addr,
+ unsigned int level)
+{
+ switch (level) {
+ case LEVEL1:
+ desc |= (addr << FIRST_LEVEL_DESC_N) | BLOCK_DESC;
+ break;
+ case LEVEL2:
+ desc |= (addr << SECOND_LEVEL_DESC_N) | BLOCK_DESC;
+ break;
+ case LEVEL3:
+ desc |= (addr << THIRD_LEVEL_DESC_N) | TABLE_DESC;
+ break;
+ default:
+ assert(0);
+ }
+
+ return desc;
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with Device nGnRE attributes.
+ ******************************************************************************/
+static unsigned long create_device_block(unsigned long output_addr,
+ unsigned int level,
+ unsigned int ns)
+{
+ unsigned long upper_attrs, lower_attrs, desc;
+
+ lower_attrs = LOWER_ATTRS(ACCESS_FLAG | OSH | AP_RW);
+ lower_attrs |= LOWER_ATTRS(ns | ATTR_DEVICE_INDEX);
+ upper_attrs = UPPER_ATTRS(XN);
+ desc = upper_attrs | lower_attrs;
+
+ return create_block_desc(desc, output_addr, level);
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with inner-shareable normal wbwa read-only memory attributes.
+ ******************************************************************************/
+static unsigned long create_romem_block(unsigned long output_addr,
+ unsigned int level,
+ unsigned int ns)
+{
+ unsigned long upper_attrs, lower_attrs, desc;
+
+ lower_attrs = LOWER_ATTRS(ACCESS_FLAG | ISH | AP_RO);
+ lower_attrs |= LOWER_ATTRS(ns | ATTR_IWBWA_OWBWA_NTR_INDEX);
+ upper_attrs = UPPER_ATTRS(0ull);
+ desc = upper_attrs | lower_attrs;
+
+ return create_block_desc(desc, output_addr, level);
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with inner-shareable normal wbwa read-write memory attributes.
+ ******************************************************************************/
+static unsigned long create_rwmem_block(unsigned long output_addr,
+ unsigned int level,
+ unsigned int ns)
+{
+ unsigned long upper_attrs, lower_attrs, desc;
+
+ lower_attrs = LOWER_ATTRS(ACCESS_FLAG | ISH | AP_RW);
+ lower_attrs |= LOWER_ATTRS(ns | ATTR_IWBWA_OWBWA_NTR_INDEX);
+ upper_attrs = UPPER_ATTRS(XN);
+ desc = upper_attrs | lower_attrs;
+
+ return create_block_desc(desc, output_addr, level);
+}
+
+/*******************************************************************************
+ * Create page tables as per the platform memory map. Certain aspects of page
+ * talble creating have been abstracted in the above routines. This can be impr-
+ * oved further.
+ * TODO: Move the page table setup helpers into the arch or lib directory
+ *******************************************************************************/
+static unsigned long fill_xlation_tables(meminfo *tzram_layout,
+ unsigned long ro_start,
+ unsigned long ro_limit,
+ unsigned long coh_start,
+ unsigned long coh_limit)
+{
+ unsigned long l2_desc, l3_desc;
+ unsigned long *xt_addr = 0, *pt_addr, off = 0;
+ unsigned long trom_start_index, trom_end_index;
+ unsigned long tzram_start_index, tzram_end_index;
+ unsigned long flash0_start_index, flash0_end_index;
+ unsigned long flash1_start_index, flash1_end_index;
+ unsigned long vram_start_index, vram_end_index;
+ unsigned long nsram_start_index, nsram_end_index;
+ unsigned long tdram_start_index, tdram_end_index;
+ unsigned long dram_start_index, dram_end_index;
+ unsigned long dev0_start_index, dev0_end_index;
+ unsigned long dev1_start_index, dev1_end_index;
+ unsigned int idx;
+
+
+ /*****************************************************************
+ * LEVEL1 PAGETABLE SETUP
+ *
+ * Find the start and end indices of the memory peripherals in the
+ * first level pagetables. These are the main areas we care about.
+ * Also bump the end index by one if its equal to the start to
+ * allow for regions which lie completely in a GB.
+ *****************************************************************/
+ trom_start_index = ONE_GB_INDEX(TZROM_BASE);
+ dev0_start_index = ONE_GB_INDEX(TZRNG_BASE);
+ dram_start_index = ONE_GB_INDEX(DRAM_BASE);
+ dram_end_index = ONE_GB_INDEX(DRAM_BASE + DRAM_SIZE);
+
+ if (dram_end_index == dram_start_index)
+ dram_end_index++;
+
+ /*
+ * Fill up the level1 translation table first
+ */
+ for (idx = 0; idx < (ADDR_SPACE_SIZE >> 30); idx++) {
+
+ /*
+ * Fill up the entry for the TZROM. This will cover
+ * everything in the first GB.
+ */
+ if (idx == trom_start_index) {
+ xt_addr = &l2_xlation_table[GB1_L2_PAGETABLE][0];
+ l1_xlation_table[idx] = create_table_desc(xt_addr);
+ continue;
+ }
+
+ /*
+ * Mark the second gb as device
+ */
+ if (idx == dev0_start_index) {
+ xt_addr = &l2_xlation_table[GB2_L2_PAGETABLE][0];
+ l1_xlation_table[idx] = create_table_desc(xt_addr);
+ continue;
+ }
+
+ /*
+ * Fill up the block entry for the DRAM with Normal
+ * inner-WBWA outer-WBWA non-transient attributes.
+ * This will cover 2-4GB. Note that the acesses are
+ * marked as non-secure.
+ */
+ if ((idx >= dram_start_index) && (idx < dram_end_index)) {
+ l1_xlation_table[idx] = create_rwmem_block(idx, LEVEL1,
+ NS);
+ continue;
+ }
+
+ assert(0);
+ }
+
+
+ /*****************************************************************
+ * LEVEL2 PAGETABLE SETUP
+ *
+ * Find the start and end indices of the memory & peripherals in the
+ * second level pagetables.
+ ******************************************************************/
+
+ /* Initializations for the 1st GB */
+ trom_start_index = TWO_MB_INDEX(TZROM_BASE);
+ trom_end_index = TWO_MB_INDEX(TZROM_BASE + TZROM_SIZE);
+ if (trom_end_index == trom_start_index)
+ trom_end_index++;
+
+ tdram_start_index = TWO_MB_INDEX(TZDRAM_BASE);
+ tdram_end_index = TWO_MB_INDEX(TZDRAM_BASE + TZDRAM_SIZE);
+ if (tdram_end_index == tdram_start_index)
+ tdram_end_index++;
+
+ flash0_start_index = TWO_MB_INDEX(FLASH0_BASE);
+ flash0_end_index = TWO_MB_INDEX(FLASH0_BASE + TZROM_SIZE);
+ if (flash0_end_index == flash0_start_index)
+ flash0_end_index++;
+
+ flash1_start_index = TWO_MB_INDEX(FLASH1_BASE);
+ flash1_end_index = TWO_MB_INDEX(FLASH1_BASE + FLASH1_SIZE);
+ if (flash1_end_index == flash1_start_index)
+ flash1_end_index++;
+
+ vram_start_index = TWO_MB_INDEX(VRAM_BASE);
+ vram_end_index = TWO_MB_INDEX(VRAM_BASE + VRAM_SIZE);
+ if (vram_end_index == vram_start_index)
+ vram_end_index++;
+
+ dev0_start_index = TWO_MB_INDEX(DEVICE0_BASE);
+ dev0_end_index = TWO_MB_INDEX(DEVICE0_BASE + DEVICE0_SIZE);
+ if (dev0_end_index == dev0_start_index)
+ dev0_end_index++;
+
+ dev1_start_index = TWO_MB_INDEX(DEVICE1_BASE);
+ dev1_end_index = TWO_MB_INDEX(DEVICE1_BASE + DEVICE1_SIZE);
+ if (dev1_end_index == dev1_start_index)
+ dev1_end_index++;
+
+ /* Since the size is < 2M this is a single index */
+ tzram_start_index = TWO_MB_INDEX(tzram_layout->total_base);
+ nsram_start_index = TWO_MB_INDEX(NSRAM_BASE);
+
+ /*
+ * Fill up the level2 translation table for the first GB next
+ */
+ for (idx = 0; idx < NUM_2MB_IN_GB; idx++) {
+
+ l2_desc = INVALID_DESC;
+ xt_addr = &l2_xlation_table[GB1_L2_PAGETABLE][idx];
+
+ /* Block entries for 64M of trusted Boot ROM */
+ if ((idx >= trom_start_index) && (idx < trom_end_index))
+ l2_desc = create_romem_block(idx, LEVEL2, 0);
+
+ /* Single L3 page table entry for 256K of TZRAM */
+ if (idx == tzram_start_index) {
+ pt_addr = &l3_xlation_table[TZRAM_PAGETABLE][0];
+ l2_desc = create_table_desc(pt_addr);
+ }
+
+ /* Block entries for 32M of trusted DRAM */
+ if ((idx >= tdram_start_index) && (idx <= tdram_end_index))
+ l2_desc = create_rwmem_block(idx, LEVEL2, 0);
+
+ /* Block entries for 64M of aliased trusted Boot ROM */
+ if ((idx >= flash0_start_index) && (idx < flash0_end_index))
+ l2_desc = create_romem_block(idx, LEVEL2, 0);
+
+ /* Block entries for 64M of flash1 */
+ if ((idx >= flash1_start_index) && (idx < flash1_end_index))
+ l2_desc = create_romem_block(idx, LEVEL2, 0);
+
+ /* Block entries for 32M of VRAM */
+ if ((idx >= vram_start_index) && (idx < vram_end_index))
+ l2_desc = create_rwmem_block(idx, LEVEL2, 0);
+
+ /* Block entries for all the devices in the first gb */
+ if ((idx >= dev0_start_index) && (idx < dev0_end_index))
+ l2_desc = create_device_block(idx, LEVEL2, 0);
+
+ /* Block entries for all the devices in the first gb */
+ if ((idx >= dev1_start_index) && (idx < dev1_end_index))
+ l2_desc = create_device_block(idx, LEVEL2, 0);
+
+ /* Single L3 page table entry for 64K of NSRAM */
+ if (idx == nsram_start_index) {
+ pt_addr = &l3_xlation_table[NSRAM_PAGETABLE][0];
+ l2_desc = create_table_desc(pt_addr);
+ }
+
+ *xt_addr = l2_desc;
+ }
+
+
+ /*
+ * Initializations for the 2nd GB. Mark everything as device
+ * for the time being as the memory map is not final. Each
+ * index will need to be offset'ed to allow absolute values
+ */
+ off = NUM_2MB_IN_GB;
+ for (idx = off; idx < (NUM_2MB_IN_GB + off); idx++) {
+ l2_desc = create_device_block(idx, LEVEL2, 0);
+ xt_addr = &l2_xlation_table[GB2_L2_PAGETABLE][idx - off];
+ *xt_addr = l2_desc;
+ }
+
+
+ /*****************************************************************
+ * LEVEL3 PAGETABLE SETUP
+ * The following setup assumes knowledge of the scatter file. This
+ * should be reasonable as this is platform specific code.
+ *****************************************************************/
+
+ /* Fill up the level3 pagetable for the trusted SRAM. */
+ tzram_start_index = FOUR_KB_INDEX(tzram_layout->total_base);
+ tzram_end_index = FOUR_KB_INDEX(tzram_layout->total_base +
+ tzram_layout->total_size);
+ if (tzram_end_index == tzram_start_index)
+ tzram_end_index++;
+
+ /*
+ * Reusing trom* to mark RO memory. BLX_STACKS follows BLX_RO in the
+ * scatter file. Using BLX_RO$$Limit does not work as it might not
+ * cross the page boundary thus leading to truncation of valid RO
+ * memory
+ */
+ trom_start_index = FOUR_KB_INDEX(ro_start);
+ trom_end_index = FOUR_KB_INDEX(ro_limit);
+ if (trom_end_index == trom_start_index)
+ trom_end_index++;
+
+ /*
+ * Reusing dev* to mark coherent device memory. $$Limit works here
+ * 'cause the coherent memory section is known to be 4k in size
+ */
+ dev0_start_index = FOUR_KB_INDEX(coh_start);
+ dev0_end_index = FOUR_KB_INDEX(coh_limit);
+ if (dev0_end_index == dev0_start_index)
+ dev0_end_index++;
+
+
+ /* Each index will need to be offset'ed to allow absolute values */
+ off = FOUR_KB_INDEX(TZRAM_BASE);
+ for (idx = off; idx < (NUM_4K_IN_2MB + off); idx++) {
+
+ l3_desc = INVALID_DESC;
+ xt_addr = &l3_xlation_table[TZRAM_PAGETABLE][idx - off];
+
+ if (idx >= tzram_start_index && idx < tzram_end_index)
+ l3_desc = create_rwmem_block(idx, LEVEL3, 0);
+
+ if (idx >= trom_start_index && idx < trom_end_index)
+ l3_desc = create_romem_block(idx, LEVEL3, 0);
+
+ if (idx >= dev0_start_index && idx < dev0_end_index)
+ l3_desc = create_device_block(idx, LEVEL3, 0);
+
+ *xt_addr = l3_desc;
+ }
+
+ /* Fill up the level3 pagetable for the non-trusted SRAM. */
+ nsram_start_index = FOUR_KB_INDEX(NSRAM_BASE);
+ nsram_end_index = FOUR_KB_INDEX(NSRAM_BASE + NSRAM_SIZE);
+ if (nsram_end_index == nsram_start_index)
+ nsram_end_index++;
+
+ /* Each index will need to be offset'ed to allow absolute values */
+ off = FOUR_KB_INDEX(NSRAM_BASE);
+ for (idx = off; idx < (NUM_4K_IN_2MB + off); idx++) {
+
+ l3_desc = INVALID_DESC;
+ xt_addr = &l3_xlation_table[NSRAM_PAGETABLE][idx - off];
+
+ if (idx >= nsram_start_index && idx < nsram_end_index)
+ l3_desc = create_rwmem_block(idx, LEVEL3, NS);
+
+ *xt_addr = l3_desc;
+ }
+
+ return (unsigned long) l1_xlation_table;
+}
+
+/*******************************************************************************
+ * Enable the MMU assuming that the pagetables have already been created
+ *******************************************************************************/
+void enable_mmu()
+{
+ unsigned long mair, tcr, ttbr, sctlr;
+ unsigned long current_el = read_current_el();
+
+ /* Set the attributes in the right indices of the MAIR */
+ mair = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);
+ mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR,
+ ATTR_IWBWA_OWBWA_NTR_INDEX);
+ write_mair(mair);
+
+ /*
+ * Set TCR bits as well. Inner & outer WBWA & shareable + T0SZ = 32
+ */
+ tcr = TCR_SH_INNER_SHAREABLE | TCR_RGN_OUTER_WBA |
+ TCR_RGN_INNER_WBA | TCR_T0SZ_4GB;
+ if (GET_EL(current_el) == MODE_EL3) {
+ tcr |= TCR_EL3_RES1;
+ /* Invalidate all TLBs */
+ tlbialle3();
+ } else {
+ /* Invalidate EL1 TLBs */
+ tlbivmalle1();
+ }
+
+ write_tcr(tcr);
+
+ /* Set TTBR bits as well */
+ assert(((unsigned long)l1_xlation_table & (sizeof(l1_xlation_table) - 1)) == 0);
+ ttbr = (unsigned long) l1_xlation_table;
+ write_ttbr0(ttbr);
+
+ sctlr = read_sctlr();
+ sctlr |= SCTLR_WXN_BIT | SCTLR_M_BIT | SCTLR_I_BIT;
+ sctlr |= SCTLR_A_BIT | SCTLR_C_BIT;
+ write_sctlr(sctlr);
+
+ return;
+}
+
+void disable_mmu(void)
+{
+ /* Zero out the MMU related registers */
+ write_mair(0);
+ write_tcr(0);
+ write_ttbr0(0);
+ write_sctlr(0);
+
+ /* Invalidate TLBs of the CurrentEL */
+ tlbiall();
+
+ /* Flush the caches */
+ dcsw_op_all(DCCISW);
+
+ return;
+}
+
+/*******************************************************************************
+ * Setup the pagetables as per the platform memory map & initialize the mmu
+ *******************************************************************************/
+void configure_mmu(meminfo *mem_layout,
+ unsigned long ro_start,
+ unsigned long ro_limit,
+ unsigned long coh_start,
+ unsigned long coh_limit)
+{
+ fill_xlation_tables(mem_layout,
+ ro_start,
+ ro_limit,
+ coh_start,
+ coh_limit);
+ enable_mmu();
+ return;
+}
+
+/* Simple routine which returns a configuration variable value */
+unsigned long platform_get_cfgvar(unsigned int var_id)
+{
+ assert(var_id < CONFIG_LIMIT);
+ return platform_config[var_id];
+}
+
+/*******************************************************************************
+ * A single boot loader stack is expected to work on both the Foundation FVP
+ * models and the two flavours of the Base FVP models (AEMv8 & Cortex). The
+ * SYS_ID register provides a mechanism for detecting the differences between
+ * these platforms. This information is stored in a per-BL array to allow the
+ * code to take the correct path.Per BL platform configuration.
+ ******************************************************************************/
+int platform_config_setup(void)
+{
+ unsigned int rev, hbi, bld, arch, sys_id, midr_pn;
+
+ sys_id = mmio_read_32(VE_SYSREGS_BASE + V2M_SYS_ID);
+ rev = (sys_id >> SYS_ID_REV_SHIFT) & SYS_ID_REV_MASK;
+ hbi = (sys_id >> SYS_ID_HBI_SHIFT) & SYS_ID_HBI_MASK;
+ bld = (sys_id >> SYS_ID_BLD_SHIFT) & SYS_ID_BLD_MASK;
+ arch = (sys_id >> SYS_ID_ARCH_SHIFT) & SYS_ID_ARCH_MASK;
+
+ assert(rev == REV_FVP);
+ assert(arch == ARCH_MODEL);
+
+ /*
+ * The build field in the SYS_ID tells which variant of the GIC
+ * memory is implemented by the model.
+ */
+ switch (bld) {
+ case BLD_GIC_VE_MMAP:
+ platform_config[CONFIG_GICD_ADDR] = VE_GICD_BASE;
+ platform_config[CONFIG_GICC_ADDR] = VE_GICC_BASE;
+ platform_config[CONFIG_GICH_ADDR] = VE_GICH_BASE;
+ platform_config[CONFIG_GICV_ADDR] = VE_GICV_BASE;
+ break;
+ case BLD_GIC_A53A57_MMAP:
+ platform_config[CONFIG_GICD_ADDR] = BASE_GICD_BASE;
+ platform_config[CONFIG_GICC_ADDR] = BASE_GICC_BASE;
+ platform_config[CONFIG_GICH_ADDR] = BASE_GICH_BASE;
+ platform_config[CONFIG_GICV_ADDR] = BASE_GICV_BASE;
+ break;
+ default:
+ assert(0);
+ }
+
+ /*
+ * The hbi field in the SYS_ID is 0x020 for the Base FVP & 0x010
+ * for the Foundation FVP.
+ */
+ switch (hbi) {
+ case HBI_FOUNDATION:
+ platform_config[CONFIG_MAX_AFF0] = 4;
+ platform_config[CONFIG_MAX_AFF1] = 1;
+ platform_config[CONFIG_CPU_SETUP] = 0;
+ platform_config[CONFIG_BASE_MMAP] = 0;
+ break;
+ case HBI_FVP_BASE:
+ midr_pn = (read_midr() >> MIDR_PN_SHIFT) & MIDR_PN_MASK;
+ if ((midr_pn == MIDR_PN_A57) || (midr_pn == MIDR_PN_A53))
+ platform_config[CONFIG_CPU_SETUP] = 1;
+ else
+ platform_config[CONFIG_CPU_SETUP] = 0;
+
+ platform_config[CONFIG_MAX_AFF0] = 4;
+ platform_config[CONFIG_MAX_AFF1] = 2;
+ platform_config[CONFIG_BASE_MMAP] = 1;
+ break;
+ default:
+ assert(0);
+ }
+
+ return 0;
+}
+
+unsigned long plat_get_ns_image_entrypoint(void) {
+ return NS_IMAGE_OFFSET;
+}
diff --git a/plat/fvp/aarch64/fvp_helpers.S b/plat/fvp/aarch64/fvp_helpers.S
new file mode 100644
index 0000000..5cb0660
--- /dev/null
+++ b/plat/fvp/aarch64/fvp_helpers.S
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <arch.h>
+#include <platform.h>
+
+ .globl plat_report_exception
+
+ .section platform_code, "ax"
+
+ /* ---------------------------------------------
+ * void plat_report_exception(unsigned int type)
+ * Function to report an unhandled exception
+ * with platform-specific means.
+ * On FVP platform, it updates the LEDs
+ * to indicate where we are
+ * ---------------------------------------------
+ */
+plat_report_exception:
+ mrs x1, CurrentEl
+ lsr x1, x1, #MODE_EL_SHIFT
+ lsl x1, x1, #SYS_LED_EL_SHIFT
+ lsl x0, x0, #SYS_LED_EC_SHIFT
+ mov x2, #(SECURE << SYS_LED_SS_SHIFT)
+ orr x0, x0, x2
+ orr x0, x0, x1
+ mov x1, #VE_SYSREGS_BASE
+ add x1, x1, #V2M_SYS_LED
+ str x0, [x1]
+ ret
diff --git a/plat/fvp/bl1_plat_setup.c b/plat/fvp/bl1_plat_setup.c
new file mode 100644
index 0000000..7131f7a
--- /dev/null
+++ b/plat/fvp/bl1_plat_setup.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <bl1.h>
+#include <console.h>
+
+/*******************************************************************************
+ * Declarations of linker defined symbols which will help us find the layout
+ * of trusted SRAM
+ ******************************************************************************/
+#if defined (__GNUC__)
+extern unsigned long __FIRMWARE_ROM_START__;
+extern unsigned long __FIRMWARE_ROM_SIZE__;
+extern unsigned long __FIRMWARE_DATA_START__;
+extern unsigned long __FIRMWARE_DATA_SIZE__;
+extern unsigned long __FIRMWARE_BSS_START__;
+extern unsigned long __FIRMWARE_BSS_SIZE__;
+extern unsigned long __DATA_RAM_START__;
+extern unsigned long __DATA_RAM_SIZE__;
+extern unsigned long __BSS_RAM_START__;
+extern unsigned long __BSS_RAM_SIZE__;
+extern unsigned long __FIRMWARE_RAM_STACKS_START__;
+extern unsigned long __FIRMWARE_RAM_STACKS_SIZE__;
+extern unsigned long __FIRMWARE_RAM_PAGETABLES_START__;
+extern unsigned long __FIRMWARE_RAM_PAGETABLES_SIZE__;
+extern unsigned long __FIRMWARE_RAM_COHERENT_START__;
+extern unsigned long __FIRMWARE_RAM_COHERENT_SIZE__;
+
+#define BL1_COHERENT_MEM_BASE (&__FIRMWARE_RAM_COHERENT_START__)
+#define BL1_COHERENT_MEM_LIMIT \
+ ((unsigned long long)&__FIRMWARE_RAM_COHERENT_START__ + \
+ (unsigned long long)&__FIRMWARE_RAM_COHERENT_SIZE__)
+
+#define BL1_FIRMWARE_RAM_GLOBALS_ZI_BASE \
+ (unsigned long)(&__BSS_RAM_START__)
+#define BL1_FIRMWARE_RAM_GLOBALS_ZI_LENGTH \
+ (unsigned long)(&__FIRMWARE_BSS_SIZE__)
+
+#define BL1_FIRMWARE_RAM_COHERENT_ZI_BASE \
+ (unsigned long)(&__FIRMWARE_RAM_COHERENT_START__)
+#define BL1_FIRMWARE_RAM_COHERENT_ZI_LENGTH\
+ (unsigned long)(&__FIRMWARE_RAM_COHERENT_SIZE__)
+
+#define BL1_NORMAL_RAM_BASE (unsigned long)(&__BSS_RAM_START__)
+#define BL1_NORMAL_RAM_LIMIT \
+ ((unsigned long)&__FIRMWARE_RAM_COHERENT_START__ + \
+ (unsigned long)&__FIRMWARE_RAM_COHERENT_SIZE__)
+#else
+ #error "Unknown compiler."
+#endif
+
+
+/* Data structure which holds the extents of the trusted SRAM for BL1*/
+static meminfo bl1_tzram_layout = {0};
+
+meminfo bl1_get_sec_mem_layout(void)
+{
+ return bl1_tzram_layout;
+}
+
+/*******************************************************************************
+ * Perform any BL1 specific platform actions.
+ ******************************************************************************/
+void bl1_early_platform_setup(void)
+{
+ unsigned long bl1_normal_ram_base;
+ unsigned long bl1_coherent_ram_limit;
+ unsigned long tzram_limit = TZRAM_BASE + TZRAM_SIZE;
+
+ /*
+ * Initialize extents of the bl1 sections as per the platform
+ * defined values.
+ */
+ bl1_normal_ram_base = BL1_NORMAL_RAM_BASE;
+ bl1_coherent_ram_limit = BL1_NORMAL_RAM_LIMIT;
+
+ /*
+ * Calculate how much ram is BL1 using & how much remains free.
+ * This also includes a rudimentary mechanism to detect whether
+ * the BL1 data is loaded at the top or bottom of memory.
+ * TODO: add support for discontigous chunks of free ram if
+ * needed. Might need dynamic memory allocation support
+ * et al.
+ * Also assuming that the section for coherent memory is
+ * the last and for globals the first in the scatter file.
+ */
+ bl1_tzram_layout.total_base = TZRAM_BASE;
+ bl1_tzram_layout.total_size = TZRAM_SIZE;
+
+ if (bl1_coherent_ram_limit == tzram_limit) {
+ bl1_tzram_layout.free_base = TZRAM_BASE;
+ bl1_tzram_layout.free_size = bl1_normal_ram_base - TZRAM_BASE;
+ } else {
+ bl1_tzram_layout.free_base = bl1_coherent_ram_limit;
+ bl1_tzram_layout.free_size =
+ tzram_limit - bl1_coherent_ram_limit;
+ }
+}
+
+/*******************************************************************************
+ * Function which will evaluate how much of the trusted ram has been gobbled
+ * up by BL1 and return the base and size of whats available for loading BL2.
+ * Its called after coherency and the MMU have been turned on.
+ ******************************************************************************/
+void bl1_platform_setup(void)
+{
+ /*
+ * This should zero out our coherent stacks as well but we don't care
+ * as they are not being used right now.
+ */
+ memset((void *) BL1_FIRMWARE_RAM_COHERENT_ZI_BASE, 0,
+ (size_t) BL1_FIRMWARE_RAM_COHERENT_ZI_LENGTH);
+
+ /* Enable and initialize the System level generic timer */
+ mmio_write_32(SYS_CNTCTL_BASE + CNTCR_OFF, CNTCR_EN);
+
+ /* Initialize the console */
+ console_init();
+
+ return;
+}
+
+/*******************************************************************************
+ * Perform the very early platform specific architecture setup here. At the
+ * moment this is only intializes the mmu in a quick and dirty way. Later arch-
+ * itectural setup (bl1_arch_setup()) does not do anything platform specific.
+ ******************************************************************************/
+void bl1_plat_arch_setup(void)
+{
+ configure_mmu(&bl1_tzram_layout,
+ TZROM_BASE, /* Read_only region start */
+ TZROM_BASE + TZROM_SIZE, /* Read_only region size */
+ /* Coherent region start */
+ BL1_FIRMWARE_RAM_COHERENT_ZI_BASE,
+ /* Coherent region size */
+ BL1_FIRMWARE_RAM_COHERENT_ZI_BASE +
+ BL1_FIRMWARE_RAM_COHERENT_ZI_LENGTH);
+}
diff --git a/plat/fvp/bl2_plat_setup.c b/plat/fvp/bl2_plat_setup.c
new file mode 100644
index 0000000..e38f00b
--- /dev/null
+++ b/plat/fvp/bl2_plat_setup.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <bl2.h>
+#include <bl_common.h>
+
+/*******************************************************************************
+ * Declarations of linker defined symbols which will help us find the layout
+ * of trusted SRAM
+ ******************************************************************************/
+#if defined (__GNUC__)
+extern unsigned long __BL2_RO_BASE__;
+extern unsigned long __BL2_STACKS_BASE__;
+extern unsigned long __BL2_COHERENT_RAM_BASE__;
+extern unsigned long __BL2_RW_BASE__;
+
+#define BL2_RO_BASE __BL2_RO_BASE__
+#define BL2_STACKS_BASE __BL2_STACKS_BASE__
+#define BL2_COHERENT_RAM_BASE __BL2_COHERENT_RAM_BASE__
+#define BL2_RW_BASE __BL2_RW_BASE__
+
+#else
+ #error "Unknown compiler."
+#endif
+
+/* Pointer to memory visible to both BL2 and BL31 for passing data */
+extern unsigned char **bl2_el_change_mem_ptr;
+
+/* Data structure which holds the extents of the trusted SRAM for BL2 */
+static meminfo bl2_tzram_layout
+__attribute__ ((aligned(PLATFORM_CACHE_LINE_SIZE),
+ section("tzfw_coherent_mem"))) = {0};
+
+/* Data structure which holds the extents of the non-trusted DRAM for BL2*/
+static meminfo dram_layout = {0};
+
+meminfo bl2_get_sec_mem_layout(void)
+{
+ return bl2_tzram_layout;
+}
+
+meminfo bl2_get_ns_mem_layout(void)
+{
+ return dram_layout;
+}
+
+/*******************************************************************************
+ * BL1 has passed the extents of the trusted SRAM that should be visible to BL2
+ * in x0. This memory layout is sitting at the base of the free trusted SRAM.
+ * Copy it to a safe loaction before its reclaimed by later BL2 functionality.
+ ******************************************************************************/
+void bl2_early_platform_setup(meminfo *mem_layout,
+ void *data)
+{
+ /* Setup the BL2 memory layout */
+ bl2_tzram_layout.total_base = mem_layout->total_base;
+ bl2_tzram_layout.total_size = mem_layout->total_size;
+ bl2_tzram_layout.free_base = mem_layout->free_base;
+ bl2_tzram_layout.free_size = mem_layout->free_size;
+ bl2_tzram_layout.attr = mem_layout->attr;
+ bl2_tzram_layout.next = 0;
+
+ /* Initialize the platform config for future decision making */
+ platform_config_setup();
+
+ return;
+}
+
+/*******************************************************************************
+ * Not much to do here aprt from finding out the extents of non-trusted DRAM
+ * which will be used for loading the non-trusted software images. We are
+ * relying on pre-iniitialized zi memory so there is nothing to zero out like
+ * in BL1. This is 'cause BL2 is raw PIC binary. Its load address is determined
+ * at runtime. The ZI section might be lost if its not already there.
+ ******************************************************************************/
+void bl2_platform_setup()
+{
+ dram_layout.total_base = DRAM_BASE;
+ dram_layout.total_size = DRAM_SIZE;
+ dram_layout.free_base = DRAM_BASE;
+ dram_layout.free_size = DRAM_SIZE;
+ dram_layout.attr = 0;
+
+ /* Use the Trusted DRAM for passing args to BL31 */
+ bl2_el_change_mem_ptr = (unsigned char **) TZDRAM_BASE;
+
+ return;
+}
+
+/*******************************************************************************
+ * Perform the very early platform specific architectural setup here. At the
+ * moment this is only intializes the mmu in a quick and dirty way.
+ ******************************************************************************/
+void bl2_plat_arch_setup()
+{
+ unsigned long sctlr;
+
+ /* Enable instruction cache. */
+ sctlr = read_sctlr();
+ sctlr |= SCTLR_I_BIT;
+ write_sctlr(sctlr);
+
+ /*
+ * Very simple exception vectors which assert if any exception other
+ * than a single SMC call from BL2 to pass control to BL31 in EL3 is
+ * received.
+ */
+ write_vbar((unsigned long) early_exceptions);
+
+ configure_mmu(&bl2_tzram_layout,
+ (unsigned long) &BL2_RO_BASE,
+ (unsigned long) &BL2_STACKS_BASE,
+ (unsigned long) &BL2_COHERENT_RAM_BASE,
+ (unsigned long) &BL2_RW_BASE);
+ return;
+}
diff --git a/plat/fvp/bl31_plat_setup.c b/plat/fvp/bl31_plat_setup.c
new file mode 100644
index 0000000..6c8635f
--- /dev/null
+++ b/plat/fvp/bl31_plat_setup.c
@@ -0,0 +1,424 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <platform.h>
+#include <bl31.h>
+#include <bl_common.h>
+#include <pl011.h>
+#include <bakery_lock.h>
+#include <cci400.h>
+#include <gic.h>
+#include <fvp_pwrc.h>
+
+/*******************************************************************************
+ * Declarations of linker defined symbols which will help us find the layout
+ * of trusted SRAM
+ ******************************************************************************/
+#if defined (__GNUC__)
+extern unsigned long __BL31_RO_BASE__;
+extern unsigned long __BL31_STACKS_BASE__;
+extern unsigned long __BL31_COHERENT_RAM_BASE__;
+extern unsigned long __BL31_RW_BASE__;
+
+#define BL31_RO_BASE __BL31_RO_BASE__
+#define BL31_STACKS_BASE __BL31_STACKS_BASE__
+#define BL31_COHERENT_RAM_BASE __BL31_COHERENT_RAM_BASE__
+#define BL31_RW_BASE __BL31_RW_BASE__
+
+#else
+ #error "Unknown compiler."
+#endif
+
+/*******************************************************************************
+ * This data structures holds information copied by BL31 from BL2 to pass
+ * control to the non-trusted software images. A per-cpu entry was created to
+ * use the same structure in the warm boot path but that's not the case right
+ * now. Persisting with this approach for the time being. TODO: Can this be
+ * moved out of device memory.
+ ******************************************************************************/
+el_change_info ns_entry_info[PLATFORM_CORE_COUNT]
+__attribute__ ((aligned(PLATFORM_CACHE_LINE_SIZE),
+ section("tzfw_coherent_mem"))) = {0};
+
+/* Data structure which holds the extents of the trusted SRAM for BL31 */
+static meminfo bl31_tzram_layout
+__attribute__ ((aligned(PLATFORM_CACHE_LINE_SIZE),
+ section("tzfw_coherent_mem"))) = {0};
+
+meminfo bl31_get_sec_mem_layout(void)
+{
+ return bl31_tzram_layout;
+}
+
+/*******************************************************************************
+ * Return information about passing control to the non-trusted software images
+ * to common code.TODO: In the initial architecture, the image after BL31 will
+ * always run in the non-secure state. In the final architecture there
+ * will be a series of images. This function will need enhancement then
+ ******************************************************************************/
+el_change_info *bl31_get_next_image_info(unsigned long mpidr)
+{
+ return &ns_entry_info[platform_get_core_pos(mpidr)];
+}
+
+/*******************************************************************************
+ * Perform any BL31 specific platform actions. Here we copy parameters passed
+ * by the calling EL (S-EL1 in BL2 & S-EL3 in BL1) before they are lost
+ * (potentially). This is done before the MMU is initialized so that the memory
+ * layout can be used while creating page tables.
+ ******************************************************************************/
+void bl31_early_platform_setup(meminfo *mem_layout,
+ void *data,
+ unsigned long mpidr)
+{
+ el_change_info *image_info = (el_change_info *) data;
+ unsigned int lin_index = platform_get_core_pos(mpidr);
+
+ /* Setup the BL31 memory layout */
+ bl31_tzram_layout.total_base = mem_layout->total_base;
+ bl31_tzram_layout.total_size = mem_layout->total_size;
+ bl31_tzram_layout.free_base = mem_layout->free_base;
+ bl31_tzram_layout.free_size = mem_layout->free_size;
+ bl31_tzram_layout.attr = mem_layout->attr;
+ bl31_tzram_layout.next = 0;
+
+ /* Save information about jumping into the NS world */
+ ns_entry_info[lin_index].entrypoint = image_info->entrypoint;
+ ns_entry_info[lin_index].spsr = image_info->spsr;
+ ns_entry_info[lin_index].args = image_info->args;
+ ns_entry_info[lin_index].security_state = image_info->security_state;
+ ns_entry_info[lin_index].next = image_info->next;
+
+ /* Initialize the platform config for future decision making */
+ platform_config_setup();
+}
+
+/*******************************************************************************
+ * Initialize the gic, configure the CLCD and zero out variables needed by the
+ * secondaries to boot up correctly.
+ ******************************************************************************/
+void bl31_platform_setup()
+{
+ unsigned int reg_val;
+
+ /* Initialize the gic cpu and distributor interfaces */
+ gic_setup();
+
+ /*
+ * TODO: Configure the CLCD before handing control to
+ * linux. Need to see if a separate driver is needed
+ * instead.
+ */
+ mmio_write_32(VE_SYSREGS_BASE + V2M_SYS_CFGDATA, 0);
+ mmio_write_32(VE_SYSREGS_BASE + V2M_SYS_CFGCTRL,
+ (1ull << 31) | (1 << 30) | (7 << 20) | (0 << 16));
+
+ /* Allow access to the System counter timer module */
+ reg_val = (1 << CNTACR_RPCT_SHIFT) | (1 << CNTACR_RVCT_SHIFT);
+ reg_val |= (1 << CNTACR_RFRQ_SHIFT) | (1 << CNTACR_RVOFF_SHIFT);
+ reg_val |= (1 << CNTACR_RWVT_SHIFT) | (1 << CNTACR_RWPT_SHIFT);
+ mmio_write_32(SYS_TIMCTL_BASE + CNTACR_BASE(0), reg_val);
+ mmio_write_32(SYS_TIMCTL_BASE + CNTACR_BASE(1), reg_val);
+
+ reg_val = (1 << CNTNSAR_NS_SHIFT(0)) | (1 << CNTNSAR_NS_SHIFT(1));
+ mmio_write_32(SYS_TIMCTL_BASE + CNTNSAR, reg_val);
+
+ /* Intialize the power controller */
+ fvp_pwrc_setup();
+
+ /* Topologies are best known to the platform. */
+ plat_setup_topology();
+}
+
+/*******************************************************************************
+ * Perform the very early platform specific architectural setup here. At the
+ * moment this is only intializes the mmu in a quick and dirty way.
+ ******************************************************************************/
+void bl31_plat_arch_setup()
+{
+ unsigned long sctlr;
+
+ /* Enable instruction cache. */
+ sctlr = read_sctlr();
+ sctlr |= SCTLR_I_BIT;
+ write_sctlr(sctlr);
+
+ write_vbar((unsigned long) runtime_exceptions);
+ configure_mmu(&bl31_tzram_layout,
+ (unsigned long) &BL31_RO_BASE,
+ (unsigned long) &BL31_STACKS_BASE,
+ (unsigned long) &BL31_COHERENT_RAM_BASE,
+ (unsigned long) &BL31_RW_BASE);
+}
+
+/*******************************************************************************
+ * TODO: Move GIC setup to a separate file in case it is needed by other BL
+ * stages or ELs
+ * TODO: Revisit if priorities are being set such that no non-secure interrupt
+ * can have a higher priority than a secure one as recommended in the GICv2 spec
+ *******************************************************************************/
+
+/*******************************************************************************
+ * This function does some minimal GICv3 configuration. The Firmware itself does
+ * not fully support GICv3 at this time and relies on GICv2 emulation as
+ * provided by GICv3. This function allows software (like Linux) in later stages
+ * to use full GICv3 features.
+ *******************************************************************************/
+void gicv3_cpuif_setup(void)
+{
+ unsigned int scr_val, val, base;
+
+ /*
+ * When CPUs come out of reset they have their GICR_WAKER.ProcessorSleep
+ * bit set. In order to allow interrupts to get routed to the CPU we
+ * need to clear this bit if set and wait for GICR_WAKER.ChildrenAsleep
+ * to clear (GICv3 Architecture specification 5.4.23).
+ * GICR_WAKER is NOT banked per CPU, compute the correct base address
+ * per CPU.
+ *
+ * TODO:
+ * For GICv4 we also need to adjust the Base address based on
+ * GICR_TYPER.VLPIS
+ */
+ base = BASE_GICR_BASE +
+ (platform_get_core_pos(read_mpidr()) << GICR_PCPUBASE_SHIFT);
+ val = gicr_read_waker(base);
+
+ val &= ~WAKER_PS;
+ gicr_write_waker(base, val);
+ dsb();
+
+ /* We need to wait for ChildrenAsleep to clear. */
+ val = gicr_read_waker(base);
+ while (val & WAKER_CA) {
+ val = gicr_read_waker(base);
+ }
+
+ /*
+ * We need to set SCR_EL3.NS in order to see GICv3 non-secure state.
+ * Restore SCR_EL3.NS again before exit.
+ */
+ scr_val = read_scr();
+ write_scr(scr_val | SCR_NS_BIT);
+
+ /*
+ * By default EL2 and NS-EL1 software should be able to enable GICv3
+ * System register access without any configuration at EL3. But it turns
+ * out that GICC PMR as set in GICv2 mode does not affect GICv3 mode. So
+ * we need to set it here again. In order to do that we need to enable
+ * register access. We leave it enabled as it should be fine and might
+ * prevent problems with later software trying to access GIC System
+ * Registers.
+ */
+ val = read_icc_sre_el3();
+ write_icc_sre_el3(val | ICC_SRE_EN | ICC_SRE_SRE);
+
+ val = read_icc_sre_el2();
+ write_icc_sre_el2(val | ICC_SRE_EN | ICC_SRE_SRE);
+
+ write_icc_pmr_el1(MAX_PRI_VAL);
+
+ /* Restore SCR_EL3 */
+ write_scr(scr_val);
+}
+
+/*******************************************************************************
+ * This function does some minimal GICv3 configuration when cores go
+ * down.
+ *******************************************************************************/
+void gicv3_cpuif_deactivate(void)
+{
+ unsigned int val, base;
+
+ /*
+ * When taking CPUs down we need to set GICR_WAKER.ProcessorSleep and
+ * wait for GICR_WAKER.ChildrenAsleep to get set.
+ * (GICv3 Architecture specification 5.4.23).
+ * GICR_WAKER is NOT banked per CPU, compute the correct base address
+ * per CPU.
+ *
+ * TODO:
+ * For GICv4 we also need to adjust the Base address based on
+ * GICR_TYPER.VLPIS
+ */
+ base = BASE_GICR_BASE +
+ (platform_get_core_pos(read_mpidr()) << GICR_PCPUBASE_SHIFT);
+ val = gicr_read_waker(base);
+ val |= WAKER_PS;
+ gicr_write_waker(base, val);
+ dsb();
+
+ /* We need to wait for ChildrenAsleep to set. */
+ val = gicr_read_waker(base);
+ while ((val & WAKER_CA) == 0) {
+ val = gicr_read_waker(base);
+ }
+}
+
+
+/*******************************************************************************
+ * Enable secure interrupts and use FIQs to route them. Disable legacy bypass
+ * and set the priority mask register to allow all interrupts to trickle in.
+ ******************************************************************************/
+void gic_cpuif_setup(unsigned int gicc_base)
+{
+ unsigned int val;
+
+ val = gicc_read_iidr(gicc_base);
+
+ /*
+ * If GICv3 we need to do a bit of additional setup. We want to
+ * allow default GICv2 behaviour but allow the next stage to
+ * enable full gicv3 features.
+ */
+ if (((val >> GICC_IIDR_ARCH_SHIFT) & GICC_IIDR_ARCH_MASK) >= 3) {
+ gicv3_cpuif_setup();
+ }
+
+ val = ENABLE_GRP0 | FIQ_EN | FIQ_BYP_DIS_GRP0;
+ val |= IRQ_BYP_DIS_GRP0 | FIQ_BYP_DIS_GRP1 | IRQ_BYP_DIS_GRP1;
+
+ gicc_write_pmr(gicc_base, MAX_PRI_VAL);
+ gicc_write_ctlr(gicc_base, val);
+}
+
+/*******************************************************************************
+ * Place the cpu interface in a state where it can never make a cpu exit wfi as
+ * as result of an asserted interrupt. This is critical for powering down a cpu
+ ******************************************************************************/
+void gic_cpuif_deactivate(unsigned int gicc_base)
+{
+ unsigned int val;
+
+ /* Disable secure, non-secure interrupts and disable their bypass */
+ val = gicc_read_ctlr(gicc_base);
+ val &= ~(ENABLE_GRP0 | ENABLE_GRP1);
+ val |= FIQ_BYP_DIS_GRP1 | FIQ_BYP_DIS_GRP0;
+ val |= IRQ_BYP_DIS_GRP0 | IRQ_BYP_DIS_GRP1;
+ gicc_write_ctlr(gicc_base, val);
+
+ val = gicc_read_iidr(gicc_base);
+
+ /*
+ * If GICv3 we need to do a bit of additional setup. Make sure the
+ * RDIST is put to sleep.
+ */
+ if (((val >> GICC_IIDR_ARCH_SHIFT) & GICC_IIDR_ARCH_MASK) >= 3) {
+ gicv3_cpuif_deactivate();
+ }
+}
+
+/*******************************************************************************
+ * Per cpu gic distributor setup which will be done by all cpus after a cold
+ * boot/hotplug. This marks out the secure interrupts & enables them.
+ ******************************************************************************/
+void gic_pcpu_distif_setup(unsigned int gicd_base)
+{
+ gicd_write_igroupr(gicd_base, 0, ~0);
+
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_PHY_TIMER);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_0);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_1);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_2);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_3);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_4);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_5);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_6);
+ gicd_clr_igroupr(gicd_base, IRQ_SEC_SGI_7);
+
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_PHY_TIMER, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_0, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_1, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_2, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_3, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_4, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_5, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_6, MAX_PRI_VAL);
+ gicd_set_ipriorityr(gicd_base, IRQ_SEC_SGI_7, MAX_PRI_VAL);
+
+ gicd_set_isenabler(gicd_base, IRQ_SEC_PHY_TIMER);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_0);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_1);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_2);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_3);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_4);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_5);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_6);
+ gicd_set_isenabler(gicd_base, IRQ_SEC_SGI_7);
+}
+
+/*******************************************************************************
+ * Global gic distributor setup which will be done by the primary cpu after a
+ * cold boot. It marks out the secure SPIs, PPIs & SGIs and enables them. It
+ * then enables the secure GIC distributor interface.
+ ******************************************************************************/
+void gic_distif_setup(unsigned int gicd_base)
+{
+ unsigned int ctr, num_ints, ctlr;
+
+ /* Disable the distributor before going further */
+ ctlr = gicd_read_ctlr(gicd_base);
+ ctlr &= ~(ENABLE_GRP0 | ENABLE_GRP1);
+ gicd_write_ctlr(gicd_base, ctlr);
+
+ /*
+ * Mark out non-secure interrupts. Calculate number of
+ * IGROUPR registers to consider. Will be equal to the
+ * number of IT_LINES
+ */
+ num_ints = gicd_read_typer(gicd_base) & IT_LINES_NO_MASK;
+ num_ints++;
+ for (ctr = 0; ctr < num_ints; ctr++)
+ gicd_write_igroupr(gicd_base, ctr << IGROUPR_SHIFT, ~0);
+
+ /* Configure secure interrupts now */
+ gicd_clr_igroupr(gicd_base, IRQ_TZ_WDOG);
+ gicd_set_ipriorityr(gicd_base, IRQ_TZ_WDOG, MAX_PRI_VAL);
+ gicd_set_itargetsr(gicd_base, IRQ_TZ_WDOG,
+ platform_get_core_pos(read_mpidr()));
+ gicd_set_isenabler(gicd_base, IRQ_TZ_WDOG);
+ gic_pcpu_distif_setup(gicd_base);
+
+ gicd_write_ctlr(gicd_base, ctlr | ENABLE_GRP0);
+}
+
+void gic_setup(void)
+{
+ unsigned int gicd_base, gicc_base;
+
+ gicd_base = platform_get_cfgvar(CONFIG_GICD_ADDR);
+ gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+
+ gic_cpuif_setup(gicc_base);
+ gic_distif_setup(gicd_base);
+}
diff --git a/plat/fvp/fvp_pm.c b/plat/fvp/fvp_pm.c
new file mode 100644
index 0000000..9621319
--- /dev/null
+++ b/plat/fvp/fvp_pm.c
@@ -0,0 +1,372 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <bl_common.h>
+#include <bl31.h>
+#include <bakery_lock.h>
+#include <cci400.h>
+#include <gic.h>
+#include <fvp_pwrc.h>
+/* Only included for error codes */
+#include <psci.h>
+
+/*******************************************************************************
+ * FVP handler called when an affinity instance is about to be turned on. The
+ * level and mpidr determine the affinity instance.
+ ******************************************************************************/
+int fvp_affinst_on(unsigned long mpidr,
+ unsigned long sec_entrypoint,
+ unsigned long ns_entrypoint,
+ unsigned int afflvl,
+ unsigned int state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned long linear_id;
+ mailbox *fvp_mboxes;
+ unsigned int psysr;
+
+ if (ns_entrypoint < DRAM_BASE) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ /*
+ * It's possible to turn on only affinity level 0 i.e. a cpu
+ * on the FVP. Ignore any other affinity level.
+ */
+ if (afflvl != MPIDR_AFFLVL0)
+ goto exit;
+
+ /*
+ * Ensure that we do not cancel an inflight power off request
+ * for the target cpu. That would leave it in a zombie wfi.
+ * Wait for it to power off, program the jump address for the
+ * target cpu and then program the power controller to turn
+ * that cpu on
+ */
+ do {
+ psysr = fvp_pwrc_read_psysr(mpidr);
+ } while (psysr & PSYSR_AFF_L0);
+
+ linear_id = platform_get_core_pos(mpidr);
+ fvp_mboxes = (mailbox *) (TZDRAM_BASE + MBOX_OFF);
+ fvp_mboxes[linear_id].value = sec_entrypoint;
+ flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
+ sizeof(unsigned long));
+
+ fvp_pwrc_write_pponr(mpidr);
+
+exit:
+ return rc;
+}
+
+/*******************************************************************************
+ * FVP handler called when an affinity instance is about to be turned off. The
+ * level and mpidr determine the affinity instance. The 'state' arg. allows the
+ * platform to decide whether the cluster is being turned off and take apt
+ * actions.
+ *
+ * CAUTION: This function is called with coherent stacks so that caches can be
+ * turned off, flushed and coherency disabled. There is no guarantee that caches
+ * will remain turned on across calls to this function as each affinity level is
+ * dealt with. So do not write & read global variables across calls. It will be
+ * wise to do flush a write to the global to prevent unpredictable results.
+ ******************************************************************************/
+int fvp_affinst_off(unsigned long mpidr,
+ unsigned int afflvl,
+ unsigned int state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int gicc_base, ectlr;
+ unsigned long cpu_setup;
+
+ switch (afflvl) {
+ case MPIDR_AFFLVL1:
+ if (state == PSCI_STATE_OFF) {
+ /*
+ * Disable coherency if this cluster is to be
+ * turned off
+ */
+ cci_disable_coherency(mpidr);
+
+ /*
+ * Program the power controller to turn the
+ * cluster off
+ */
+ fvp_pwrc_write_pcoffr(mpidr);
+
+ }
+ break;
+
+ case MPIDR_AFFLVL0:
+ if (state == PSCI_STATE_OFF) {
+
+ /*
+ * Take this cpu out of intra-cluster coherency if
+ * the FVP flavour supports the SMP bit.
+ */
+ cpu_setup = platform_get_cfgvar(CONFIG_CPU_SETUP);
+ if (cpu_setup) {
+ ectlr = read_cpuectlr();
+ ectlr &= ~CPUECTLR_SMP_BIT;
+ write_cpuectlr(ectlr);
+ }
+
+ /*
+ * Prevent interrupts from spuriously waking up
+ * this cpu
+ */
+ gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+ gic_cpuif_deactivate(gicc_base);
+
+ /*
+ * Program the power controller to power this
+ * cpu off
+ */
+ fvp_pwrc_write_ppoffr(mpidr);
+ }
+ break;
+
+ default:
+ assert(0);
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * FVP handler called when an affinity instance is about to be suspended. The
+ * level and mpidr determine the affinity instance. The 'state' arg. allows the
+ * platform to decide whether the cluster is being turned off and take apt
+ * actions.
+ *
+ * CAUTION: This function is called with coherent stacks so that caches can be
+ * turned off, flushed and coherency disabled. There is no guarantee that caches
+ * will remain turned on across calls to this function as each affinity level is
+ * dealt with. So do not write & read global variables across calls. It will be
+ * wise to do flush a write to the global to prevent unpredictable results.
+ ******************************************************************************/
+int fvp_affinst_suspend(unsigned long mpidr,
+ unsigned long sec_entrypoint,
+ unsigned long ns_entrypoint,
+ unsigned int afflvl,
+ unsigned int state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned int gicc_base, ectlr;
+ unsigned long cpu_setup, linear_id;
+ mailbox *fvp_mboxes;
+
+ /* Cannot allow NS world to execute trusted firmware code */
+ if (ns_entrypoint < DRAM_BASE) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ switch (afflvl) {
+ case MPIDR_AFFLVL1:
+ if (state == PSCI_STATE_OFF) {
+ /*
+ * Disable coherency if this cluster is to be
+ * turned off
+ */
+ cci_disable_coherency(mpidr);
+
+ /*
+ * Program the power controller to turn the
+ * cluster off
+ */
+ fvp_pwrc_write_pcoffr(mpidr);
+
+ }
+ break;
+
+ case MPIDR_AFFLVL0:
+ if (state == PSCI_STATE_OFF) {
+ /*
+ * Take this cpu out of intra-cluster coherency if
+ * the FVP flavour supports the SMP bit.
+ */
+ cpu_setup = platform_get_cfgvar(CONFIG_CPU_SETUP);
+ if (cpu_setup) {
+ ectlr = read_cpuectlr();
+ ectlr &= ~CPUECTLR_SMP_BIT;
+ write_cpuectlr(ectlr);
+ }
+
+ /* Program the jump address for the target cpu */
+ linear_id = platform_get_core_pos(mpidr);
+ fvp_mboxes = (mailbox *) (TZDRAM_BASE + MBOX_OFF);
+ fvp_mboxes[linear_id].value = sec_entrypoint;
+ flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
+ sizeof(unsigned long));
+
+ /*
+ * Prevent interrupts from spuriously waking up
+ * this cpu
+ */
+ gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+ gic_cpuif_deactivate(gicc_base);
+
+ /*
+ * Program the power controller to power this
+ * cpu off and enable wakeup interrupts.
+ */
+ fvp_pwrc_write_pwkupr(mpidr);
+ fvp_pwrc_write_ppoffr(mpidr);
+ }
+ break;
+
+ default:
+ assert(0);
+ }
+
+exit:
+ return rc;
+}
+
+/*******************************************************************************
+ * FVP handler called when an affinity instance has just been powered on after
+ * being turned off earlier. The level and mpidr determine the affinity
+ * instance. The 'state' arg. allows the platform to decide whether the cluster
+ * was turned off prior to wakeup and do what's necessary to setup it up
+ * correctly.
+ ******************************************************************************/
+int fvp_affinst_on_finish(unsigned long mpidr,
+ unsigned int afflvl,
+ unsigned int state)
+{
+ int rc = PSCI_E_SUCCESS;
+ unsigned long linear_id, cpu_setup;
+ mailbox *fvp_mboxes;
+ unsigned int gicd_base, gicc_base, reg_val, ectlr;
+
+ switch (afflvl) {
+
+ case MPIDR_AFFLVL1:
+ /* Enable coherency if this cluster was off */
+ if (state == PSCI_STATE_OFF)
+ cci_enable_coherency(mpidr);
+ break;
+
+ case MPIDR_AFFLVL0:
+ /*
+ * Ignore the state passed for a cpu. It could only have
+ * been off if we are here.
+ */
+
+ /*
+ * Turn on intra-cluster coherency if the FVP flavour supports
+ * it.
+ */
+ cpu_setup = platform_get_cfgvar(CONFIG_CPU_SETUP);
+ if (cpu_setup) {
+ ectlr = read_cpuectlr();
+ ectlr |= CPUECTLR_SMP_BIT;
+ write_cpuectlr(ectlr);
+ }
+
+ /* Zero the jump address in the mailbox for this cpu */
+ fvp_mboxes = (mailbox *) (TZDRAM_BASE + MBOX_OFF);
+ linear_id = platform_get_core_pos(mpidr);
+ fvp_mboxes[linear_id].value = 0;
+ flush_dcache_range((unsigned long) &fvp_mboxes[linear_id],
+ sizeof(unsigned long));
+
+ gicd_base = platform_get_cfgvar(CONFIG_GICD_ADDR);
+ gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+
+ /* Enable the gic cpu interface */
+ gic_cpuif_setup(gicc_base);
+
+ /* TODO: This setup is needed only after a cold boot */
+ gic_pcpu_distif_setup(gicd_base);
+
+ /* Allow access to the System counter timer module */
+ reg_val = (1 << CNTACR_RPCT_SHIFT) | (1 << CNTACR_RVCT_SHIFT);
+ reg_val |= (1 << CNTACR_RFRQ_SHIFT) | (1 << CNTACR_RVOFF_SHIFT);
+ reg_val |= (1 << CNTACR_RWVT_SHIFT) | (1 << CNTACR_RWPT_SHIFT);
+ mmio_write_32(SYS_TIMCTL_BASE + CNTACR_BASE(0), reg_val);
+ mmio_write_32(SYS_TIMCTL_BASE + CNTACR_BASE(1), reg_val);
+
+ reg_val = (1 << CNTNSAR_NS_SHIFT(0)) |
+ (1 << CNTNSAR_NS_SHIFT(1));
+ mmio_write_32(SYS_TIMCTL_BASE + CNTNSAR, reg_val);
+
+ break;
+
+ default:
+ assert(0);
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * FVP handler called when an affinity instance has just been powered on after
+ * having been suspended earlier. The level and mpidr determine the affinity
+ * instance.
+ * TODO: At the moment we reuse the on finisher and reinitialize the secure
+ * context. Need to implement a separate suspend finisher.
+ ******************************************************************************/
+int fvp_affinst_suspend_finish(unsigned long mpidr,
+ unsigned int afflvl,
+ unsigned int state)
+{
+ return fvp_affinst_on_finish(mpidr, afflvl, state);
+}
+
+
+/*******************************************************************************
+ * Export the platform handlers to enable psci to invoke them
+ ******************************************************************************/
+static plat_pm_ops fvp_plat_pm_ops = {
+ 0,
+ fvp_affinst_on,
+ fvp_affinst_off,
+ fvp_affinst_suspend,
+ fvp_affinst_on_finish,
+ fvp_affinst_suspend_finish,
+};
+
+/*******************************************************************************
+ * Export the platform specific power ops & initialize the fvp power controller
+ ******************************************************************************/
+int platform_setup_pm(plat_pm_ops **plat_ops)
+{
+ *plat_ops = &fvp_plat_pm_ops;
+ return 0;
+}
diff --git a/plat/fvp/fvp_topology.c b/plat/fvp/fvp_topology.c
new file mode 100644
index 0000000..20f3324
--- /dev/null
+++ b/plat/fvp/fvp_topology.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <string.h>
+#include <assert.h>
+#include <platform.h>
+#include <fvp_pwrc.h>
+/* TODO: Reusing psci error codes & state information. Get our own! */
+#include <psci.h>
+
+/* We treat '255' as an invalid affinity instance */
+#define AFFINST_INVAL 0xff
+
+/*******************************************************************************
+ * We support 3 flavours of the FVP: Foundation, Base AEM & Base Cortex. Each
+ * flavour has a different topology. The common bit is that there can be a max.
+ * of 2 clusters (affinity 1) and 4 cpus (affinity 0) per cluster. So we define
+ * a tree like data structure which caters to these maximum bounds. It simply
+ * marks the absent affinity level instances as PSCI_AFF_ABSENT e.g. there is no
+ * cluster 1 on the Foundation FVP. The 'data' field is currently unused.
+ ******************************************************************************/
+typedef struct {
+ unsigned char sibling;
+ unsigned char child;
+ unsigned char state;
+ unsigned int data;
+} affinity_info;
+
+/*******************************************************************************
+ * The following two data structures store the topology tree for the fvp. There
+ * is a separate array for each affinity level i.e. cpus and clusters. The child
+ * and sibling references allow traversal inside and in between the two arrays.
+ ******************************************************************************/
+static affinity_info fvp_aff1_topology_map[PLATFORM_CLUSTER_COUNT];
+static affinity_info fvp_aff0_topology_map[PLATFORM_CORE_COUNT];
+
+/* Simple global variable to safeguard us from stupidity */
+static unsigned int topology_setup_done;
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform to allow the former to detect the platform
+ * topology. psci queries the platform to determine how many affinity instances
+ * are present at a particular level for a given mpidr e.g. consider a dual
+ * cluster platform where each cluster has 4 cpus. A call to this function with
+ * (0, 0x100) will return the number of cpus implemented under cluster 1 i.e. 4.
+ * Similarly a call with (1, 0x100) will return 2 i.e. the number of clusters.
+ * This is 'cause we are effectively asking how many affinity level 1 instances
+ * are implemented under affinity level 2 instance 0.
+ ******************************************************************************/
+unsigned int plat_get_aff_count(unsigned int aff_lvl,
+ unsigned long mpidr)
+{
+ unsigned int aff_count = 1, ctr;
+ unsigned char parent_aff_id;
+
+ assert(topology_setup_done == 1);
+
+ switch (aff_lvl) {
+ case 3:
+ case 2:
+ /*
+ * Assert if the parent affinity instance is not 0.
+ * This also takes care of level 3 in an obfuscated way
+ */
+ parent_aff_id = (mpidr >> MPIDR_AFF3_SHIFT) & MPIDR_AFFLVL_MASK;
+ assert(parent_aff_id == 0);
+
+ /*
+ * Report that we implement a single instance of
+ * affinity levels 2 & 3 which are AFF_ABSENT
+ */
+ break;
+ case 1:
+ /* Assert if the parent affinity instance is not 0. */
+ parent_aff_id = (mpidr >> MPIDR_AFF2_SHIFT) & MPIDR_AFFLVL_MASK;
+ assert(parent_aff_id == 0);
+
+ /* Fetch the starting index in the aff1 array */
+ for (ctr = 0;
+ fvp_aff1_topology_map[ctr].sibling != AFFINST_INVAL;
+ ctr = fvp_aff1_topology_map[ctr].sibling) {
+ aff_count++;
+ }
+
+ break;
+ case 0:
+ /* Assert if the cluster id is anything apart from 0 or 1 */
+ parent_aff_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+ assert(parent_aff_id < PLATFORM_CLUSTER_COUNT);
+
+ /* Fetch the starting index in the aff0 array */
+ for (ctr = fvp_aff1_topology_map[parent_aff_id].child;
+ fvp_aff0_topology_map[ctr].sibling != AFFINST_INVAL;
+ ctr = fvp_aff0_topology_map[ctr].sibling) {
+ aff_count++;
+ }
+
+ break;
+ default:
+ assert(0);
+ }
+
+ return aff_count;
+}
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform to allow the former to detect the state of a
+ * affinity instance in the platform topology. psci queries the platform to
+ * determine whether an affinity instance is present or absent. This caters for
+ * topologies where an intermediate affinity level instance is missing e.g.
+ * consider a platform which implements a single cluster with 4 cpus and there
+ * is another cpu sitting directly on the interconnect along with the cluster.
+ * The mpidrs of the cluster would range from 0x0-0x3. The mpidr of the single
+ * cpu would be 0x100 to highlight that it does not belong to cluster 0. Cluster
+ * 1 is however missing but needs to be accounted to reach this single cpu in
+ * the topology tree. Hence it will be marked as PSCI_AFF_ABSENT. This is not
+ * applicable to the FVP but depicted as an example.
+ ******************************************************************************/
+unsigned int plat_get_aff_state(unsigned int aff_lvl,
+ unsigned long mpidr)
+{
+ unsigned int aff_state = PSCI_AFF_ABSENT, idx;
+ idx = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+
+ assert(topology_setup_done == 1);
+
+ switch (aff_lvl) {
+ case 3:
+ case 2:
+ /* Report affinity levels 2 & 3 as absent */
+ break;
+ case 1:
+ aff_state = fvp_aff1_topology_map[idx].state;
+ break;
+ case 0:
+ /*
+ * First get start index of the aff0 in its array & then add
+ * to it the affinity id that we want the state of
+ */
+ idx = fvp_aff1_topology_map[idx].child;
+ idx += (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+ aff_state = fvp_aff0_topology_map[idx].state;
+ break;
+ default:
+ assert(0);
+ }
+
+ return aff_state;
+}
+
+/*******************************************************************************
+ * Handy optimization to prevent the psci implementation from traversing through
+ * affinity levels which are not present while detecting the platform topology.
+ ******************************************************************************/
+int plat_get_max_afflvl()
+{
+ return MPIDR_AFFLVL1;
+}
+
+/*******************************************************************************
+ * This function populates the FVP specific topology information depending upon
+ * the FVP flavour its running on. We construct all the mpidrs we can handle
+ * and rely on the PWRC.PSYSR to flag absent cpus when their status is queried.
+ ******************************************************************************/
+int plat_setup_topology()
+{
+ unsigned char aff0, aff1, aff_state, aff0_offset = 0;
+ unsigned long mpidr;
+
+ topology_setup_done = 0;
+
+ for (aff1 = 0; aff1 < PLATFORM_CLUSTER_COUNT; aff1++) {
+
+ fvp_aff1_topology_map[aff1].child = aff0_offset;
+ fvp_aff1_topology_map[aff1].sibling = aff1 + 1;
+
+ for (aff0 = 0; aff0 < PLATFORM_MAX_CPUS_PER_CLUSTER; aff0++) {
+
+ mpidr = aff1 << MPIDR_AFF1_SHIFT;
+ mpidr |= aff0 << MPIDR_AFF0_SHIFT;
+
+ if (fvp_pwrc_read_psysr(mpidr) != PSYSR_INVALID) {
+ /*
+ * Presence of even a single aff0 indicates
+ * presence of parent aff1 on the FVP.
+ */
+ aff_state = PSCI_AFF_PRESENT;
+ fvp_aff1_topology_map[aff1].state =
+ PSCI_AFF_PRESENT;
+ } else {
+ aff_state = PSCI_AFF_ABSENT;
+ }
+
+ fvp_aff0_topology_map[aff0_offset].child = AFFINST_INVAL;
+ fvp_aff0_topology_map[aff0_offset].state = aff_state;
+ fvp_aff0_topology_map[aff0_offset].sibling =
+ aff0_offset + 1;
+
+ /* Increment the absolute number of aff0s traversed */
+ aff0_offset++;
+ }
+
+ /* Tie-off the last aff0 sibling to -1 to avoid overflow */
+ fvp_aff0_topology_map[aff0_offset - 1].sibling = AFFINST_INVAL;
+ }
+
+ /* Tie-off the last aff1 sibling to AFFINST_INVAL to avoid overflow */
+ fvp_aff1_topology_map[aff1 - 1].sibling = AFFINST_INVAL;
+
+ topology_setup_done = 1;
+ return 0;
+}
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
new file mode 100644
index 0000000..21a7912
--- /dev/null
+++ b/plat/fvp/platform.h
@@ -0,0 +1,341 @@
+/*
+ * Copyright (c) 2013, ARM Limited. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 EV