18 files changed, 919 insertions, 76 deletions

diff --git a/drivers/arm/gic/common/gic_common.c b/drivers/arm/gic/common/gic_common.c
index 38b2f6719..bf6405f7f 100644
--- a/drivers/arm/gic/common/gic_common.c
+++ b/drivers/arm/gic/common/gic_common.c
@@ -1,9 +1,11 @@
 /*
- * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#pragma message __FILE__ " is deprecated, use gicv2.mk instead"
+
 #include <assert.h>
 
 #include <drivers/arm/gic_common.h>
diff --git a/drivers/arm/gic/v2/gicdv2_helpers.c b/drivers/arm/gic/v2/gicdv2_helpers.c
new file mode 100644
index 000000000..db9ba87c4
--- /dev/null
+++ b/drivers/arm/gic/v2/gicdv2_helpers.c
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/arm/gic_common.h>
+#include <lib/mmio.h>
+
+#include "../common/gic_common_private.h"
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+/*
+ * Accessor to read the GIC Distributor IGROUPR corresponding to the interrupt
+ * `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	return mmio_read_32(base + GICD_IGROUPR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ISPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	return mmio_read_32(base + GICD_IPRIORITYR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICGFR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICFGR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	return mmio_read_32(base + GICD_NSACR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+/*
+ * Accessor to write the GIC Distributor IGROUPR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	mmio_write_32(base + GICD_IGROUPR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ISENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ICENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ISPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ICPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ISACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ICACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	mmio_write_32(base + GICD_IPRIORITYR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICFGR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	mmio_write_32(base + GICD_ICFGR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	mmio_write_32(base + GICD_NSACR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val | (1U << bit_num));
+}
+
+void gicd_clr_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val & ~(1U << bit_num));
+}
+
+void gicd_set_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISENABLER_SHIFT) - 1U);
+
+	gicd_write_isenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICENABLER_SHIFT) - 1U);
+
+	gicd_write_icenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISPENDR_SHIFT) - 1U);
+
+	gicd_write_ispendr(base, id, (1U << bit_num));
+}
+
+void gicd_set_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICPENDR_SHIFT) - 1U);
+
+	gicd_write_icpendr(base, id, (1U << bit_num));
+}
+
+unsigned int gicd_get_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_isactiver(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+
+	gicd_write_isactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICACTIVER_SHIFT) - 1U);
+
+	gicd_write_icactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	uint8_t val = pri & GIC_PRI_MASK;
+
+	mmio_write_8(base + GICD_IPRIORITYR + id, val);
+}
+
+void gicd_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg)
+{
+	/* Interrupt configuration is a 2-bit field */
+	unsigned int bit_num = id & ((1U << ICFGR_SHIFT) - 1U);
+	unsigned int bit_shift = bit_num << 1;
+
+	uint32_t reg_val = gicd_read_icfgr(base, id);
+
+	/* Clear the field, and insert required configuration */
+	reg_val &= ~(GIC_CFG_MASK << bit_shift);
+	reg_val |= ((cfg & GIC_CFG_MASK) << bit_shift);
+
+	gicd_write_icfgr(base, id, reg_val);
+}
diff --git a/drivers/arm/gic/v2/gicv2.mk b/drivers/arm/gic/v2/gicv2.mk
new file mode 100644
index 000000000..49996bb51
--- /dev/null
+++ b/drivers/arm/gic/v2/gicv2.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2020, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# No support for extended PPI and SPI range
+GIC_EXT_INTID	:=	0
+
+GICV2_SOURCES	+=	drivers/arm/gic/v2/gicv2_main.c		\
+			drivers/arm/gic/v2/gicv2_helpers.c	\
+			drivers/arm/gic/v2/gicdv2_helpers.c
+
+# Set GICv2 build option
+$(eval $(call add_define,GIC_EXT_INTID))
\ No newline at end of file
diff --git a/drivers/arm/gic/v3/gic-x00.c b/drivers/arm/gic/v3/gic-x00.c
index cc9717451..c9e9cb98f 100644
--- a/drivers/arm/gic/v3/gic-x00.c
+++ b/drivers/arm/gic/v3/gic-x00.c
@@ -24,6 +24,7 @@
 #define GICR_PWRR			0x24
 #define IIDR_MODEL_ARM_GIC_600		(0x0200043b)
 #define IIDR_MODEL_ARM_GIC_600AE	(0x0300043b)
+#define IIDR_MODEL_ARM_GIC_CLAYTON	(0x0400043b)
 
 /* GICR_PWRR fields */
 #define PWRR_RDPD_SHIFT			0
@@ -45,7 +46,7 @@
 
 #if GICV3_SUPPORT_GIC600
 
