Base infrastructure for partitioner integrated on ATF

.gitignore

@@ -44,3 +44,7 @@ tags
 
 # Node.js
 node_modules/
+
+# Partitioner: ignore temp files
+_tmp_*
+.clang-format

bl31/aarch64/bl31_entrypoint.S

@@ -13,6 +13,8 @@
 #include <lib/runtime_instr.h>
 #include <lib/xlat_tables/xlat_mmu_helpers.h>
 
+#include <atf_stubs.h>
+
 	.globl	bl31_entrypoint
 	.globl	bl31_warm_entrypoint
 
@@ -124,6 +126,16 @@ func bl31_entrypoint
 	sub	x1, x1, x0
 	bl	clean_dcache_range
 
+	/* --------------------------------------------------------------------
+	 * Jump to partitioner init function
+	 * --------------------------------------------------------------------
+	 */
+	bl	partitioner_init
+	bl	plat_my_core_pos
+	bl	partition_set_cpu
+	bl	plat_my_core_pos
+	bl	partition_prepare_atfctx
+
 	b	el3_exit
 endfunc bl31_entrypoint
 
@@ -218,6 +230,12 @@ func bl31_warm_entrypoint
 
 	bl	psci_warmboot_entrypoint
 
+	/* TODO: Call partition_prepare_atfctx() */
+	bl	plat_my_core_pos
+	bl	partition_set_cpu
+	bl	plat_my_core_pos
+	bl	partition_prepare_atfctx
+
 #if ENABLE_RUNTIME_INSTRUMENTATION
 	pmf_calc_timestamp_addr rt_instr_svc, RT_INSTR_EXIT_PSCI
 	mov	x19, x0

bl31/aarch64/runtime_exceptions.S

@@ -15,6 +15,7 @@
 #include <el3_common_macros.S>
 #include <lib/el3_runtime/cpu_data.h>
 #include <lib/smccc.h>
+#include <syscalls.h>
 
 	.globl	runtime_exceptions
 
@@ -520,38 +521,40 @@ smc_handler64:
 
 	mov	sp, x12
 
-	/* Get the unique owning entity number */
-	ubfx	x16, x0, #FUNCID_OEN_SHIFT, #FUNCID_OEN_WIDTH
-	ubfx	x15, x0, #FUNCID_TYPE_SHIFT, #FUNCID_TYPE_WIDTH
-	orr	x16, x16, x15, lsl #FUNCID_OEN_WIDTH
-
-	/* Load descriptor index from array of indices */
-	adrp	x14, rt_svc_descs_indices
-	add	x14, x14, :lo12:rt_svc_descs_indices
-	ldrb	w15, [x14, x16]
-
-	/* Any index greater than 127 is invalid. Check bit 7. */
-	tbnz	w15, 7, smc_unknown
-
-	/*
-	 * Get the descriptor using the index
-	 * x11 = (base + off), w15 = index
-	 *
-	 * handler = (base + off) + (index << log2(size))
-	 */
-	adr	x11, (__RT_SVC_DESCS_START__ + RT_SVC_DESC_HANDLE)
-	lsl	w10, w15, #RT_SVC_SIZE_LOG2
-	ldr	x15, [x11, w10, uxtw]
-
-	/*
-	 * Call the Secure Monitor Call handler and then drop directly into
-	 * el3_exit() which will program any remaining architectural state
-	 * prior to issuing the ERET to the desired lower EL.
-	 */
-#if DEBUG
-	cbz	x15, rt_svc_fw_critical_error
-#endif
-	blr	x15
+// 	/* Get the unique owning entity number */
+// 	ubfx	x16, x0, #FUNCID_OEN_SHIFT, #FUNCID_OEN_WIDTH
+// 	ubfx	x15, x0, #FUNCID_TYPE_SHIFT, #FUNCID_TYPE_WIDTH
+// 	orr	x16, x16, x15, lsl #FUNCID_OEN_WIDTH
+
+// 	/* Load descriptor index from array of indices */
+// 	adrp	x14, rt_svc_descs_indices
+// 	add	x14, x14, :lo12:rt_svc_descs_indices
+// 	ldrb	w15, [x14, x16]
+
+// 	/* Any index greater than 127 is invalid. Check bit 7. */
+// 	tbnz	w15, 7, smc_unknown
+
+// 	/*
+// 	 * Get the descriptor using the index
+// 	 * x11 = (base + off), w15 = index
+// 	 *
+// 	 * handler = (base + off) + (index << log2(size))
+// 	 */
+// 	adr	x11, (__RT_SVC_DESCS_START__ + RT_SVC_DESC_HANDLE)
+// 	lsl	w10, w15, #RT_SVC_SIZE_LOG2
+// 	ldr	x15, [x11, w10, uxtw]
+
+// 	/*
+// 	 * Call the Secure Monitor Call handler and then drop directly into
+// 	 * el3_exit() which will program any remaining architectural state
+// 	 * prior to issuing the ERET to the desired lower EL.
+// 	 */
+// #if DEBUG
+// 	cbz	x15, rt_svc_fw_critical_error
+// #endif
+// 	blr	x15
+
+	bl	partitioner_systemcall
 
 	b	el3_exit
 

