feat(s32g274a): enable BL31 stage

Add BL31 prerequisites for the S32G274ARDB2 board to allow single-core
cold boot without MMU and PSCI services.

Change-Id: I8a10fd62f3cc9430083758043ea82e3803f61060
Signed-off-by: Bogdan Hamciuc <bogdan.hamciuc@nxp.com>
Signed-off-by: Bogdan Roman <bogdan-gabriel.roman@nxp.com>
Signed-off-by: Andra-Teodora Ilie <andra.ilie@nxp.com>
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>

plat/nxp/s32/s32g274ardb2/include/plat_helpers.h

@@ -0,0 +1,12 @@
+/*
+ * Copyright 2024 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_HELPERS_H
+#define PLAT_HELPERS_H
+
+unsigned int s32g2_core_pos_by_mpidr(u_register_t mpidr);
+
+#endif /* PLAT_HELPERS_H */

plat/nxp/s32/s32g274ardb2/include/platform_def.h

@@ -18,10 +18,15 @@
 /* CPU Topology */
 #define PLATFORM_CORE_COUNT		U(4)
 #define PLATFORM_SYSTEM_COUNT		U(1)
+#define PLATFORM_CLUSTER_COUNT		U(2)
 #define PLATFORM_PRIMARY_CPU		U(0)
 #define PLATFORM_MPIDR_CPU_MASK_BITS	U(1)
+#define PLATFORM_MAX_CPUS_PER_CLUSTER	U(2)
 
 /* Power Domains */
+#define PLAT_NUM_PWR_DOMAINS		(PLATFORM_SYSTEM_COUNT + \
+					 PLATFORM_CLUSTER_COUNT + \
+					 PLATFORM_CORE_COUNT)
 #define PLAT_MAX_PWR_LVL		MPIDR_AFFLVL2
 #define PLAT_MAX_OFF_STATE		U(2)
 #define PLAT_MAX_RET_STATE		U(1)
@@ -39,6 +44,13 @@
 #define BL33_BASE			UL(0x34500000)
 #define BL33_LIMIT			UL(0x345FF000)
 
+#define PLAT_PHY_ADDR_SPACE_SIZE	(ULL(1) << 36)
+/* We'll be doing a 1:1 mapping anyway */
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(ULL(1) << 36)
+
+#define MAX_MMAP_REGIONS		U(8)
+#define MAX_XLAT_TABLES			U(32)
+
 /* Console settings */
 #define UART_BASE			UL(0x401C8000)
 #define UART_BAUDRATE			U(115200)
@@ -51,4 +63,16 @@
 #define MAX_IO_HANDLES			U(2)
 #define MAX_IO_DEVICES			U(2)
 
+/* GIC settings */
+#define S32G_GIC_BASE			UL(0x50800000)
+#define PLAT_GICD_BASE			S32G_GIC_BASE
+#define PLAT_GICR_BASE			(S32G_GIC_BASE + UL(0x80000))
+
+/* Generic timer frequency; this goes directly into CNTFRQ_EL0.
+ * Its end-value is 5MHz; this is based on the assumption that
+ * GPR00[CA53_COUNTER_CLK_DIV_VAL] contains the reset value of 0x7, hence
+ * producing a divider value of 8, applied to the FXOSC frequency of 40MHz.
+ */
+#define COUNTER_FREQUENCY		U(5000000)
+
 #endif /* PLATFORM_DEF_H */

plat/nxp/s32/s32g274ardb2/plat_bl31_setup.c