-/* GIC-600 specific accessor functions */
+/* GIC-600/Clayton specific accessor functions */
 static void gicr_write_pwrr(uintptr_t base, unsigned int val)
 {
 	mmio_write_32(base + GICR_PWRR, val);
@@ -113,12 +114,17 @@ static uintptr_t get_gicr_base(unsigned int proc_num)
 	return gicr_base;
 }
 
-static bool gicv3_is_gic600(uintptr_t gicr_base)
+static bool gicv3_redists_need_power_mgmt(uintptr_t gicr_base)
 {
 	uint32_t reg = mmio_read_32(gicr_base + GICR_IIDR);
 
+	/*
+	 * The Arm GIC-600 and GIC-Clayton models have their redistributors
+	 * powered down at reset.
+	 */
 	return (((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600) ||
-		((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE));
+		((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE) ||
+		((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_CLAYTON));
 }
 
 #endif
@@ -143,7 +149,7 @@ void gicv3_rdistif_off(unsigned int proc_num)
 	uintptr_t gicr_base = get_gicr_base(proc_num);
 
 	/* Attempt to power redistributor off */
-	if (gicv3_is_gic600(gicr_base)) {
+	if (gicv3_redists_need_power_mgmt(gicr_base)) {
 		gic600_pwr_off(gicr_base);
 	}
 #endif
@@ -158,7 +164,7 @@ void gicv3_rdistif_on(unsigned int proc_num)
 	uintptr_t gicr_base = get_gicr_base(proc_num);
 
 	/* Power redistributor on */
-	if (gicv3_is_gic600(gicr_base)) {
+	if (gicv3_redists_need_power_mgmt(gicr_base)) {
 		gic600_pwr_on(gicr_base);
 	}
 #endif
diff --git a/drivers/brcm/rng.c b/drivers/brcm/rng.c
new file mode 100644
index 000000000..ee2e6561e
--- /dev/null
+++ b/drivers/brcm/rng.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2017 - 2020, Broadcom
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define RNG_CTRL_REG		(RNG_BASE_ADDR + 0x00)
+#define RNG_CTRL_MASK		0x00001FFF
+#define RNG_CTRL_ENABLE		0x00000001
+#define RNG_CTRL_DISABLE	0x00000000
+
+#define RNG_SOFT_RESET_REG	(RNG_BASE_ADDR + 0x04)
+#define RNG_SOFT_RESET_MASK	0x00000001
+
+#define RNG_FIFO_DATA_REG	(RNG_BASE_ADDR + 0x20)
+
+#define RNG_FIFO_COUNT_REG	(RNG_BASE_ADDR + 0x24)
+#define RNG_FIFO_COUNT_MASK	0x000000FF
+
+#define RNG_FIFO_WORDS_MAX	16
+#define MAX_WAIT_COUNT_50US	20000
+
+
+static void rng_reset(void)
+{
+	/* Disable RBG */
+	mmio_clrbits_32(RNG_CTRL_REG, RNG_CTRL_MASK);
+
+	/* Reset RNG and RBG */
+	mmio_setbits_32(RNG_SOFT_RESET_REG, RNG_SOFT_RESET_MASK);
+
+	/* Take all out of reset */
+	mmio_clrbits_32(RNG_SOFT_RESET_REG, RNG_SOFT_RESET_MASK);
+}
+
+static void rng_enable(void)
+{
+	/* Setup RNG. */
+	mmio_clrsetbits_32(RNG_CTRL_REG, RNG_CTRL_MASK, RNG_CTRL_ENABLE);
+}
+
+int rng_init(void)
+{
+	rng_reset();
+
+	rng_enable();
+
+	return 0;
+}
+
+int rng_read(uint32_t *p_out, uint32_t *words_read)
+{
+	uint32_t available_words;
+	uint32_t i;
+	uint32_t word_processed = 0;
+	uint32_t wait_count = MAX_WAIT_COUNT_50US;
+
+	if (*words_read == 0) {
+		ERROR("RNG Parameter: No word requested\n");
+		return -1;
+	}
+
+	do {
+		available_words = mmio_read_32(RNG_FIFO_COUNT_REG);
+		available_words &= RNG_FIFO_COUNT_MASK;
+
+		if (available_words != 0) {
+			available_words = MIN(available_words,
+					*words_read - word_processed);
+
+			for (i = 0; i < available_words; i++)
+				p_out[word_processed + i] =
+					mmio_read_32(RNG_FIFO_DATA_REG);
+			word_processed += available_words;
+		} else {
+			udelay(50);
+		}
+
+		if (word_processed == *words_read)
+			break;
+
+	} while (--wait_count);
+
+	if (word_processed != *words_read) {
+		ERROR("RNG Timeout: requested %d word(s) got %d\n",
+				*words_read, word_processed);
+		*words_read = word_processed;
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
index 7aa717da8..6e152959f 100644
--- a/drivers/io/io_fip.c
+++ b/drivers/io/io_fip.c
@@ -36,7 +36,7 @@
 typedef struct {
 	unsigned int file_pos;
 	fip_toc_entry_t entry;
-} file_state_t;
+} fip_file_state_t;
 
 /*
  * Maintain dev_spec per FIP Device
@@ -49,7 +49,6 @@ typedef struct {
 	uint16_t plat_toc_flag;
 } fip_dev_state_t;
 
-static const uuid_t uuid_null;
 /*
  * Only one file can be open across all FIP device
  * as backends like io_memmap don't support
@@ -57,7 +56,7 @@ static const uuid_t uuid_null;
  * backend handle should be maintained per FIP device
  * if the same support is available in the backend
  */
-static file_state_t current_file = {0};
+static fip_file_state_t current_fip_file = {0};
 static uintptr_t backend_dev_handle;
 static uintptr_t backend_image_spec;
 
@@ -288,6 +287,7 @@ static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
 	int result;
 	uintptr_t backend_handle;
 	const io_uuid_spec_t *uuid_spec = (io_uuid_spec_t *)spec;
+	static const uuid_t uuid_null = { {0} }; /* Double braces for clang */
 	size_t bytes_read;
 	int found_file = 0;
 
@@ -300,7 +300,7 @@ static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
 	 * 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) {
+	if (current_fip_file.entry.offset_address != 0U) {
 		WARN("fip_file_open : Only one open file at a time.\n");
 		return -ENFILE;
 	}
@@ -326,31 +326,32 @@ static int fip_file_open(io_dev_info_t *dev_info, const uintptr_t spec,
 	found_file = 0;
 	do {
 		result = io_read(backend_handle,
-				 (uintptr_t)&current_file.entry,
-				 sizeof(current_file.entry),
+				 (uintptr_t)&current_fip_file.entry,
+				 sizeof(current_fip_file.entry),
 				 &bytes_read);
 		if (result == 0) {
-			if (compare_uuids(&current_file.entry.uuid,
+			if (compare_uuids(&current_fip_file.entry.uuid,
 					  &uuid_spec->uuid) == 0) {
 				found_file = 1;
-				break;
 			}
 		} else {
 			WARN("Failed to read FIP (%i)\n", result);
 			goto fip_file_open_close;
 		}
-	} while (compare_uuids(&current_file.entry.uuid, &uuid_null) != 0);
+	} while ((found_file == 0) &&
+			(compare_uuids(&current_fip_file.entry.uuid,
+				&uuid_null) != 0));
 
 	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.
+		 * the file position to 0. The 'current_fip_file.entry' holds
+		 * the base and size of the file.
 		 */
-		current_file.file_pos = 0;
-		entity->info = (uintptr_t)&current_file;
+		current_fip_file.file_pos = 0;
+		entity->info = (uintptr_t)&current_fip_file;
 	} else {
 		/* Did not find the file in the FIP. */
-		current_file.entry.offset_address = 0;
+		current_fip_file.entry.offset_address = 0;
 		result = -ENOENT;
 	}
 
@@ -368,7 +369,7 @@ 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;
+	*length =  ((fip_file_state_t *)entity->info)->entry.size;
 
 	return 0;
 }
@@ -379,7 +380,7 @@ static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
 			  size_t *length_read)
 {
 	int result;
-	file_state_t *fp;
+	fip_file_state_t *fp;
 	size_t file_offset;
 	size_t bytes_read;
 	uintptr_t backend_handle;
@@ -397,7 +398,7 @@ static int fip_file_read(io_entity_t *entity, uintptr_t buffer, size_t length,
 		goto fip_file_read_exit;
 	}
 
-	fp = (file_state_t *)entity->info;
+	fp = (fip_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;
@@ -436,8 +437,8 @@ 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) {
-		zeromem(&current_file, sizeof(current_file));
+	if (current_fip_file.entry.offset_address != 0U) {
+		zeromem(&current_fip_file, sizeof(current_fip_file));
 	}
 
 	/* Clear the Entity info. */
diff --git a/drivers/io/io_memmap.c b/drivers/io/io_memmap.c
index eed50cc08..eb69163b7 100644
--- a/drivers/io/io_memmap.c
+++ b/drivers/io/io_memmap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -27,9 +27,9 @@ typedef struct {
 	uintptr_t		base;
 	unsigned long long	file_pos;
 	unsigned long long	size;
-} file_state_t;
+} memmap_file_state_t;
 
-static file_state_t current_file = {0};
+static memmap_file_state_t current_memmap_file = {0};
 
 /* Identify the device type as memmap */
 static io_type_t device_type_memmap(void)
@@ -71,7 +71,7 @@ static const io_dev_funcs_t memmap_dev_funcs = {
 
 
 /* No state associated with this device so structure can be const */
-static const io_dev_info_t memmap_dev_info = {
+static io_dev_info_t memmap_dev_info = {
 	.funcs = &memmap_dev_funcs,
 	.info = (uintptr_t)NULL
 };
@@ -82,8 +82,7 @@ static int memmap_dev_open(const uintptr_t dev_spec __unused,
 			   io_dev_info_t **dev_info)
 {
 	assert(dev_info != NULL);
-	*dev_info = (io_dev_info_t *)&memmap_dev_info; /* cast away const */
-
+	*dev_info = &memmap_dev_info;
 	return 0;
 }
 
@@ -109,16 +108,16 @@ static int memmap_block_open(io_dev_info_t *dev_info, const uintptr_t spec,
 	 * spec at a time. When we have dynamic memory we can malloc and set
 	 * entity->info.
 	 */
-	if (current_file.in_use == 0) {
+	if (current_memmap_file.in_use == 0) {
 		assert(block_spec != NULL);
 		assert(entity != NULL);
 
-		current_file.in_use = 1;
-		current_file.base = block_spec->offset;
+		current_memmap_file.in_use = 1;
+		current_memmap_file.base = block_spec->offset;
 		/* File cursor offset for seek and incremental reads etc. */
-		current_file.file_pos = 0;
-		current_file.size = block_spec->length;
-		entity->info = (uintptr_t)&current_file;
+		current_memmap_file.file_pos = 0;
+		current_memmap_file.size = block_spec->length;
+		entity->info = (uintptr_t)&current_memmap_file;
 		result = 0;
 	} else {
 		WARN("A Memmap device is already active. Close first.\n");
@@ -133,13 +132,13 @@ static int memmap_block_seek(io_entity_t *entity, int mode,
 			     signed long long offset)
 {
 	int result = -ENOENT;
-	file_state_t *fp;
+	memmap_file_state_t *fp;
 
 	/* We only support IO_SEEK_SET for the moment. */
 	if (mode == IO_SEEK_SET) {
 		assert(entity != NULL);
 
-		fp = (file_state_t *) entity->info;
+		fp = (memmap_file_state_t *) entity->info;
 
 		/* Assert that new file position is valid */
 		assert((offset >= 0) &&
@@ -160,7 +159,7 @@ static int memmap_block_len(io_entity_t *entity, size_t *length)
 	assert(entity != NULL);
 	assert(length != NULL);
 
-	*length = (size_t)((file_state_t *)entity->info)->size;
+	*length = (size_t)((memmap_file_state_t *)entity->info)->size;
 
 	return 0;
 }
@@ -170,13 +169,13 @@ static int memmap_block_len(io_entity_t *entity, size_t *length)
 static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
 			     size_t length, size_t *length_read)
 {
-	file_state_t *fp;
+	memmap_file_state_t *fp;
 	unsigned long long pos_after;
 
 	assert(entity != NULL);
 	assert(length_read != NULL);
 
-	fp = (file_state_t *) entity->info;
+	fp = (memmap_file_state_t *) entity->info;
 
 	/* Assert that file position is valid for this read operation */
 	pos_after = fp->file_pos + length;
@@ -198,13 +197,13 @@ static int memmap_block_read(io_entity_t *entity, uintptr_t buffer,
 static int memmap_block_write(io_entity_t *entity, const uintptr_t buffer,
 			      size_t length, size_t *length_written)
 {
-	file_state_t *fp;
+	memmap_file_state_t *fp;
 	unsigned long long pos_after;
 
 	assert(entity != NULL);
 	assert(length_written != NULL);
 
-	fp = (file_state_t *) entity->info;
+	fp = (memmap_file_state_t *) entity->info;
 
 	/* Assert that file position is valid for this write operation */
 	pos_after = fp->file_pos + length;
@@ -230,7 +229,7 @@ static int memmap_block_close(io_entity_t *entity)
 	entity->info = 0;
 
 	/* This would be a mem free() if we had malloc.*/
-	zeromem((void *)&current_file, sizeof(current_file));
+	zeromem((void *)&current_memmap_file, sizeof(current_memmap_file));
 
 	return 0;
 }
diff --git a/drivers/io/io_semihosting.c b/drivers/io/io_semihosting.c
index 4ceddc6cc..1c2f84d54 100644
--- a/drivers/io/io_semihosting.c
+++ b/drivers/io/io_semihosting.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -51,8 +51,7 @@ static const io_dev_funcs_t sh_dev_funcs = {
 };
 
 
-/* No state associated with this device so structure can be const */
-static const io_dev_info_t sh_dev_info = {
+static io_dev_info_t sh_dev_info = {
 	.funcs = &sh_dev_funcs,
 	.info = (uintptr_t)NULL
 };
@@ -63,7 +62,7 @@ static int sh_dev_open(const uintptr_t dev_spec __unused,
 		io_dev_info_t **dev_info)
 {
 	assert(dev_info != NULL);
-	*dev_info = (io_dev_info_t *)&sh_dev_info; /* cast away const */
+	*dev_info = &sh_dev_info;
 	return 0;
 }
 
diff --git a/drivers/io/io_storage.c b/drivers/io/io_storage.c
index b8c1d6479..053426891 100644
--- a/drivers/io/io_storage.c
+++ b/drivers/io/io_storage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,36 +32,34 @@ static unsigned int dev_count;
 #if ENABLE_ASSERTIONS
 
 /* Return a boolean value indicating whether a device connector is valid */
-static int is_valid_dev_connector(const io_dev_connector_t *dev_con)
+static bool is_valid_dev_connector(const io_dev_connector_t *dev_con)
 {
-	int result = (dev_con != NULL) && (dev_con->dev_open != NULL);
-	return result;
+	return (dev_con != NULL) && (dev_con->dev_open != NULL);
 }
 
-
 /* Return a boolean value indicating whether a device handle is valid */
-static int is_valid_dev(const uintptr_t dev_handle)
+static bool 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) &&
+
+	return (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)
+static bool is_valid_entity(const uintptr_t handle)
 {
 	const io_entity_t *entity = (io_entity_t *)handle;
-	int result = (entity != NULL) &&
+
+	return (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)
+static bool is_valid_seek_mode(io_seek_mode_t mode)
 {
 	return ((mode != IO_SEEK_INVALID) && (mode < IO_SEEK_MAX));
 }
@@ -71,15 +69,14 @@ static int is_valid_seek_mode(io_seek_mode_t mode)
 
 
 /* Open a connection to a specific device */
-static int dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+static int io_storage_dev_open(const io_dev_connector_t *dev_con,
+		const uintptr_t dev_spec,
 		io_dev_info_t **dev_info)
 {
-	int result;
 	assert(dev_info != NULL);
 	assert(is_valid_dev_connector(dev_con));
 
-	result = dev_con->dev_open(dev_spec, dev_info);
-	return result;
+	return dev_con->dev_open(dev_spec, dev_info);
 }
 
 
@@ -116,7 +113,8 @@ static int allocate_entity(io_entity_t **entity)
 		unsigned int index = 0;
 		result = find_first_entity(NULL, &index);
 		assert(result == 0);
-		*entity = entity_map[index] = &entity_pool[index];
+		*entity = &entity_pool[index];
+		entity_map[index] = &entity_pool[index];
 		++entity_count;
 	}
 
@@ -163,11 +161,8 @@ int io_register_device(const io_dev_info_t *dev_info)
 int io_dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
 		uintptr_t *handle)
 {
-	int result;
 	assert(handle != NULL);
-
-	result = dev_open(dev_con, dev_spec, (io_dev_info_t **)handle);
-	return result;
+	return io_storage_dev_open(dev_con, dev_spec, (io_dev_info_t **)handle);
 }
 
 
diff --git a/drivers/marvell/cache_llc.c b/drivers/marvell/cache_llc.c
index 836aae7b8..4b06b4752 100644
--- a/drivers/marvell/cache_llc.c
+++ b/drivers/marvell/cache_llc.c
@@ -111,28 +111,36 @@ void llc_runtime_enable(int ap_index)
 }
 
 #if LLC_SRAM
-void llc_sram_enable(int ap_index)
+int llc_sram_enable(int ap_index, int size)
 {
-	uint32_t tc, way;
+	uint32_t tc, way, ways_to_allocate;
 	uint32_t way_addr;
 
+	if ((size <= 0) || (size > LLC_SIZE) || (size % LLC_WAY_SIZE))
+		return -1;
+
+	llc_enable(ap_index, 1);
+	llc_inv_all(ap_index);
+
+	ways_to_allocate = size / LLC_WAY_SIZE;
+
 	/* Lockdown all available ways for all traffic classes */
 	for (tc = 0; tc < LLC_TC_NUM; tc++)
-		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_WAY_MASK);
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_ALL_WAYS_MASK);
 
 	/* Clear the high bits of SRAM address */
 	mmio_write_32(LLC_BANKED_MNT_AHR(ap_index), 0);
 
 	way_addr = PLAT_MARVELL_TRUSTED_RAM_BASE;
-	for (way = 0; way < LLC_WAYS; way++) {
+	for (way = 0; way < ways_to_allocate; way++) {
 		/* Trigger allocation block command */
 		mmio_write_32(LLC_BLK_ALOC(ap_index),
 			      LLC_BLK_ALOC_BASE_ADDR(way_addr) |
-			      LLC_BLK_ALOC_WAY_DATA_CLR |
+			      LLC_BLK_ALOC_WAY_DATA_SET |
 			      LLC_BLK_ALOC_WAY_ID(way));
 		way_addr += LLC_WAY_SIZE;
 	}
-	llc_enable(ap_index, 1);
+	return 0;
 }
 
 void llc_sram_disable(int ap_index)
@@ -146,4 +154,36 @@ void llc_sram_disable(int ap_index)
 	/* Invalidate all ways */
 	llc_inv_all(ap_index);
 }
+
+int llc_sram_test(int ap_index, int size, char *msg)
+{
+	uintptr_t addr, end_addr;
+	uint32_t data = 0;
+
+	if ((size <= 0) || (size > LLC_SIZE))
+		return -1;
+
+	INFO("=== LLC SRAM WRITE test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		mmio_write_32(addr, addr);
+	}
+	INFO("=== LLC SRAM WRITE test %s PASSED\n", msg);
+	INFO("=== LLC SRAM READ test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		data = mmio_read_32(addr);
+		if (data != addr) {
+			INFO("=== LLC SRAM READ test %s FAILED @ 0x%08lx)\n",
+			     msg, addr);
+			return -1;
+		}
+	}
+	INFO("=== LLC SRAM READ test %s PASSED (last read = 0x%08x)\n",
+	     msg, data);
+	return 0;
+}
+
 #endif /* LLC_SRAM */
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index 2760f4603..1d5b6f564 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -11,6 +11,7 @@
 
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <mg_conf_cm3/mg_conf_cm3.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
 
@@ -2231,6 +2232,7 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 					  uint32_t comphy_mode)
 {
 	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr;
 	uintptr_t comphy_addr = comphy_addr =
 				COMPHY_ADDR(comphy_base, comphy_index);
 
@@ -2247,6 +2249,10 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
 	debug_exit();
 
+	/* Start AP Firmware */
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+	mg_start_ap_fw(cp_nr, comphy_index);
+
 	return 0;
 }
 
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
new file mode 100644
index 000000000..98e189687
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a8k_plat_def.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mss_scp_bl2_format.h>
+
+/* CONFI REGISTERS */
+#define MG_CM3_CONFI_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
+#define MG_CM3_SRAM_BASE(CP)		MG_CM3_CONFI_BASE(CP)
+#define MG_CM3_CONFI_GLOB_CFG_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B500)
+#define CM3_CPU_EN_BIT			BIT(28)
+#define MG_CM3_MG_INT_MFX_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B054)
+#define CM3_SYS_RESET_BIT		BIT(0)
+
+#define MG_CM3_SHARED_MEM_BASE(CP)	(MG_CM3_SRAM_BASE(CP) + 0x1FC00ULL)
+
+#define MG_SRAM_SIZE	0x20000 /* 128KB */
+
+#define MG_ACK_TIMEOUT 10
+
+/**
+ * struct ap_sharedmem_ctrl - used to pass information between the HOST and CM3
+ * @init_done:	Set by CM3 when ap_proces initialzied. Host check if CM3 set
+ *		this flag to confirm that the process is running
+ * @lane_nr:	Set by Host to mark which comphy lane should be configure. E.g.:
+ *		- A8K development board uses comphy lane 2 for eth0
+ *		- CN913x development board uses comphy lane 4 for eth0
+ */
+struct ap_sharedmem_ctrl {
+	uint32_t init_done;
+	uint32_t lane_nr;
+};
+
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index)
+{
+	uintptr_t mg_regs = MG_CM3_SRAM_BASE(cp_index);
+
+	if (size > MG_SRAM_SIZE) {
+		ERROR("image is too big to fit into MG CM3 memory\n");
+		return 1;
+	}
+
+	NOTICE("Loading MG image from address 0x%lx Size 0x%x to MG at 0x%lx\n",
+	       src_addr, size, mg_regs);
+
+	/* Copy image to MG CM3 SRAM */
+	memcpy((void *)mg_regs, (void *)src_addr, size);
+
+	/* Don't release MG CM3 from reset - it will be done by next step
+	 * bootloader (e.g. U-Boot), when appriopriate device-tree setup (which
+	 * has enabeld 802.3. auto-neg) will be choosen.
+	 */
+
+	return 0;
+}
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index)
+{
+	volatile struct ap_sharedmem_ctrl *ap_shared_ctrl =
+					(void *)MG_CM3_SHARED_MEM_BASE(cp_nr);
+	int timeout = MG_ACK_TIMEOUT;
+
+	if (mmio_read_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr)) & CM3_CPU_EN_BIT) {
+		VERBOSE("cm3 already running\n");
+		return;  /* cm3 already running */
+	}
+
+	/*
+	 * Mark which comphy lane should be used - it will be read via shared
+	 * mem by ap process
+	 */
+	ap_shared_ctrl->lane_nr = comphy_index;
+	/* Make sure it took place before enabling cm3 */
+	dmbst();
+
+	mmio_setbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr), CM3_CPU_EN_BIT);
+	mmio_setbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr), CM3_SYS_RESET_BIT);
+
+	/* Check for ap process initialization by fw */
+	while (ap_shared_ctrl->init_done != 1 && timeout--)
+		VERBOSE("Waiting for ap process ack, timeout %d\n", timeout);
+
+	if (timeout == 0) {
+		ERROR("AP process failed, disabling cm3\n");
+		mmio_clrbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr),
+				CM3_SYS_RESET_BIT);
+		mmio_clrbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr),
+				CM3_CPU_EN_BIT);
+	}
+}
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
new file mode 100644
index 000000000..e2756de9c
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -0,0 +1,9 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index);
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index);
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 2f4dcadfc..d6cd8b1ce 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -577,6 +577,43 @@ static const uint8_t stm32mp1_axi_div[8] = {
 	1, 2, 3, 4, 4, 4, 4, 4
 };
 
