Merge pull request #1603 from antonio-nino-diaz-arm/db/reclaim-init

Reclaim BL31 initialization code memory for runtime data
2018-10-04 14:23:31 +01:00 · 2018-10-04 14:23:31 +01:00 · 7e0a38a4d8
parent e44b8a9d48 cb4adb0d8c
commit 7e0a38a4d8
40 changed files with 192 additions and 62 deletions
--- a/1
+++ b/1
@ -634,6 +634,7 @@ $(eval $(call add_define,PSCI_EXTENDED_STATE_ID))
 $(eval $(call add_define,RAS_EXTENSION))
 $(eval $(call add_define,RESET_TO_BL31))
 $(eval $(call add_define,SEPARATE_CODE_AND_RODATA))
+$(eval $(call add_define,RECLAIM_INIT_CODE))
 $(eval $(call add_define,SMCCC_MAJOR_VERSION))
 $(eval $(call add_define,SPD_${SPD}))
 $(eval $(call add_define,SPIN_ON_BL1_EXIT))
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@ -57,7 +57,7 @@ uintptr_t get_arm_std_svc_args(unsigned int svc_mask)
 /*******************************************************************************
 * Simple function to initialise all BL31 helper libraries.
 ******************************************************************************/
-void bl31_lib_init(void)
+void __init bl31_lib_init(void)
 {
 	cm_init();
 }
@ -149,7 +149,7 @@ uint32_t bl31_get_next_image_type(void)
 * This function programs EL3 registers and performs other setup to enable entry
 * into the next image after BL31 at the next ERET.
 ******************************************************************************/
