[PATCH 16/19] ARM: atf: add bl31 v2 calling method

[Sascha Hauer <s.hauer@pengutronix.de>]

Newer bl31 binaries require a slightly different parameter structure.
This patch adds support for it. The code is taken from U-Boot and
changed to static initializers for improved readability.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
---
 arch/arm/cpu/atf.c                | 94 +++++++++++++++++++++++++++++++
 arch/arm/include/asm/atf_common.h | 40 +++++++++++++
 2 files changed, 134 insertions(+)

diff --git a/arch/arm/cpu/atf.c b/arch/arm/cpu/atf.c
index ccd540d32a..d01e20508c 100644
--- a/arch/arm/cpu/atf.c
+++ b/arch/arm/cpu/atf.c
@@ -80,3 +80,97 @@ void bl31_entry(uintptr_t bl31_entry, uintptr_t bl32_entry,
 
 	atf_entry(&bl31_params, fdt_addr);
 }
+
+struct bl2_to_bl31_params_mem_v2 *bl2_plat_get_bl31_params_v2(uintptr_t bl32_entry,
+					uintptr_t bl33_entry, uintptr_t fdt_addr)
+{
+	static struct bl2_to_bl31_params_mem_v2 p = {
+		.bl_params = {
+			.h = {
+				.type = ATF_PARAM_BL_PARAMS,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct bl_params),
+				.attr = 0,
+			},
+			.head = &p.bl31_params_node,
+		},
+		.bl31_params_node = {
+			.image_id = ATF_BL31_IMAGE_ID,
+			.image_info = &p.bl31_image_info,
+			.ep_info = &p.bl31_ep_info,
+			.next_params_info = &p.bl32_params_node,
+		},
+		.bl32_params_node = {
+			.image_id = ATF_BL32_IMAGE_ID,
+			.image_info = &p.bl32_image_info,
+			.ep_info = &p.bl32_ep_info,
+			.next_params_info = &p.bl33_params_node,
+		},
+		.bl33_params_node = {
+			.image_id = ATF_BL33_IMAGE_ID,
+			.image_info = &p.bl33_image_info,
+			.ep_info = &p.bl33_ep_info,
+			.next_params_info = NULL,
+		},
+		.bl32_ep_info = {
+			.h = {
+				.type = ATF_PARAM_EP,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct entry_point_info),
+				.attr = ATF_EP_SECURE,
+			},
+			.spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXECPTIONS),
+		},
+		.bl33_ep_info = {
+			.h = {
+				.type = ATF_PARAM_EP,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct entry_point_info),
+				.attr = ATF_EP_NON_SECURE,
+			},
+			.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX, DISABLE_ALL_EXECPTIONS),
+		},
+		.bl33_image_info = {
+			.h = {
+				.type = ATF_PARAM_IMAGE_BINARY,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct atf_image_info),
+				.attr = 0,
+			},
+		},
+		.bl32_image_info = {
+			.h = {
+				.type = ATF_PARAM_IMAGE_BINARY,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct atf_image_info),
+				.attr = ATF_EP_SECURE,
+			},
+		},
+		.bl31_image_info = {
+			.h = {
+				.type = ATF_PARAM_IMAGE_BINARY,
+				.version = ATF_VERSION_2,
+				.size = sizeof(struct atf_image_info),
+				.attr = 0,
+			},
+		},
+	};
+
+	p.bl33_ep_info.args.arg0 = 0xffff & read_mpidr();
+	p.bl33_ep_info.pc = bl33_entry;
+	p.bl32_ep_info.args.arg3 = fdt_addr;
+	p.bl32_ep_info.pc = bl32_entry;
+
+	return &p;
+}
+
+void bl31_entry_v2(uintptr_t bl31_entry, struct bl_params *params, void *fdt_addr)
+{
+	void (*atf_entry)(struct bl_params *params, uintptr_t plat_params);
+
+	raw_write_daif(SPSR_EXCEPTION_MASK);
+
+	atf_entry = (void *)bl31_entry;
+
+	atf_entry(params, (uintptr_t)fdt_addr);
+}
diff --git a/arch/arm/include/asm/atf_common.h b/arch/arm/include/asm/atf_common.h
index 56e2d3ffaa..ab99cd3ac1 100644
--- a/arch/arm/include/asm/atf_common.h
+++ b/arch/arm/include/asm/atf_common.h
@@ -14,8 +14,14 @@
 #define ATF_PARAM_EP		0x01
 #define ATF_PARAM_IMAGE_BINARY	0x02
 #define ATF_PARAM_BL31		0x03
+#define ATF_PARAM_BL_PARAMS	0x05
 
 #define ATF_VERSION_1	0x01
+#define ATF_VERSION_2	0x02
+
+#define ATF_BL31_IMAGE_ID	0x03
+#define ATF_BL32_IMAGE_ID	0x04
+#define ATF_BL33_IMAGE_ID	0x05
 
 #define ATF_EP_SECURE	0x0
 #define ATF_EP_NON_SECURE	0x1
@@ -158,6 +164,40 @@ struct bl31_params {
 void bl31_entry(uintptr_t bl31_entry, uintptr_t bl32_entry,
 		uintptr_t bl33_entry, uintptr_t fdt_addr);
 
+/* BL image node in the BL image execution sequence */
+struct bl_params_node {
+	unsigned int image_id;
+	struct atf_image_info *image_info;
+	struct entry_point_info *ep_info;
+	struct bl_params_node *next_params_info;
+};
+
+/*
+ * BL image head node in the BL image execution sequence
+ * It is also used to pass information to next BL image.
+ */
+struct bl_params {
+	struct param_header h;
+	struct bl_params_node *head;
+};
+
+struct bl2_to_bl31_params_mem_v2 {
+	struct bl_params bl_params;
+	struct bl_params_node bl31_params_node;
+	struct bl_params_node bl32_params_node;
+	struct bl_params_node bl33_params_node;
+	struct atf_image_info bl31_image_info;
+	struct atf_image_info bl32_image_info;
+	struct atf_image_info bl33_image_info;
+	struct entry_point_info bl31_ep_info;
+	struct entry_point_info bl32_ep_info;
+	struct entry_point_info bl33_ep_info;
+};
+
+struct bl2_to_bl31_params_mem_v2 *bl2_plat_get_bl31_params_v2(uintptr_t bl32_entry,
+					uintptr_t bl33_entry, uintptr_t fdt_addr);
+void bl31_entry_v2(uintptr_t bl31_entry, struct bl_params *params, void *fdt_addr);
+
 #endif /*__ASSEMBLY__ */
 
 #endif /* __BL_COMMON_H__ */
-- 
2.39.2