Push old Amlogic BL2 sourcesHEADmaster

8 files changed, 1044 insertions, 0 deletions

diff --git a/drivers/arm/cci400/cci400.c b/drivers/arm/cci400/cci400.c
new file mode 100644
index 0000000..af10f21
--- /dev/null
+++ b/drivers/arm/cci400/cci400.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2013-2014, 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 <cci400.h>
+#include <mmio.h>
+#include <platform_def.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/gic/aarch64/gic_v3_sysregs.S b/drivers/arm/gic/aarch64/gic_v3_sysregs.S
new file mode 100644
index 0000000..ddf85a8
--- /dev/null
+++ b/drivers/arm/gic/aarch64/gic_v3_sysregs.S
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2013-2014, 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 <asm_macros.S>
+
+	.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
+
+func read_icc_sre_el1
+	mrs	x0, ICC_SRE_EL1
+	ret
+
+
+func read_icc_sre_el2
+	mrs	x0, ICC_SRE_EL2
+	ret
+
+
+func read_icc_sre_el3
+	mrs	x0, ICC_SRE_EL3
+	ret
+
+
+func write_icc_sre_el1
+	msr	ICC_SRE_EL1, x0
+	ret
+
+
+func write_icc_sre_el2
+	msr	ICC_SRE_EL2, x0
+	ret
+
+
+func write_icc_sre_el3
+	msr	ICC_SRE_EL3, x0
+	ret
+
+
+func write_icc_pmr_el1
+	msr	ICC_PMR_EL1, x0
+	ret
diff --git a/drivers/arm/gic/gic_v2.c b/drivers/arm/gic/gic_v2.c
new file mode 100644
index 0000000..27a39b9
--- /dev/null
+++ b/drivers/arm/gic/gic_v2.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (c) 2013-2014, 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.h>
+#include <assert.h>
+#include <gic_v2.h>
+#include <interrupt_mgmt.h>
+#include <mmio.h>
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+
+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_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 accessors for writing entire registers
+ ******************************************************************************/
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors 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));
+}
+
+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));
+}
+
+void gicd_set_isenabler(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ISENABLER_SHIFT) - 1);
+
+	gicd_write_isenabler(base, id, (1 << bit_num));
+}
+
+void gicd_set_icenabler(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ICENABLER_SHIFT) - 1);
+
+	gicd_write_icenabler(base, id, (1 << bit_num));
+}
+
+void gicd_set_ispendr(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ISPENDR_SHIFT) - 1);
+
+	gicd_write_ispendr(base, id, (1 << bit_num));
+}
+
+void gicd_set_icpendr(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ICPENDR_SHIFT) - 1);
+
+	gicd_write_icpendr(base, id, (1 << bit_num));
+}
+
+void gicd_set_isactiver(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ISACTIVER_SHIFT) - 1);
+
+	gicd_write_isactiver(base, id, (1 << bit_num));
+}
+
+void gicd_set_icactiver(unsigned int base, unsigned int id)
+{
+	unsigned bit_num = id & ((1 << ICACTIVER_SHIFT) - 1);
+
+	gicd_write_icactiver(base, id, (1 << bit_num));
+}
+
+/*
+ * 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 int reg = base + GICD_IPRIORITYR + (id & ~3);
+	unsigned int shift = (id & 3) << 3;
+	unsigned int reg_val = mmio_read_32(reg);
+
+	/*
+	 * Enforce ARM recommendation to manage priority values such
+	 * that group1 interrupts always have a lower priority than
+	 * group0 interrupts.
+	 * Note, lower numerical values are higher priorities so the comparison
+	 * checks below are reversed from what might be expected.
+	 */
+	assert(gicd_get_igroupr(base, id) == GRP1 ?
+		pri >= GIC_HIGHEST_NS_PRIORITY &&
+			pri <= GIC_LOWEST_NS_PRIORITY :
+		pri >= GIC_HIGHEST_SEC_PRIORITY &&
+			pri <= GIC_LOWEST_SEC_PRIORITY);
+
+	reg_val &= ~(GIC_PRI_MASK << shift);
+	reg_val |= (pri & GIC_PRI_MASK) << shift;
+	mmio_write_32(reg, reg_val);
+}
+
+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));
+}
+
+/*******************************************************************************
+ * This function allows the interrupt management framework to determine (through
+ * the platform) which interrupt line (IRQ/FIQ) to use for an interrupt type to
+ * route it to EL3. The interrupt line is represented as the bit position of the
+ * IRQ or FIQ bit in the SCR_EL3.
+ ******************************************************************************/
+uint32_t gicv2_interrupt_type_to_line(uint32_t cpuif_base, uint32_t type)
+{
+	uint32_t gicc_ctlr;
+
+	/* Non-secure interrupts are signalled on the IRQ line always */
+	if (type == INTR_TYPE_NS)
+		return __builtin_ctz(SCR_IRQ_BIT);
+
+	/*
+	 * Secure interrupts are signalled using the IRQ line if the FIQ_EN
+	 * bit is not set else they are signalled using the FIQ line.
+	 */
+	gicc_ctlr = gicc_read_ctlr(cpuif_base);
+	if (gicc_ctlr & FIQ_EN)
+		return __builtin_ctz(SCR_FIQ_BIT);
+	else
+		return __builtin_ctz(SCR_IRQ_BIT);
+}
diff --git a/drivers/arm/gic/gic_v3.c b/drivers/arm/gic/gic_v3.c
new file mode 100644
index 0000000..f429662
--- /dev/null
+++ b/drivers/arm/gic/gic_v3.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2014, 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.h>
+#include <debug.h>
+#include <gic_v3.h>
+
+uintptr_t gicv3_get_rdist(uintptr_t gicr_base, uint64_t mpidr)
+{
+	uint32_t  cpu_aff, gicr_aff;
+	uint64_t  gicr_typer;
+	uintptr_t addr;
+
+	/* Construct the affinity as used by GICv3. MPIDR and GIC affinity level
+	 * mask is the same.
+	 */
+	cpu_aff  = ((mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK) <<
+			GICV3_AFF0_SHIFT;
+	cpu_aff |= ((mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK) <<
+			GICV3_AFF1_SHIFT;
+	cpu_aff |= ((mpidr >> MPIDR_AFF2_SHIFT) & MPIDR_AFFLVL_MASK) <<
+			GICV3_AFF2_SHIFT;
+	cpu_aff |= ((mpidr >> MPIDR_AFF3_SHIFT) & MPIDR_AFFLVL_MASK) <<
+			GICV3_AFF3_SHIFT;
+
+	addr = gicr_base;
+	do {
+		gicr_typer = gicr_read_typer(addr);
+
+		gicr_aff = (gicr_typer >> GICR_TYPER_AFF_SHIFT) &
+				GICR_TYPER_AFF_MASK;
+		if (cpu_aff == gicr_aff) {
+			/* Disable this print for now as it appears every time
+			 * when using PSCI CPU_SUSPEND.
+			 * TODO: Print this only the first time for each CPU.
+			 * INFO("GICv3 - Found RDIST for MPIDR(0x%lx) at 0x%lx\n",
+			 *	mpidr, addr);
+			 */
+			return addr;
+		}
+
+		/* TODO:
+		 * For GICv4 we need to adjust the Base address based on
+		 * GICR_TYPER.VLPIS
+		 */
+		addr += (1 << GICR_PCPUBASE_SHIFT);
+
+	} while (!(gicr_typer & GICR_TYPER_LAST));
+
+	/* If we get here we did not find a match. */
+	ERROR("GICv3 - Did not find RDIST for CPU with MPIDR 0x%lx\n", mpidr);
+	return (uintptr_t)NULL;
+}
diff --git a/drivers/arm/pl011/pl011.c b/drivers/arm/pl011/pl011.c
new file mode 100644
index 0000000..e296c23
--- /dev/null
+++ b/drivers/arm/pl011/pl011.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2013-2014, 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 <assert.h>
+#include <pl011.h>
+
+void pl011_setbaudrate(unsigned long base_addr, unsigned int baudrate)
+{
+	unsigned int divisor;
+	assert(baudrate);
+	divisor = (PL011_CLK_IN_HZ * 4) / baudrate;
+	pl011_write_ibrd(base_addr, divisor >> 6);
+	pl011_write_fbrd(base_addr, divisor & 0x3F);
+}
diff --git a/drivers/arm/pl011/pl011_console.c b/drivers/arm/pl011/pl011_console.c
new file mode 100644
index 0000000..a26c00e
--- /dev/null
+++ b/drivers/arm/pl011/pl011_console.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2013-2014, 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 <assert.h>
+#include <console.h>
+#include <pl011.h>
+
+static unsigned long uart_base;
+
+void console_init(unsigned long base_addr)
+{
+	/* TODO: assert() internally calls printf() and will result in
+	 * an infinite loop. This needs to be fixed with some kind of
+	 * exception  mechanism or early panic support. This also applies
+	 * to the other assert() calls below.
+	 */
+	assert(base_addr);
+
+	/* Initialise internal base address variable */
+	uart_base = base_addr;
+
+	/* Baud Rate */
+#if defined(PL011_INTEGER) && defined(PL011_FRACTIONAL)
+	pl011_write_ibrd(uart_base, PL011_INTEGER);
+	pl011_write_fbrd(uart_base, PL011_FRACTIONAL);
+#else
+	pl011_setbaudrate(uart_base, PL011_BAUDRATE);
+#endif
+
+	pl011_write_lcr_h(uart_base, PL011_LINE_CONTROL);
+
+	/* Clear any pending errors */
+	pl011_write_ecr(uart_base, 0);
+
+	/* Enable tx, rx, and uart overall */
+	pl011_write_cr(uart_base, PL011_UARTCR_RXE | PL011_UARTCR_TXE |
+			PL011_UARTCR_UARTEN);
+
+}
+
+#define WAIT_UNTIL_UART_FREE(base)				\
+	while ((pl011_read_fr(base) & PL011_UARTFR_TXFF))	\
+		continue
+
+int console_putc(int c)
+{
+	assert(uart_base);
+
+	if (c == '\n') {
+		WAIT_UNTIL_UART_FREE(uart_base);
+		pl011_write_dr(uart_base, '\r');
+	}
+
+	WAIT_UNTIL_UART_FREE(uart_base);
+	pl011_write_dr(uart_base, c);
+	return c;
+}
+
+int console_getc(void)
+{
+	assert(uart_base);
+
+	while ((pl011_read_fr(uart_base) & PL011_UARTFR_RXFE) != 0)
+		;
+	return pl011_read_dr(uart_base);
+}
diff --git a/drivers/arm/serial/serial.c b/drivers/arm/serial/serial.c
new file mode 100644
index 0000000..8f7f0b1
--- /dev/null
+++ b/drivers/arm/serial/serial.c
@@ -0,0 +1,101 @@
+#include <serial.h>
+#include <io.h>
+#include <stdint.h>
+#include <asm/arch/secure_apb.h>
+
+void serial_set_pin_port(unsigned port_base)
+{
+    setbits_le32(P_AO_RTI_PIN_MUX_REG,3<<11);
+}
+
+#define UART_STP_BIT UART_CNTL_MASK_STP_1BIT
+#define UART_PRTY_BIT 0
+#define UART_CHAR_LEN   UART_CNTL_MASK_CHAR_8BIT
+
+void serial_init(unsigned set)
+{
+    /* baud rate */
+    writel(set| UART_STP_BIT
+	        | UART_PRTY_BIT
+	        | UART_CHAR_LEN
+	        //please do not remove these setting , jerry.yu
+	        | UART_CNTL_MASK_TX_EN
+	        | UART_CNTL_MASK_RX_EN
+	        | UART_CNTL_MASK_RST_TX
+	        | UART_CNTL_MASK_RST_RX
+	        | UART_CNTL_MASK_CLR_ERR
+	,P_UART_CONTROL(UART_PORT_CONS));
+    serial_set_pin_port(UART_PORT_CONS);
+    clrbits_le32(P_UART_CONTROL(UART_PORT_CONS),
+	    UART_CNTL_MASK_RST_TX | UART_CNTL_MASK_RST_RX | UART_CNTL_MASK_CLR_ERR);
+}
+
+int serial_putc(int c)
+{
+    if (c == '\n') {
+        while ((readl(P_UART_STATUS(UART_PORT_CONS)) & UART_STAT_MASK_TFIFO_FULL));
+        writel('\r', P_UART_WFIFO(UART_PORT_CONS));
+    }
+    /* Wait till dataTx register is not full */
+    while ((readl(P_UART_STATUS(UART_PORT_CONS)) & UART_STAT_MASK_TFIFO_FULL));
+    writel(c, P_UART_WFIFO(UART_PORT_CONS));
+    /* Wait till dataTx register is empty */
+    return c;
+}
+
+int serial_getc(void)
+{
+   unsigned char ch;
+    /* Wait till character is placed in fifo */
+	while ((readl(P_UART_STATUS(UART_PORT_CONS)) & UART_STAT_MASK_RFIFO_CNT) == 0) ;
+
+    /* Also check for overflow errors */
+    if (readl(P_UART_STATUS(UART_PORT_CONS)) & (UART_STAT_MASK_PRTY_ERR | UART_STAT_MASK_FRAM_ERR))
+	{
+	    setbits_le32(P_UART_CONTROL(UART_PORT_CONS),UART_CNTL_MASK_CLR_ERR);
+	    clrbits_le32(P_UART_CONTROL(UART_PORT_CONS),UART_CNTL_MASK_CLR_ERR);
+	}
+
+    ch = readl(P_UART_RFIFO(UART_PORT_CONS)) & 0x00ff;
+    return ((int)ch);
+}
+
+int serial_puts(const char *s){
+	while (*s) {
+		serial_putc(*s++);
+	}
+	return 0;
+}
+
+//support 64bit number, serial_put_hex(data, 64);
+void serial_put_hex(unsigned long data,unsigned int bitlen)
+{
+	int i;
+	for (i=bitlen-4;i>=0;i-=4) {
+        if ((data>>i) == 0)
+        {
+            serial_putc(0x30);
+            continue;
+        }
+        unsigned char s = (data>>i)&0xf;
+        if (s<10)
+            serial_putc(0x30+s);
+        else
+            serial_putc(0x61+s-10);
+    }
+}
+
+void serial_put_dec(unsigned long data)
+{
+	char szTxt[10];
+	szTxt[0] = 0x30;
+	int i = 0;
+
+	do {
+		szTxt[i++] = (data % 10) + 0x30;
+		data = data / 10;
+	} while(data);
+
+	for (--i;i >=0;--i)
+		serial_putc(szTxt[i]);
+}
\ No newline at end of file
diff --git a/drivers/arm/tzc400/tzc400.c b/drivers/arm/tzc400/tzc400.c
new file mode 100644
index 0000000..c1716db
--- /dev/null
+++ b/drivers/arm/tzc400/tzc400.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <debug.h>
+#include <mmio.h>
+#include <stddef.h>
+#include <tzc400.h>
+
+static uint32_t tzc_read_build_config(uint64_t base)
+{
+	return mmio_read_32(base + BUILD_CONFIG_OFF);
+}
+
+static uint32_t tzc_read_gate_keeper(uint64_t base)
+{
+	return mmio_read_32(base + GATE_KEEPER_OFF);
+}
+
+static void tzc_write_gate_keeper(uint64_t base, uint32_t val)
+{
+	mmio_write_32(base + GATE_KEEPER_OFF, val);
+}
+
+static void tzc_write_action(uint64_t base, tzc_action_t action)
+{
+	mmio_write_32(base + ACTION_OFF, action);
+}
+
+static void tzc_write_region_base_low(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_BASE_LOW_OFF + REGION_NUM_OFF(region), val);
+}
+
+static void tzc_write_region_base_high(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_BASE_HIGH_OFF + REGION_NUM_OFF(region), val);
+}
+
+static void tzc_write_region_top_low(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_TOP_LOW_OFF + REGION_NUM_OFF(region), val);
+}
+
+static void tzc_write_region_top_high(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_TOP_HIGH_OFF + REGION_NUM_OFF(region), val);
+}
+
+static void tzc_write_region_attributes(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_ATTRIBUTES_OFF + REGION_NUM_OFF(region), val);
+}
+
+static void tzc_write_region_id_access(uint64_t base, uint32_t region, uint32_t val)
+{
+	mmio_write_32(base + REGION_ID_ACCESS_OFF + REGION_NUM_OFF(region), val);
+}
+
+static uint32_t tzc_read_component_id(uint64_t base)
+{
+	uint32_t id;
+
+	id = mmio_read_8(base + CID0_OFF);
+	id |= (mmio_read_8(base + CID1_OFF) << 8);
+	id |= (mmio_read_8(base + CID2_OFF) << 16);
+	id |= (mmio_read_8(base + CID3_OFF) << 24);
+
+	return id;
+}
+
+static uint32_t tzc_get_gate_keeper(uint64_t base, uint8_t filter)
+{
+	uint32_t tmp;
+
+	tmp = (tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK;
+
+	return tmp >> filter;
+}
+
+/* This function is not MP safe. */
+static void tzc_set_gate_keeper(uint64_t base, uint8_t filter, uint32_t val)
+{
+	uint32_t tmp;
+
+	/* Upper half is current state. Lower half is requested state. */
+	tmp = (tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK;
+
+	if (val)
+		tmp |=  (1 << filter);
+	else
+		tmp &= ~(1 << filter);
+
+	tzc_write_gate_keeper(base, (tmp & GATE_KEEPER_OR_MASK) <<
+			      GATE_KEEPER_OR_SHIFT);
+
+	/* Wait here until we see the change reflected in the TZC status. */
+	while (((tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK) != tmp)
+	  ;
+}
+
+
+void tzc_init(tzc_instance_t *controller)
+{
+	uint32_t tzc_id, tzc_build;
+
+	assert(controller != NULL);
+
+	/*
+	 * We expect to see a tzc400. Check component ID. The TZC-400 TRM shows
+	 * component ID is expected to be "0xB105F00D".
+	 */
+	tzc_id = tzc_read_component_id(controller->base);
+	if (tzc_id != TZC400_COMPONENT_ID) {
+		ERROR("TZC : Wrong device ID (0x%x).\n", tzc_id);
+		panic();
+	}
+
+	/* Save values we will use later. */
+	tzc_build = tzc_read_build_config(controller->base);
+	controller->num_filters = ((tzc_build >> BUILD_CONFIG_NF_SHIFT) &
+			   BUILD_CONFIG_NF_MASK) + 1;
+	controller->addr_width  = ((tzc_build >> BUILD_CONFIG_AW_SHIFT) &
+			   BUILD_CONFIG_AW_MASK) + 1;
+	controller->num_regions = ((tzc_build >> BUILD_CONFIG_NR_SHIFT) &
+			   BUILD_CONFIG_NR_MASK) + 1;
+}
+
+
+/*
+ * `tzc_configure_region` is used to program regions into the TrustZone
+ * controller. A region can be associated with more than one filter. The
+ * associated filters are passed in as a bitmap (bit0 = filter0).
+ * NOTE:
+ * The region 0 covers the whole address space and is enabled on all filters,
+ * this cannot be changed. It is, however, possible to change some region 0
+ * permissions.
+ */
+void tzc_configure_region(const tzc_instance_t *controller,
+			  uint32_t filters,
+			  uint8_t  region,
+			  uint64_t region_base,
+			  uint64_t region_top,
+			  tzc_region_attributes_t sec_attr,
+			  uint32_t ns_device_access)
+{
+	uint64_t max_addr;
+
+	assert(controller != NULL);
+
+	/* Do range checks on filters and regions. */
+	assert(((filters >> controller->num_filters) == 0) &&
+	       (region < controller->num_regions));
+
+	/*
+	 * Do address range check based on TZC configuration. A 64bit address is
+	 * the max and expected case.
+	 */
+	max_addr = UINT64_MAX >> (64 - controller->addr_width);
+	if ((region_top > max_addr) || (region_base >= region_top))
+		assert(0);
+
+	/* region_base and (region_top + 1) must be 4KB aligned */
+	assert(((region_base | (region_top + 1)) & (4096 - 1)) == 0);
+
+	assert(sec_attr <= TZC_REGION_S_RDWR);
+
+	/*
+	 * Inputs look ok, start programming registers.
+	 * All the address registers are 32 bits wide and have a LOW and HIGH
+	 * component used to construct a up to a 64bit address.
+	 */
+	tzc_write_region_base_low(controller->base, region, (uint32_t)(region_base));
+	tzc_write_region_base_high(controller->base, region, (uint32_t)(region_base >> 32));
+
+	tzc_write_region_top_low(controller->base, region, (uint32_t)(region_top));
+	tzc_write_region_top_high(controller->base, region, (uint32_t)(region_top >> 32));
+
+	/* Assign the region to a filter and set secure attributes */
+	tzc_write_region_attributes(controller->base, region,
+		(sec_attr << REGION_ATTRIBUTES_SEC_SHIFT) | filters);
+
+	/*
+	 * Specify which non-secure devices have permission to access this
+	 * region.
+	 */
+	tzc_write_region_id_access(controller->base, region, ns_device_access);
+}
+
+
+void tzc_set_action(const tzc_instance_t *controller, tzc_action_t action)
+{
+	assert(controller != NULL);
+
+	/*
+	 * - Currently no handler is provided to trap an error via interrupt
+	 *   or exception.
+	 * - The interrupt action has not been tested.
+	 */
+	tzc_write_action(controller->base, action);
+}
+
+
+void tzc_enable_filters(const tzc_instance_t *controller)
+{
+	uint32_t state;
+	uint32_t filter;
+
+	assert(controller != NULL);
+
+	for (filter = 0; filter < controller->num_filters; filter++) {
+		state = tzc_get_gate_keeper(controller->base, filter);
+		if (state) {
+			ERROR("TZC : Filter %d Gatekeeper already enabled.\n",
+				filter);
+			panic();
+		}
+		tzc_set_gate_keeper(controller->base, filter, 1);
+	}
+}
+
+
+void tzc_disable_filters(const tzc_instance_t *controller)
+{
+	uint32_t filter;
+
+	assert(controller != NULL);
+
+	/*
+	 * We don't do the same state check as above as the Gatekeepers are
+	 * disabled after reset.
+	 */
+	for (filter = 0; filter < controller->num_filters; filter++)
+		tzc_set_gate_keeper(controller->base, filter, 0);
+}