Trusted Firmware-A Tests, version 2.0
This is the first public version of the tests for the Trusted
Firmware-A project. Please see the documentation provided in the
source tree for more details.
Change-Id: I6f3452046a1351ac94a71b3525c30a4ca8db7867
Signed-off-by: Sandrine Bailleux <sandrine.bailleux@arm.com>
Co-authored-by: amobal01 <amol.balasokamble@arm.com>
Co-authored-by: Antonio Nino Diaz <antonio.ninodiaz@arm.com>
Co-authored-by: Asha R <asha.r@arm.com>
Co-authored-by: Chandni Cherukuri <chandni.cherukuri@arm.com>
Co-authored-by: David Cunado <david.cunado@arm.com>
Co-authored-by: Dimitris Papastamos <dimitris.papastamos@arm.com>
Co-authored-by: Douglas Raillard <douglas.raillard@arm.com>
Co-authored-by: dp-arm <dimitris.papastamos@arm.com>
Co-authored-by: Jeenu Viswambharan <jeenu.viswambharan@arm.com>
Co-authored-by: Jonathan Wright <jonathan.wright@arm.com>
Co-authored-by: Kévin Petit <kevin.petit@arm.com>
Co-authored-by: Roberto Vargas <roberto.vargas@arm.com>
Co-authored-by: Sathees Balya <sathees.balya@arm.com>
Co-authored-by: Shawon Roy <Shawon.Roy@arm.com>
Co-authored-by: Soby Mathew <soby.mathew@arm.com>
Co-authored-by: Thomas Abraham <thomas.abraham@arm.com>
Co-authored-by: Vikram Kanigiri <vikram.kanigiri@arm.com>
Co-authored-by: Yatharth Kochar <yatharth.kochar@arm.com>
diff --git a/drivers/arm/gic/arm_gic_v2.c b/drivers/arm/gic/arm_gic_v2.c
new file mode 100644
index 0000000..025d48d
--- /dev/null
+++ b/drivers/arm/gic/arm_gic_v2.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <gic_v2.h>
+
+void arm_gic_enable_interrupts_local(void)
+{
+ gicv2_enable_cpuif();
+}
+
+void arm_gic_setup_local(void)
+{
+ gicv2_probe_gic_cpu_id();
+ gicv2_setup_cpuif();
+}
+
+void arm_gic_disable_interrupts_local(void)
+{
+ gicv2_disable_cpuif();
+}
+
+void arm_gic_save_context_local(void)
+{
+ gicv2_save_cpuif_context();
+}
+
+void arm_gic_restore_context_local(void)
+{
+ gicv2_restore_cpuif_context();
+}
+
+void arm_gic_save_context_global(void)
+{
+ gicv2_save_sgi_ppi_context();
+}
+
+void arm_gic_restore_context_global(void)
+{
+ gicv2_setup_distif();
+ gicv2_restore_sgi_ppi_context();
+}
+
+void arm_gic_setup_global(void)
+{
+ gicv2_setup_distif();
+}
+
+unsigned int arm_gic_get_intr_priority(unsigned int num)
+{
+ return gicv2_gicd_get_ipriorityr(num);
+}
+
+void arm_gic_set_intr_priority(unsigned int num,
+ unsigned int priority)
+{
+ gicv2_gicd_set_ipriorityr(num, priority);
+}
+
+void arm_gic_send_sgi(unsigned int sgi_id, unsigned int core_pos)
+{
+ gicv2_send_sgi(sgi_id, core_pos);
+}
+
+void arm_gic_set_intr_target(unsigned int num, unsigned int core_pos)
+{
+ gicv2_set_itargetsr(num, core_pos);
+}
+
+unsigned int arm_gic_intr_enabled(unsigned int num)
+{
+ return gicv2_gicd_get_isenabler(num) != 0;
+}
+
+void arm_gic_intr_enable(unsigned int num)
+{
+ gicv2_gicd_set_isenabler(num);
+}
+
+void arm_gic_intr_disable(unsigned int num)
+{
+ gicv2_gicd_set_icenabler(num);
+}
+
+unsigned int arm_gic_intr_ack(unsigned int *raw_iar)
+{
+ assert(raw_iar);
+
+ *raw_iar = gicv2_gicc_read_iar();
+ return get_gicc_iar_intid(*raw_iar);
+}
+
+unsigned int arm_gic_is_intr_pending(unsigned int num)
+{
+ return gicv2_gicd_get_ispendr(num);
+}
+
+void arm_gic_intr_clear(unsigned int num)
+{
+ gicv2_gicd_set_icpendr(num);
+}
+
+void arm_gic_end_of_intr(unsigned int raw_iar)
+{
+ gicv2_gicc_write_eoir(raw_iar);
+}
+
+void arm_gic_init(uintptr_t gicc_base,
+ uintptr_t gicd_base,
+ uintptr_t gicr_base)
+{
+ gicv2_init(gicc_base, gicd_base);
+ INFO("ARM GIC v2 driver initialized\n");
+}
+
diff --git a/drivers/arm/gic/arm_gic_v2v3.c b/drivers/arm/gic/arm_gic_v2v3.c
new file mode 100644
index 0000000..576c611
--- /dev/null
+++ b/drivers/arm/gic/arm_gic_v2v3.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <gic_common.h>
+#include <gic_v2.h>
+#include <gic_v3.h>
+
+/* Record whether a GICv3 was detected on the system */
+static unsigned int gicv3_detected;
+
+void arm_gic_enable_interrupts_local(void)
+{
+ if (gicv3_detected)
+ gicv3_enable_cpuif();
+ else
+ gicv2_enable_cpuif();
+}
+
+void arm_gic_setup_local(void)
+{
+ if (gicv3_detected) {
+ gicv3_probe_redistif_addr();
+ gicv3_setup_cpuif();
+ } else {
+ gicv2_probe_gic_cpu_id();
+ gicv2_setup_cpuif();
+ }
+}
+
+void arm_gic_disable_interrupts_local(void)
+{
+ if (gicv3_detected)
+ gicv3_disable_cpuif();
+ else
+ gicv2_disable_cpuif();
+}
+
+void arm_gic_save_context_local(void)
+{
+ if (gicv3_detected)
+ gicv3_save_cpuif_context();
+ else
+ gicv2_save_cpuif_context();
+}
+
+void arm_gic_restore_context_local(void)
+{
+ if (gicv3_detected)
+ gicv3_restore_cpuif_context();
+ else
+ gicv2_restore_cpuif_context();
+}
+
+void arm_gic_save_context_global(void)
+{
+ if (gicv3_detected)
+ gicv3_save_sgi_ppi_context();
+ else
+ gicv2_save_sgi_ppi_context();
+}
+
+void arm_gic_restore_context_global(void)
+{
+ if (gicv3_detected) {
+ gicv3_setup_distif();
+ gicv3_restore_sgi_ppi_context();
+ } else {
+ gicv2_setup_distif();
+ gicv2_restore_sgi_ppi_context();
+ }
+}
+
+void arm_gic_setup_global(void)
+{
+ if (gicv3_detected)
+ gicv3_setup_distif();
+ else
+ gicv2_setup_distif();
+}
+
+unsigned int arm_gic_get_intr_priority(unsigned int num)
+{
+ if (gicv3_detected)
+ return gicv3_get_ipriorityr(num);
+ else
+ return gicv2_gicd_get_ipriorityr(num);
+}
+
+void arm_gic_set_intr_priority(unsigned int num,
+ unsigned int priority)
+{
+ if (gicv3_detected)
+ gicv3_set_ipriorityr(num, priority);
+ else
+ gicv2_gicd_set_ipriorityr(num, priority);
+}
+
+void arm_gic_send_sgi(unsigned int sgi_id, unsigned int core_pos)
+{
+ if (gicv3_detected)
+ gicv3_send_sgi(sgi_id, core_pos);
+ else
+ gicv2_send_sgi(sgi_id, core_pos);
+}
+
+void arm_gic_set_intr_target(unsigned int num, unsigned int core_pos)
+{
+ if (gicv3_detected)
+ gicv3_set_intr_route(num, core_pos);
+ else
+ gicv2_set_itargetsr(num, core_pos);
+}
+
+unsigned int arm_gic_intr_enabled(unsigned int num)
+{
+ if (gicv3_detected)
+ return gicv3_get_isenabler(num) != 0;
+ else
+ return gicv2_gicd_get_isenabler(num) != 0;
+}
+
+void arm_gic_intr_enable(unsigned int num)
+{
+ if (gicv3_detected)
+ gicv3_set_isenabler(num);
+ else
+ gicv2_gicd_set_isenabler(num);
+}
+
+void arm_gic_intr_disable(unsigned int num)
+{
+ if (gicv3_detected)
+ gicv3_set_icenabler(num);
+ else
+ gicv2_gicd_set_icenabler(num);
+}
+
+unsigned int arm_gic_intr_ack(unsigned int *raw_iar)
+{
+ assert(raw_iar);
+
+ if (gicv3_detected) {
+ *raw_iar = gicv3_acknowledge_interrupt();
+ return *raw_iar;
+ } else {
+ *raw_iar = gicv2_gicc_read_iar();
+ return get_gicc_iar_intid(*raw_iar);
+ }
+}
+
+unsigned int arm_gic_is_intr_pending(unsigned int num)
+{
+ if (gicv3_detected)
+ return gicv3_get_ispendr(num);
+ else
+ return gicv2_gicd_get_ispendr(num);
+}
+
+void arm_gic_intr_clear(unsigned int num)
+{
+ if (gicv3_detected)
+ gicv3_set_icpendr(num);
+ else
+ gicv2_gicd_set_icpendr(num);
+}
+
+void arm_gic_end_of_intr(unsigned int raw_iar)
+{
+ if (gicv3_detected)
+ gicv3_end_of_interrupt(raw_iar);
+ else
+ gicv2_gicc_write_eoir(raw_iar);
+}
+
+void arm_gic_init(uintptr_t gicc_base,
+ uintptr_t gicd_base,
+ uintptr_t gicr_base)
+{
+
+ if (is_gicv3_mode()) {
+ gicv3_detected = 1;
+ gicv3_init(gicr_base, gicd_base);
+ } else {
+ gicv2_init(gicc_base, gicd_base);
+ }
+
+ INFO("%s mode detected\n", (gicv3_detected) ?
+ "GICv3" : "GICv2");
+}
diff --git a/drivers/arm/gic/gic_common.c b/drivers/arm/gic/gic_common.c
new file mode 100644
index 0000000..207ee15
--- /dev/null
+++ b/drivers/arm/gic/gic_common.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <gic_common.h>
+#include <gic_v3.h>
+#include <mmio.h>
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+
+unsigned int gicd_read_isenabler(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ISENABLER_SHIFT;
+ return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+unsigned int gicd_read_icenabler(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ICENABLER_SHIFT;
+ return mmio_read_32(base + GICD_ICENABLER + (n << 2));
+}
+
+unsigned int gicd_read_ispendr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ISPENDR_SHIFT;
+ return mmio_read_32(base + GICD_ISPENDR + (n << 2));
+}
+
+unsigned int gicd_read_icpendr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ICPENDR_SHIFT;
+ return mmio_read_32(base + GICD_ICPENDR + (n << 2));
+}
+
+unsigned int gicd_read_isactiver(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ISACTIVER_SHIFT;
+ return mmio_read_32(base + GICD_ISACTIVER + (n << 2));
+}
+
+unsigned int gicd_read_icactiver(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ICACTIVER_SHIFT;
+ return mmio_read_32(base + GICD_ICACTIVER + (n << 2));
+}
+
+unsigned int gicd_read_ipriorityr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> IPRIORITYR_SHIFT;
+ return mmio_read_32(base + GICD_IPRIORITYR + (n << 2));
+}
+
+unsigned int gicd_read_icfgr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int n = interrupt_id >> ICFGR_SHIFT;
+ return mmio_read_32(base + GICD_ICFGR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+
+void gicd_write_isenabler(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ISENABLER_SHIFT;
+ mmio_write_32(base + GICD_ISENABLER + (n << 2), val);
+}
+
+void gicd_write_icenabler(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ICENABLER_SHIFT;
+ mmio_write_32(base + GICD_ICENABLER + (n << 2), val);
+}
+
+void gicd_write_ispendr(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ISPENDR_SHIFT;
+ mmio_write_32(base + GICD_ISPENDR + (n << 2), val);
+}
+
+void gicd_write_icpendr(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ICPENDR_SHIFT;
+ mmio_write_32(base + GICD_ICPENDR + (n << 2), val);
+}
+
+void gicd_write_isactiver(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ISACTIVER_SHIFT;
+ mmio_write_32(base + GICD_ISACTIVER + (n << 2), val);
+}
+
+void gicd_write_icactiver(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ICACTIVER_SHIFT;
+ mmio_write_32(base + GICD_ICACTIVER + (n << 2), val);
+}
+
+void gicd_write_ipriorityr(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> IPRIORITYR_SHIFT;
+ mmio_write_32(base + GICD_IPRIORITYR + (n << 2), val);
+}
+
+void gicd_write_icfgr(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned int n = interrupt_id >> ICFGR_SHIFT;
+ mmio_write_32(base + GICD_ICFGR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for individual interrupt manipulation
+ ******************************************************************************/
+unsigned int gicd_get_isenabler(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ISENABLER_SHIFT) - 1);
+
+ return gicd_read_isenabler(base, interrupt_id) & (1 << bit_num);
+}
+
+void gicd_set_isenabler(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ISENABLER_SHIFT) - 1);
+
+ gicd_write_isenabler(base, interrupt_id, (1 << bit_num));
+}
+
+void gicd_set_icenabler(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ICENABLER_SHIFT) - 1);
+
+ gicd_write_icenabler(base, interrupt_id, (1 << bit_num));
+}
+
+void gicd_set_ispendr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ISPENDR_SHIFT) - 1);
+
+ gicd_write_ispendr(base, interrupt_id, (1 << bit_num));
+}
+
+void gicd_set_icpendr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ICPENDR_SHIFT) - 1);
+
+ gicd_write_icpendr(base, interrupt_id, (1 << bit_num));
+}
+
+void gicd_set_isactiver(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ISACTIVER_SHIFT) - 1);
+
+ gicd_write_isactiver(base, interrupt_id, (1 << bit_num));
+}
+
+void gicd_set_icactiver(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned int bit_num = interrupt_id & ((1 << ICACTIVER_SHIFT) - 1);
+
+ gicd_write_icactiver(base, interrupt_id, (1 << bit_num));
+}
+
+unsigned int gicd_get_ipriorityr(unsigned int base, unsigned int interrupt_id)
+{
+ return gicd_read_ipriorityr(base, interrupt_id) & GIC_PRI_MASK;
+}
+
+void gicd_set_ipriorityr(unsigned int base, unsigned int interrupt_id,
+ unsigned int priority)
+{
+ mmio_write_8(base + GICD_IPRIORITYR + interrupt_id,
+ priority & GIC_PRI_MASK);
+}
+
+unsigned int is_gicv3_mode(void)
+{
+ /* Check if GICv3 system register available */
+#ifndef AARCH32
+ if (!(read_id_aa64pfr0_el1() & (ID_AA64PFR0_GIC_MASK << ID_AA64PFR0_GIC_SHIFT)))
+ return 0;
+#else
+ if (!(read_id_pfr1() & (ID_PFR1_GIC_MASK << ID_PFR1_GIC_SHIFT)))
+ return 0;
+#endif
+
+ /* Check whether the system register interface is enabled */
+ return !!is_sre_enabled();
+}
diff --git a/drivers/arm/gic/gic_v2.c b/drivers/arm/gic/gic_v2.c
new file mode 100644
index 0000000..48ee29e
--- /dev/null
+++ b/drivers/arm/gic/gic_v2.c
@@ -0,0 +1,353 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <arm_gic.h>
+#include <assert.h>
+#include <gic_common.h>
+#include <gic_v2.h>
+#include <mmio.h>
+#include <platform.h>
+
+/*
+ * Data structure to store the GIC per CPU context before entering
+ * system suspend. Only the GIC context of first 32 interrupts (SGIs and PPIs)
+ * will be saved. The GIC SPI context needs to be restored by the respective
+ * drivers. The GICC_PMR is not saved here as it will be reinitialized during
+ * GIC restore.
+ */
+struct gicv2_pcpu_ctx {
+ unsigned int gicc_ctlr;
+ unsigned int gicd_isenabler0;
+ unsigned int gicd_ipriorityr[NUM_PCPU_INTR >> IPRIORITYR_SHIFT];
+ unsigned int gicd_icfgr;
+};
+
+static struct gicv2_pcpu_ctx pcpu_gic_ctx[PLATFORM_CORE_COUNT];
+
+static uintptr_t gicc_base_addr;
+static uintptr_t gicd_base_addr;
+
+static unsigned int gic_cpu_id[PLATFORM_CORE_COUNT] = {UINT32_MAX};
+
+/* Helper function to convert core pos to gic id */
+static unsigned int core_pos_to_gic_id(unsigned int core_pos)
+{
+ assert(gic_cpu_id[core_pos] != UINT32_MAX);
+ return gic_cpu_id[core_pos];
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+unsigned int gicd_read_itargetsr(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned n = interrupt_id >> ITARGETSR_SHIFT;
+ return mmio_read_32(base + GICD_ITARGETSR + (n << 2));
+}
+
+unsigned int gicd_read_cpendsgir(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned n = interrupt_id >> CPENDSGIR_SHIFT;
+ return mmio_read_32(base + GICD_CPENDSGIR + (n << 2));
+}
+
+unsigned int gicd_read_spendsgir(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned n = interrupt_id >> SPENDSGIR_SHIFT;
+ return mmio_read_32(base + GICD_SPENDSGIR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+void gicd_write_itargetsr(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned n = interrupt_id >> ITARGETSR_SHIFT;
+ mmio_write_32(base + GICD_ITARGETSR + (n << 2), val);
+}
+
+void gicd_write_itargetsr_byte(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ mmio_write_8(base + GICD_ITARGETSR + interrupt_id, val);
+}
+
+void gicd_write_cpendsgir(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned n = interrupt_id >> CPENDSGIR_SHIFT;
+ mmio_write_32(base + GICD_CPENDSGIR + (n << 2), val);
+}
+
+void gicd_write_spendsgir(unsigned int base,
+ unsigned int interrupt_id, unsigned int val)
+{
+ unsigned n = interrupt_id >> SPENDSGIR_SHIFT;
+ mmio_write_32(base + GICD_SPENDSGIR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for individual interrupt manipulation
+ ******************************************************************************/
+void gicd_set_itargetsr(unsigned int base,
+ unsigned int interrupt_id, unsigned int iface)
+{
+ mmio_write_8(base + GICD_ITARGETSR + interrupt_id, (1 << iface));
+}
+
+/******************************************************************************
+ * GICv2 public driver API
+ *****************************************************************************/
+
+void gicv2_enable_cpuif(void)
+{
+ unsigned int gicc_ctlr;
+
+ assert(gicc_base_addr);
+
+ /* Enable the GICC and disable bypass */
+ gicc_ctlr = GICC_CTLR_ENABLE | FIQ_BYP_DIS_GRP1
+ | IRQ_BYP_DIS_GRP1;
+ gicc_write_ctlr(gicc_base_addr, gicc_ctlr);
+}
+
+void gicv2_probe_gic_cpu_id(void)
+{
+ unsigned int gicd_itargets_val, core_pos;
+
+ assert(gicd_base_addr);
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ gicd_itargets_val = gicd_read_itargetsr(gicd_base_addr, 0);
+
+ assert(gicd_itargets_val);
+
+ /* Convert the bit pos returned by read of ITARGETSR0 to GIC CPU ID */
+ gic_cpu_id[core_pos] = __builtin_ctz(gicd_itargets_val);
+}
+
+void gicv2_setup_cpuif(void)
+{
+ assert(gicc_base_addr);
+
+ /* Set the priority mask register to allow all interrupts to trickle in */
+ gicc_write_pmr(gicc_base_addr, GIC_PRI_MASK);
+ gicv2_enable_cpuif();
+}
+
+void gicv2_disable_cpuif(void)
+{
+ unsigned int gicc_ctlr;
+
+ assert(gicc_base_addr);
+
+ /* Disable non-secure interrupts and disable their bypass */
+ gicc_ctlr = gicc_read_ctlr(gicc_base_addr);
+ gicc_ctlr &= ~GICC_CTLR_ENABLE;
+ gicc_ctlr |= FIQ_BYP_DIS_GRP1 | IRQ_BYP_DIS_GRP1;
+ gicc_write_ctlr(gicc_base_addr, gicc_ctlr);
+}
+
+void gicv2_save_cpuif_context(void)
+{
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ assert(gicc_base_addr);
+ pcpu_gic_ctx[core_pos].gicc_ctlr =
+ gicc_read_ctlr(gicc_base_addr);
+}
+
+void gicv2_restore_cpuif_context(void)
+{
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ assert(gicc_base_addr);
+
+ /* The GICC_PMR is never modified, hence we initialize this register */
+ gicc_write_pmr(gicc_base_addr, GIC_PRI_MASK);
+
+ gicc_write_ctlr(gicc_base_addr,
+ pcpu_gic_ctx[core_pos].gicc_ctlr);
+}
+
+void gicv2_setup_distif(void)
+{
+ unsigned int gicd_ctlr;
+
+ assert(gicd_base_addr);
+
+ /* Enable the forwarding of interrupts to CPU interface */
+ gicd_ctlr = gicd_read_ctlr(gicd_base_addr);
+ gicd_ctlr |= GICD_CTLR_ENABLE;
+ gicd_write_ctlr(gicd_base_addr, gicd_ctlr);
+}
+
+/* Save the per-cpu GICD ISENABLER, IPRIORITYR and ICFGR registers */
+void gicv2_save_sgi_ppi_context(void)
+{
+ unsigned int i;
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ assert(gicd_base_addr);
+ pcpu_gic_ctx[core_pos].gicd_isenabler0 =
+ gicd_read_isenabler(gicd_base_addr, 0);
+
+ /* Read the ipriority registers, 4 at a time */
+ for (i = 0; i < (NUM_PCPU_INTR >> IPRIORITYR_SHIFT); i++)
+ pcpu_gic_ctx[core_pos].gicd_ipriorityr[i] =
+ gicd_read_ipriorityr(gicd_base_addr, i << IPRIORITYR_SHIFT);
+
+ pcpu_gic_ctx[core_pos].gicd_icfgr =
+ gicd_read_icfgr(gicd_base_addr, MIN_PPI_ID);
+}
+
+/* Restore the per-cpu GICD ISENABLER, IPRIORITYR and ICFGR registers */
+void gicv2_restore_sgi_ppi_context(void)
+{
+ unsigned int i;
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ assert(gicd_base_addr);
+
+ /* Write the ipriority registers, 4 at a time */
+ for (i = 0; i < (NUM_PCPU_INTR >> IPRIORITYR_SHIFT); i++)
+ gicd_write_ipriorityr(gicd_base_addr, i << IPRIORITYR_SHIFT,
+ pcpu_gic_ctx[core_pos].gicd_ipriorityr[i]);
+
+ gicd_write_icfgr(gicd_base_addr, MIN_PPI_ID,
+ pcpu_gic_ctx[core_pos].gicd_icfgr);
+
+ gicd_write_isenabler(gicd_base_addr, 0,
+ pcpu_gic_ctx[core_pos].gicd_isenabler0);
+}
+
+unsigned int gicv2_gicd_get_ipriorityr(unsigned int interrupt_id)
+{
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ return gicd_get_ipriorityr(gicd_base_addr, interrupt_id);
+}
+
+void gicv2_gicd_set_ipriorityr(unsigned int interrupt_id,
+ unsigned int priority)
+{
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ gicd_set_ipriorityr(gicd_base_addr, interrupt_id, priority);
+}
+
+void gicv2_send_sgi(unsigned int sgi_id, unsigned int core_pos)
+{
+ unsigned int sgir_val;
+
+ assert(gicd_base_addr);
+ assert(IS_SGI(sgi_id));
+
+ sgir_val = sgi_id << GICD_SGIR_INTID_SHIFT;
+ sgir_val |= (1 << core_pos_to_gic_id(core_pos)) << GICD_SGIR_CPUTL_SHIFT;
+
+ gicd_write_sgir(gicd_base_addr, sgir_val);
+}
+
+void gicv2_set_itargetsr(unsigned int num, unsigned int core_pos)
+{
+ unsigned int gic_cpu_id;
+ assert(gicd_base_addr);
+ assert(IS_SPI(num));
+
+ gic_cpu_id = core_pos_to_gic_id(core_pos);
+ gicd_set_itargetsr(gicd_base_addr, num, gic_cpu_id);
+}
+
+void gicv2_set_itargetsr_value(unsigned int num, unsigned int val)
+{
+ assert(gicd_base_addr);
+ assert(IS_SPI(num));
+
+ gicd_write_itargetsr_byte(gicd_base_addr, num, val);
+}
+
+unsigned int gicv2_gicd_get_isenabler(unsigned int num)
+{
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(num));
+
+ return gicd_get_isenabler(gicd_base_addr, num);
+}
+
+void gicv2_gicd_set_isenabler(unsigned int num)
+{
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(num));
+
+ gicd_set_isenabler(gicd_base_addr, num);
+}
+
+void gicv2_gicd_set_icenabler(unsigned int num)
+{
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(num));
+
+ gicd_set_icenabler(gicd_base_addr, num);
+}
+
+unsigned int gicv2_gicc_read_iar(void)
+{
+ assert(gicc_base_addr);
+ return gicc_read_iar(gicc_base_addr);
+}
+
+unsigned int gicv2_gicd_get_ispendr(unsigned int interrupt_id)
+{
+ unsigned int ispendr;
+ unsigned int bit_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ ispendr = gicd_read_ispendr(gicd_base_addr, interrupt_id);
+ bit_pos = interrupt_id % (1 << ISPENDR_SHIFT);
+
+ return !!(ispendr & (1 << bit_pos));
+}
+
+void gicv2_gicd_set_ispendr(unsigned int interrupt_id)
+{
+ assert(gicd_base_addr);
+ assert(IS_PPI(interrupt_id) || IS_SPI(interrupt_id));
+ gicd_set_ispendr(gicd_base_addr, interrupt_id);
+}
+
+void gicv2_gicd_set_icpendr(unsigned int interrupt_id)
+{
+ assert(gicd_base_addr);
+ assert(IS_PPI(interrupt_id) || IS_SPI(interrupt_id));
+
+ gicd_set_icpendr(gicd_base_addr, interrupt_id);
+}
+
+void gicv2_gicc_write_eoir(unsigned int val)
+{
+ assert(gicc_base_addr);
+
+ gicc_write_eoir(gicc_base_addr, val);
+}
+
+void gicv2_init(uintptr_t gicc_base,
+ uintptr_t gicd_base)
+{
+ assert(gicc_base);
+ assert(gicd_base);
+
+ /* Assert that this is a GICv2 system */
+ assert(!is_gicv3_mode());
+ gicc_base_addr = gicc_base;
+ gicd_base_addr = gicd_base;
+}
diff --git a/drivers/arm/gic/gic_v3.c b/drivers/arm/gic/gic_v3.c
new file mode 100644
index 0000000..76b0863
--- /dev/null
+++ b/drivers/arm/gic/gic_v3.c
@@ -0,0 +1,507 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <arm_gic.h>
+#include <assert.h>
+#include <debug.h>
+#include <gic_common.h>
+#include <gic_v3.h>
+#include <mmio.h>
+#include <platform.h>
+
+/* Global variables to store the GIC base addresses */
+static uintptr_t gicr_base_addr;
+static uintptr_t gicd_base_addr;
+
+#ifndef AARCH32
+#define MPIDR_AFFLVL3_MASK ((unsigned long long)MPIDR_AFFLVL_MASK << MPIDR_AFF3_SHIFT)
+#define gic_typer_affinity_from_mpidr(mpidr) \
+ (((mpidr) & (~MPIDR_AFFLVL3_MASK)) | (((mpidr) & MPIDR_AFFLVL3_MASK) >> 8))
+#else
+#define gic_typer_affinity_from_mpidr(mpidr) \
+ ((mpidr) & ((MPIDR_AFFLVL_MASK << MPIDR_AFF2_SHIFT) | MPID_MASK))
+#endif
+
+/*
+ * Data structure to store the GIC per CPU context before entering
+ * system suspend. Only the GIC context of first 32 interrupts (SGIs and PPIs)
+ * will be saved. The GIC SPI context needs to be restored by the respective
+ * drivers.
+ */
+struct gicv3_pcpu_ctx {
+ /* Flag to indicate whether the CPU is suspended */
+ unsigned int is_suspended;
+ unsigned int icc_igrpen1;
+ unsigned int gicr_isenabler;
+ unsigned int gicr_ipriorityr[NUM_PCPU_INTR >> IPRIORITYR_SHIFT];
+ unsigned int gicr_icfgr;
+};
+
+/* Array to store the per-cpu GICv3 context when being suspended.*/
+static struct gicv3_pcpu_ctx pcpu_ctx[PLATFORM_CORE_COUNT];
+
+/* Array to store the per-cpu redistributor frame addresses */
+static uintptr_t rdist_pcpu_base[PLATFORM_CORE_COUNT];
+
+/*
+ * Array to store the mpidr corresponding to each initialized per-CPU
+ * redistributor interface.
+ */
+static unsigned long long mpidr_list[PLATFORM_CORE_COUNT] = {UINT64_MAX};
+
+/******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ *****************************************************************************/
+static void gicd_write_irouter(unsigned int base,
+ unsigned int interrupt_id,
+ unsigned long long route)
+{
+ assert(interrupt_id >= MIN_SPI_ID);
+ mmio_write_64(base + GICD_IROUTER + (interrupt_id << 3), route);
+}
+
+/******************************************************************************
+ * GIC Re-distributor interface accessors for writing entire registers
+ *****************************************************************************/
+static void gicr_write_isenabler0(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICR_ISENABLER0, val);
+}
+
+static void gicr_write_icenabler0(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICR_ICENABLER0, val);
+}
+
+static void gicr_write_icpendr0(unsigned int base, unsigned int val)
+{
+ mmio_write_32(base + GICR_ICPENDR0, val);
+}
+
+static void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+ unsigned n = id >> IPRIORITYR_SHIFT;
+ mmio_write_32(base + GICR_IPRIORITYR + (n << 2), val);
+}
+
+static void gicr_write_icfgr1(uintptr_t base, unsigned int val)
+{
+ mmio_write_32(base + GICR_ICFGR1, val);
+}
+
+/******************************************************************************
+ * GIC Re-distributor interface accessors for reading entire registers
+ *****************************************************************************/
+static unsigned long long gicr_read_typer(uintptr_t base)
+{
+ return mmio_read_64(base + GICR_TYPER);
+}
+
+static unsigned int gicr_read_icfgr1(uintptr_t base)
+{
+ return mmio_read_32(base + GICR_ICFGR1);
+}
+
+static unsigned int gicr_read_isenabler0(unsigned int base)
+{
+ return mmio_read_32(base + GICR_ISENABLER0);
+}
+
+static unsigned int gicr_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+ unsigned n = id >> IPRIORITYR_SHIFT;
+ return mmio_read_32(base + GICR_IPRIORITYR + (n << 2));
+}
+
+static unsigned int gicr_read_ispendr0(unsigned int base)
+{
+ return mmio_read_32(base + GICR_ISPENDR0);
+}
+
+/******************************************************************************
+ * GIC Re-distributor interface accessors for individual interrupt
+ * manipulation
+ *****************************************************************************/
+static unsigned int gicr_get_isenabler0(unsigned int base,
+ unsigned int interrupt_id)
+{
+ unsigned bit_num = interrupt_id & ((1 << ISENABLER_SHIFT) - 1);
+ return gicr_read_isenabler0(base) & (1 << bit_num);
+}
+
+static void gicr_set_isenabler0(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned bit_num = interrupt_id & ((1 << ISENABLER_SHIFT) - 1);
+ gicr_write_isenabler0(base, (1 << bit_num));
+}
+
+static void gicr_set_icenabler0(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned bit_num = interrupt_id & ((1 << ISENABLER_SHIFT) - 1);
+ gicr_write_icenabler0(base, (1 << bit_num));
+}
+
+static void gicr_set_icpendr0(unsigned int base, unsigned int interrupt_id)
+{
+ unsigned bit_num = interrupt_id & ((1 << ICPENDR_SHIFT) - 1);
+ gicr_write_icpendr0(base, (1 << bit_num));
+}
+
+/******************************************************************************
+ * GICv3 public driver API
+ *****************************************************************************/
+void gicv3_enable_cpuif(void)
+{
+ /* Assert that system register access is enabled */
+ assert(IS_IN_EL2() ? (read_icc_sre_el2() & ICC_SRE_SRE_BIT) :
+ (read_icc_sre_el1() & ICC_SRE_SRE_BIT));
+
+ /* Enable Group1 non secure interrupts */
+ write_icc_igrpen1_el1(read_icc_igrpen1_el1() | IGRPEN1_EL1_ENABLE_BIT);
+ isb();
+}
+
+void gicv3_setup_cpuif(void)
+{
+ /* Set the priority mask register to allow all interrupts to trickle in */
+ write_icc_pmr_el1(GIC_PRI_MASK);
+ isb();
+ gicv3_enable_cpuif();
+}
+
+void gicv3_disable_cpuif(void)
+{
+ /* Disable Group1 non secure interrupts */
+ write_icc_igrpen1_el1(read_icc_igrpen1_el1() &
+ ~IGRPEN1_EL1_ENABLE_BIT);
+ isb();
+}
+
+void gicv3_save_cpuif_context(void)
+{
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ /* Set the `is_suspended` flag as this core is being suspended. */
+ pcpu_ctx[core_pos].is_suspended = 1;
+ pcpu_ctx[core_pos].icc_igrpen1 = read_icc_igrpen1_el1();
+}
+
+void gicv3_restore_cpuif_context(void)
+{
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ /* Reset the `is_suspended` flag as this core has resumed from suspend. */
+ pcpu_ctx[core_pos].is_suspended = 0;
+ write_icc_pmr_el1(GIC_PRI_MASK);
+ write_icc_igrpen1_el1(pcpu_ctx[core_pos].icc_igrpen1);
+ isb();
+}
+
+void gicv3_save_sgi_ppi_context(void)
+{
+ unsigned int i, core_pos;
+ unsigned int my_core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ /* Save the context for all the suspended cores */
+ for (core_pos = 0; core_pos < PLATFORM_CORE_COUNT; core_pos++) {
+ /*
+ * Continue if the core pos is not the current core
+ * and has not suspended
+ */
+ if ((core_pos != my_core_pos) &&
+ (!pcpu_ctx[core_pos].is_suspended))
+ continue;
+
+ assert(rdist_pcpu_base[core_pos]);
+
+ pcpu_ctx[core_pos].gicr_isenabler =
+ gicr_read_isenabler0(rdist_pcpu_base[core_pos]);
+
+ /* Read the ipriority registers, 4 at a time */
+ for (i = 0; i < (NUM_PCPU_INTR >> IPRIORITYR_SHIFT); i++)
+ pcpu_ctx[core_pos].gicr_ipriorityr[i] =
+ gicr_read_ipriorityr(rdist_pcpu_base[core_pos],
+ i << IPRIORITYR_SHIFT);
+
+ pcpu_ctx[core_pos].gicr_icfgr =
+ gicr_read_icfgr1(rdist_pcpu_base[core_pos]);
+ }
+}
+
+void gicv3_restore_sgi_ppi_context(void)
+{
+ unsigned int i, core_pos;
+ unsigned int my_core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ /* Restore the context for all the suspended cores */
+ for (core_pos = 0; core_pos < PLATFORM_CORE_COUNT; core_pos++) {
+ /*
+ * Continue if the core pos is not the current core
+ * and has not suspended
+ */
+ if ((core_pos != my_core_pos) &&
+ (!pcpu_ctx[core_pos].is_suspended))
+ continue;
+
+ assert(rdist_pcpu_base[core_pos]);
+
+ /* Read the ipriority registers, 4 at a time */
+ for (i = 0; i < (NUM_PCPU_INTR >> IPRIORITYR_SHIFT); i++)
+ gicr_write_ipriorityr(rdist_pcpu_base[core_pos],
+ i << IPRIORITYR_SHIFT,
+ pcpu_ctx[core_pos].gicr_ipriorityr[i]);
+
+ gicr_write_icfgr1(rdist_pcpu_base[core_pos],
+ pcpu_ctx[core_pos].gicr_icfgr);
+ gicr_write_isenabler0(rdist_pcpu_base[core_pos],
+ pcpu_ctx[core_pos].gicr_isenabler);
+ }
+}
+
+unsigned int gicv3_get_ipriorityr(unsigned int interrupt_id)
+{
+ unsigned int core_pos;
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ return mmio_read_8(rdist_pcpu_base[core_pos] + GICR_IPRIORITYR
+ + interrupt_id);
+ } else {
+ return mmio_read_8(gicd_base_addr +
+ GICD_IPRIORITYR + interrupt_id);
+ }
+}
+
+void gicv3_set_ipriorityr(unsigned int interrupt_id,
+ unsigned int priority)
+{
+ unsigned int core_pos;
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ mmio_write_8(rdist_pcpu_base[core_pos] + GICR_IPRIORITYR
+ + interrupt_id, priority & GIC_PRI_MASK);
+ } else {
+ mmio_write_8(gicd_base_addr + GICD_IPRIORITYR + interrupt_id,
+ priority & GIC_PRI_MASK);
+ }
+}
+
+void gicv3_send_sgi(unsigned int sgi_id, unsigned int core_pos)
+{
+ unsigned long long aff0, aff1, aff2;
+ unsigned long long sgir, target_list;
+
+ assert(IS_SGI(sgi_id));
+ assert(core_pos < PLATFORM_CORE_COUNT);
+
+ assert(mpidr_list[core_pos] != UINT64_MAX);
+
+ /* Extract the affinity information */
+ aff0 = MPIDR_AFF_ID(mpidr_list[core_pos], 0);
+ aff1 = MPIDR_AFF_ID(mpidr_list[core_pos], 1);
+ aff2 = MPIDR_AFF_ID(mpidr_list[core_pos], 2);
+#ifndef AARCH32
+ unsigned long long aff3;
+ aff3 = MPIDR_AFF_ID(mpidr_list[core_pos], 3);
+#endif
+
+ /* Construct the SGI target list using Affinity 0 */
+ assert(aff0 < SGI_TARGET_MAX_AFF0);
+ target_list = 1 << aff0;
+
+ /* Construct the SGI target affinity */
+ sgir =
+#ifndef AARCH32
+ ((aff3 & SGI1R_AFF_MASK) << SGI1R_AFF3_SHIFT) |
+#endif
+ ((aff2 & SGI1R_AFF_MASK) << SGI1R_AFF2_SHIFT) |
+ ((aff1 & SGI1R_AFF_MASK) << SGI1R_AFF1_SHIFT) |
+ ((target_list & SGI1R_TARGET_LIST_MASK)
+ << SGI1R_TARGET_LIST_SHIFT);
+
+ /* Combine SGI target affinity with the SGI ID */
+ sgir |= ((sgi_id & SGI1R_INTID_MASK) << SGI1R_INTID_SHIFT);
+#ifndef AARCH32
+ write_icc_sgi1r(sgir);
+#else
+ write64_icc_sgi1r(sgir);
+#endif
+ isb();
+}
+
+void gicv3_set_intr_route(unsigned int interrupt_id,
+ unsigned int core_pos)
+{
+ unsigned long long route_affinity;
+
+ assert(gicd_base_addr);
+ assert(core_pos < PLATFORM_CORE_COUNT);
+ assert(mpidr_list[core_pos] != UINT64_MAX);
+
+ /* Routing information can be set only for SPIs */
+ assert(IS_SPI(interrupt_id));
+ route_affinity = mpidr_list[core_pos];
+
+ gicd_write_irouter(gicd_base_addr, interrupt_id, route_affinity);
+}
+
+unsigned int gicv3_get_isenabler(unsigned int interrupt_id)
+{
+ unsigned int core_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ return gicr_get_isenabler0(rdist_pcpu_base[core_pos], interrupt_id);
+ } else
+ return gicd_get_isenabler(gicd_base_addr, interrupt_id);
+}
+
+void gicv3_set_isenabler(unsigned int interrupt_id)
+{
+ unsigned int core_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ gicr_set_isenabler0(rdist_pcpu_base[core_pos], interrupt_id);
+ } else
+ gicd_set_isenabler(gicd_base_addr, interrupt_id);
+}
+
+void gicv3_set_icenabler(unsigned int interrupt_id)
+{
+ unsigned int core_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ gicr_set_icenabler0(rdist_pcpu_base[core_pos], interrupt_id);
+ } else
+ gicd_set_icenabler(gicd_base_addr, interrupt_id);
+}
+
+unsigned int gicv3_get_ispendr(unsigned int interrupt_id)
+{
+ unsigned int ispendr;
+ unsigned int bit_pos, core_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_VALID_INTR_ID(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ ispendr = gicr_read_ispendr0(rdist_pcpu_base[core_pos]);
+ } else
+ ispendr = gicd_read_ispendr(gicd_base_addr, interrupt_id);
+
+ bit_pos = interrupt_id % (1 << ISPENDR_SHIFT);
+ return !!(ispendr & (1 << bit_pos));
+}
+
+void gicv3_set_icpendr(unsigned int interrupt_id)
+{
+ unsigned int core_pos;
+
+ assert(gicd_base_addr);
+ assert(IS_SPI(interrupt_id) || IS_PPI(interrupt_id));
+
+ if (interrupt_id < MIN_SPI_ID) {
+ core_pos = platform_get_core_pos(read_mpidr_el1());
+ assert(rdist_pcpu_base[core_pos]);
+ gicr_set_icpendr0(rdist_pcpu_base[core_pos], interrupt_id);
+
+ } else
+ gicd_set_icpendr(gicd_base_addr, interrupt_id);
+}
+
+void gicv3_probe_redistif_addr(void)
+{
+ unsigned long long typer_val;
+ uintptr_t rdistif_base;
+ unsigned long long affinity;
+ unsigned int core_pos = platform_get_core_pos(read_mpidr_el1());
+
+ assert(gicr_base_addr);
+
+ /*
+ * Return if the re-distributor base address is already populated
+ * for this core.
+ */
+ if (rdist_pcpu_base[core_pos])
+ return;
+
+ /* Iterate over the GICR frames and find the matching frame*/
+ rdistif_base = gicr_base_addr;
+ affinity = gic_typer_affinity_from_mpidr(read_mpidr_el1() & MPIDR_AFFINITY_MASK);
+ do {
+ typer_val = gicr_read_typer(rdistif_base);
+ if (affinity == ((typer_val >> TYPER_AFF_VAL_SHIFT) & TYPER_AFF_VAL_MASK)) {
+ rdist_pcpu_base[core_pos] = rdistif_base;
+ mpidr_list[core_pos] = read_mpidr_el1() & MPIDR_AFFINITY_MASK;
+ return;
+ }
+ rdistif_base += (1 << GICR_PCPUBASE_SHIFT);
+ } while (!(typer_val & TYPER_LAST_BIT));
+
+ ERROR("Re-distributor address not found for core %d\n", core_pos);
+ panic();
+}
+
+void gicv3_setup_distif(void)
+{
+ unsigned int gicd_ctlr;
+
+ assert(gicd_base_addr);
+
+ /* Check for system register support */
+#ifndef AARCH32
+ assert(read_id_aa64pfr0_el1() &
+ (ID_AA64PFR0_GIC_MASK << ID_AA64PFR0_GIC_SHIFT));
+#else
+ assert(read_id_pfr1() & (ID_PFR1_GIC_MASK << ID_PFR1_GIC_SHIFT));
+#endif
+
+ /* Assert that system register access is enabled */
+ assert(is_sre_enabled());
+
+ /* Enable the forwarding of interrupts to CPU interface */
+ gicd_ctlr = gicd_read_ctlr(gicd_base_addr);
+
+ /* Assert ARE_NS bit in GICD */
+ assert(gicd_ctlr & (GICD_CTLR_ARE_NS_MASK << GICD_CTLR_ARE_NS_SHIFT));
+
+ gicd_ctlr |= GICD_CTLR_ENABLE_GRP1A;
+ gicd_write_ctlr(gicd_base_addr, gicd_ctlr);
+}
+
+void gicv3_init(uintptr_t gicr_base, uintptr_t gicd_base)
+{
+ assert(gicr_base);
+ assert(gicd_base);
+
+ gicr_base_addr = gicr_base;
+ gicd_base_addr = gicd_base;
+}
diff --git a/drivers/arm/pl011/aarch32/pl011_console.S b/drivers/arm/pl011/aarch32/pl011_console.S
new file mode 100644
index 0000000..96da6f9
--- /dev/null
+++ b/drivers/arm/pl011/aarch32/pl011_console.S
@@ -0,0 +1,275 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <console.h>
+#include <pl011.h>
+
+ .globl console_init
+ .globl console_putc
+ .globl console_getc
+ .globl console_try_getc
+ .globl console_flush
+ .globl console_core_init
+ .globl console_core_putc
+ .globl console_core_getc
+ .globl console_core_flush
+
+ /*
+ * The console base is in the data section and not in .bss
+ * even though it is zero-init. In particular, this allows
+ * the console functions to start using this variable before
+ * the runtime memory is initialized for images which do not
+ * need to copy the .data section from ROM to RAM.
+ */
+ .section .data.console_base
+ .align 2
+console_base: .word 0x0
+
+ /* -----------------------------------------------
+ * int console_init(uintptr_t base_addr,
+ * unsigned int uart_clk, unsigned int baud_rate)
+ * Function to initialize the console without a
+ * C Runtime to print debug information. It saves
+ * the console base to the data section.
+ * In: r0 - Console base address
+ * r1 - Uart clock in Hz
+ * r2 - Baud rate
+ * Out: r0 - Return 1 on success, 0 on error.
+ * Clobber list : r1 - r3
+ * -----------------------------------------------
+ */
+func console_init
+ ldr r3, =console_base
+ str r0, [r3]
+ b console_core_init
+endfunc console_init
+
+ /* -----------------------------------------------
+ * int console_core_init(uintptr_t base_addr,
+ * unsigned int uart_clk, unsigned int baud_rate)
+ * Function to initialize the console without a
+ * C Runtime to print debug information. This
+ * function will be accessed by console_init and
+ * crash reporting.
+ * In: r0 - Console base address
+ * r1 - Uart clock in Hz
+ * r2 - Baud rate
+ * Out: r0 - Return 1 on success, 0 on error.
+ * Clobber list : r1 - r3
+ * -----------------------------------------------
+ */
+func console_core_init
+ /* Check the input base address */
+ cmp r0, #0
+ beq core_init_fail
+ /* Check baud rate and uart clock for sanity */
+ cmp r1, #0
+ beq core_init_fail
+ cmp r2, #0
+ beq core_init_fail
+ /* Disable the UART before initialization */
+ ldr r3, [r0, #UARTCR]
+ bic r3, r3, #PL011_UARTCR_UARTEN
+ str r3, [r0, #UARTCR]
+ /* Program the baudrate */
+ /* Divisor = (Uart clock * 4) / baudrate */
+ lsl r1, r1, #2
+ udiv r2, r1, r2
+ /* IBRD = Divisor >> 6 */
+ lsr r1, r2, #6
+ /* Write the IBRD */
+ str r1, [r0, #UARTIBRD]
+ /* FBRD = Divisor & 0x3F */
+ and r1, r2, #0x3f
+ /* Write the FBRD */
+ str r1, [r0, #UARTFBRD]
+ mov r1, #PL011_LINE_CONTROL
+ str r1, [r0, #UARTLCR_H]
+ /* Clear any pending errors */
+ mov r1, #0
+ str r1, [r0, #UARTECR]
+ /* Enable tx, rx, and uart overall */
+ ldr r1, =(PL011_UARTCR_RXE | PL011_UARTCR_TXE | PL011_UARTCR_UARTEN)
+ str r1, [r0, #UARTCR]
+ mov r0, #1
+ bx lr
+core_init_fail:
+ mov r0, #0
+ bx lr
+endfunc console_core_init
+
+ /* ---------------------------------------------
+ * int console_putc(int c)
+ * Function to output a character over the
+ * console. It returns the character printed on
+ * success or an error code.
+ * In : r0 - Character to be printed
+ * Out : r0 - Input character or error code.
+ * Clobber list : r1, r2
+ * ---------------------------------------------
+ */
+func console_putc
+ ldr r1, =console_base
+ ldr r1, [r1]
+ b console_core_putc
+endfunc console_putc
+
+ /* --------------------------------------------------------
+ * int console_core_putc(int c, uintptr_t base_addr)
+ * Function to output a character over the console. It
+ * returns the character printed on success or an error
+ * code.
+ * In : r0 - Character to be printed
+ * r1 - Console base address
+ * Out : r0 - Input character or error code.
+ * Clobber list : r2
+ * --------------------------------------------------------
+ */
+func console_core_putc
+ /* Check the input parameter */
+ cmp r1, #0
+ beq putc_error
+ /* Prepend '\r' to '\n' */
+ cmp r0, #0xA
+ bne 2f
+1:
+ /* Check if the transmit FIFO is full */
+ ldr r2, [r1, #UARTFR]
+ tst r2, #PL011_UARTFR_TXFF
+ bne 1b
+ mov r2, #0xD
+ str r2, [r1, #UARTDR]
+2:
+ /* Check if the transmit FIFO is full */
+ ldr r2, [r1, #UARTFR]
+ tst r2, #PL011_UARTFR_TXFF
+ bne 2b
+
+ /* Only write 8 bits */
+ and r0, r0, #0xFF
+ str r0, [r1, #UARTDR]
+ bx lr
+putc_error:
+ mov r0, #ERROR_NO_VALID_CONSOLE
+ bx lr
+endfunc console_core_putc
+
+ /* ---------------------------------------------
+ * int console_getc(void)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code on error. This function is
+ * blocking, it waits until there is an
+ * available character to return.
+ * Out : r0 - Return character or error code.
+ * Clobber list : r0 - r3
+ * ---------------------------------------------
+ */
+func console_getc
+ ldr r2, =console_base
+ ldr r2, [r2]
+ mov r3, lr
+
+ /* Loop until it returns a character or an error. */
+1: mov r0, r2
+ bl console_core_getc
+ cmp r0, #ERROR_NO_PENDING_CHAR
+ beq 1b
+
+ bx r3
+endfunc console_getc
+
+ /* ---------------------------------------------
+ * int console_try_getc(void)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code on error. This function is
+ * non-blocking, it returns immediately.
+ * Out : r0 - Return character or error code.
+ * Clobber list : r0, r1
+ * ---------------------------------------------
+ */
+func console_try_getc
+ ldr r0, =console_base
+ ldr r0, [r0]
+ b console_core_getc
+endfunc console_try_getc
+
+ /* ---------------------------------------------
+ * int console_core_getc(uintptr_t base_addr)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code.
+ * In : r0 - Console base address
+ * Out : r0 - Return character or error code.
+ * Clobber list : r0, r1
+ * ---------------------------------------------
+ */
+func console_core_getc
+ cmp r0, #0
+ beq getc_error
+
+ /* Check if the receive FIFO is empty */
+ ldr r1, [r0, #UARTFR]
+ tst r1, #PL011_UARTFR_RXFE
+ bne getc_empty
+
+ /* Read a character from the FIFO */
+ ldr r1, [r0, #UARTDR]
+ /* Mask out error flags */
+ and r0, r1, #0xFF
+ bx lr
+
+getc_empty:
+ mov r0, #ERROR_NO_PENDING_CHAR
+ bx lr
+getc_error:
+ mov r0, #ERROR_NO_VALID_CONSOLE
+ bx lr
+endfunc console_core_getc
+
+ /* ---------------------------------------------
+ * int console_flush(void)
+ * Function to force a write of all buffered
+ * data that hasn't been output. It returns 0
+ * upon successful completion, otherwise it
+ * returns an error code.
+ * Out: r0 - Error code or 0.
+ * Clobber list : r0, r1
+ * ---------------------------------------------
+ */
+func console_flush
+ ldr r0, =console_base
+ ldr r0, [r0]
+ b console_core_flush
+endfunc console_flush
+
+ /* ---------------------------------------------
+ * int console_core_flush(uintptr_t base_addr)
+ * Function to force a write of all buffered
+ * data that hasn't been output.
+ * In : r0 - Console base address
+ * Out : r0 - Error code or 0.
+ * Clobber list : r0, r1
+ * ---------------------------------------------
+ */
+func console_core_flush
+ cmp r0, #0
+ beq flush_error
+
+1:
+ /* Loop while the transmit FIFO is busy */
+ ldr r1, [r0, #UARTFR]
+ tst r1, #PL011_UARTFR_BUSY
+ bne 1b
+
+ mov r0, #0
+ bx lr
+flush_error:
+ mov r0, #ERROR_NO_VALID_CONSOLE
+ bx lr
+endfunc console_core_flush
diff --git a/drivers/arm/pl011/aarch64/pl011_console.S b/drivers/arm/pl011/aarch64/pl011_console.S
new file mode 100644
index 0000000..d87982a
--- /dev/null
+++ b/drivers/arm/pl011/aarch64/pl011_console.S
@@ -0,0 +1,264 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <console.h>
+#include <pl011.h>
+
+ .globl console_init
+ .globl console_putc
+ .globl console_getc
+ .globl console_try_getc
+ .globl console_flush
+ .globl console_core_init
+ .globl console_core_putc
+ .globl console_core_getc
+ .globl console_core_flush
+
+ /*
+ * The console base is in the data section and not in .bss
+ * even though it is zero-init. In particular, this allows
+ * the console functions to start using this variable before
+ * the runtime memory is initialized for images which do not
+ * need to copy the .data section from ROM to RAM.
+ */
+ .section .data.console_base
+ .align 3
+console_base: .quad 0x0
+
+ /* -----------------------------------------------
+ * int console_init(uintptr_t base_addr,
+ * unsigned int uart_clk, unsigned int baud_rate)
+ * Function to initialize the console without a
+ * C Runtime to print debug information. It saves
+ * the console base to the data section.
+ * In: x0 - Console base address
+ * w1 - Uart clock in Hz
+ * w2 - Baud rate
+ * Out: w0 - Return 1 on success, 0 on error.
+ * Clobber list : x1 - x3
+ * -----------------------------------------------
+ */
+func console_init
+ adrp x3, console_base
+ str x0, [x3, :lo12:console_base]
+ b console_core_init
+endfunc console_init
+
+ /* -----------------------------------------------
+ * int console_core_init(uintptr_t base_addr,
+ * unsigned int uart_clk, unsigned int baud_rate)
+ * Function to initialize the console without a
+ * C Runtime to print debug information. This
+ * function will be accessed by console_init and
+ * crash reporting.
+ * In: x0 - Console base address
+ * w1 - Uart clock in Hz
+ * w2 - Baud rate
+ * Out: w0 - Return 1 on success, 0 on error.
+ * Clobber list : x1 - x3
+ * -----------------------------------------------
+ */
+func console_core_init
+ /* Check the input base address */
+ cbz x0, init_fail
+ /* Check baud rate and uart clock for sanity */
+ cbz w1, init_fail
+ cbz w2, init_fail
+ /* Disable uart before programming */
+ ldr w3, [x0, #UARTCR]
+ bic w3, w3, #PL011_UARTCR_UARTEN
+ str w3, [x0, #UARTCR]
+ /* Program the baudrate */
+ /* Divisor = (Uart clock * 4) / baudrate */
+ lsl w1, w1, #2
+ udiv w2, w1, w2
+ /* IBRD = Divisor >> 6 */
+ lsr w1, w2, #6
+ /* Write the IBRD */
+ str w1, [x0, #UARTIBRD]
+ /* FBRD = Divisor & 0x3F */
+ and w1, w2, #0x3f
+ /* Write the FBRD */
+ str w1, [x0, #UARTFBRD]
+ mov w1, #PL011_LINE_CONTROL
+ str w1, [x0, #UARTLCR_H]
+ /* Clear any pending errors */
+ str wzr, [x0, #UARTECR]
+ /* Enable tx, rx, and uart overall */
+ mov w1, #(PL011_UARTCR_RXE | PL011_UARTCR_TXE | PL011_UARTCR_UARTEN)
+ str w1, [x0, #UARTCR]
+ mov w0, #1
+ ret
+init_fail:
+ mov w0, wzr
+ ret
+endfunc console_core_init
+
+ /* ---------------------------------------------
+ * int console_putc(int c)
+ * Function to output a character over the
+ * console. It returns the character printed on
+ * success or an error code.
+ * In : x0 - Character to be printed
+ * Out : w0 - Input character or error code.
+ * Clobber list : x1, x2
+ * ---------------------------------------------
+ */
+func console_putc
+ adrp x1, console_base
+ ldr x1, [x1, :lo12:console_base]
+ b console_core_putc
+endfunc console_putc
+
+ /* ---------------------------------------------
+ * int console_core_putc(int c, uintptr_t base_addr)
+ * Function to output a character over the console. It
+ * returns the character printed on success or an error
+ * code.
+ * In : w0 - Character to be printed
+ * x1 - Console base address
+ * Out : w0 - Input character or error code.
+ * Clobber list : x2
+ * ---------------------------------------------
+ */
+func console_core_putc
+ /* Check the input parameter */
+ cbz x1, putc_error
+ /* Prepend '\r' to '\n' */
+ cmp w0, #0xA
+ b.ne 2f
+1:
+ /* Check if the transmit FIFO is full */
+ ldr w2, [x1, #UARTFR]
+ tbnz w2, #PL011_UARTFR_TXFF_BIT, 1b
+ mov w2, #0xD
+ str w2, [x1, #UARTDR]
+2:
+ /* Check if the transmit FIFO is full */
+ ldr w2, [x1, #UARTFR]
+ tbnz w2, #PL011_UARTFR_TXFF_BIT, 2b
+
+ /* Only write 8 bits */
+ and w0, w0, #0xFF
+ str w0, [x1, #UARTDR]
+ ret
+putc_error:
+ mov w0, #ERROR_NO_VALID_CONSOLE
+ ret
+endfunc console_core_putc
+
+ /* ---------------------------------------------
+ * int console_getc(void)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code on error. This function is
+ * blocking, it waits until there is an
+ * available character to return.
+ * Out : w0 - Return character or error code.
+ * Clobber list : x0 - x3
+ * ---------------------------------------------
+ */
+func console_getc
+ adrp x2, console_base
+ ldr x2, [x2, :lo12:console_base]
+ mov x3, x30
+
+ /* Loop until it returns a character or an error. */
+1: mov x0, x2
+ bl console_core_getc
+ cmp w0, #ERROR_NO_PENDING_CHAR
+ b.eq 1b
+
+ ret x3
+endfunc console_getc
+
+ /* ---------------------------------------------
+ * int console_try_getc(void)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code on error. This function is
+ * non-blocking, it returns immediately.
+ * Out : w0 - Return character or error code.
+ * Clobber list : x0, x1
+ * ---------------------------------------------
+ */
+func console_try_getc
+ adrp x0, console_base
+ ldr x0, [x0, :lo12:console_base]
+ b console_core_getc
+endfunc console_try_getc
+
+ /* ---------------------------------------------
+ * int console_core_getc(uintptr_t base_addr)
+ * Function to get a character from the console.
+ * It returns the character grabbed on success
+ * or an error code.
+ * In : x0 - Console base address
+ * Out : w0 - Return character or error code.
+ * Clobber list : x0, x1
+ * ---------------------------------------------
+ */
+func console_core_getc
+ cbz x0, getc_error
+
+ /* Check if the receive FIFO is empty */
+ ldr w1, [x0, #UARTFR]
+ tbnz w1, #PL011_UARTFR_RXFE_BIT, getc_empty
+
+ /* Read a character from the FIFO */
+ ldr w0, [x0, #UARTDR]
+ /* Mask out error flags */
+ and w0, w0, #0xFF
+ ret
+
+getc_empty:
+ mov w0, #ERROR_NO_PENDING_CHAR
+ ret
+getc_error:
+ mov w0, #ERROR_NO_VALID_CONSOLE
+ ret
+endfunc console_core_getc
+
+ /* ---------------------------------------------
+ * int console_flush(void)
+ * Function to force a write of all buffered
+ * data that hasn't been output. It returns 0
+ * upon successful completion, otherwise it
+ * returns an error code.
+ * Out: w0 - Error code or 0.
+ * Clobber list : x0, x1
+ * ---------------------------------------------
+ */
+func console_flush
+ adrp x0, console_base
+ ldr x0, [x0, :lo12:console_base]
+ b console_core_flush
+endfunc console_flush
+
+ /* ---------------------------------------------
+ * int console_core_flush(uintptr_t base_addr)
+ * Function to force a write of all buffered
+ * data that hasn't been output.
+ * In : x0 - Console base address
+ * Out : w0 - Error code or 0.
+ * Clobber list : x0, x1
+ * ---------------------------------------------
+ */
+func console_core_flush
+ cbz x0, flush_error
+
+1:
+ /* Loop until the transmit FIFO is empty */
+ ldr w1, [x0, #UARTFR]
+ tbnz w1, #PL011_UARTFR_BUSY_BIT, 1b
+
+ mov w0, wzr
+ ret
+flush_error:
+ mov w0, #ERROR_NO_VALID_CONSOLE
+ ret
+endfunc console_core_flush
diff --git a/drivers/arm/sp805/sp805.c b/drivers/arm/sp805/sp805.c
new file mode 100644
index 0000000..72a668d
--- /dev/null
+++ b/drivers/arm/sp805/sp805.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <mmio.h>
+#include <platform_def.h>
+#include <sp805.h>
+#include <stdint.h>
+
+static inline uint32_t sp805_read_wdog_load(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_LOAD_OFF);
+}
+
+static inline void sp805_write_wdog_load(unsigned long base, uint32_t value)
+{
+ assert(base);
+ mmio_write_32(base + SP805_WDOG_LOAD_OFF, value);
+}
+
+static inline uint32_t sp805_read_wdog_value(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_VALUE_0FF);
+}
+
+static inline uint32_t sp805_read_wdog_ctrl(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_CTRL_OFF) & SP805_WDOG_CTRL_MASK;
+}
+
+static inline void sp805_write_wdog_ctrl(unsigned long base, uint32_t value)
+{
+ assert(base);
+ /* Not setting reserved bits */
+ assert(!(value & ~SP805_WDOG_CTRL_MASK));
+ mmio_write_32(base + SP805_WDOG_CTRL_OFF, value);
+}
+
+static inline void sp805_write_wdog_int_clr(unsigned long base, uint32_t value)
+{
+ assert(base);
+ mmio_write_32(base + SP805_WDOG_INT_CLR_OFF, value);
+}
+
+static inline uint32_t sp805_read_wdog_ris(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_RIS_OFF) & SP805_WDOG_RIS_MASK;
+}
+
+static inline uint32_t sp805_read_wdog_mis(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_MIS_OFF) & SP805_WDOG_MIS_MASK;
+}
+
+static inline uint32_t sp805_read_wdog_lock(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_LOCK_OFF);
+}
+
+static inline void sp805_write_wdog_lock(unsigned long base, uint32_t value)
+{
+ assert(base);
+ mmio_write_32(base + SP805_WDOG_LOCK_OFF, value);
+}
+
+static inline uint32_t sp805_read_wdog_itcr(unsigned long base)
+{
+ assert(base);
+ return mmio_read_32(base + SP805_WDOG_ITCR_OFF) & SP805_WDOG_ITCR_MASK;
+}
+
+static inline void sp805_write_wdog_itcr(unsigned long base, uint32_t value)
+{
+ assert(base);
+ /* Not setting reserved bits */
+ assert(!(value & ~SP805_WDOG_ITCR_MASK));
+ mmio_write_32(base + SP805_WDOG_ITCR_OFF, value);
+}
+
+static inline void sp805_write_wdog_itop(unsigned long base, uint32_t value)
+{
+ assert(base);
+ /* Not setting reserved bits */
+ assert(!(value & ~SP805_WDOG_ITOP_MASK));
+ mmio_write_32(base + SP805_WDOG_ITOP_OFF, value);
+}
+
+static inline uint32_t sp805_read_wdog_periph_id(unsigned long base, unsigned int id)
+{
+ assert(base);
+ assert(id < 4);
+ return mmio_read_32(base + SP805_WDOG_PERIPH_ID_OFF + (id << 2));
+}
+
+static inline uint32_t sp805_read_wdog_pcell_id(unsigned long base, unsigned int id)
+{
+ assert(base);
+ assert(id < 4);
+ return mmio_read_32(base + SP805_WDOG_PCELL_ID_OFF + (id << 2));
+}
+
+void sp805_wdog_start(uint32_t wdog_cycles)
+{
+ /* Unlock to access the watchdog registers */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, SP805_WDOG_UNLOCK_ACCESS);
+
+ /* Write the number of cycles needed */
+ sp805_write_wdog_load(SP805_WDOG_BASE, wdog_cycles);
+
+ /* Enable reset interrupt and watchdog interrupt on expiry */
+ sp805_write_wdog_ctrl(SP805_WDOG_BASE,
+ SP805_WDOG_CTRL_RESEN | SP805_WDOG_CTRL_INTEN);
+
+ /* Lock registers so that they can't be accidently overwritten */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, 0x0);
+}
+
+void sp805_wdog_stop(void)
+{
+ /* Unlock to access the watchdog registers */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, SP805_WDOG_UNLOCK_ACCESS);
+
+ /* Clearing INTEN bit stops the counter */
+ sp805_write_wdog_ctrl(SP805_WDOG_BASE, 0x00);
+
+ /* Lock registers so that they can't be accidently overwritten */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, 0x0);
+}
+
+void sp805_wdog_refresh(void)
+{
+ /* Unlock to access the watchdog registers */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, SP805_WDOG_UNLOCK_ACCESS);
+
+ /*
+ * Write of any value to WdogIntClr clears interrupt and reloads
+ * the counter from the value in WdogLoad Register.
+ */
+ sp805_write_wdog_int_clr(SP805_WDOG_BASE, 1);
+
+ /* Lock registers so that they can't be accidently overwritten */
+ sp805_write_wdog_lock(SP805_WDOG_BASE, 0x0);
+}
diff --git a/drivers/arm/timer/private_timer.c b/drivers/arm/timer/private_timer.c
new file mode 100644
index 0000000..c629448
--- /dev/null
+++ b/drivers/arm/timer/private_timer.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <platform.h>
+
+/*******************************************************************************
+ * Data structure to keep track of per-cpu secure generic timer context across
+ * power management operations.
+ ******************************************************************************/
+typedef struct timer_context {
+ uint64_t cval;
+ uint32_t ctl;
+} timer_context_t;
+
+static timer_context_t pcpu_timer_context[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * This function initializes the generic timer to fire every `timeo` ms.
+ ******************************************************************************/
+void private_timer_start(unsigned long timeo)
+{
+ uint64_t cval, freq;
+ uint32_t ctl = 0;
+
+ /* Any previous pending timer activation will be disabled. */
+ cval = read_cntpct_el0();
+ freq = read_cntfrq_el0();
+ cval += (freq * timeo) / 1000;
+ write_cnthp_cval_el2(cval);
+
+ /* Enable the secure physical timer */
+ set_cntp_ctl_enable(ctl);
+ write_cnthp_ctl_el2(ctl);
+}
+
+/*******************************************************************************
+ * This function deasserts the timer interrupt prior to cpu power down
+ ******************************************************************************/
+void private_timer_stop(void)
+{
+ /* Disable the timer */
+ write_cnthp_ctl_el2(0);
+}
+
+/*******************************************************************************
+ * This function saves the timer context prior to cpu suspension
+ ******************************************************************************/
+void private_timer_save(void)
+{
+ uint32_t linear_id = platform_get_core_pos(read_mpidr_el1());
+
+ pcpu_timer_context[linear_id].cval = read_cnthp_cval_el2();
+ pcpu_timer_context[linear_id].ctl = read_cnthp_ctl_el2();
+ flush_dcache_range((uintptr_t) &pcpu_timer_context[linear_id],
+ sizeof(pcpu_timer_context[linear_id]));
+}
+
+/*******************************************************************************
+ * This function restores the timer context post cpu resummption
+ ******************************************************************************/
+void private_timer_restore(void)
+{
+ uint32_t linear_id = platform_get_core_pos(read_mpidr_el1());
+
+ write_cnthp_cval_el2(pcpu_timer_context[linear_id].cval);
+ write_cnthp_ctl_el2(pcpu_timer_context[linear_id].ctl);
+}
diff --git a/drivers/arm/timer/sp804.c b/drivers/arm/timer/sp804.c
new file mode 100644
index 0000000..de88cda
--- /dev/null
+++ b/drivers/arm/timer/sp804.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <arm_gic.h>
+#include <assert.h>
+#include <gic_v2.h>
+#include <mmio.h>
+#include <sp804.h>
+
+static unsigned int sp804_freq;
+static uintptr_t sp804_base;
+
+int sp804_timer_program(unsigned long time_out_ms)
+{
+ unsigned int load_val;
+ unsigned char ctrl_reg;
+
+ assert(sp804_base);
+ assert(time_out_ms);
+
+ /* Disable the timer */
+ ctrl_reg = mmio_read_8(sp804_base + SP804_CTRL_OFFSET);
+ ctrl_reg &= ~(TIMER_EN | INT_ENABLE);
+ mmio_write_8(sp804_base + SP804_CTRL_OFFSET, ctrl_reg);
+
+ /* Calculate the load value */
+ load_val = (sp804_freq * time_out_ms) / 1000;
+
+ /* Write the load value to sp804 timer */
+ mmio_write_32(sp804_base + SP804_LOAD_OFFSET, load_val);
+
+ /* Enable the timer */
+ ctrl_reg |= (TIMER_EN | INT_ENABLE);
+ mmio_write_8(sp804_base + SP804_CTRL_OFFSET, ctrl_reg);
+
+ return 0;
+}
+
+static void sp804_timer_disable(void)
+{
+ unsigned char ctrl_reg;
+
+ /*
+ * The interrupt line should be cleared prior to timer disable.
+ * Otherwise the interrupt line level decay from high to quiescent
+ * level is not quick enough which may trigger spurious interrupt.
+ * Write a dummy load value to sp804 timer to clear the interrupt.
+ */
+ mmio_write_32(sp804_base + SP804_LOAD_OFFSET, 0xffff);
+
+ /* De-assert the timer interrupt */
+ mmio_write_8(sp804_base + SP804_INT_CLR_OFFSET, 0x0);
+
+ /* Disable the timer */
+ ctrl_reg = mmio_read_8(sp804_base + SP804_CTRL_OFFSET);
+ ctrl_reg &= ~(TIMER_EN | INT_ENABLE);
+ mmio_write_8(sp804_base + SP804_CTRL_OFFSET, ctrl_reg);
+}
+
+int sp804_timer_cancel(void)
+{
+ assert(sp804_base);
+ sp804_timer_disable();
+ return 0;
+}
+
+int sp804_timer_handler(void)
+{
+ assert(sp804_base);
+ sp804_timer_disable();
+ return 0;
+}
+
+int sp804_timer_init(uintptr_t base_addr, unsigned int timer_freq)
+{
+ unsigned char ctrl_reg;
+
+ /* Check input parameters */
+ assert(base_addr && timer_freq);
+
+ /* Check for duplicate initialization */
+ assert(sp804_base == 0);
+
+ sp804_base = base_addr;
+ sp804_freq = timer_freq;
+
+ /*
+ * Configure the timer in one shot mode, pre-scalar divider to 1,
+ * timer counter width to 32 bits and un-mask the interrupt.
+ */
+ ctrl_reg = ONESHOT_MODE | TIMER_PRE_DIV1 | TIMER_SIZE;
+ mmio_write_8(sp804_base + SP804_CTRL_OFFSET, ctrl_reg);
+
+ return 0;
+}
diff --git a/drivers/arm/timer/system_timer.c b/drivers/arm/timer/system_timer.c
new file mode 100644
index 0000000..3415e41
--- /dev/null
+++ b/drivers/arm/timer/system_timer.c
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <arm_gic.h>
+#include <assert.h>
+#include <debug.h>
+#include <gic_v2.h>
+#include <irq.h>
+#include <mmio.h>
+#include <system_timer.h>
+
+static uintptr_t g_systimer_base;
+
+int program_systimer(unsigned long time_out_ms)
+{
+ unsigned int cntp_ctl;
+ unsigned long long count_val;
+ unsigned int freq;
+
+ /* Check timer base is initialised */
+ assert(g_systimer_base);
+
+ count_val = mmio_read_64(g_systimer_base + CNTPCT_LO);
+ freq = read_cntfrq_el0();
+ count_val += (freq * time_out_ms) / 1000;
+ mmio_write_64(g_systimer_base + CNTP_CVAL_LO, count_val);
+
+ /* Enable the timer */
+ cntp_ctl = mmio_read_32(g_systimer_base + CNTP_CTL);
+ set_cntp_ctl_enable(cntp_ctl);
+ clr_cntp_ctl_imask(cntp_ctl);
+ mmio_write_32(g_systimer_base + CNTP_CTL, cntp_ctl);
+
+ /*
+ * Ensure that we have programmed a timer interrupt for a time in
+ * future. Else we will have to wait for the systimer to rollover
+ * for the interrupt to fire (which is 64 years).
+ */
+ if (count_val < mmio_read_64(g_systimer_base + CNTPCT_LO))
+ panic();
+
+ VERBOSE("%s : interrupt requested at sys_counter: %lld "
+ "time_out_ms: %ld\n", __func__, count_val, time_out_ms);
+
+ return 0;
+}
+
+static void disable_systimer(void)
+{
+ uint32_t val;
+
+ /* Check timer base is initialised */
+ assert(g_systimer_base);
+
+ /* Deassert and disable the timer interrupt */
+ val = 0;
+ set_cntp_ctl_imask(val);
+ mmio_write_32(g_systimer_base + CNTP_CTL, val);
+}
+
+int cancel_systimer(void)
+{
+ disable_systimer();
+ return 0;
+}
+
+int handler_systimer(void)
+{
+ disable_systimer();
+ return 0;
+}
+
+int init_systimer(uintptr_t systimer_base)
+{
+ /* Check timer base is not initialised */
+ assert(!g_systimer_base);
+
+ g_systimer_base = systimer_base;
+
+ /* Disable the timer as the reset value is unknown */
+ disable_systimer();
+
+ /* Initialise CVAL to zero */
+ mmio_write_64(g_systimer_base + CNTP_CVAL_LO, 0);
+
+ return 0;
+}
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
new file mode 100644
index 0000000..8b5af6e
--- /dev/null
+++ b/drivers/io/io_fip.c
@@ -0,0 +1,334 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <firmware_image_package.h>
+#include <image_loader.h>
+#include <io_driver.h>
+#include <io_fip.h>
+#include <io_storage.h>
+#include <platform.h>
+#include <platform_def.h>
+#include <stdint.h>
+#include <string.h>
+#include <uuid.h>
+#include <uuid_utils.h>
+
+
+typedef struct {
+ unsigned int file_pos;
+ fip_toc_entry_t entry;
+} file_state_t;
+
+static file_state_t current_file = {0};
+static uintptr_t backend_dev_handle;
+static uintptr_t backend_image_spec;
+
+
+/* Firmware Image Package driver functions */
+static int fip_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity);
+static int fip_file_len(io_entity_t *entity, size_t *length);
+static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+ size_t *length_read);
+static int fip_file_close(io_entity_t *entity);
+static int fip_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params);
+static int fip_dev_close(io_dev_info_t *dev_info);
+
+
+/* TODO: We could check version numbers or do a package checksum? */
+static inline int is_valid_header(fip_toc_header_t *header)
+{
+ if ((header->name == TOC_HEADER_NAME) && (header->serial_number != 0)) {
+ return 1;
+ } else {
+ return 0;
+ }
+}
+
+
+/* Identify the device type as a virtual driver */
+io_type_t device_type_fip(void)
+{
+ return IO_TYPE_FIRMWARE_IMAGE_PACKAGE;
+}
+
+
+static const io_dev_connector_t fip_dev_connector = {
+ .dev_open = fip_dev_open
+};
+
+
+static const io_dev_funcs_t fip_dev_funcs = {
+ .type = device_type_fip,
+ .open = fip_file_open,
+ .seek = NULL,
+ .size = fip_file_len,
+ .read = fip_file_read,
+ .write = NULL,
+ .close = fip_file_close,
+ .dev_init = fip_dev_init,
+ .dev_close = fip_dev_close,
+};
+
+
+/* No state associated with this device so structure can be const */
+static const io_dev_info_t fip_dev_info = {
+ .funcs = &fip_dev_funcs,
+ .info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the FIP device */
+static int fip_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+ io_dev_info_t **dev_info)
+{
+ assert(dev_info != NULL);
+ *dev_info = (io_dev_info_t *)&fip_dev_info; /* cast away const */
+
+ return IO_SUCCESS;
+}
+
+
+/* Do some basic package checks. */
+static int fip_dev_init(io_dev_info_t *dev_info, const uintptr_t init_params)
+{
+ int result = IO_FAIL;
+ unsigned int image_id = (unsigned int)init_params;
+ uintptr_t backend_handle;
+ fip_toc_header_t header;
+ size_t bytes_read;
+
+ /* Obtain a reference to the image by querying the platform layer */
+ result = plat_get_image_source(image_id, &backend_dev_handle,
+ &backend_image_spec);
+ if (result != IO_SUCCESS) {
+ WARN("Failed to obtain reference to image id=%u (%i)\n",
+ image_id, result);
+ result = IO_FAIL;
+ goto fip_dev_init_exit;
+ }
+
+ /* Attempt to access the FIP image */
+ result = io_open(backend_dev_handle, backend_image_spec,
+ &backend_handle);
+ if (result != IO_SUCCESS) {
+ WARN("Failed to access image id=%u (%i)\n", image_id, result);
+ result = IO_FAIL;
+ goto fip_dev_init_exit;
+ }
+
+ result = io_read(backend_handle, (uintptr_t)&header, sizeof(header),
+ &bytes_read);
+ if (result == IO_SUCCESS) {
+ if (!is_valid_header(&header)) {
+ WARN("Firmware Image Package header check failed.\n");
+ result = IO_FAIL;
+ } else {
+ VERBOSE("FIP header looks OK.\n");
+ }
+ }
+
+ io_close(backend_handle);
+
+ fip_dev_init_exit:
+ return result;
+}
+
+/* Close a connection to the FIP device */
+static int fip_dev_close(io_dev_info_t *dev_info)
+{
+ /* TODO: Consider tracking open files and cleaning them up here */
+
+ /* Clear the backend. */
+ backend_dev_handle = (uintptr_t)NULL;
+ backend_image_spec = (uintptr_t)NULL;
+
+ return IO_SUCCESS;
+}
+
+
+/* Open a file for access from package. */
+static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity)
+{
+ int result = IO_FAIL;
+ uintptr_t backend_handle;
+ const io_uuid_spec_t *uuid_spec = (io_uuid_spec_t *)spec;
+ size_t bytes_read;
+ int found_file = 0;
+
+ assert(uuid_spec != NULL);
+ assert(entity != NULL);
+
+ /* Can only have one file open at a time for the moment. We need to
+ * track state like file cursor position. We know the header lives at
+ * offset zero, so this entry should never be zero for an active file.
+ * When the system supports dynamic memory allocation we can allow more
+ * than one open file at a time if needed.
+ */
+ if (current_file.entry.offset_address != 0) {
+ WARN("fip_file_open : Only one open file at a time.\n");
+ return IO_RESOURCES_EXHAUSTED;
+ }
+
+ /* Attempt to access the FIP image */
+ result = io_open(backend_dev_handle, backend_image_spec,
+ &backend_handle);
+ if (result != IO_SUCCESS) {
+ WARN("Failed to open Firmware Image Package (%i)\n", result);
+ result = IO_FAIL;
+ goto fip_file_open_exit;
+ }
+
+ /* Seek past the FIP header into the Table of Contents */
+ result = io_seek(backend_handle, IO_SEEK_SET, sizeof(fip_toc_header_t));
+ if (result != IO_SUCCESS) {
+ WARN("fip_file_open: failed to seek\n");
+ result = IO_FAIL;
+ goto fip_file_open_close;
+ }
+
+ found_file = 0;
+ do {
+ result = io_read(backend_handle,
+ (uintptr_t)¤t_file.entry,
+ sizeof(current_file.entry),
+ &bytes_read);
+ if (result == IO_SUCCESS) {
+ if (uuid_equal(¤t_file.entry.uuid,
+ &uuid_spec->uuid)) {
+ found_file = 1;
+ break;
+ }
+ } else {
+ WARN("Failed to read FIP (%i)\n", result);
+ goto fip_file_open_close;
+ }
+ } while (!is_uuid_null(¤t_file.entry.uuid));
+
+ if (found_file == 1) {
+ /* All fine. Update entity info with file state and return. Set
+ * the file position to 0. The 'current_file.entry' holds the
+ * base and size of the file.
+ */
+ current_file.file_pos = 0;
+ entity->info = (uintptr_t)¤t_file;
+ } else {
+ /* Did not find the file in the FIP. */
+ current_file.entry.offset_address = 0;
+ result = IO_FAIL;
+ }
+
+ fip_file_open_close:
+ io_close(backend_handle);
+
+ fip_file_open_exit:
+ return result;
+}
+
+
+/* Return the size of a file in package */
+static int fip_file_len(io_entity_t *entity, size_t *length)
+{
+ assert(entity != NULL);
+ assert(length != NULL);
+
+ *length = ((file_state_t *)entity->info)->entry.size;
+
+ return IO_SUCCESS;
+}
+
+
+/* Read data from a file in package */
+static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+ size_t *length_read)
+{
+ int result = IO_FAIL;
+ file_state_t *fp;
+ size_t file_offset;
+ size_t bytes_read;
+ uintptr_t backend_handle;
+
+ assert(entity != NULL);
+ assert(buffer != (uintptr_t)NULL);
+ assert(length_read != NULL);
+ assert(entity->info != (uintptr_t)NULL);
+
+ /* Open the backend, attempt to access the blob image */
+ result = io_open(backend_dev_handle, backend_image_spec,
+ &backend_handle);
+ if (result != IO_SUCCESS) {
+ WARN("Failed to open FIP (%i)\n", result);
+ result = IO_FAIL;
+ goto fip_file_read_exit;
+ }
+
+ fp = (file_state_t *)entity->info;
+
+ /* Seek to the position in the FIP where the payload lives */
+ file_offset = fp->entry.offset_address + fp->file_pos;
+ result = io_seek(backend_handle, IO_SEEK_SET, file_offset);
+ if (result != IO_SUCCESS) {
+ WARN("fip_file_read: failed to seek\n");
+ result = IO_FAIL;
+ goto fip_file_read_close;
+ }
+
+ result = io_read(backend_handle, buffer, length, &bytes_read);
+ if (result != IO_SUCCESS) {
+ /* We cannot read our data. Fail. */
+ WARN("Failed to read payload (%i)\n", result);
+ result = IO_FAIL;
+ goto fip_file_read_close;
+ } else {
+ /* Set caller length and new file position. */
+ *length_read = bytes_read;
+ fp->file_pos += bytes_read;
+ }
+
+/* Close the backend. */
+ fip_file_read_close:
+ io_close(backend_handle);
+
+ fip_file_read_exit:
+ return result;
+}
+
+
+/* Close a file in package */
+static int fip_file_close(io_entity_t *entity)
+{
+ /* Clear our current file pointer.
+ * If we had malloc() we would free() here.
+ */
+ if (current_file.entry.offset_address != 0) {
+ memset(¤t_file, 0, sizeof(current_file));
+ }
+
+ /* Clear the Entity info. */
+ entity->info = 0;
+
+ return IO_SUCCESS;
+}
+
+/* Exported functions */
+
+/* Register the Firmware Image Package driver with the IO abstraction */
+int register_io_dev_fip(const io_dev_connector_t **dev_con)
+{
+ int result = IO_FAIL;
+ assert(dev_con != NULL);
+
+ result = io_register_device(&fip_dev_info);
+ if (result == IO_SUCCESS)
+ *dev_con = &fip_dev_connector;
+
+ return result;
+}
diff --git a/drivers/io/io_memmap.c b/drivers/io/io_memmap.c
new file mode 100644
index 0000000..3f4b2db
--- /dev/null
+++ b/drivers/io/io_memmap.c
@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <string.h>
+
+/* As we need to be able to keep state for seek, only one file can be open
+ * at a time. Make this a structure and point to the entity->info. When we
+ * can malloc memory we can change this to support more open files.
+ */
+typedef struct {
+ /* Use the 'in_use' flag as any value for base and file_pos could be
+ * valid.
+ */
+ int in_use;
+ uintptr_t base;
+ size_t file_pos;
+} file_state_t;
+
+static file_state_t current_file = {0};
+
+/* Identify the device type as memmap */
+io_type_t device_type_memmap(void)
+{
+ return IO_TYPE_MEMMAP;
+}
+
+/* Memmap device functions */
+static int memmap_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity);
+static int memmap_block_seek(io_entity_t *entity, int mode,
+ ssize_t offset);
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+ size_t length, size_t *length_read);
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written);
+static int memmap_block_close(io_entity_t *entity);
+static int memmap_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t memmap_dev_connector = {
+ .dev_open = memmap_dev_open
+};
+
+
+static const io_dev_funcs_t memmap_dev_funcs = {
+ .type = device_type_memmap,
+ .open = memmap_block_open,
+ .seek = memmap_block_seek,
+ .size = NULL,
+ .read = memmap_block_read,
+ .write = memmap_block_write,
+ .close = memmap_block_close,
+ .dev_init = NULL,
+ .dev_close = memmap_dev_close,
+};
+
+
+/* No state associated with this device so structure can be const */
+static const io_dev_info_t memmap_dev_info = {
+ .funcs = &memmap_dev_funcs,
+ .info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the memmap device */
+static int memmap_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+ io_dev_info_t **dev_info)
+{
+ assert(dev_info != NULL);
+ *dev_info = (io_dev_info_t *)&memmap_dev_info; /* cast away const */
+
+ return IO_SUCCESS;
+}
+
+
+
+/* Close a connection to the memmap device */
+static int memmap_dev_close(io_dev_info_t *dev_info)
+{
+ /* NOP */
+ /* TODO: Consider tracking open files and cleaning them up here */
+ return IO_SUCCESS;
+}
+
+
+/* Open a file on the memmap device */
+/* TODO: Can we do any sensible limit checks on requested memory */
+static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity)
+{
+ int result = IO_FAIL;
+ const io_block_spec_t *block_spec = (io_block_spec_t *)spec;
+
+ /* Since we need to track open state for seek() we only allow one open
+ * spec at a time. When we have dynamic memory we can malloc and set
+ * entity->info.
+ */
+ if (current_file.in_use == 0) {
+ assert(block_spec != NULL);
+ assert(entity != NULL);
+
+ current_file.in_use = 1;
+ current_file.base = block_spec->offset;
+ /* File cursor offset for seek and incremental reads etc. */
+ current_file.file_pos = 0;
+ entity->info = (uintptr_t)¤t_file;
+ result = IO_SUCCESS;
+ } else {
+ WARN("A Memmap device is already active. Close first.\n");
+ result = IO_RESOURCES_EXHAUSTED;
+ }
+
+ return result;
+}
+
+
+/* Seek to a particular file offset on the memmap device */
+static int memmap_block_seek(io_entity_t *entity, int mode, ssize_t offset)
+{
+ int result = IO_FAIL;
+
+ /* We only support IO_SEEK_SET for the moment. */
+ if (mode == IO_SEEK_SET) {
+ assert(entity != NULL);
+
+ /* TODO: can we do some basic limit checks on seek? */
+ ((file_state_t *)entity->info)->file_pos = offset;
+ result = IO_SUCCESS;
+ } else {
+ result = IO_FAIL;
+ }
+
+ return result;
+}
+
+
+/* Read data from a file on the memmap device */
+static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
+ size_t length, size_t *length_read)
+{
+ file_state_t *fp;
+
+ assert(entity != NULL);
+ assert(buffer != (uintptr_t)NULL);
+ assert(length_read != NULL);
+
+ fp = (file_state_t *)entity->info;
+
+ memcpy((void *)buffer, (void *)(fp->base + fp->file_pos), length);
+
+ *length_read = length;
+ /* advance the file 'cursor' for incremental reads */
+ fp->file_pos += length;
+
+ return IO_SUCCESS;
+}
+
+
+/* Write data to a file on the memmap device */
+static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written)
+{
+ file_state_t *fp;
+
+ assert(entity != NULL);
+ assert(buffer != (uintptr_t)NULL);
+ assert(length_written != NULL);
+
+ fp = (file_state_t *)entity->info;
+
+ memcpy((void *)(fp->base + fp->file_pos), (void *)buffer, length);
+
+ *length_written = length;
+
+ /* advance the file 'cursor' for incremental writes */
+ fp->file_pos += length;
+
+ return IO_SUCCESS;
+}
+
+
+/* Close a file on the memmap device */
+static int memmap_block_close(io_entity_t *entity)
+{
+ assert(entity != NULL);
+
+ entity->info = 0;
+
+ /* This would be a mem free() if we had malloc.*/
+ memset((void *)¤t_file, 0, sizeof(current_file));
+
+ return IO_SUCCESS;
+}
+
+
+/* Exported functions */
+
+/* Register the memmap driver with the IO abstraction */
+int register_io_dev_memmap(const io_dev_connector_t **dev_con)
+{
+ int result = IO_FAIL;
+ assert(dev_con != NULL);
+
+ result = io_register_device(&memmap_dev_info);
+ if (result == IO_SUCCESS)
+ *dev_con = &memmap_dev_connector;
+
+ return result;
+}
diff --git a/drivers/io/io_storage.c b/drivers/io/io_storage.c
new file mode 100644
index 0000000..57dc761
--- /dev/null
+++ b/drivers/io/io_storage.c
@@ -0,0 +1,349 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <platform_def.h>
+#include <stddef.h>
+
+
+/* Storage for a fixed maximum number of IO entities, definable by platform */
+static io_entity_t entity_pool[MAX_IO_HANDLES];
+
+/* Simple way of tracking used storage - each entry is NULL or a pointer to an
+ * entity */
+static io_entity_t *entity_map[MAX_IO_HANDLES];
+
+/* Track number of allocated entities */
+static unsigned int entity_count;
+
+/* Array of fixed maximum of registered devices, definable by platform */
+static const io_dev_info_t *devices[MAX_IO_DEVICES];
+
+/* Number of currently registered devices */
+static unsigned int dev_count;
+
+
+#if DEBUG /* Extra validation functions only used in debug builds */
+
+/* Return a boolean value indicating whether a device connector is valid */
+static int is_valid_dev_connector(const io_dev_connector_t *dev_con)
+{
+ int result = (dev_con != NULL) && (dev_con->dev_open != NULL);
+ return result;
+}
+
+
+/* Return a boolean value indicating whether a device handle is valid */
+static int is_valid_dev(const uintptr_t dev_handle)
+{
+ const io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+ int result = (dev != NULL) && (dev->funcs != NULL) &&
+ (dev->funcs->type != NULL) &&
+ (dev->funcs->type() < IO_TYPE_MAX);
+ return result;
+}
+
+
+/* Return a boolean value indicating whether an IO entity is valid */
+static int is_valid_entity(const uintptr_t handle)
+{
+ const io_entity_t *entity = (io_entity_t *)handle;
+ int result = (entity != NULL) &&
+ (is_valid_dev((uintptr_t)entity->dev_handle));
+ return result;
+}
+
+
+/* Return a boolean value indicating whether a seek mode is valid */
+static int is_valid_seek_mode(io_seek_mode_t mode)
+{
+ return ((mode != IO_SEEK_INVALID) && (mode < IO_SEEK_MAX));
+}
+
+#endif /* End of debug-only validation functions */
+
+
+/* Open a connection to a specific device */
+static int dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+ io_dev_info_t **dev_info)
+{
+ int result = IO_FAIL;
+ assert(dev_info != NULL);
+ assert(is_valid_dev_connector(dev_con));
+
+ result = dev_con->dev_open(dev_spec, dev_info);
+ return result;
+}
+
+
+/* Set a handle to track an entity */
+static void set_handle(uintptr_t *handle, io_entity_t *entity)
+{
+ assert(handle != NULL);
+ *handle = (uintptr_t)entity;
+}
+
+
+/* Locate an entity in the pool, specified by address */
+static int find_first_entity(const io_entity_t *entity, unsigned int *index_out)
+{
+ int result = IO_FAIL;
+ for (int index = 0; index < MAX_IO_HANDLES; ++index) {
+ if (entity_map[index] == entity) {
+ result = IO_SUCCESS;
+ *index_out = index;
+ break;
+ }
+ }
+ return result;
+}
+
+
+/* Allocate an entity from the pool and return a pointer to it */
+static int allocate_entity(io_entity_t **entity)
+{
+ int result = IO_FAIL;
+ assert(entity != NULL);
+
+ if (entity_count < MAX_IO_HANDLES) {
+ unsigned int index = 0;
+ result = find_first_entity(NULL, &index);
+ assert(result == IO_SUCCESS);
+ *entity = entity_map[index] = &entity_pool[index];
+ ++entity_count;
+ } else
+ result = IO_RESOURCES_EXHAUSTED;
+
+ return result;
+}
+
+
+/* Release an entity back to the pool */
+static int free_entity(const io_entity_t *entity)
+{
+ int result = IO_FAIL;
+ unsigned int index = 0;
+ assert(entity != NULL);
+
+ result = find_first_entity(entity, &index);
+ if (result == IO_SUCCESS) {
+ entity_map[index] = NULL;
+ --entity_count;
+ }
+
+ return result;
+}
+
+
+/* Exported API */
+
+/* Register a device driver */
+int io_register_device(const io_dev_info_t *dev_info)
+{
+ int result = IO_FAIL;
+ assert(dev_info != NULL);
+
+ if (dev_count < MAX_IO_DEVICES) {
+ devices[dev_count] = dev_info;
+ dev_count++;
+ result = IO_SUCCESS;
+ } else {
+ result = IO_RESOURCES_EXHAUSTED;
+ }
+
+ return result;
+}
+
+
+/* Open a connection to an IO device */
+int io_dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+ uintptr_t *handle)
+{
+ int result = IO_FAIL;
+ assert(handle != NULL);
+
+ result = dev_open(dev_con, dev_spec, (io_dev_info_t **)handle);
+ return result;
+}
+
+
+/* Initialise an IO device explicitly - to permit lazy initialisation or
+ * re-initialisation */
+int io_dev_init(uintptr_t dev_handle, const uintptr_t init_params)
+{
+ int result = IO_FAIL;
+ assert(dev_handle != (uintptr_t)NULL);
+ assert(is_valid_dev(dev_handle));
+
+ io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+ if (dev->funcs->dev_init != NULL) {
+ result = dev->funcs->dev_init(dev, init_params);
+ } else {
+ /* Absence of registered function implies NOP here */
+ result = IO_SUCCESS;
+ }
+ return result;
+}
+
+
+/* TODO: Consider whether an explicit "shutdown" API should be included */
+
+/* Close a connection to a device */
+int io_dev_close(uintptr_t dev_handle)
+{
+ int result = IO_FAIL;
+ assert(dev_handle != (uintptr_t)NULL);
+ assert(is_valid_dev(dev_handle));
+
+ io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+ if (dev->funcs->dev_close != NULL) {
+ result = dev->funcs->dev_close(dev);
+ } else {
+ /* Absence of registered function implies NOP here */
+ result = IO_SUCCESS;
+ }
+
+ return result;
+}
+
+
+/* Synchronous operations */
+
+
+/* Open an IO entity */
+int io_open(uintptr_t dev_handle, const uintptr_t spec, uintptr_t *handle)
+{
+ int result = IO_FAIL;
+ assert((spec != (uintptr_t)NULL) && (handle != NULL));
+ assert(is_valid_dev(dev_handle));
+
+ io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+ io_entity_t *entity;
+
+ result = allocate_entity(&entity);
+
+ if (result == IO_SUCCESS) {
+ assert(dev->funcs->open != NULL);
+ result = dev->funcs->open(dev, spec, entity);
+
+ if (result == IO_SUCCESS) {
+ entity->dev_handle = dev;
+ set_handle(handle, entity);
+ } else
+ free_entity(entity);
+ }
+ return result;
+}
+
+
+/* Seek to a specific position in an IO entity */
+int io_seek(uintptr_t handle, io_seek_mode_t mode, ssize_t offset)
+{
+ int result = IO_FAIL;
+ assert(is_valid_entity(handle) && is_valid_seek_mode(mode));
+
+ io_entity_t *entity = (io_entity_t *)handle;
+
+ io_dev_info_t *dev = entity->dev_handle;
+
+ if (dev->funcs->seek != NULL)
+ result = dev->funcs->seek(entity, mode, offset);
+ else
+ result = IO_NOT_SUPPORTED;
+
+ return result;
+}
+
+
+/* Determine the length of an IO entity */
+int io_size(uintptr_t handle, size_t *length)
+{
+ int result = IO_FAIL;
+ assert(is_valid_entity(handle) && (length != NULL));
+
+ io_entity_t *entity = (io_entity_t *)handle;
+
+ io_dev_info_t *dev = entity->dev_handle;
+
+ if (dev->funcs->size != NULL)
+ result = dev->funcs->size(entity, length);
+ else
+ result = IO_NOT_SUPPORTED;
+
+ return result;
+}
+
+
+/* Read data from an IO entity */
+int io_read(uintptr_t handle,
+ uintptr_t buffer,
+ size_t length,
+ size_t *length_read)
+{
+ int result = IO_FAIL;
+ assert(is_valid_entity(handle) && (buffer != (uintptr_t)NULL));
+
+ io_entity_t *entity = (io_entity_t *)handle;
+
+ io_dev_info_t *dev = entity->dev_handle;
+
+ if (dev->funcs->read != NULL)
+ result = dev->funcs->read(entity, buffer, length, length_read);
+ else
+ result = IO_NOT_SUPPORTED;
+
+ return result;
+}
+
+
+/* Write data to an IO entity */
+int io_write(uintptr_t handle,
+ const uintptr_t buffer,
+ size_t length,
+ size_t *length_written)
+{
+ int result = IO_FAIL;
+ assert(is_valid_entity(handle) && (buffer != (uintptr_t)NULL));
+
+ io_entity_t *entity = (io_entity_t *)handle;
+
+ io_dev_info_t *dev = entity->dev_handle;
+
+ if (dev->funcs->write != NULL) {
+ result = dev->funcs->write(entity, buffer, length,
+ length_written);
+ } else
+ result = IO_NOT_SUPPORTED;
+
+ return result;
+}
+
+
+/* Close an IO entity */
+int io_close(uintptr_t handle)
+{
+ int result = IO_FAIL;
+ assert(is_valid_entity(handle));
+
+ io_entity_t *entity = (io_entity_t *)handle;
+
+ io_dev_info_t *dev = entity->dev_handle;
+
+ if (dev->funcs->close != NULL)
+ result = dev->funcs->close(entity);
+ else {
+ /* Absence of registered function implies NOP here */
+ result = IO_SUCCESS;
+ }
+ /* Ignore improbable free_entity failure */
+ (void)free_entity(entity);
+
+ return result;
+}
diff --git a/drivers/io/vexpress_nor/io_vexpress_nor_hw.c b/drivers/io/vexpress_nor/io_vexpress_nor_hw.c
new file mode 100644
index 0000000..de7b4ce
--- /dev/null
+++ b/drivers/io/vexpress_nor/io_vexpress_nor_hw.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <mmio.h>
+#include <string.h>
+#include "io_vexpress_nor_internal.h"
+#include "norflash.h"
+
+/* Device Id information */
+#define NOR_DEVICE_ID_LOCK_CONFIGURATION 0x02
+#define NOR_DEVICE_ID_BLOCK_LOCKED (1 << 0)
+#define NOR_DEVICE_ID_BLOCK_LOCKED_DOWN (1 << 1)
+
+/* Status Register Bits */
+#define NOR_SR_BIT_WRITE ((1 << 23) | (1 << 7))
+#define NOR_SR_BIT_ERASE ((1 << 21) | (1 << 5))
+#define NOR_SR_BIT_PROGRAM ((1 << 20) | (1 << 4))
+#define NOR_SR_BIT_VPP ((1 << 19) | (1 << 3))
+#define NOR_SR_BIT_BLOCK_LOCKED ((1 << 17) | (1 << 1))
+
+/*
+ * On chip buffer size for buffered programming operations
+ * There are 2 chips, each chip can buffer up to 32 (16-bit)words.
+ * Therefore the total size of the buffer is 2 x 32 x 2 = 128 bytes.
+ */
+#define NOR_MAX_BUFFER_SIZE_IN_BYTES 128
+#define NOR_MAX_BUFFER_SIZE_IN_WORDS (NOR_MAX_BUFFER_SIZE_IN_BYTES / 4)
+
+#define MAX_BUFFERED_PROG_ITERATIONS 1000
+#define LOW_16_BITS 0x0000FFFF
+#define FOLD_32BIT_INTO_16BIT(value) ((value >> 16) | (value & LOW_16_BITS))
+#define BOUNDARY_OF_32_WORDS 0x7F
+
+#define CHECK_VPP_RANGE_ERROR(status_register, address) \
+ do { \
+ if ((status_register) & NOR_SR_BIT_VPP) { \
+ ERROR("%s (address:0x%X): " \
+ "VPP Range Error\n", __func__, address);\
+ err = IO_FAIL; \
+ } \
+ } while (0)
+
+#define CHECK_BLOCK_LOCK_ERROR(status_register, address) \
+ do { \
+ if ((status_register) & NOR_SR_BIT_BLOCK_LOCKED) { \
+ ERROR("%s (address:0x%X): Device Protect " \
+ "Error\n", __func__, address); \
+ err = IO_FAIL; \
+ } \
+ } while (0)
+
+#define CHECK_BLOCK_ERASE_ERROR(status_register, block_offset) \
+ do { \
+ if ((status_register) & NOR_SR_BIT_ERASE) { \
+ ERROR("%s (block_offset=0x%08x: " \
+ "Block Erase Error status_register" \
+ ":0x%x\n", __func__, block_offset, \
+ status_register); \
+ err = IO_FAIL; \
+ } \
+ } while (0)
+
+#define CHECK_SR_BIT_PROGRAM_ERROR(status_register, address) \
+ do { \
+ if ((status_register) & NOR_SR_BIT_PROGRAM) { \
+ ERROR("%s(address:0x%X): Program Error\n", \
+ __func__, address); \
+ err = IO_FAIL; \
+ } \
+ } while (0)
+
+/* Helper macros to access two flash banks in parallel */
+#define NOR_2X16(d) ((d << 16) | (d & 0xffff))
+
+static inline void nor_send_cmd(uintptr_t base_addr, unsigned long cmd)
+{
+ mmio_write_32(base_addr, NOR_2X16(cmd));
+}
+
+static uint32_t flash_read_status(const io_nor_flash_spec_t *device)
+{
+ /* Prepare to read the status register */
+ nor_send_cmd(device->device_address, NOR_CMD_READ_STATUS_REG);
+
+ return mmio_read_32(device->device_address);
+}
+
+static uint32_t flash_wait_until_complete(const io_nor_flash_spec_t *device)
+{
+ uint32_t lock_status;
+
+ /* Wait until the status register gives us the all clear */
+ do {
+ lock_status = flash_read_status(device);
+ } while ((lock_status & NOR_SR_BIT_WRITE) != NOR_SR_BIT_WRITE);
+
+ return lock_status;
+}
+
+static int flash_block_is_locked(uint32_t block_offset)
+{
+ uint32_t lock_status;
+
+ uintptr_t addr = block_offset + (NOR_DEVICE_ID_LOCK_CONFIGURATION << 2);
+
+ /* Send command for reading device id */
+ nor_send_cmd(addr, NOR_CMD_READ_ID_CODE);
+
+ /* Read block lock status */
+ lock_status = mmio_read_32(addr);
+
+ /* Decode block lock status */
+ lock_status = FOLD_32BIT_INTO_16BIT(lock_status);
+
+ if ((lock_status & NOR_DEVICE_ID_BLOCK_LOCKED_DOWN) != 0)
+ WARN("flash_block_is_locked: Block LOCKED DOWN\n");
+
+ return lock_status & NOR_DEVICE_ID_BLOCK_LOCKED;
+}
+
+
+static void flash_perform_lock_operation(const io_nor_flash_spec_t *device,
+ uint32_t block_offset,
+ uint32_t lock_operation)
+{
+ assert ((lock_operation == NOR_UNLOCK_BLOCK) ||
+ (lock_operation == NOR_LOCK_BLOCK));
+
+ /* Request a lock setup */
+ nor_send_cmd(block_offset, NOR_CMD_LOCK_UNLOCK);
+
+ /* Request lock or unlock */
+ nor_send_cmd(block_offset, lock_operation);
+
+ /* Wait until status register shows device is free */
+ flash_wait_until_complete(device);
+
+ /* Put device back into Read Array mode */
+ nor_send_cmd(block_offset, NOR_CMD_READ_ARRAY);
+}
+
+static void flash_unlock_block_if_necessary(const io_nor_flash_spec_t *device,
+ uint32_t block_offset)
+{
+ if (flash_block_is_locked(block_offset) != 0)
+ flash_perform_lock_operation(device, block_offset,
+ NOR_UNLOCK_BLOCK);
+}
+
+
+static int flash_erase_block(const io_nor_flash_spec_t *device,
+ uint32_t block_offset)
+{
+ int err = IO_SUCCESS;
+ uint32_t status_register;
+
+ VERBOSE("%s : 0x%x\n", __func__, block_offset);
+ /* Request a block erase and then confirm it */
+ nor_send_cmd(block_offset, NOR_CMD_BLOCK_ERASE);
+ nor_send_cmd(block_offset, NOR_CMD_BLOCK_ERASE_ACK);
+
+ /* Wait for the write to complete and then check for any errors;
+ * i.e. check the Status Register */
+ status_register = flash_wait_until_complete(device);
+
+ CHECK_VPP_RANGE_ERROR(status_register, block_offset);
+
+ if ((status_register & (NOR_SR_BIT_ERASE | NOR_SR_BIT_PROGRAM)) ==
+ (NOR_SR_BIT_ERASE | NOR_SR_BIT_PROGRAM)) {
+ ERROR("%s(block_offset=0x%08x: "
+ "Command Sequence Error\n", __func__, block_offset);
+ err = IO_FAIL;
+ }
+
+ CHECK_BLOCK_ERASE_ERROR(status_register, block_offset);
+
+ CHECK_BLOCK_LOCK_ERROR(status_register, block_offset);
+
+ if (err) {
+ /* Clear the Status Register */
+ nor_send_cmd(device->device_address, NOR_CMD_CLEAR_STATUS_REG);
+ }
+
+ /* Put device back into Read Array mode */
+ nor_send_cmd(device->device_address, NOR_CMD_READ_ARRAY);
+
+ return err;
+}
+
+/*
+ * Writes data to the NOR Flash using the Buffered Programming method.
+ *
+ * The maximum size of the on-chip buffer is 32-words, because of hardware
+ * restrictions. Therefore this function will only handle buffers up to 32
+ * words or 128 bytes. To deal with larger buffers, call this function again.
+ *
+ * This function presumes that both the offset and the offset+BufferSize
+ * fit entirely within the NOR Flash. Therefore these conditions will not
+ * be checked here.
+ *
+ * In buffered programming, if the target address is not at the beginning of a
+ * 32-bit word boundary, then programming time is doubled and power consumption
+ * is increased. Therefore, it is a requirement to align buffer writes to
+ * 32-bit word boundaries.
+ */
+static int flash_write_buffer(const io_nor_flash_spec_t *device,
+ uint32_t offset,
+ const uint32_t *buffer,
+ uint32_t buffer_size)
+{
+ int err = IO_SUCCESS;
+ uint32_t size_in_words;
+ uint32_t count;
+ volatile uint32_t *data;
+ uint32_t timeout;
+ int is_buffer_available = 0;
+ uint32_t status_register;
+
+ timeout = MAX_BUFFERED_PROG_ITERATIONS;
+ is_buffer_available = 0;
+
+ /* Check that the target offset does not cross a 32-word boundary. */
+ if ((offset & BOUNDARY_OF_32_WORDS) != 0)
+ return IO_FAIL;
+
+ /* This implementation requires the buffer to be 32bit aligned. */
+ if (((uintptr_t)buffer & (sizeof(uint32_t) - 1)) != 0)
+ return IO_FAIL;
+
+ /* Check there are some data to program */
+ assert(buffer_size > 0);
+
+ /* Check that the buffer size does not exceed the maximum hardware
+ * buffer size on chip.
+ */
+ assert(buffer_size <= NOR_MAX_BUFFER_SIZE_IN_BYTES);
+
+ /* Check that the buffer size is a multiple of 32-bit words */
+ assert((buffer_size % 4) == 0);
+
+ /* Pre-programming conditions checked, now start the algorithm. */
+
+ /* Prepare the data destination address */
+ data = (uint32_t *)(uintptr_t)offset;
+
+ /* Check the availability of the buffer */
+ do {
+ /* Issue the Buffered Program Setup command */
+ nor_send_cmd(offset, NOR_CMD_BUFFERED_PROGRAM);
+
+ /* Read back the status register bit#7 from the same offset */
+ if (((*data) & NOR_SR_BIT_WRITE) == NOR_SR_BIT_WRITE)
+ is_buffer_available = 1;
+
+ /* Update the loop counter */
+ timeout--;
+ } while ((timeout > 0) && (is_buffer_available == 0));
+
+ /* The buffer was not available for writing */
+ if (timeout == 0) {
+ err = IO_FAIL;
+ goto exit;
+ }
+
+ /* From now on we work in 32-bit words */
+ size_in_words = buffer_size / sizeof(uint32_t);
+
+ /* Write the word count, which is (buffer_size_in_words - 1),
+ * because word count 0 means one word. */
+ nor_send_cmd(offset, size_in_words - 1);
+
+ /* Write the data to the NOR Flash, advancing each address by 4 bytes */
+ for (count = 0; count < size_in_words; count++, data++, buffer++)
+ *data = *buffer;
+
+ /* Issue the Buffered Program Confirm command, to start the programming
+ * operation */
+ nor_send_cmd(device->device_address, NOR_CMD_BUFFERED_PROGRAM_ACK);
+
+ /* Wait for the write to complete and then check for any errors;
+ * i.e. check the Status Register */
+ status_register = flash_wait_until_complete(device);
+
+ /* Perform a full status check:
+ * Mask the relevant bits of Status Register.
+ * Everything should be zero, if not, we have a problem */
+
+ CHECK_VPP_RANGE_ERROR(status_register, offset);
+
+ CHECK_SR_BIT_PROGRAM_ERROR(status_register, offset);
+
+ CHECK_BLOCK_LOCK_ERROR(status_register, offset);
+
+ if (err != IO_SUCCESS) {
+ /* Clear the Status Register */
+ nor_send_cmd(device->device_address,
+ NOR_CMD_CLEAR_STATUS_REG);
+ }
+
+exit:
+ /* Put device back into Read Array mode */
+ nor_send_cmd(device->device_address, NOR_CMD_READ_ARRAY);
+
+ return err;
+}
+
+static int flash_write_single_word(const io_nor_flash_spec_t *device,
+ int32_t offset, uint32_t data)
+{
+ int err = IO_SUCCESS;
+ uint32_t status_register;
+
+ /* NOR Flash Programming: Request a write single word command */
+ nor_send_cmd(offset, NOR_CMD_WORD_PROGRAM);
+
+ /* Store the word into NOR Flash; */
+ mmio_write_32(offset, data);
+
+ /* Wait for the write to complete and then check for any errors;
+ * i.e. check the Status Register */
+ status_register = flash_wait_until_complete(device);
+
+ /* Perform a full status check: */
+ /* Mask the relevant bits of Status Register.
+ * Everything should be zero, if not, we have a problem */
+
+ CHECK_VPP_RANGE_ERROR(status_register, offset);
+
+ CHECK_SR_BIT_PROGRAM_ERROR(status_register, offset);
+
+ CHECK_BLOCK_LOCK_ERROR(status_register, offset);
+
+ if (err != IO_SUCCESS)
+ /* Clear the Status Register */
+ nor_send_cmd(device->device_address,
+ NOR_CMD_CLEAR_STATUS_REG);
+
+ /* Put device back into Read Array mode */
+ nor_send_cmd(device->device_address, NOR_CMD_READ_ARRAY);
+
+ return err;
+}
+
+int flash_block_write(file_state_t *fp, uint32_t offset,
+ const uintptr_t buffer, size_t *written)
+{
+ int ret;
+ uintptr_t buffer_ptr = buffer;
+ uint32_t buffer_size;
+ uint32_t remaining = fp->block_spec->block_size;
+ uint32_t flash_pos = fp->block_spec->region_address + offset;
+ uint32_t block_offset = flash_pos;
+
+ /* address passed should be block aligned */
+ assert(!(offset % fp->block_spec->block_size));
+
+ VERBOSE("%s : 0x%x\n", __func__, flash_pos);
+ /* Unlock block */
+ flash_unlock_block_if_necessary(fp->block_spec, block_offset);
+
+ /* Erase one block */
+ ret = flash_erase_block(fp->block_spec, flash_pos);
+
+ if (ret != IO_SUCCESS)
+ /* Perform lock operation as we unlocked it */
+ goto lock_block;
+
+ /* Start by using NOR Flash buffer while the buffer size is a multiple
+ * of 32-bit */
+ while ((remaining >= sizeof(uint32_t)) && (ret == IO_SUCCESS)) {
+ if (remaining >= NOR_MAX_BUFFER_SIZE_IN_BYTES)
+ buffer_size = NOR_MAX_BUFFER_SIZE_IN_BYTES;
+ else
+ /* Copy the remaining 32bit words of the buffer */
+ buffer_size = remaining & (sizeof(uint32_t) - 1);
+
+ ret = flash_write_buffer(fp->block_spec, flash_pos,
+ (const uint32_t *)buffer_ptr, buffer_size);
+ flash_pos += buffer_size;
+ remaining -= buffer_size;
+ buffer_ptr += buffer_size;
+
+ }
+
+ /* Write the remaining bytes */
+ while ((remaining > 0) && (ret == IO_SUCCESS)) {
+ ret = flash_write_single_word(fp->block_spec,
+ flash_pos++, buffer_ptr++);
+ remaining--;
+ }
+
+ if (ret == IO_SUCCESS)
+ *written = fp->block_spec->block_size;
+
+lock_block:
+ VERBOSE("%s : 0x%x\n", __func__, block_offset);
+ /* Lock the block once done */
+ flash_perform_lock_operation(fp->block_spec,
+ block_offset,
+ NOR_LOCK_BLOCK);
+
+ return ret;
+}
+
+/* In case of partial write we need to save the block into a temporary buffer */
+static char block_buffer[NOR_FLASH_BLOCK_SIZE];
+
+int flash_partial_write(file_state_t *fp, uint32_t offset,
+ const uintptr_t buffer, size_t length, size_t *written)
+{
+ uintptr_t block_start;
+ uint32_t block_size;
+ uint32_t block_offset;
+ int ret;
+
+ assert((fp != NULL) && (fp->block_spec != NULL));
+ assert(written != NULL);
+
+ block_size = fp->block_spec->block_size;
+ /* Start address of the block to write */
+ block_start = (offset / block_size) * block_size;
+
+ /* Ensure 'block_buffer' is big enough to contain a copy of the block.
+ * 'block_buffer' is reserved at build time - so it might not match */
+ assert(sizeof(block_buffer) >= block_size);
+
+ /*
+ * Check the buffer fits inside a single block.
+ * It must not span several blocks
+ */
+ assert((offset / block_size) ==
+ ((offset + length - 1) / block_size));
+
+ /* Make a copy of the block from flash to a temporary buffer */
+ memcpy(block_buffer, (void *)(fp->block_spec->region_address +
+ block_start), block_size);
+
+ /* Calculate the offset of the buffer into the block */
+ block_offset = offset % block_size;
+
+ /* update the content of the block buffer */
+ memcpy(block_buffer + block_offset, (void *)buffer, length);
+
+ /* Write the block buffer back */
+ ret = flash_block_write(fp, block_start,
+ (uintptr_t)block_buffer, written);
+ if (ret == IO_SUCCESS)
+ *written = length;
+
+ return ret;
+}
diff --git a/drivers/io/vexpress_nor/io_vexpress_nor_internal.h b/drivers/io/vexpress_nor/io_vexpress_nor_internal.h
new file mode 100644
index 0000000..e06d492
--- /dev/null
+++ b/drivers/io/vexpress_nor/io_vexpress_nor_internal.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __IO_VEXPRESS_NOR_INTERNAL_H__
+#define __IO_VEXPRESS_NOR_INTERNAL_H__
+
+#include <io_driver.h>
+#include <io_nor_flash.h>
+#include <io_storage.h>
+
+#define IS_FLASH_ADDRESS_BLOCK_ALIGNED(fp, addr) \
+ (((addr) & ((fp)->block_spec->block_size - 1)) == 0)
+
+/* As we need to be able to keep state for seek, only one file can be open
+ * at a time. Make this a structure and point to the entity->info. When we
+ * can malloc memory we can change this to support more open files.
+ */
+typedef struct {
+ /* Use the 'in_use' flag as any value for base and file_pos could be
+ * valid.
+ */
+ int in_use;
+ uintptr_t base;
+ size_t file_pos;
+
+ /* Definition of the flash block device */
+ const io_nor_flash_spec_t *block_spec;
+} file_state_t;
+
+int flash_block_write(file_state_t *fp, uint32_t address,
+ const uintptr_t buffer, size_t *written);
+
+int flash_partial_write(file_state_t *fp, uint32_t address,
+ const uintptr_t buffer, size_t length, size_t *written);
+
+#endif /* __IO_VEXPRESS_NOR_INTERNAL_H__ */
diff --git a/drivers/io/vexpress_nor/io_vexpress_nor_ops.c b/drivers/io/vexpress_nor/io_vexpress_nor_ops.c
new file mode 100644
index 0000000..d31da57
--- /dev/null
+++ b/drivers/io/vexpress_nor/io_vexpress_nor_ops.c
@@ -0,0 +1,278 @@
+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <debug.h>
+#include <string.h>
+#include "io_vexpress_nor_internal.h"
+
+static file_state_t current_file = {0};
+
+/* Identify the device type as flash */
+io_type_t device_type_flash(void)
+{
+ return IO_TYPE_FLASH;
+}
+
+/* NOR Flash device functions */
+static int flash_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int flash_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity);
+static int flash_seek(io_entity_t *entity, int mode,
+ ssize_t offset);
+static int flash_read(io_entity_t *entity, uintptr_t buffer,
+ size_t length, size_t *length_read);
+static int flash_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written);
+static int flash_close(io_entity_t *entity);
+static int flash_dev_close(io_dev_info_t *dev_info);
+
+
+static const io_dev_connector_t flash_dev_connector = {
+ .dev_open = flash_dev_open
+};
+
+static const io_dev_funcs_t flash_dev_funcs = {
+ .type = device_type_flash,
+ .open = flash_open,
+ .seek = flash_seek,
+ .size = NULL,
+ .read = flash_read,
+ .write = flash_write,
+ .close = flash_close,
+ .dev_init = NULL,
+ .dev_close = flash_dev_close,
+};
+
+/* No state associated with this device so structure can be const */
+static const io_dev_info_t flash_dev_info = {
+ .funcs = &flash_dev_funcs,
+ .info = (uintptr_t)NULL
+};
+
+/* Open a connection to the flash device */
+static int flash_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+ io_dev_info_t **dev_info)
+{
+ assert(dev_info != NULL);
+ *dev_info = (io_dev_info_t *)&flash_dev_info; /* cast away const */
+
+ return IO_SUCCESS;
+}
+
+/* Close a connection to the flash device */
+static int flash_dev_close(io_dev_info_t *dev_info)
+{
+ /* NOP */
+ /* TODO: Consider tracking open files and cleaning them up here */
+ return IO_SUCCESS;
+}
+
+
+/* Open a file on the flash device */
+/* TODO: Can we do any sensible limit checks on requested memory */
+static int flash_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity)
+{
+ int result;
+ const io_nor_flash_spec_t *block_spec = (io_nor_flash_spec_t *)spec;
+
+ /* Since we need to track open state for seek() we only allow one open
+ * spec at a time. When we have dynamic memory we can malloc and set
+ * entity->info.
+ */
+ if (current_file.in_use == 0) {
+ assert(block_spec != NULL);
+ assert(entity != NULL);
+
+ current_file.in_use = 1;
+ current_file.base = block_spec->region_address;
+ /* File cursor offset for seek and incremental reads etc. */
+ current_file.file_pos = 0;
+ /* Attach the device specification to this file */
+ current_file.block_spec = block_spec;
+
+ entity->info = (uintptr_t)¤t_file;
+ result = IO_SUCCESS;
+ } else {
+ WARN("A Flash device is already active. Close first.\n");
+ result = IO_RESOURCES_EXHAUSTED;
+ }
+
+ return result;
+}
+
+/* Seek to a particular file offset on the flash device */
+static int flash_seek(io_entity_t *entity, int mode, ssize_t offset)
+{
+ const io_nor_flash_spec_t *block_spec;
+ int result;
+ uintptr_t flash_base;
+ size_t flash_size;
+
+ assert(entity != NULL);
+
+ block_spec = ((file_state_t *)entity->info)->block_spec;
+ flash_size = block_spec->block_count * block_spec->block_size;
+
+ if (mode == IO_SEEK_SET) {
+ /* Ensure the offset is into the flash */
+ if ((offset >= 0) && (offset < flash_size)) {
+ ((file_state_t *)entity->info)->file_pos = offset;
+ result = IO_SUCCESS;
+ } else {
+ result = IO_FAIL;
+ }
+ } else if (mode == IO_SEEK_END) {
+ flash_base = block_spec->region_address;
+ /* Ensure the offset is into the flash */
+ if ((offset <= 0) && (flash_size + offset >= 0)) {
+ ((file_state_t *)entity->info)->file_pos =
+ flash_base + flash_size + offset;
+ result = IO_SUCCESS;
+ } else {
+ result = IO_FAIL;
+ }
+ } else if (mode == IO_SEEK_CUR) {
+ flash_base = block_spec->region_address;
+ /* Ensure the offset is into the flash */
+ if ((((file_state_t *)entity->info)->file_pos +
+ offset >= flash_base) &&
+ (((file_state_t *)entity->info)->file_pos +
+ offset < flash_base + flash_size)) {
+ ((file_state_t *)entity->info)->file_pos += offset;
+ result = IO_SUCCESS;
+ } else {
+ result = IO_FAIL;
+ }
+ } else {
+ result = IO_FAIL;
+ }
+
+ return result;
+}
+
+/* Read data from a file on the flash device */
+static int flash_read(io_entity_t *entity, uintptr_t buffer,
+ size_t length, size_t *length_read)
+{
+ file_state_t *fp;
+
+ assert(entity != NULL);
+ assert(buffer != (uintptr_t)NULL);
+ assert(length_read != NULL);
+
+ fp = (file_state_t *)entity->info;
+
+ memcpy((void *)buffer, (void *)(fp->base + fp->file_pos), length);
+
+ *length_read = length;
+ /* advance the file 'cursor' for incremental reads */
+ fp->file_pos += length;
+
+ return IO_SUCCESS;
+}
+
+/* Write data to a file on the flash device */
+static int flash_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written)
+{
+ file_state_t *fp;
+ const io_nor_flash_spec_t *block_spec;
+ size_t file_pos;
+ int ret;
+ uint32_t remaining_block_size;
+ size_t written;
+ uintptr_t buffer_ptr = buffer;
+
+ assert(entity != NULL);
+ assert(buffer != (uintptr_t)NULL);
+ assert(length_written != NULL);
+
+ fp = (file_state_t *)entity->info;
+ block_spec = fp->block_spec;
+ file_pos = fp->file_pos;
+
+ *length_written = 0;
+
+ while (length > 0) {
+ /* Check if we can do a block write */
+ if (IS_FLASH_ADDRESS_BLOCK_ALIGNED(fp, file_pos)) {
+ if (length / block_spec->block_size > 0) {
+ ret = flash_block_write(fp, file_pos,
+ buffer_ptr, &written);
+ } else {
+ /* Case when the length is smaller than a
+ * block size
+ */
+ ret = flash_partial_write(fp, file_pos,
+ buffer_ptr, length, &written);
+ }
+ } else {
+ /* Case when the buffer does not start at the beginning
+ * of a block
+ */
+
+ /* Length between the current file_pos and the end of
+ * the block
+ */
+ remaining_block_size = block_spec->block_size -
+ (file_pos % block_spec->block_size);
+
+ if (length < remaining_block_size) {
+ ret = flash_partial_write(fp, file_pos,
+ buffer_ptr, length, &written);
+ } else {
+ ret = flash_partial_write(fp, file_pos,
+ buffer_ptr, remaining_block_size,
+ &written);
+ }
+ }
+
+ /* If one of the write operations fails then we do not pursue */
+ if (ret != IO_SUCCESS) {
+ return ret;
+ } else {
+ buffer_ptr += written;
+ file_pos += written;
+
+ *length_written += written;
+ length -= written;
+ }
+ }
+
+ /* advance the file 'cursor' for incremental writes */
+ fp->file_pos += length;
+
+ return IO_SUCCESS;
+}
+
+/* Close a file on the flash device */
+static int flash_close(io_entity_t *entity)
+{
+ assert(entity != NULL);
+
+ entity->info = 0;
+
+ /* This would be a mem free() if we had malloc.*/
+ memset((void *)¤t_file, 0, sizeof(current_file));
+
+ return IO_SUCCESS;
+}
+
+/* Exported functions */
+
+/* Register the flash driver with the IO abstraction */
+int register_io_dev_nor_flash(const io_dev_connector_t **dev_con)
+{
+ int result;
+ assert(dev_con != NULL);
+
+ result = io_register_device(&flash_dev_info);
+ if (result == IO_SUCCESS)
+ *dev_con = &flash_dev_connector;
+
+ return result;
+}
diff --git a/drivers/io/vexpress_nor/norflash.h b/drivers/io/vexpress_nor/norflash.h
new file mode 100644
index 0000000..893a2c5
--- /dev/null
+++ b/drivers/io/vexpress_nor/norflash.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2015-2017, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __NORFLASH_H_
+#define __NORFLASH_H_
+
+#include <stdint.h>
+
+/* First bus cycle */
+#define NOR_CMD_READ_ARRAY 0xFF
+#define NOR_CMD_READ_ID_CODE 0x90
+#define NOR_CMD_READ_QUERY 0x98
+#define NOR_CMD_READ_STATUS_REG 0x70
+#define NOR_CMD_CLEAR_STATUS_REG 0x50
+#define NOR_CMD_WRITE_TO_BUFFER 0xE8
+#define NOR_CMD_WORD_PROGRAM 0x40
+#define NOR_CMD_BLOCK_ERASE 0x20
+#define NOR_CMD_LOCK_UNLOCK 0x60
+#define NOR_CMD_BLOCK_ERASE_ACK 0xD0
+#define NOR_CMD_BUFFERED_PROGRAM 0xE8
+
+/* Second bus cycle */
+#define NOR_LOCK_BLOCK 0x01
+#define NOR_UNLOCK_BLOCK 0xD0
+#define NOR_CMD_BUFFERED_PROGRAM_ACK 0xD0
+
+/* Status register bits */
+#define NOR_DWS (1 << 7)
+#define NOR_ESS (1 << 6)
+#define NOR_ES (1 << 5)
+#define NOR_PS (1 << 4)
+#define NOR_VPPS (1 << 3)
+#define NOR_PSS (1 << 2)
+#define NOR_BLS (1 << 1)
+#define NOR_BWS (1 << 0)
+
+#endif /* __NORFLASH_H_ */
+