+static const char * const stm32mp1_clk_parent_name[_PARENT_NB] __unused = {
+	[_HSI] = "HSI",
+	[_HSE] = "HSE",
+	[_CSI] = "CSI",
+	[_LSI] = "LSI",
+	[_LSE] = "LSE",
+	[_I2S_CKIN] = "I2S_CKIN",
+	[_HSI_KER] = "HSI_KER",
+	[_HSE_KER] = "HSE_KER",
+	[_HSE_KER_DIV2] = "HSE_KER_DIV2",
+	[_CSI_KER] = "CSI_KER",
+	[_PLL1_P] = "PLL1_P",
+	[_PLL1_Q] = "PLL1_Q",
+	[_PLL1_R] = "PLL1_R",
+	[_PLL2_P] = "PLL2_P",
+	[_PLL2_Q] = "PLL2_Q",
+	[_PLL2_R] = "PLL2_R",
+	[_PLL3_P] = "PLL3_P",
+	[_PLL3_Q] = "PLL3_Q",
+	[_PLL3_R] = "PLL3_R",
+	[_PLL4_P] = "PLL4_P",
+	[_PLL4_Q] = "PLL4_Q",
+	[_PLL4_R] = "PLL4_R",
+	[_ACLK] = "ACLK",
+	[_PCLK1] = "PCLK1",
+	[_PCLK2] = "PCLK2",
+	[_PCLK3] = "PCLK3",
+	[_PCLK4] = "PCLK4",
+	[_PCLK5] = "PCLK5",
+	[_HCLK6] = "KCLK6",
+	[_HCLK2] = "HCLK2",
+	[_CK_PER] = "CK_PER",
+	[_CK_MPU] = "CK_MPU",
+	[_CK_MCU] = "CK_MCU",
+	[_USB_PHY_48] = "USB_PHY_48",
+};
+
 /* RCC clock device driver private */
 static unsigned long stm32mp1_osc[NB_OSC];
 static struct spinlock reg_lock;