-void bl31_prepare_next_image_entry(void)
+void __init bl31_prepare_next_image_entry(void)
 {
 	entry_point_info_t *next_image_info;
 	uint32_t image_type;
--- a/bl31/ehf.c
+++ b/bl31/ehf.c
@ -451,7 +451,7 @@ static uint64_t ehf_el3_interrupt_handler(uint32_t id, uint32_t flags,
 /*
 * Initialize the EL3 exception handling.
 */
-void ehf_init(void)
+void __init ehf_init(void)
 {
 	unsigned int flags = 0;
 	int ret __unused;
--- a/common/runtime_svc.c
+++ b/common/runtime_svc.c
@ -93,7 +93,7 @@ static int32_t validate_rt_svc_desc(const rt_svc_desc_t *desc)
 * The unique oen is used as an index into the 'rt_svc_descs_indices' array.
 * The index of the runtime service descriptor is stored at this index.
 ******************************************************************************/
-void runtime_svc_init(void)
+void __init runtime_svc_init(void)
 {
 	int rc = 0;
 	unsigned int index, start_idx, end_idx;
--- a/docs/firmware-design.rst
+++ b/docs/firmware-design.rst
@ -2336,6 +2336,29 @@ implement:

   SUBSCRIBE_TO_EVENT(foo, foo_handler);

+
+Reclaiming the BL31 initialization code
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+A significant amount of the code used for the initialization of BL31 is never
+needed again after boot time. In order to reduce the runtime memory
+footprint, the memory used for this code can be reclaimed after initialization
+has finished and be used for runtime data.
+
+The build option ``RECLAIM_INIT_CODE`` can be set to mark this boot time code
+with a ``.text.init.*`` attribute which can be filtered and placed suitably
+within the BL image for later reclaimation by the platform. The platform can
+specify the fiter and the memory region for this init section in BL31 via the
+plat.ld.S linker script. For example, on the FVP, this section is placed
+overlapping the secondary CPU stacks so that after the cold boot is done, this
+memory can be reclaimed for the stacks. The init memory section is initially
+mapped with ``RO``, ``EXECUTE`` attributes. After BL31 initilization has
+completed, the FVP changes the attributes of this section to ``RW``,
+``EXECUTE_NEVER`` allowing it to be used for runtime data. The memory attributes
+are changed within the ``bl31_plat_runtime_setup`` platform hook. The init
+section section can be reclaimed for any data which is accessed after cold
+boot initialization and it is upto the platform to make the decision.
+
 Performance Measurement Framework
 ---------------------------------

--- a/drivers/arm/cci/cci.c
+++ b/drivers/arm/cci/cci.c
@ -107,7 +107,8 @@ static int get_slave_ports(unsigned int part_num)
 }
 #endif /* ENABLE_ASSERTIONS */

-void cci_init(uintptr_t base, const int *map, unsigned int num_cci_masters)
+void __init cci_init(uintptr_t base, const int *map,
+				unsigned int num_cci_masters)
 {
 	assert(map != NULL);
 	assert(base != 0U);
--- a/drivers/arm/ccn/ccn.c
+++ b/drivers/arm/ccn/ccn.c
@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
@ -167,7 +167,7 @@ static unsigned int ccn_get_rn_master_info(uintptr_t periphbase,
 * It compares this with the information provided by the platform to determine
 * the validity of the latter.
 ******************************************************************************/
-static void ccn_validate_plat_params(const ccn_desc_t *plat_desc)
+static void __init ccn_validate_plat_params(const ccn_desc_t *plat_desc)
 {
 	unsigned int master_id, num_rn_masters;
 	rn_info_t info = { {0} };
@ -208,7 +208,7 @@ static void ccn_validate_plat_params(const ccn_desc_t *plat_desc)
 * simultaneous CCN operations at runtime (only BL31) to add and remove Request
 * nodes from coherency.
 ******************************************************************************/
-void ccn_init(const ccn_desc_t *plat_desc)
+void __init ccn_init(const ccn_desc_t *plat_desc)
 {
 #if ENABLE_ASSERTIONS
 	ccn_validate_plat_params(plat_desc);
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@ -55,7 +55,7 @@ static spinlock_t gic_lock;
 * This function initialises the ARM GICv3 driver in EL3 with provided platform
 * inputs.
 ******************************************************************************/
-void gicv3_driver_init(const gicv3_driver_data_t *plat_driver_data)
+void __init gicv3_driver_init(const gicv3_driver_data_t *plat_driver_data)
 {
 	unsigned int gic_version;

@ -129,7 +129,7 @@ void gicv3_driver_init(const gicv3_driver_data_t *plat_driver_data)
 * This function initialises the GIC distributor interface based upon the data
 * provided by the platform while initialising the driver.
 ******************************************************************************/
-void gicv3_distif_init(void)
+void __init gicv3_distif_init(void)
 {
 	unsigned int bitmap = 0;

--- a/drivers/arm/smmu/smmu_v3.c
+++ b/drivers/arm/smmu/smmu_v3.c
@ -4,21 +4,22 @@
 * SPDX-License-Identifier: BSD-3-Clause
 */

+#include <cdefs.h>
 #include <mmio.h>
 #include <smmu_v3.h>
 #include <stdbool.h>

-static inline uint32_t smmuv3_read_s_idr1(uintptr_t base)
+static inline uint32_t __init smmuv3_read_s_idr1(uintptr_t base)
 {
 	return mmio_read_32(base + SMMU_S_IDR1);
 }

-static inline uint32_t smmuv3_read_s_init(uintptr_t base)
+static inline uint32_t __init smmuv3_read_s_init(uintptr_t base)
 {
 	return mmio_read_32(base + SMMU_S_INIT);
 }

-static inline void smmuv3_write_s_init(uintptr_t base, uint32_t value)
+static inline void __init smmuv3_write_s_init(uintptr_t base, uint32_t value)
 {
 	mmio_write_32(base + SMMU_S_INIT, value);
 }
@ -34,7 +35,7 @@ static inline bool smmuv3_inval_pending(uintptr_t base)
 *
 * Returns 0 on success, and -1 on failure.
 */
-int smmuv3_init(uintptr_t smmu_base)
+int __init smmuv3_init(uintptr_t smmu_base)
 {
 	uint32_t idr1_reg;

--- a/include/lib/libc/cdefs.h
+++ b/include/lib/libc/cdefs.h
@ -14,6 +14,15 @@
 #define __unused	__attribute__((__unused__))
 #define __aligned(x)	__attribute__((__aligned__(x)))
 #define __section(x)	__attribute__((__section__(x)))
+#if RECLAIM_INIT_CODE
+/*
+ * Add each function to a section that is unique so the functions can still
+ * be garbage collected
+ */
+#define __init		__section(".text.init." __FILE__ "." __XSTRING(__LINE__))
+#else
+#define __init
+#endif

 #define __printflike(fmtarg, firstvararg) \
 		__attribute__((__format__ (__printf__, fmtarg, firstvararg)))
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@ -280,7 +280,7 @@
 * The max number of regions like RO(code), coherent and data required by
 * different BL stages which need to be mapped in the MMU.
 */
-# define ARM_BL_REGIONS			4
+#define ARM_BL_REGIONS			5

 #define MAX_MMAP_REGIONS		(PLAT_ARM_MMAP_ENTRIES +	\
 					 ARM_BL_REGIONS)
--- a/include/plat/arm/common/arm_reclaim_init.ld.S
+++ b/include/plat/arm/common/arm_reclaim_init.ld.S
@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef ARM_RECLAIM_INIT_LD_S
+#define ARM_RECLAIM_INIT_LD_S
+
+SECTIONS
+{
+        .init __STACKS_START__ : {
+            . = . + PLATFORM_STACK_SIZE;
+            . = ALIGN(PAGE_SIZE);
+            __INIT_CODE_START__ = .;
+            /*
+             * Exclude PSCI initialization functions to ensure the init section
+             * does not become larger than the overlaid stack region
+             */
+            *(EXCLUDE_FILE (*psci_setup.o).text.init*)
+            __INIT_CODE_UNALIGNED__ = .;
+            .  = ALIGN(PAGE_SIZE);
+            __INIT_CODE_END__ = .;
+        } >RAM
+
+#ifdef BL31_PROGBITS_LIMIT
+    ASSERT(__INIT_CODE_END__ <= BL31_PROGBITS_LIMIT,
+            "BL31 init has exceeded progbits limit.")
+#endif
+
+#if RECLAIM_INIT_CODE
+    ASSERT(__INIT_CODE_END__ <= __STACKS_END__,
+        "Init code ends past the end of the stacks")
+#endif
+}
+
+#endif /* ARM_RECLAIM_INIT_LD_S */
--- a/include/plat/arm/common/arm_tzc_dram.ld.S
+++ b/include/plat/arm/common/arm_tzc_dram.ld.S
@ -3,8 +3,8 @@
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
-#ifndef __ARM_COMMON_LD_S__
-#define __ARM_COMMON_LD_S__
+#ifndef ARM_TZC_DRAM_LD_S__
+#define ARM_TZC_DRAM_LD_S__

 #include <xlat_tables_defs.h>

@ -27,4 +27,4 @@ SECTIONS
 	} >EL3_SEC_DRAM
 }

-#endif /* __ARM_COMMON_LD_S__ */
+#endif /* ARM_TZC_DRAM_LD_S__ */
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@ -220,6 +220,12 @@ void arm_bl2_dyn_cfg_init(void);
 void arm_bl1_set_mbedtls_heap(void);
 int arm_get_mbedtls_heap(void **heap_addr, size_t *heap_size);

+/*
+ * Free the memory storing initialization code only used during an images boot
+ * time so it can be reclaimed for runtime data
+ */
+void arm_free_init_memory(void);
+
 /*
 * Mandatory functions required in ARM standard platforms
 */
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@ -36,7 +36,7 @@
 * which will used for programming an entry into a lower EL. The same context
 * will used to save state upon exception entry from that EL.
 ******************************************************************************/
-void cm_init(void)
+void __init cm_init(void)
 {
 	/*
 	 * The context management library has only global data to intialize, but
--- a/lib/extensions/ras/ras_common.c
+++ b/lib/extensions/ras/ras_common.c
@ -128,7 +128,7 @@ static int ras_interrupt_handler(uint32_t intr_raw, uint32_t flags,
 	return 0;
 }

-void ras_init(void)
+void __init ras_init(void)
 {
 #if ENABLE_ASSERTIONS
 	/* Check RAS interrupts are sorted */
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@ -216,7 +216,7 @@ static void psci_set_req_local_pwr_state(unsigned int pwrlvl,
 /******************************************************************************
 * This function initializes the psci_req_local_pwr_states.
 *****************************************************************************/
-void psci_init_req_local_pwr_states(void)
+void __init psci_init_req_local_pwr_states(void)
 {
 	/* Initialize the requested state of all non CPU power domains as OFF */
 	unsigned int pwrlvl;
--- a/lib/psci/psci_setup.c
+++ b/lib/psci/psci_setup.c
@ -32,7 +32,7 @@ unsigned int psci_caps;
 * Function which initializes the 'psci_non_cpu_pd_nodes' or the
 * 'psci_cpu_pd_nodes' corresponding to the power level.
 ******************************************************************************/
-static void psci_init_pwr_domain_node(unsigned char node_idx,
+static void __init psci_init_pwr_domain_node(unsigned char node_idx,
 					unsigned int parent_idx,
 					unsigned char level)
 {
@ -80,7 +80,7 @@ static void psci_init_pwr_domain_node(unsigned char node_idx,
 * mapping of the CPUs to indices via plat_core_pos_by_mpidr() and
 * plat_my_core_pos() APIs.
 *******************************************************************************/
-static void psci_update_pwrlvl_limits(void)
+static void __init psci_update_pwrlvl_limits(void)
 {
 	int j, cpu_idx;
 	unsigned int nodes_idx[PLAT_MAX_PWR_LVL] = {0};
@ -107,7 +107,7 @@ static void psci_update_pwrlvl_limits(void)
 * informs the number of root power domains. The parent nodes of the root nodes
 * will point to an invalid entry(-1).
 ******************************************************************************/
-static void populate_power_domain_tree(const unsigned char *topology)
+static void __init populate_power_domain_tree(const unsigned char *topology)
 {
 	unsigned int i, j = 0U, num_nodes_at_lvl = 1U, num_nodes_at_next_lvl;
 	unsigned int node_index = 0U, num_children;
@ -184,7 +184,7 @@ static void populate_power_domain_tree(const unsigned char *topology)
 * |   CPU 0   |   CPU 1   |   CPU 2   |   CPU 3  |
 * ------------------------------------------------
 ******************************************************************************/
-int psci_setup(const psci_lib_args_t *lib_args)
+int __init psci_setup(const psci_lib_args_t *lib_args)
 {
 	const unsigned char *topology_tree;

--- a/lib/xlat_tables/xlat_tables_common.c
+++ b/lib/xlat_tables/xlat_tables_common.c
@ -176,7 +176,7 @@ void mmap_add(const mmap_region_t *mm)
 {
 	const mmap_region_t *mm_cursor = mm;

-	while (mm_cursor->size != 0U) {
+	while (mm_cursor->attr != 0U) {
 		mmap_add_region(mm_cursor->base_pa, mm_cursor->base_va,
 				mm_cursor->size, mm_cursor->attr);
 		mm_cursor++;
--- a/lib/xlat_tables_v2/xlat_tables_context.c
+++ b/lib/xlat_tables_v2/xlat_tables_context.c
@ -56,7 +56,7 @@ int mmap_remove_dynamic_region(uintptr_t base_va, size_t size)

 #endif /* PLAT_XLAT_TABLES_DYNAMIC */

-void init_xlat_tables(void)
+void __init init_xlat_tables(void)
 {
 	assert(tf_xlat_ctx.xlat_regime == EL_REGIME_INVALID);

--- a/lib/xlat_tables_v2/xlat_tables_core.c
+++ b/lib/xlat_tables_v2/xlat_tables_core.c
@ -815,7 +815,7 @@ void mmap_add_ctx(xlat_ctx_t *ctx, const mmap_region_t *mm)
 {
 	const mmap_region_t *mm_cursor = mm;

-	while (mm_cursor->size != 0U) {
+	while (mm_cursor->attr != 0U) {
 		mmap_add_region_ctx(ctx, mm_cursor);
 		mm_cursor++;
 	}
@ -1012,7 +1012,7 @@ int mmap_remove_dynamic_region_ctx(xlat_ctx_t *ctx, uintptr_t base_va,

 #endif /* PLAT_XLAT_TABLES_DYNAMIC */

-void init_xlat_tables_ctx(xlat_ctx_t *ctx)
+void __init init_xlat_tables_ctx(xlat_ctx_t *ctx)
 {
 	assert(ctx != NULL);
 	assert(!ctx->initialized);
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@ -252,10 +252,11 @@ endef
 define MAKE_LD

 $(eval DEP := $(1).d)
+$(eval IMAGE := IMAGE_BL$(call uppercase,$(3)))

 $(1): $(2) $(filter-out %.d,$(MAKEFILE_LIST)) | bl$(3)_dirs
 	@echo "  PP      $$<"
-	$$(Q)$$(CPP) $$(CPPFLAGS) -P -D__ASSEMBLY__ -D__LINKER__ $(MAKE_DEP) -o $$@ $$<
+	$$(Q)$$(CPP) $$(CPPFLAGS) -P -D__ASSEMBLY__ -D__LINKER__ $(MAKE_DEP) -D$(IMAGE) -o $$@ $$<

 -include $(DEP)

--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@ -146,6 +146,10 @@ SDEI_SUPPORT            	:= 0
 # platform Makefile is free to override this value.
 SEPARATE_CODE_AND_RODATA	:= 0

+# If the BL31 image initialisation code is recalimed after use for the secondary
+# cores stack
+RECLAIM_INIT_CODE		:= 0
+
 # Default to SMCCC Version 1.X
 SMCCC_MAJOR_VERSION		:= 1

--- a/plat/arm/board/fvp/drivers/pwrc/fvp_pwrc.c
+++ b/plat/arm/board/fvp/drivers/pwrc/fvp_pwrc.c
@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
@ -70,7 +70,7 @@ void fvp_pwrc_write_pcoffr(u_register_t mpidr)
 }

 /* Nothing else to do here apart from initializing the lock */
-void plat_arm_pwrc_setup(void)
+void __init plat_arm_pwrc_setup(void)
 {
 	arm_lock_init();
 }
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c
@ -10,8 +10,8 @@
 #include <smmu_v3.h>
 #include "fvp_private.h"

-void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
-		u_register_t arg2, u_register_t arg3)
+void __init bl31_early_platform_setup2(u_register_t arg0,
+		u_register_t arg1, u_register_t arg2, u_register_t arg3)
 {
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);

--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@ -241,7 +241,7 @@ const struct secure_partition_boot_info *plat_get_secure_partition_boot_info(
 * these platforms. This information is stored in a per-BL array to allow the
 * code to take the correct path.Per BL platform configuration.
 ******************************************************************************/
-void fvp_config_setup(void)
+void __init fvp_config_setup(void)
 {
 	unsigned int rev, hbi, bld, arch, sys_id;

@ -331,7 +331,7 @@ void fvp_config_setup(void)
 }


-void fvp_interconnect_init(void)
+void __init fvp_interconnect_init(void)
 {
 #if FVP_INTERCONNECT_DRIVER == FVP_CCN
 	if (ccn_get_part0_id(PLAT_ARM_CCN_BASE) != CCN_502_PART0_ID) {
--- a/plat/arm/board/fvp/include/plat.ld.S
+++ b/plat/arm/board/fvp/include/plat.ld.S
@ -6,6 +6,7 @@
 #ifndef __PLAT_LD_S__
 #define __PLAT_LD_S__

-#include <arm_common.ld.S>
+#include <arm_tzc_dram.ld.S>
+#include <arm_reclaim_init.ld.S>

 #endif /* __PLAT_LD_S__ */
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@ -201,6 +201,9 @@ ENABLE_AMU			:=	1
 # Enable dynamic mitigation support by default
 DYNAMIC_WORKAROUND_CVE_2018_3639	:=	1

+# Enable reclaiming of BL31 initialisation code for secondary cores stacks for FVP
+RECLAIM_INIT_CODE	:=	1
+
 ifeq (${ENABLE_AMU},1)
 BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a75_pubsub.c	\
 				lib/cpus/aarch64/cortex_ares_pubsub.c	\
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@ -15,6 +15,8 @@
 #include <plat_arm.h>
 #include <platform.h>
 #include <ras.h>
+#include <utils.h>
+#include <arm_xlat_tables.h>

 /*
 * Placeholder variables for copying the arguments that have been passed to
@ -35,10 +37,20 @@ CASSERT(BL31_BASE >= ARM_TB_FW_CONFIG_LIMIT, assert_bl31_base_overflows);
 #pragma weak bl31_plat_arch_setup
 #pragma weak bl31_plat_get_next_image_ep_info

-#define MAP_BL31_TOTAL	MAP_REGION_FLAT(			\
+#define MAP_BL31_TOTAL		MAP_REGION_FLAT(			\
 					BL31_BASE,			\
 					BL31_END - BL31_BASE,		\
 					MT_MEMORY | MT_RW | MT_SECURE)
+#if RECLAIM_INIT_CODE
+IMPORT_SYM(unsigned long, __INIT_CODE_START__, BL_INIT_CODE_BASE);
+IMPORT_SYM(unsigned long, __INIT_CODE_END__, BL_INIT_CODE_END);
+
+#define MAP_BL_INIT_CODE	MAP_REGION_FLAT(			\
+					BL_INIT_CODE_BASE,		\
+					BL_INIT_CODE_END		\
+						- BL_INIT_CODE_BASE,	\
+					MT_CODE | MT_SECURE)
+#endif

 /*******************************************************************************
 * Return a pointer to the 'entry_point_info' structure of the next image for the
@ -71,7 +83,7 @@ struct entry_point_info *bl31_plat_get_next_image_ep_info(uint32_t type)
 * while creating page tables. BL2 has flushed this information to memory, so
 * we are guaranteed to pick up good data.
 ******************************************************************************/
-void arm_bl31_early_platform_setup(void *from_bl2, uintptr_t soc_fw_config,
+void __init arm_bl31_early_platform_setup(void *from_bl2, uintptr_t soc_fw_config,
 				uintptr_t hw_config, void *plat_params_from_bl2)
 {
 	/* Initialize the console to provide early debug support */
@ -233,9 +245,30 @@ void arm_bl31_plat_runtime_setup(void)

 	/* Initialize the runtime console */
 	arm_console_runtime_init();
+#if RECLAIM_INIT_CODE
+	arm_free_init_memory();
+#endif
 }

-void bl31_platform_setup(void)
+#if RECLAIM_INIT_CODE
+/*
+ * Zero out and make RW memory used to store image boot time code so it can
+ * be reclaimed during runtime
+ */
+void arm_free_init_memory(void)
+{
+	int ret = xlat_change_mem_attributes(BL_INIT_CODE_BASE,
+				BL_INIT_CODE_END - BL_INIT_CODE_BASE,
+				MT_RW_DATA);
+
+	if (ret != 0) {
+		ERROR("Could not reclaim initialization code");
+		panic();
+	}
+}
+#endif
+
+void __init bl31_platform_setup(void)
 {
 	arm_bl31_platform_setup();
 }
@ -251,16 +284,13 @@ void bl31_plat_runtime_setup(void)
 * architectural setup (bl31_arch_setup()) does not do anything platform
 * specific.
 ******************************************************************************/
-void arm_bl31_plat_arch_setup(void)
+void __init arm_bl31_plat_arch_setup(void)
 {
-
-#define ARM_MAP_BL_ROMLIB	MAP_REGION_FLAT(			\
-					BL31_BASE,			\
-					BL31_END - BL31_BASE,		\
-					MT_MEMORY | MT_RW | MT_SECURE)
-
 	const mmap_region_t bl_regions[] = {
 		MAP_BL31_TOTAL,
+#if RECLAIM_INIT_CODE
+		MAP_BL_INIT_CODE,
+#endif
 		ARM_MAP_BL_RO,
 #if USE_ROMLIB
 		ARM_MAP_ROMLIB_CODE,
@ -279,7 +309,7 @@ void arm_bl31_plat_arch_setup(void)
 	arm_setup_romlib();
 }

-void bl31_plat_arch_setup(void)
+void __init bl31_plat_arch_setup(void)
 {
 	arm_bl31_plat_arch_setup();
 }
--- a/plat/arm/common/arm_cci.c
+++ b/plat/arm/common/arm_cci.c
@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
@ -27,7 +27,7 @@ static const int cci_map[] = {
 /******************************************************************************
 * Helper function to initialize ARM CCI driver.
 *****************************************************************************/
-void plat_arm_interconnect_init(void)
+void __init plat_arm_interconnect_init(void)
 {
 	cci_init(PLAT_ARM_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
 }
--- a/plat/arm/common/arm_ccn.c
+++ b/plat/arm/common/arm_ccn.c
@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
@ -34,7 +34,7 @@ CASSERT(PLAT_ARM_CLUSTER_COUNT == ARRAY_SIZE(master_to_rn_id_map),
 /******************************************************************************
 * Helper function to initialize ARM CCN driver.
 *****************************************************************************/
-void plat_arm_interconnect_init(void)
+void __init plat_arm_interconnect_init(void)
 {
 	ccn_init(&arm_ccn_desc);
 }
--- a/plat/arm/common/arm_common.c
+++ b/plat/arm/common/arm_common.c
@ -38,10 +38,11 @@ void arm_setup_romlib(void)
 * as an array specifying the generic memory regions which can be;
 * - Code section;
 * - Read-only data section;
+ * - Init code section, if applicable
 * - Coherent memory region, if applicable.
 */

-void arm_setup_page_tables(const mmap_region_t bl_regions[],
+void __init arm_setup_page_tables(const mmap_region_t bl_regions[],
 			   const mmap_region_t plat_regions[])
 {
 #if LOG_LEVEL >= LOG_LEVEL_VERBOSE
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@ -273,3 +273,14 @@ endif
    include ${IMG_PARSER_LIB_MK}

 endif
+
+# RECLAIM_INIT_CODE can only be set when LOAD_IMAGE_V2=2 and xlat tables v2
+# are used
+ifeq (${RECLAIM_INIT_CODE}, 1)
+    ifeq (${LOAD_IMAGE_V2}, 0)
+        $(error "LOAD_IMAGE_V2 must be enabled to use RECLAIM_INIT_CODE")
+    endif
+    ifeq (${ARM_XLAT_TABLES_LIB_V1}, 1)
+        $(error "To reclaim init code xlat tables v2 must be used")
+    endif
+endif
--- a/plat/arm/common/arm_console.c
+++ b/plat/arm/common/arm_console.c
@ -19,7 +19,7 @@ static console_pl011_t arm_runtime_console;
 #endif

 /* Initialize the console to provide early debug support */
-void arm_console_boot_init(void)
+void __init arm_console_boot_init(void)
 {
 #if MULTI_CONSOLE_API
 	int rc = console_pl011_register(PLAT_ARM_BOOT_UART_BASE,
--- a/plat/arm/common/arm_gicv3.c
+++ b/plat/arm/common/arm_gicv3.c
@ -68,7 +68,7 @@ static const gicv3_driver_data_t arm_gic_data __unused = {
 	.mpidr_to_core_pos = arm_gicv3_mpidr_hash
 };

-void plat_arm_gic_driver_init(void)
+void __init plat_arm_gic_driver_init(void)
 {
 	/*
 	 * The GICv3 driver is initialized in EL3 and does not need
@ -85,7 +85,7 @@ void plat_arm_gic_driver_init(void)
 /******************************************************************************
 * ARM common helper to initialize the GIC. Only invoked by BL31
 *****************************************************************************/
-void plat_arm_gic_init(void)
+void __init plat_arm_gic_init(void)
 {
 	gicv3_distif_init();
 	gicv3_rdistif_init(plat_my_core_pos());
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@ -208,7 +208,7 @@ void plat_arm_program_trusted_mailbox(uintptr_t address)
 * The ARM Standard platform definition of platform porting API
 * `plat_setup_psci_ops`.
 ******************************************************************************/
-int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+int __init plat_setup_psci_ops(uintptr_t sec_entrypoint,
 				const plat_psci_ops_t **psci_ops)
 {
 	*psci_ops = plat_arm_psci_override_pm_ops(&plat_arm_psci_pm_ops);
--- a/plat/arm/css/drivers/mhu/css_mhu.c
+++ b/plat/arm/css/drivers/mhu/css_mhu.c
@ -81,7 +81,7 @@ void mhu_secure_message_end(unsigned int slot_id)
 	arm_lock_release();
 }

-void mhu_secure_init(void)
+void __init mhu_secure_init(void)
 {
 	arm_lock_init();

@ -93,7 +93,7 @@ void mhu_secure_init(void)
 	assert(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) == 0);
 }

-void plat_arm_pwrc_setup(void)
+void __init plat_arm_pwrc_setup(void)
 {
 	mhu_secure_init();
 }
--- a/plat/arm/css/drivers/scp/css_pm_scmi.c
+++ b/plat/arm/css/drivers/scp/css_pm_scmi.c
@ -328,7 +328,7 @@ static int scmi_ap_core_init(scmi_channel_t *ch)
 	return 0;
 }

-void plat_arm_pwrc_setup(void)
+void __init plat_arm_pwrc_setup(void)
 {
 	channel.info = &plat_css_scmi_plat_info;
 	channel.lock = ARM_LOCK_GET_INSTANCE;
--- a/plat/arm/css/sgi/sgi_interconnect.c
+++ b/plat/arm/css/sgi/sgi_interconnect.c
@ -17,7 +17,7 @@
 /******************************************************************************
 * Helper function to initialize ARM interconnect driver.
 *****************************************************************************/
-void plat_arm_interconnect_init(void)
+void __init plat_arm_interconnect_init(void)
 {
 }

--- a/plat/arm/css/sgm/sgm_interconnect.c
+++ b/plat/arm/css/sgm/sgm_interconnect.c
@ -4,6 +4,8 @@
 * SPDX-License-Identifier: BSD-3-Clause
 */

+#include <cdefs.h>
+
 /*
 * As the SGM platform supports FCM (with automatic interconnect
 * enter/exit), we should not do anything in these interface functions.
@ -13,7 +15,7 @@
 /******************************************************************************
 * Helper function to initialize ARM interconnect driver.
 *****************************************************************************/
-void plat_arm_interconnect_init(void)
+void __init plat_arm_interconnect_init(void)
 {
 }