lib/el3_runtime/aarch64/context.S

@@ -965,6 +965,7 @@ sve_not_enabled:
 #ifdef IMAGE_BL31
 	str	xzr, [sp, #CTX_EL3STATE_OFFSET + CTX_IS_IN_EL3]
 #endif
+
 	exception_return
 
 endfunc el3_exit

partitioner/partitioner.mk

@@ -1,3 +1,71 @@
+# Define directories of partitioner-atf and partitioner-standalone
+PART_DIR:=partitioner
+PART_ATF_DIR:=$(PART_DIR)
+PART_STD_DIR:=$(PART_DIR)/partitioner/src
+root_dir:=/home/daniel/workspace/bao-partitioner/arm-trusted-firmware/partitioner/partitioner
 
+ifeq ($(PLAT), qemu)
+PART_PLAT=qemu-aarch64-virt
+PART_ARCH=armv8
+else
+$(error Partitioner does not support the $(PLAT) board)
+endif
 
-PARTITIONER_SOURCES :=
+# Round-up source files and include directories
+PART_ATF_INC+=																\
+				$(PART_ATF_DIR)/src/core/inc								\
+
+PART_ATF_SRC+=																\
+				$(PART_ATF_DIR)/src/core/atf_partition.c					\
+				$(PART_ATF_DIR)/src/core/atf_cpu.c							\
+
+PART_STD_INC+=																\
+				$(PART_STD_DIR)/core/inc									\
+				$(PART_STD_DIR)/arch/$(PART_ARCH)/inc						\
+				$(PART_STD_DIR)/arch/$(PART_ARCH)/inc/arch					\
+
+PART_STD_SRC+=																\
+				$(PART_STD_DIR)/core/partition.c							\
+				$(PART_STD_DIR)/core/syscalls.c								\
+				$(PART_STD_DIR)/arch/$(PART_ARCH)/vmm.c						\
+				$(PART_STD_DIR)/arch/$(PART_ARCH)/partition.c				\
+
+PART_STD_SRC:=	$(PART_STD_SRC:$(PART_STD_DIR)%=%)
+
+PART_STD_SRC:=	$(subst /,_tmp_,$(PART_STD_SRC))
+
+define COPY_PART_STD_SRC
+$(1): $$(PART_STD_DIR)/$$$(subst _tmp_,/,$(1))
+	@cp $$< $$@
+endef
+
+$(foreach src, $(PART_STD_SRC), $(eval $(call COPY_PART_STD_SRC, $(src))))
+
+# Instantiate CI rules
+include $(PART_DIR)/partitioner/ci/ci.mk
+
+atf_hdrs_rec=$(foreach dir, $1, $(wildcard $(dir)/*.h) \
+    $(call atf_hdrs_rec, $(wildcard $(basename $(wildcard $(dir)/*)))))
+atf_hdrs+=$(call atf_hdrs_rec, $(PART_ATF_INC))
+
+## Grab defines and includes after second expansion
+_cpp_flags+=-DATF_BUILD $$(DEFINES) $$(INCLUDES)
+clang-arch:=arm64
+_misra_sup:=--suppress=*:include/*
+
+$(call ci, format, $(PART_ATF_SRC) $(atf_hdrs))
+$(call ci, tidy, $(PART_ATF_SRC) $(PART_STD_SRC) $(atf_hdrs), $(_cpp_flags))
+$(call ci, cppcheck, $(PART_ATF_SRC), $(_cpp_flags))
+$(call ci, misra, $(PART_ATF_SRC), $(PART_ATF_INC), $(_cpp_flags), $(_misra_sup))
+
+TF_CFLAGS+=-DATF_BUILD
+INCLUDES+=$(addprefix -I, $(PART_STD_INC))
+INCLUDES+=$(addprefix -I, $(PART_ATF_INC))
+BL31_SOURCES+=$(PART_ATF_SRC)
+BL31_SOURCES+=$(PART_STD_SRC)
+
+# ATF clean rules
+distclean realclean clean: partitioner_clean
+
+partitioner_clean:
+	$(call SHELL_DELETE_ALL,$(PART_STD_SRC))

partitioner/src/core/atf_cpu.c

@@ -0,0 +1,16 @@
+#include <atf_cpu.h>
+
+extern unsigned int plat_is_my_cpu_primary(void);
+
+bool cpu_is_primary(void)
+{
+    return plat_is_my_cpu_primary();
+}
+
+void cpu_idle(void) { }
+
+void cpu_init(unsigned int cpuid, unsigned int entrypoint)
+{
+    /* Call PSCI to wake-up the partition master CPU */
+    psci_cpu_on(cpuid, entrypoint, 0x0);
+}

partitioner/src/core/atf_partition.c

@@ -0,0 +1,72 @@
+#include <atf_partition.h>
+#include <partition.h>
+#include <atf_cpu.h>
+
+static spinlock_t console_lock;
+
+void partition_prepare_atfctx(uint32_t cpuid)
+{
+    cpu_context_t* ctx = NULL;
+    el3_state_t* state = NULL;
+    partition_cfg_t* prttn = NULL;
+
+    /* Get partition struct corresponding to the running CPU */
+    prttn = partition_get_prttn_by_coreid(cpuid);
+
+    if (prttn != NULL) {
+        /* Get ATF context to be modified before jumping to EL2-NS */
+        ctx = cm_get_context(NON_SECURE);
+        state = get_el3state_ctx(ctx);
+
+        /* Modify ATF context before jumping to EL2-NS */
+        write_ctx_reg(state, CTX_SCR_EL3,
+            prttn->prttn_cfg_header.prttn_arch.scr_el3);
+        write_ctx_reg(state, CTX_SPSR_EL3,
+            prttn->prttn_cfg_header.prttn_arch.spsr_el3);
+        write_ctx_reg(state, CTX_ELR_EL3, prttn->prttn_cfg_header.entrypoint);
+
+        spin_lock(&console_lock);
+        console_printf("BAO_PARTITIONER: Jump to partition %d entrypoint \n",
+            prttn->prttn_id);
+        spin_unlock(&console_lock);
+    } else {
+        ERROR("BAO_PARTITIONER: No partition defined for cpuid %d", cpuid);
+        while (1) { };
+    }
+}
+
+void partitions_init(void)
+{
+    unsigned int prttn_index = 0;
+
+    for (; prttn_index < partitions.partitions_size; prttn_index++) {
+        partition_cfg_t* partition = &partitions.partitions_list[prttn_index];
+        uint32_t cpuid = partition->prttn_cfg_header.cpuids[0];
+        uint64_t entrypoint = partition->prttn_cfg_header.entrypoint;
+
+        spin_lock(&console_lock);
+        console_printf("BAO_PARTITIONER: Initializing partition %d\n",
+            prttn_index);
+        spin_unlock(&console_lock);
+
+        /* Awake master CPU from current iterated partition, except if it
+        the primary CPU that is already running*/
+        if (cpuid != plat_my_core_pos()) {
+            spin_lock(&console_lock);
+            console_printf("BAO_PARTITIONER: Wake-up partition %d master "
+                           "CPU-%d\n",
+                prttn_index, cpuid);
+            spin_unlock(&console_lock);
+            /* Wake-up the partition master CPU */
+            cpu_init(cpuid, entrypoint);
+        }
+    }
+}
+
+void partitioner_init(void)
+{
+    console_printf("BAO_PARTITIONER: Initializing partitioner\n");
+
+    /* Init each partition */
+    partitions_init();
+}

partitioner/src/core/inc/atf_cpu.h

@@ -0,0 +1,15 @@
+#ifndef ATF_CPU_H
+#define ATF_CPU_H
+
+#ifndef __ASSEMBLER__
+
+#include <atf_stubs.h>
+
+/* CPU-related stubs declaration */
+bool cpu_is_primary(void);
+void cpu_idle(void);
+void cpu_init(unsigned int cpuid, unsigned int entrypoint);
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* ATF_CPU_H */

partitioner/src/core/inc/atf_partition.h

@@ -0,0 +1,13 @@
+#ifndef ATF_PARTITION_H
+#define ATF_PARTITION_H
+
+#ifndef __ASSEMBLER__
+
+#include <atf_stubs.h>
+
+void partition_prepare_atfctx(uint32_t cpuid);
+void partitioner_init(void);
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* ATF_PARTITION_H */

partitioner/src/core/inc/atf_stubs.h

@@ -0,0 +1,39 @@
+#ifndef ATF_STUBS_H
+#define ATF_STUBS_H
+
+#ifndef __ASSEMBLER__
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <common/debug.h>
+#include <arch_helpers.h>
+#include <plat/common/platform.h>
+#include <lib/psci/psci.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+
+#define console_printf(...)            tf_log(LOG_MARKER_INFO __VA_ARGS__)
+
+#define spinlock_lock(lock)            spin_lock((spinlock_t*)(lock))
+#define spinlock_unlock(lock)          spin_unlock((spinlock_t*)(lock))
+
+/* Partition cookie registers accessors */
+#define sysreg_elr_el3_write(_value)   write_elr_el3(_value)
+#define sysreg_sctlr_el2_write(_value) write_sctlr_el2(_value)
+#define sysreg_mair_el2_write(_value)  write_mair_el2(_value)
+#define sysreg_tcr_el2_write(_value)   write_tcr_el2(_value)
+#define sysreg_ttbr0_el2_write(_value) write_ttbr0_el2(_value)
+#define sysreg_hcr_el2_write(_value)   write_hcr_el2(_value)
+#define sysreg_vtcr_el2_write(_value)  write_vtcr_el2(_value)
+#define sysreg_vttbr_el2_write(_value) write_vttbr_el2(_value)
+#define sysreg_scr_el3_write(_value)   write_scr_el3(_value)
+#define sysreg_scr_el3_read()          read_scr_el3()
+
+DEFINE_SYSREG_RW_FUNCS(hacr_el2)
+#define sysreg_hacr_el2_write(_value) write_hacr_el2(_value)
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* ATF_STUBS_H */