@@ -2007,6 +2044,165 @@ static void stm32mp1_osc_init(void)
 	}
 }
 
+#ifdef STM32MP_SHARED_RESOURCES
+/*
+ * Get the parent ID of the target parent clock, for tagging as secure
+ * shared clock dependencies.
+ */
+static int get_parent_id_parent(unsigned int parent_id)
+{
+	enum stm32mp1_parent_sel s = _UNKNOWN_SEL;
+	enum stm32mp1_pll_id pll_id;
+	uint32_t p_sel;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (parent_id) {
+	case _ACLK:
+	case _PCLK4:
+	case _PCLK5:
+		s = _AXIS_SEL;
+		break;
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+		pll_id = _PLL1;
+		break;
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+		pll_id = _PLL2;
+		break;
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		pll_id = _PLL3;
+		break;
+	case _PLL4_P:
+	case _PLL4_Q:
+	case _PLL4_R:
+		pll_id = _PLL4;
+		break;
+	case _PCLK1:
+	case _PCLK2:
+	case _HCLK2:
+	case _HCLK6:
+	case _CK_PER:
+	case _CK_MPU:
+	case _CK_MCU:
+	case _USB_PHY_48:
+		/* We do not expect to access these */
+		panic();
+		break;
+	default:
+		/* Other parents have no parent */
+		return -1;
+	}
+
+	if (s != _UNKNOWN_SEL) {
+		const struct stm32mp1_clk_sel *sel = clk_sel_ref(s);
+
+		p_sel = (mmio_read_32(rcc_base + sel->offset) >> sel->src) &
+			sel->msk;
+
+		if (p_sel < sel->nb_parent) {
+			return (int)sel->parent[p_sel];
+		}
+	} else {
+		const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+
+		p_sel = mmio_read_32(rcc_base + pll->rckxselr) &
+			RCC_SELR_REFCLK_SRC_MASK;
+
+		if (pll->refclk[p_sel] != _UNKNOWN_OSC_ID) {
+			return (int)pll->refclk[p_sel];
+		}
+	}
+
+	VERBOSE("No parent selected for %s\n",
+		stm32mp1_clk_parent_name[parent_id]);
+
+	return -1;
+}
+
+static void secure_parent_clocks(unsigned long parent_id)
+{
+	int grandparent_id;
+
+	switch (parent_id) {
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+
+	/* These clocks are always secure when RCC is secure */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+	case _HSI:
+	case _HSI_KER:
+	case _LSI:
+	case _CSI:
+	case _CSI_KER:
+	case _HSE:
+	case _HSE_KER:
+	case _HSE_KER_DIV2:
+	case _LSE:
+		break;
+
+	default:
+		VERBOSE("Cannot secure parent clock %s\n",
+			stm32mp1_clk_parent_name[parent_id]);
+		panic();
+	}
+
+	grandparent_id = get_parent_id_parent(parent_id);
+	if (grandparent_id >= 0) {
+		secure_parent_clocks(grandparent_id);
+	}
+}
+
+void stm32mp1_register_clock_parents_secure(unsigned long clock_id)
+{
+	int parent_id;
+
+	if (!stm32mp1_rcc_is_secure()) {
+		return;
+	}
+
+	switch (clock_id) {
+	case PLL1:
+	case PLL2:
+		/* PLL1/PLL2 are always secure: nothing to do */
+		break;
+	case PLL3:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3);
+		break;
+	case PLL4:
+		ERROR("PLL4 cannot be secured\n");
+		panic();
+		break;
+	default:
+		/* Others are expected gateable clock */
+		parent_id = stm32mp1_clk_get_parent(clock_id);
+		if (parent_id < 0) {
+			INFO("No parent found for clock %lu\n", clock_id);
+		} else {
+			secure_parent_clocks(parent_id);
+		}
+		break;
+	}
+}
+#endif /* STM32MP_SHARED_RESOURCES */
+
 static void sync_earlyboot_clocks_state(void)
 {
 	unsigned int idx;
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
index 3184df9de..515947c10 100644
--- a/drivers/st/crypto/stm32_hash.c
+++ b/drivers/st/crypto/stm32_hash.c
@@ -300,7 +300,9 @@ int stm32_hash_register(void)
 			break;
 		}
 #else
