Merge pull request #853 from vwadekar/tegra-changes-from-downstream-v3
Tegra changes from downstream v3
diff --git a/docs/plat/nvidia-tegra.md b/docs/plat/nvidia-tegra.md
index f82085b..b45fec6 100644
--- a/docs/plat/nvidia-tegra.md
+++ b/docs/plat/nvidia-tegra.md
@@ -84,3 +84,10 @@
parameter to be used during the 'SYSTEM SUSPEND' call. The state-id field
is implementation defined on Tegra SoCs and is preferably defined by
tegra_def.h.
+
+Tegra configs
+=============
+
+* 'tegra_enable_l2_ecc_parity_prot': This flag enables the L2 ECC and Parity
+ Protection bit, for ARM Cortex-A57 CPUs, during CPU boot. This flag will
+ be enabled by Tegrs SoCs during 'Cluster power up' or 'System Suspend' exit.
diff --git a/include/lib/cpus/aarch64/cortex_a57.h b/include/lib/cpus/aarch64/cortex_a57.h
index c5a218b..9229a56 100644
--- a/include/lib/cpus/aarch64/cortex_a57.h
+++ b/include/lib/cpus/aarch64/cortex_a57.h
@@ -87,6 +87,8 @@
#define L2_DATA_RAM_LATENCY_3_CYCLES 0x2
#define L2_TAG_RAM_LATENCY_3_CYCLES 0x2
+#define L2_ECC_PARITY_PROTECTION_BIT (1 << 21)
+
/*******************************************************************************
* L2 Extended Control register specific definitions.
******************************************************************************/
diff --git a/include/lib/stdlib/string.h b/include/lib/stdlib/string.h
index 902d9c1..56677b2 100644
--- a/include/lib/stdlib/string.h
+++ b/include/lib/stdlib/string.h
@@ -52,6 +52,7 @@
void *memchr(const void *, int, size_t) __pure;
int memcmp(const void *, const void *, size_t) __pure;
void *memcpy(void * __restrict, const void * __restrict, size_t);
+void *memcpy16(void * __restrict, const void * __restrict, size_t);
void *memmove(void *, const void *, size_t);
void *memset(void *, int, size_t);
diff --git a/lib/cpus/aarch64/denver.S b/lib/cpus/aarch64/denver.S
index 3e238a1..fcfae8f 100644
--- a/lib/cpus/aarch64/denver.S
+++ b/lib/cpus/aarch64/denver.S
@@ -59,7 +59,6 @@
mov x1, #1
lsl x1, x1, x0
msr s3_0_c15_c0_2, x1
- isb
ret
endfunc denver_enable_dco
diff --git a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
index 6851b15..70a7f3a 100644
--- a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
+++ b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
@@ -33,6 +33,7 @@
#include <cpu_macros.S>
#include <cortex_a57.h>
#include <cortex_a53.h>
+#include <platform_def.h>
#include <tegra_def.h>
#define MIDR_PN_CORTEX_A57 0xD07
@@ -67,6 +68,7 @@
.globl ns_image_entrypoint
.globl tegra_bl31_phys_base
.globl tegra_console_base
+ .globl tegra_enable_l2_ecc_parity_prot
/* ---------------------
* Common CPU init code
@@ -75,8 +77,8 @@
.macro cpu_init_common
/* ------------------------------------------------
- * We enable procesor retention and L2/CPUECTLR NS
- * access for A57 CPUs only.
+ * We enable procesor retention, L2/CPUECTLR NS
+ * access and ECC/Parity protection for A57 CPUs
* ------------------------------------------------
*/
mrs x0, midr_el1
@@ -89,7 +91,7 @@
/* ---------------------------
* Enable processor retention
* ---------------------------
- */
+ */
mrs x0, L2ECTLR_EL1
mov x1, #RETENTION_ENTRY_TICKS_512 << L2ECTLR_RET_CTRL_SHIFT
bic x0, x0, #L2ECTLR_RET_CTRL_MASK
@@ -107,12 +109,26 @@
/* -------------------------------------------------------
* Enable L2 and CPU ECTLR RW access from non-secure world
* -------------------------------------------------------
- */
+ */
mov x0, #ACTLR_EL3_ENABLE_ALL_ACCESS
msr actlr_el3, x0
msr actlr_el2, x0
isb
+ /* -------------------------------------------------------
+ * Enable L2 ECC and Parity Protection
+ * -------------------------------------------------------
+ */
+ adr x0, tegra_enable_l2_ecc_parity_prot
+ ldr x0, [x0]
+ cbz x0, 1f
+ mrs x0, L2CTLR_EL1
+ and x1, x0, #L2_ECC_PARITY_PROTECTION_BIT
+ cbnz x1, 1f
+ orr x0, x0, #L2_ECC_PARITY_PROTECTION_BIT
+ msr L2CTLR_EL1, x0
+ isb
+
/* --------------------------------
* Enable the cycle count register
* --------------------------------
@@ -254,6 +270,47 @@
*/
func plat_reset_handler
+ /* ----------------------------------------------------
+ * Verify if we are running from BL31_BASE address
+ * ----------------------------------------------------
+ */
+ adr x18, bl31_entrypoint
+ mov x17, #BL31_BASE
+ cmp x18, x17
+ b.eq 1f
+
+ /* ----------------------------------------------------
+ * Copy the entire BL31 code to BL31_BASE if we are not
+ * running from it already
+ * ----------------------------------------------------
+ */
+ mov x0, x17
+ mov x1, x18
+ mov x2, #BL31_SIZE
+_loop16:
+ cmp x2, #16
+ b.lt _loop1
+ ldp x3, x4, [x1], #16
+ stp x3, x4, [x0], #16
+ sub x2, x2, #16
+ b _loop16
+ /* copy byte per byte */
+_loop1:
+ cbz x2, _end
+ ldrb w3, [x1], #1
+ strb w3, [x0], #1
+ subs x2, x2, #1
+ b.ne _loop1
+
+ /* ----------------------------------------------------
+ * Jump to BL31_BASE and start execution again
+ * ----------------------------------------------------
+ */
+_end: mov x0, x20
+ mov x1, x21
+ br x17
+1:
+
/* -----------------------------------
* derive and save the phys_base addr
* -----------------------------------
@@ -412,3 +469,10 @@
*/
tegra_console_base:
.quad 0
+
+ /* --------------------------------------------------
+ * Enable L2 ECC and Parity Protection
+ * --------------------------------------------------
+ */
+tegra_enable_l2_ecc_parity_prot:
+ .quad 0
diff --git a/plat/nvidia/tegra/common/tegra_bl31_setup.c b/plat/nvidia/tegra/common/tegra_bl31_setup.c
index 72da4b3..246a03e 100644
--- a/plat/nvidia/tegra/common/tegra_bl31_setup.c
+++ b/plat/nvidia/tegra/common/tegra_bl31_setup.c
@@ -44,9 +44,12 @@
#include <platform.h>
#include <platform_def.h>
#include <stddef.h>
+#include <string.h>
#include <tegra_def.h>
#include <tegra_private.h>
+extern void zeromem16(void *mem, unsigned int length);
+
/*******************************************************************************
* Declarations of linker defined symbols which will help us find the layout
* of trusted SRAM
@@ -80,6 +83,29 @@
extern uint64_t ns_image_entrypoint;
/*******************************************************************************
+ * The following platform setup functions are weakly defined. They
+ * provide typical implementations that will be overridden by a SoC.
+ ******************************************************************************/
+#pragma weak plat_early_platform_setup
+#pragma weak plat_get_bl31_params
+#pragma weak plat_get_bl31_plat_params
+
+void plat_early_platform_setup(void)
+{
+ ; /* do nothing */
+}
+
+bl31_params_t *plat_get_bl31_params(void)
+{
+ return NULL;
+}
+
+plat_params_from_bl2_t *plat_get_bl31_plat_params(void)
+{
+ return NULL;
+}
+
+/*******************************************************************************
* Return a pointer to the 'entry_point_info' structure of the next image for
* security state specified. BL33 corresponds to the non-secure image type
* while BL32 corresponds to the secure image type.
@@ -89,7 +115,8 @@
if (type == NON_SECURE)
return &bl33_image_ep_info;
- if (type == SECURE)
+ /* return BL32 entry point info if it is valid */
+ if (type == SECURE && bl32_image_ep_info.pc)
return &bl32_image_ep_info;
return NULL;
@@ -116,11 +143,25 @@
#if DEBUG
int impl = (read_midr() >> MIDR_IMPL_SHIFT) & MIDR_IMPL_MASK;
#endif
+ image_info_t bl32_img_info = { {0} };
+ uint64_t tzdram_start, tzdram_end, bl32_start, bl32_end;
+
+ /*
+ * For RESET_TO_BL31 systems, BL31 is the first bootloader to run so
+ * there's no argument to relay from a previous bootloader. Platforms
+ * might use custom ways to get arguments, so provide handlers which
+ * they can override.
+ */
+ if (from_bl2 == NULL)
+ from_bl2 = plat_get_bl31_params();
+ if (plat_params == NULL)
+ plat_params = plat_get_bl31_plat_params();
/*
* Copy BL3-3, BL3-2 entry point information.
* They are stored in Secure RAM, in BL2's address space.
*/
+ assert(from_bl2);
assert(from_bl2->bl33_ep_info);
bl33_image_ep_info = *from_bl2->bl33_ep_info;
@@ -136,6 +177,14 @@
plat_bl31_params_from_bl2.uart_id = plat_params->uart_id;
/*
+ * It is very important that we run either from TZDRAM or TZSRAM base.
+ * Add an explicit check here.
+ */
+ if ((plat_bl31_params_from_bl2.tzdram_base != BL31_BASE) &&
+ (TEGRA_TZRAM_BASE != BL31_BASE))
+ panic();
+
+ /*
* Get the base address of the UART controller to be used for the
* console
*/
@@ -152,6 +201,51 @@
/* Initialise crash console */
plat_crash_console_init();
+ /*
+ * Do initial security configuration to allow DRAM/device access.
+ */
+ tegra_memctrl_tzdram_setup(plat_bl31_params_from_bl2.tzdram_base,
+ plat_bl31_params_from_bl2.tzdram_size);
+
+ /*
+ * The previous bootloader might not have placed the BL32 image
+ * inside the TZDRAM. We check the BL32 image info to find out
+ * the base/PC values and relocate the image if necessary.
+ */
+ if (from_bl2->bl32_image_info) {
+
+ bl32_img_info = *from_bl2->bl32_image_info;
+
+ /* Relocate BL32 if it resides outside of the TZDRAM */
+ tzdram_start = plat_bl31_params_from_bl2.tzdram_base;
+ tzdram_end = plat_bl31_params_from_bl2.tzdram_base +
+ plat_bl31_params_from_bl2.tzdram_size;
+ bl32_start = bl32_img_info.image_base;
+ bl32_end = bl32_img_info.image_base + bl32_img_info.image_size;
+
+ assert(tzdram_end > tzdram_start);
+ assert(bl32_end > bl32_start);
+ assert(bl32_image_ep_info.pc > tzdram_start);
+ assert(bl32_image_ep_info.pc < tzdram_end);
+
+ /* relocate BL32 */
+ if (bl32_start >= tzdram_end || bl32_end <= tzdram_start) {
+
+ INFO("Relocate BL32 to TZDRAM\n");
+
+ memcpy16((void *)(uintptr_t)bl32_image_ep_info.pc,
+ (void *)(uintptr_t)bl32_start,
+ bl32_img_info.image_size);
+
+ /* clean up non-secure intermediate buffer */
+ zeromem16((void *)(uintptr_t)bl32_start,
+ bl32_img_info.image_size);
+ }
+ }
+
+ /* Early platform setup for Tegra SoCs */
+ plat_early_platform_setup();
+
INFO("BL3-1: Boot CPU: %s Processor [%lx]\n", (impl == DENVER_IMPL) ?
"Denver" : "ARM", read_mpidr());
}
@@ -163,6 +257,9 @@
{
uint32_t tmp_reg;
+ /* Initialize the gic cpu and distributor interfaces */
+ plat_gic_setup();
+
/*
* Initialize delay timer
*/
@@ -179,12 +276,6 @@
tegra_memctrl_setup();
/*
- * Do initial security configuration to allow DRAM/device access.
- */
- tegra_memctrl_tzdram_setup(plat_bl31_params_from_bl2.tzdram_base,
- plat_bl31_params_from_bl2.tzdram_size);
-
- /*
* Set up the TZRAM memory aperture to allow only secure world
* access
*/
@@ -194,9 +285,6 @@
tmp_reg = SCR_RES1_BITS | SCR_RW_BIT;
write_scr(tmp_reg);
- /* Initialize the gic cpu and distributor interfaces */
- tegra_gic_setup();
-
INFO("BL3-1: Tegra platform setup complete\n");
}
diff --git a/plat/nvidia/tegra/common/tegra_common.mk b/plat/nvidia/tegra/common/tegra_common.mk
index c9e9255..7f789c5 100644
--- a/plat/nvidia/tegra/common/tegra_common.mk
+++ b/plat/nvidia/tegra/common/tegra_common.mk
@@ -47,16 +47,15 @@
COMMON_DIR := plat/nvidia/tegra/common
BL31_SOURCES += drivers/arm/gic/gic_v2.c \
- drivers/arm/gic/gic_v3.c \
drivers/console/aarch64/console.S \
drivers/delay_timer/delay_timer.c \
drivers/ti/uart/aarch64/16550_console.S \
plat/common/aarch64/platform_mp_stack.S \
- plat/common/plat_psci_common.c \
${COMMON_DIR}/aarch64/tegra_helpers.S \
${COMMON_DIR}/drivers/pmc/pmc.c \
${COMMON_DIR}/tegra_bl31_setup.c \
${COMMON_DIR}/tegra_delay_timer.c \
+ ${COMMON_DIR}/tegra_fiq_glue.c \
${COMMON_DIR}/tegra_gic.c \
${COMMON_DIR}/tegra_pm.c \
${COMMON_DIR}/tegra_sip_calls.c \
diff --git a/plat/nvidia/tegra/common/tegra_fiq_glue.c b/plat/nvidia/tegra/common/tegra_fiq_glue.c
new file mode 100644
index 0000000..7fcc114
--- /dev/null
+++ b/plat/nvidia/tegra/common/tegra_fiq_glue.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. 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 <assert.h>
+#include <bakery_lock.h>
+#include <bl_common.h>
+#include <context.h>
+#include <context_mgmt.h>
+#include <debug.h>
+#include <denver.h>
+#include <gic_v2.h>
+#include <interrupt_mgmt.h>
+#include <platform.h>
+#include <tegra_def.h>
+#include <tegra_private.h>
+
+DEFINE_BAKERY_LOCK(tegra_fiq_lock);
+
+/*******************************************************************************
+ * Static variables
+ ******************************************************************************/
+static uint64_t ns_fiq_handler_addr;
+static unsigned int fiq_handler_active;
+static pcpu_fiq_state_t fiq_state[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * Handler for FIQ interrupts
+ ******************************************************************************/
+static uint64_t tegra_fiq_interrupt_handler(uint32_t id,
+ uint32_t flags,
+ void *handle,
+ void *cookie)
+{
+ cpu_context_t *ctx = cm_get_context(NON_SECURE);
+ el3_state_t *el3state_ctx = get_el3state_ctx(ctx);
+ int cpu = plat_my_core_pos();
+ uint32_t irq;
+
+ bakery_lock_get(&tegra_fiq_lock);
+
+ /*
+ * The FIQ was generated when the execution was in the non-secure
+ * world. Save the context registers to start with.
+ */
+ cm_el1_sysregs_context_save(NON_SECURE);
+
+ /*
+ * Save elr_el3 and spsr_el3 from the saved context, and overwrite
+ * the context with the NS fiq_handler_addr and SPSR value.
+ */
+ fiq_state[cpu].elr_el3 = read_ctx_reg(el3state_ctx, CTX_ELR_EL3);
+ fiq_state[cpu].spsr_el3 = read_ctx_reg(el3state_ctx, CTX_SPSR_EL3);
+
+ /*
+ * Set the new ELR to continue execution in the NS world using the
+ * FIQ handler registered earlier.
+ */
+ assert(ns_fiq_handler_addr);
+ write_ctx_reg(el3state_ctx, CTX_ELR_EL3, ns_fiq_handler_addr);
+
+ /*
+ * Mark this interrupt as complete to avoid a FIQ storm.
+ */
+ irq = plat_ic_acknowledge_interrupt();
+ if (irq < 1022)
+ plat_ic_end_of_interrupt(irq);
+
+ bakery_lock_release(&tegra_fiq_lock);
+
+ return 0;
+}
+
+/*******************************************************************************
+ * Setup handler for FIQ interrupts
+ ******************************************************************************/
+void tegra_fiq_handler_setup(void)
+{
+ uint64_t flags;
+ int rc;
+
+ /* return if already registered */
+ if (fiq_handler_active)
+ return;
+
+ /*
+ * Register an interrupt handler for FIQ interrupts generated for
+ * NS interrupt sources
+ */
+ flags = 0;
+ set_interrupt_rm_flag(flags, NON_SECURE);
+ rc = register_interrupt_type_handler(INTR_TYPE_EL3,
+ tegra_fiq_interrupt_handler,
+ flags);
+ if (rc)
+ panic();
+
+ /* handler is now active */
+ fiq_handler_active = 1;
+}
+
+/*******************************************************************************
+ * Validate and store NS world's entrypoint for FIQ interrupts
+ ******************************************************************************/
+void tegra_fiq_set_ns_entrypoint(uint64_t entrypoint)
+{
+ ns_fiq_handler_addr = entrypoint;
+}
+
+/*******************************************************************************
+ * Handler to return the NS EL1/EL0 CPU context
+ ******************************************************************************/
+int tegra_fiq_get_intr_context(void)
+{
+ cpu_context_t *ctx = cm_get_context(NON_SECURE);
+ gp_regs_t *gpregs_ctx = get_gpregs_ctx(ctx);
+ el1_sys_regs_t *el1state_ctx = get_sysregs_ctx(ctx);
+ int cpu = plat_my_core_pos();
+ uint64_t val;
+
+ /*
+ * We store the ELR_EL3, SPSR_EL3, SP_EL0 and SP_EL1 registers so
+ * that el3_exit() sends these values back to the NS world.
+ */
+ write_ctx_reg(gpregs_ctx, CTX_GPREG_X0, fiq_state[cpu].elr_el3);
+ write_ctx_reg(gpregs_ctx, CTX_GPREG_X1, fiq_state[cpu].spsr_el3);
+
+ val = read_ctx_reg(gpregs_ctx, CTX_GPREG_SP_EL0);
+ write_ctx_reg(gpregs_ctx, CTX_GPREG_X2, val);
+
+ val = read_ctx_reg(el1state_ctx, CTX_SP_EL1);
+ write_ctx_reg(gpregs_ctx, CTX_GPREG_X3, val);
+
+ return 0;
+}
diff --git a/plat/nvidia/tegra/common/tegra_gic.c b/plat/nvidia/tegra/common/tegra_gic.c
index ee12975..6864f8b 100644
--- a/plat/nvidia/tegra/common/tegra_gic.c
+++ b/plat/nvidia/tegra/common/tegra_gic.c
@@ -47,6 +47,9 @@
(GIC_HIGHEST_NS_PRIORITY << 16) | \
(GIC_HIGHEST_NS_PRIORITY << 24))
+static const irq_sec_cfg_t *g_irq_sec_ptr;
+static unsigned int g_num_irqs;
+
/*******************************************************************************
* 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
@@ -110,7 +113,9 @@
******************************************************************************/
static void tegra_gic_distif_setup(unsigned int gicd_base)
{
- unsigned int index, num_ints;
+ unsigned int index, num_ints, irq_num;
+ uint8_t target_cpus;
+ uint32_t val;
/*
* Mark out non-secure interrupts. Calculate number of
@@ -128,6 +133,39 @@
GICD_IPRIORITYR_DEF_VAL);
}
+ /* Configure SPI secure interrupts now */
+ if (g_irq_sec_ptr) {
+
+ for (index = 0; index < g_num_irqs; index++) {
+ irq_num = (g_irq_sec_ptr + index)->irq;
+ target_cpus = (g_irq_sec_ptr + index)->target_cpus;
+
+ if (irq_num >= MIN_SPI_ID) {
+
+ /* Configure as a secure interrupt */
+ gicd_clr_igroupr(gicd_base, irq_num);
+
+ /* Configure SPI priority */
+ mmio_write_8(gicd_base + GICD_IPRIORITYR +
+ irq_num,
+ GIC_HIGHEST_SEC_PRIORITY &
+ GIC_PRI_MASK);
+
+ /* Configure as level triggered */
+ val = gicd_read_icfgr(gicd_base, irq_num);
+ val |= (3 << ((irq_num & 0xF) << 1));
+ gicd_write_icfgr(gicd_base, irq_num, val);
+
+ /* Route SPI to the target CPUs */
+ gicd_set_itargetsr(gicd_base, irq_num,
+ target_cpus);
+
+ /* Enable this interrupt */
+ gicd_set_isenabler(gicd_base, irq_num);
+ }
+ }
+ }
+
/*
* Configure the SGI and PPI. This is done in a separated function
* because each CPU is responsible for initializing its own private
@@ -139,8 +177,11 @@
gicd_write_ctlr(gicd_base, ENABLE_GRP0 | ENABLE_GRP1);
}
-void tegra_gic_setup(void)
+void tegra_gic_setup(const irq_sec_cfg_t *irq_sec_ptr, unsigned int num_irqs)
{
+ g_irq_sec_ptr = irq_sec_ptr;
+ g_num_irqs = num_irqs;
+
tegra_gic_cpuif_setup(TEGRA_GICC_BASE);
tegra_gic_distif_setup(TEGRA_GICD_BASE);
}
@@ -185,12 +226,17 @@
uint32_t tegra_gic_get_pending_interrupt_type(void)
{
uint32_t id;
+ unsigned int index;
id = gicc_read_hppir(TEGRA_GICC_BASE) & INT_ID_MASK;
- /* Assume that all secure interrupts are S-EL1 interrupts */
- if (id < 1022)
- return INTR_TYPE_S_EL1;
+ /* get the interrupt type */
+ if (id < 1022) {
+ for (index = 0; index < g_num_irqs; index++) {
+ if (id == (g_irq_sec_ptr + index)->irq)
+ return (g_irq_sec_ptr + index)->type;
+ }
+ }
if (id == GIC_SPURIOUS_INTERRUPT)
return INTR_TYPE_INVAL;
@@ -248,14 +294,19 @@
uint32_t tegra_gic_get_interrupt_type(uint32_t id)
{
uint32_t group;
+ unsigned int index;
group = gicd_get_igroupr(TEGRA_GICD_BASE, id);
- /* Assume that all secure interrupts are S-EL1 interrupts */
- if (group == GRP0)
- return INTR_TYPE_S_EL1;
- else
- return INTR_TYPE_NS;
+ /* get the interrupt type */
+ if (group == GRP0) {
+ for (index = 0; index < g_num_irqs; index++) {
+ if (id == (g_irq_sec_ptr + index)->irq)
+ return (g_irq_sec_ptr + index)->type;
+ }
+ }
+
+ return INTR_TYPE_NS;
}
#else
diff --git a/plat/nvidia/tegra/common/tegra_pm.c b/plat/nvidia/tegra/common/tegra_pm.c
index f5ef3e7..b2703dd 100644
--- a/plat/nvidia/tegra/common/tegra_pm.c
+++ b/plat/nvidia/tegra/common/tegra_pm.c
@@ -57,6 +57,7 @@
#pragma weak tegra_soc_pwr_domain_power_down_wfi
#pragma weak tegra_soc_prepare_system_reset
#pragma weak tegra_soc_prepare_system_off
+#pragma weak tegra_soc_get_target_pwr_state
int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
{
@@ -94,6 +95,23 @@
panic();
}
+plat_local_state_t tegra_soc_get_target_pwr_state(unsigned int lvl,
+ const plat_local_state_t *states,
+ unsigned int ncpu)
+{
+ plat_local_state_t target = PLAT_MAX_RET_STATE, temp;
+
+ assert(ncpu);
+
+ do {
+ temp = *states++;
+ if ((temp > target) && (temp != PLAT_MAX_OFF_STATE))
+ target = temp;
+ } while (--ncpu);
+
+ return target;
+}
+
/*******************************************************************************
* This handler is called by the PSCI implementation during the `SYSTEM_SUSPEND`
* call to get the `power_state` parameter. This allows the platform to encode
@@ -102,12 +120,9 @@
******************************************************************************/
void tegra_get_sys_suspend_power_state(psci_power_state_t *req_state)
{
- /* lower affinities use PLAT_MAX_OFF_STATE */
- for (int i = MPIDR_AFFLVL0; i < PLAT_MAX_PWR_LVL; i++)
- req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
-
- /* max affinity uses system suspend state id */
- req_state->pwr_domain_state[PLAT_MAX_PWR_LVL] = PSTATE_ID_SOC_POWERDN;
+ /* all affinities use system suspend state id */
+ for (int i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++)
+ req_state->pwr_domain_state[i] = PSTATE_ID_SOC_POWERDN;
}
/*******************************************************************************
@@ -183,7 +198,7 @@
/*
* Initialize the GIC cpu and distributor interfaces
*/
- tegra_gic_setup();
+ plat_gic_setup();
/*
* Check if we are exiting from deep sleep.
@@ -203,6 +218,12 @@
plat_params = bl31_get_plat_params();
tegra_memctrl_tzdram_setup(plat_params->tzdram_base,
plat_params->tzdram_size);
+
+ /*
+ * Set up the TZRAM memory aperture to allow only secure world
+ * access
+ */
+ tegra_memctrl_tzram_setup(TEGRA_TZRAM_BASE, TEGRA_TZRAM_SIZE);
}
/*
@@ -318,3 +339,14 @@
return 0;
}
+
+/*******************************************************************************
+ * Platform handler to calculate the proper target power level at the
+ * specified affinity level
+ ******************************************************************************/
+plat_local_state_t plat_get_target_pwr_state(unsigned int lvl,
+ const plat_local_state_t *states,
+ unsigned int ncpu)
+{
+ return tegra_soc_get_target_pwr_state(lvl, states, ncpu);
+}
diff --git a/plat/nvidia/tegra/common/tegra_sip_calls.c b/plat/nvidia/tegra/common/tegra_sip_calls.c
index 3bcd441..ba0e1ef 100644
--- a/plat/nvidia/tegra/common/tegra_sip_calls.c
+++ b/plat/nvidia/tegra/common/tegra_sip_calls.c
@@ -42,6 +42,8 @@
* Common Tegra SiP SMCs
******************************************************************************/
#define TEGRA_SIP_NEW_VIDEOMEM_REGION 0x82000003
+#define TEGRA_SIP_FIQ_NS_ENTRYPOINT 0x82000005
+#define TEGRA_SIP_FIQ_NS_GET_CONTEXT 0x82000006
/*******************************************************************************
* SoC specific SiP handler
@@ -60,7 +62,7 @@
}
/*******************************************************************************
- * This function is responsible for handling all SiP calls from the NS world
+ * This function is responsible for handling all SiP calls
******************************************************************************/
uint64_t tegra_sip_handler(uint32_t smc_fid,
uint64_t x1,
@@ -71,14 +73,8 @@
void *handle,
uint64_t flags)
{
- uint32_t ns;
int err;
- /* Determine which security state this SMC originated from */
- ns = is_caller_non_secure(flags);
- if (!ns)
- SMC_RET1(handle, SMC_UNK);
-
/* Check if this is a SoC specific SiP */
err = plat_sip_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
if (err == 0)
@@ -114,6 +110,41 @@
SMC_RET1(handle, 0);
break;
+ /*
+ * The NS world registers the address of its handler to be
+ * used for processing the FIQ. This is normally used by the
+ * NS FIQ debugger driver to detect system hangs by programming
+ * a watchdog timer to fire a FIQ interrupt.
+ */
+ case TEGRA_SIP_FIQ_NS_ENTRYPOINT:
+
+ if (!x1)
+ SMC_RET1(handle, SMC_UNK);
+
+ /*
+ * TODO: Check if x1 contains a valid DRAM address
+ */
+
+ /* store the NS world's entrypoint */
+ tegra_fiq_set_ns_entrypoint(x1);
+
+ SMC_RET1(handle, 0);
+ break;
+
+ /*
+ * The NS world's FIQ handler issues this SMC to get the NS EL1/EL0
+ * CPU context when the FIQ interrupt was triggered. This allows the
+ * NS world to understand the CPU state when the watchdog interrupt
+ * triggered.
+ */
+ case TEGRA_SIP_FIQ_NS_GET_CONTEXT:
+
+ /* retrieve context registers when FIQ triggered */
+ tegra_fiq_get_intr_context();
+
+ SMC_RET0(handle);
+ break;
+
default:
ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
break;
diff --git a/plat/nvidia/tegra/include/tegra_private.h b/plat/nvidia/tegra/include/tegra_private.h
index 75416ec..012bfd7 100644
--- a/plat/nvidia/tegra/include/tegra_private.h
+++ b/plat/nvidia/tegra/include/tegra_private.h
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
@@ -42,6 +42,9 @@
#define TEGRA_DRAM_BASE 0x80000000
#define TEGRA_DRAM_END 0x27FFFFFFF
+/*******************************************************************************
+ * Struct for parameters received from BL2
+ ******************************************************************************/
typedef struct plat_params_from_bl2 {
/* TZ memory size */
uint64_t tzdram_size;
@@ -51,6 +54,26 @@
int uart_id;
} plat_params_from_bl2_t;
+/*******************************************************************************
+ * Per-CPU struct describing FIQ state to be stored
+ ******************************************************************************/
+typedef struct pcpu_fiq_state {
+ uint64_t elr_el3;
+ uint64_t spsr_el3;
+} pcpu_fiq_state_t;
+
+/*******************************************************************************
+ * Struct describing per-FIQ configuration settings
+ ******************************************************************************/
+typedef struct irq_sec_cfg {
+ /* IRQ number */
+ unsigned int irq;
+ /* Target CPUs servicing this interrupt */
+ unsigned int target_cpus;
+ /* type = INTR_TYPE_S_EL1 or INTR_TYPE_EL3 */
+ uint32_t type;
+} irq_sec_cfg_t;
+
/* Declarations for plat_psci_handlers.c */
int32_t tegra_soc_validate_power_state(unsigned int power_state,
psci_power_state_t *req_state);
@@ -58,13 +81,21 @@
/* Declarations for plat_setup.c */
const mmap_region_t *plat_get_mmio_map(void);
uint32_t plat_get_console_from_id(int id);
+void plat_gic_setup(void);
+bl31_params_t *plat_get_bl31_params(void);
+plat_params_from_bl2_t *plat_get_bl31_plat_params(void);
/* Declarations for plat_secondary.c */
void plat_secondary_setup(void);
int plat_lock_cpu_vectors(void);
+/* Declarations for tegra_fiq_glue.c */
+void tegra_fiq_handler_setup(void);
+int tegra_fiq_get_intr_context(void);
+void tegra_fiq_set_ns_entrypoint(uint64_t entrypoint);
+
/* Declarations for tegra_gic.c */
-void tegra_gic_setup(void);
+void tegra_gic_setup(const irq_sec_cfg_t *irq_sec_ptr, unsigned int num_irqs);
void tegra_gic_cpuif_deactivate(void);
/* Declarations for tegra_security.c */
@@ -83,6 +114,7 @@
/* Declarations for tegra_bl31_setup.c */
plat_params_from_bl2_t *bl31_get_plat_params(void);
int bl31_check_ns_address(uint64_t base, uint64_t size_in_bytes);
+void plat_early_platform_setup(void);
/* Declarations for tegra_delay_timer.c */
void tegra_delay_timer_init(void);
diff --git a/plat/nvidia/tegra/platform.mk b/plat/nvidia/tegra/platform.mk
index 756899c..b168634 100644
--- a/plat/nvidia/tegra/platform.mk
+++ b/plat/nvidia/tegra/platform.mk
@@ -36,6 +36,14 @@
# Disable the PSCI platform compatibility layer
ENABLE_PLAT_COMPAT := 0
+# Disable cache non-temporal hint for A57
+A57_DISABLE_NON_TEMPORAL_HINT := 0
+$(eval $(call add_define,A57_DISABLE_NON_TEMPORAL_HINT))
+
+# Disable cache non-temporal hint for A53
+A53_DISABLE_NON_TEMPORAL_HINT := 0
+$(eval $(call add_define,A53_DISABLE_NON_TEMPORAL_HINT))
+
include plat/nvidia/tegra/common/tegra_common.mk
include ${SOC_DIR}/platform_${TARGET_SOC}.mk
diff --git a/plat/nvidia/tegra/soc/t132/plat_setup.c b/plat/nvidia/tegra/soc/t132/plat_setup.c
index 337a2c5..651bd08 100644
--- a/plat/nvidia/tegra/soc/t132/plat_setup.c
+++ b/plat/nvidia/tegra/soc/t132/plat_setup.c
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
@@ -28,8 +28,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <xlat_tables.h>
+#include <arch_helpers.h>
+#include <bl_common.h>
#include <tegra_def.h>
+#include <tegra_private.h>
+#include <xlat_tables.h>
/*******************************************************************************
* The Tegra power domain tree has a single system level power domain i.e. a
@@ -106,3 +109,11 @@
return tegra132_uart_addresses[id];
}
+
+/*******************************************************************************
+ * Initialize the GIC and SGIs
+ ******************************************************************************/
+void plat_gic_setup(void)
+{
+ tegra_gic_setup(NULL, 0);
+}
diff --git a/plat/nvidia/tegra/soc/t210/plat_setup.c b/plat/nvidia/tegra/soc/t210/plat_setup.c
index 246faf8..42eefe7 100644
--- a/plat/nvidia/tegra/soc/t210/plat_setup.c
+++ b/plat/nvidia/tegra/soc/t210/plat_setup.c
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
@@ -28,8 +28,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+#include <arch_helpers.h>
+#include <bl_common.h>
#include <console.h>
#include <tegra_def.h>
+#include <tegra_private.h>
#include <xlat_tables.h>
/*******************************************************************************
@@ -112,3 +115,11 @@
return tegra210_uart_addresses[id];
}
+
+/*******************************************************************************
+ * Initialize the GIC and SGIs
+ ******************************************************************************/
+void plat_gic_setup(void)
+{
+ tegra_gic_setup(NULL, 0);
+}