@@ -0,0 +1,75 @@
+/*
+ * Copyright 2024 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/arm/gicv3.h>
+#include <plat/common/platform.h>
+#include <plat_console.h>
+
+static entry_point_info_t bl33_image_ep_info;
+
+static unsigned int s32g2_mpidr_to_core_pos(unsigned long mpidr);
+
+static uint32_t get_spsr_for_bl33_entry(void)
+{
+	unsigned long mode = MODE_EL1;
+	uint32_t spsr;
+
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+
+	return spsr;
+}
+
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				u_register_t arg2, u_register_t arg3)
+{
+	console_s32g2_register();
+
+	SET_PARAM_HEAD(&bl33_image_ep_info, PARAM_EP, VERSION_1, 0);
+	bl33_image_ep_info.pc = BL33_BASE;
+	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
+	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+}
+
+void bl31_plat_arch_setup(void)
+{
+}
+
+struct entry_point_info *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	return &bl33_image_ep_info;
+}
+
+void bl31_platform_setup(void)
+{
+	static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
+	static gicv3_driver_data_t plat_gic_data = {
+		.gicd_base = PLAT_GICD_BASE,
+		.gicr_base = PLAT_GICR_BASE,
+		.rdistif_num = PLATFORM_CORE_COUNT,
+		.rdistif_base_addrs = rdistif_base_addrs,
+		.mpidr_to_core_pos = s32g2_mpidr_to_core_pos,
+	};
+
+	unsigned int pos = plat_my_core_pos();
+
+	gicv3_driver_init(&plat_gic_data);
+	gicv3_distif_init();
+	gicv3_rdistif_init(pos);
+	gicv3_cpuif_enable(pos);
+}
+
+static unsigned int s32g2_mpidr_to_core_pos(unsigned long mpidr)
+{
+	int core;
+
+	core = plat_core_pos_by_mpidr(mpidr);
+	if (core < 0) {
+		return 0;
+	}
+
+	return (unsigned int)core;
+}
+

plat/nxp/s32/s32g274ardb2/plat_helpers.S

@@ -15,6 +15,7 @@
 .globl	plat_crash_console_init
 .globl	plat_crash_console_putc
 .globl	plat_is_my_cpu_primary
+.globl	plat_my_core_pos
 .globl	plat_reset_handler
 .globl	plat_secondary_cold_boot_setup
 .globl	platform_mem_init

plat/nxp/s32/s32g274ardb2/platform.mk

@@ -54,3 +54,12 @@ BL2_SOURCES += \
 	drivers/io/io_storage.c \
 	lib/cpus/aarch64/cortex_a53.S \
 
+BL31_SOURCES += \
+	${GICV3_SOURCES} \
+	${PLAT_S32G274ARDB2}/plat_bl31_setup.c \
+	${PLAT_S32G274ARDB2}/s32g2_psci.c \
+	${PLAT_S32G274ARDB2}/s32g2_soc.c \
+	${XLAT_TABLES_LIB_SRCS} \
+	lib/cpus/aarch64/cortex_a53.S \
+	plat/common/plat_gicv3.c \
+	plat/common/plat_psci_common.c \

plat/nxp/s32/s32g274ardb2/s32g2_psci.c

@@ -0,0 +1,20 @@
+/*
+ * Copyright 2024 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/psci/psci.h>
+#include <plat/common/platform.h>
+
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	static const plat_psci_ops_t s32g2_psci_ops = {
+	};
+
+	*psci_ops = &s32g2_psci_ops;
+
+	return 0;
+}
+

plat/nxp/s32/s32g274ardb2/s32g2_soc.c

@@ -0,0 +1,52 @@
+/*
+ * Copyright 2024 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/common/platform.h>
+#include <plat_helpers.h>
+
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	static const unsigned char s32g_power_domain_tree_desc[] = {
+		PLATFORM_SYSTEM_COUNT,
+		PLATFORM_CLUSTER_COUNT,
+		PLATFORM_CORE_COUNT / U(2),
+		PLATFORM_CORE_COUNT / U(2),
+	};
+
+	return s32g_power_domain_tree_desc;
+}
+
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	unsigned int cluster_id, cpu_id, core_id;
+	u_register_t mpidr_priv = mpidr;
+
+	mpidr_priv &= MPIDR_AFFINITY_MASK;
+
+	if ((mpidr_priv & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) != 0) {
+		return -1;
+	}
+
+	cluster_id = MPIDR_AFFLVL1_VAL(mpidr_priv);
+	cpu_id = MPIDR_AFFLVL0_VAL(mpidr_priv);
+
+	if ((cluster_id >= PLATFORM_CLUSTER_COUNT) ||
+	    (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER)) {
+		return -1;
+	}
+
+	core_id = s32g2_core_pos_by_mpidr(mpidr_priv);
+	if (core_id >= PLATFORM_CORE_COUNT) {
+		return -1;
+	}
+
+	return (int)core_id;
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return COUNTER_FREQUENCY;
+}