Merge branch 'for-next/rockchip'

3 files changed, 91 insertions, 3 deletions

diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index e7a6e3e6fb..6344ab5066 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -50,6 +50,7 @@ AFLAGS-cache-armv8.pbl.o   :=-Wa,-march=armv8-a
 
 pbl-y += entry.o entry_ll$(S64).o
 pbl-y += uncompress.o
+pbl-$(CONFIG_ARM_ATF) += atf.o
 
 obj-pbl-y += common.o sections.o
 KASAN_SANITIZE_common.o := n
diff --git a/arch/arm/cpu/atf.c b/arch/arm/cpu/atf.c
new file mode 100644
index 0000000000..4753a8a559
--- /dev/null
+++ b/arch/arm/cpu/atf.c
@@ -0,0 +1,80 @@
+#include <common.h>
+#include <asm/atf_common.h>
+#include <asm/system.h>
+
+static inline void raw_write_daif(unsigned int daif)
+{
+	__asm__ __volatile__("msr DAIF, %0\n\t" : : "r" (daif) : "memory");
+}
+
+void bl31_entry(uintptr_t bl31_entry, uintptr_t bl32_entry,
+		uintptr_t bl33_entry, uintptr_t fdt_addr)
+{
+	struct atf_image_info bl31_image_info = {
+		.h = {
+			.type = ATF_PARAM_IMAGE_BINARY,
+			.version = ATF_VERSION_1,
+			.size = sizeof(bl31_image_info),
+		},
+	};
+	struct atf_image_info bl32_image_info = {
+		.h = {
+			.type = ATF_PARAM_IMAGE_BINARY,
+			.version = ATF_VERSION_1,
+			.size = sizeof(bl32_image_info),
+		},
+	};
+	struct entry_point_info bl32_ep_info = {
+		.h = {
+			.type = ATF_PARAM_EP,
+			.version = ATF_VERSION_1,
+			.attr = ATF_EP_SECURE,
+			.size = sizeof(bl32_ep_info),
+		},
+		.pc = bl32_entry,
+		.spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXECPTIONS),
+		.args = {
+			.arg3 = fdt_addr,
+		},
+	};
+	struct atf_image_info bl33_image_info = {
+		.h = {
+			.type = ATF_PARAM_IMAGE_BINARY,
+			.version = ATF_VERSION_1,
+			.size = sizeof(bl33_image_info),
+		},
+	};
+	struct entry_point_info bl33_ep_info = {
+		.h = {
+			.type = ATF_PARAM_EP,
+			.version = ATF_VERSION_1,
+			.attr = ATF_EP_NON_SECURE,
+			.size = sizeof(bl33_ep_info),
+		},
+		.pc = bl33_entry,
+		.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX, DISABLE_ALL_EXECPTIONS),
+		.args = {
+			/* BL33 expects to receive the primary CPU MPID (through x0) */
+			.arg0 = 0xffff & read_mpidr(),
+		},
+	};
+	struct bl31_params bl31_params = {
+		.h = {
+			.type = ATF_PARAM_BL31,
+			.version = ATF_VERSION_1,
+			.size = sizeof(bl31_params),
+		},
+		.bl31_image_info = &bl31_image_info,
+		.bl32_ep_info = &bl32_ep_info,
+		.bl32_image_info = &bl32_image_info,
+		.bl33_ep_info = &bl33_ep_info,
+		.bl33_image_info = &bl33_image_info,
+	};
+	void (*atf_entry)(struct bl31_params *params, uintptr_t plat_params);
+
+	raw_write_daif(SPSR_EXCEPTION_MASK);
+
+	atf_entry = (void *)bl31_entry;
+
+	atf_entry(&bl31_params, fdt_addr);
+}
diff --git a/arch/arm/cpu/setupc_64.S b/arch/arm/cpu/setupc_64.S
index ee9ea6cfc0..e24acbd0da 100644
--- a/arch/arm/cpu/setupc_64.S
+++ b/arch/arm/cpu/setupc_64.S
@@ -24,9 +24,18 @@ ENDPROC(setup_c)
  * executing at new address.
  */
 .section .text.relocate_to_adr
-ENTRY(relocate_to_adr)
 					/* x0: target address */
+#ifdef __PBL__
+ENTRY(relocate_to_adr_full)
+	ldr	x2, =__piggydata_end
+	b	1f
+#endif
+
+ENTRY(relocate_to_adr)
+	ldr	x2, =__bss_start
+	b	1f
 
+1:
 	stp	x19, x20, [sp, #-16]!
 	stp	x21, x22, [sp, #-16]!
 
@@ -45,8 +54,6 @@ ENTRY(relocate_to_adr)
 	cmp	x1, x21			/* already at correct address? */
 	beq	1f			/* yes, skip copy to new address */
 
-	ldr	x2, =__bss_start
-
 	sub	x2, x2, x0		/* x2: size */
 	mov	x0, x21			/* x0: target */