+		/* BL32 uses hash if it is assigned only to secure world */
 		if (hash_info.status == DT_SECURE) {
+			stm32mp_register_secure_periph_iomem(hash_info.base);
 			break;
 		}
 #endif
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index a13c341a8..bb77371bf 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -254,6 +254,15 @@ void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 		mmio_read_32(base + GPIO_AFRH_OFFSET));
 
 	stm32mp_clk_disable(clock);
+
+	if (status == DT_SECURE) {
+		stm32mp_register_secure_gpio(bank, pin);
+		set_gpio_secure_cfg(bank, pin, true);
+
+	} else {
+		stm32mp_register_non_secure_gpio(bank, pin);
+		set_gpio_secure_cfg(bank, pin, false);
+	}
 }
 
 void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
index ea6fbb2b9..c052b4dfb 100644
--- a/drivers/st/iwdg/stm32_iwdg.c
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -137,6 +137,12 @@ int stm32_iwdg_init(void)
 			((dt_info.status & DT_NON_SECURE) != 0) ?
 			"non-" : "");
 
+		if ((dt_info.status & DT_NON_SECURE) != 0) {
+			stm32mp_register_non_secure_periph_iomem(iwdg->base);
+		} else {
+			stm32mp_register_secure_periph_iomem(iwdg->base);
+		}
+
 #if defined(IMAGE_BL2)
 		if (stm32_iwdg_shadow_update(idx, iwdg->flags) != BSEC_OK) {
 			return -1;
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
index 9e9dddc4d..b2bb482f9 100644
--- a/drivers/st/pmic/stm32mp_pmic.c
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -54,6 +54,15 @@ int dt_pmic_status(void)
 	return fdt_get_status(node);
 }
 
+static bool dt_pmic_is_secure(void)
+{
+	int status = dt_pmic_status();
+
+	return (status >= 0) &&
+	       (status == DT_SECURE) &&
+	       (i2c_handle.dt_status == DT_SECURE);
+}
+
 /*
  * Get PMIC and its I2C bus configuration from the device tree.
  * Return 0 on success, negative on error, 1 if no PMIC node is found.
@@ -223,6 +232,19 @@ bool initialize_pmic_i2c(void)
 	return true;
 }
 
+static void register_pmic_shared_peripherals(void)
+{
+	uintptr_t i2c_base = i2c_handle.i2c_base_addr;
+
+	if (dt_pmic_is_secure()) {
+		stm32mp_register_secure_periph_iomem(i2c_base);
+	} else {
+		if (i2c_base != 0U) {
+			stm32mp_register_non_secure_periph_iomem(i2c_base);
+		}
+	}
+}
+
 void initialize_pmic(void)
 {
 	unsigned long pmic_version;
@@ -232,6 +254,8 @@ void initialize_pmic(void)
 		return;
 	}
 
+	register_pmic_shared_peripherals();
+
 	if (stpmic1_get_version(&pmic_version) != 0) {
 		ERROR("Failed to access PMIC\n");
 		panic();