diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc
new file mode 100644
index 0000000..8ffa0aa
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc
@@ -0,0 +1,36 @@
+COMPATIBLE_MACHINE = "fvp-baser-aemv8r64"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/${MACHINE}:"
+SRC_URI:append = " \
+    file://0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch \
+    file://0002-aarch64-Prepare-for-EL1-booting.patch \
+    file://0003-aarch64-Prepare-for-lower-EL-booting.patch \
+    file://0004-gic-v3-Prepare-for-gicv3-with-EL2.patch \
+    file://0005-aarch64-Prepare-for-booting-with-EL2.patch \
+    file://0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch \
+    file://0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch \
+    file://0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch \
+    file://0009-lds-Mark-the-mem-range.patch \
+    file://0010-common-Introduce-the-libfdt.patch \
+    file://0011-common-Add-essential-libc-functions.patch \
+    file://0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch \
+    file://0013-platform-Add-print_hex-func.patch \
+    file://0014-common-Add-mem-usage-to-memreserve.patch \
+    file://0015-boot-Add-the-enable-keep-el-compile-option.patch \
+    file://0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch \
+    file://0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch \
+    file://0018-PSCI-Add-function-call-entry-point.patch \
+    file://0019-lds-Rearrange-and-mark-the-sections.patch \
+    file://0020-common-Provide-firmware-info-using-libfdt.patch \
+    file://0021-boot-Enable-firmware-node-initialization.patch \
+    "
+
+BOOT_WRAPPER_AARCH64_CMDLINE = "\
+earlycon console=ttyAMA0 loglevel=8 rootfstype=ext4 root=/dev/vda1 rw"
+
+EXTRA_OECONF += "--enable-psci=hvc --enable-keep-el"
+
+TUNE_CCARGS = ""
+
+BOOT_WRAPPER_AARCH64_KERNEL = "u-boot.bin"
+do_deploy[depends] += "u-boot:do_deploy"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
new file mode 100644
index 0000000..c2e7e6e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
@@ -0,0 +1,4 @@
+MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE ?= ""
+MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE:fvp-baser-aemv8r64 ?= "boot-wrapper-aarch64-fvp-baser-aemv8r64.inc"
+
+require ${MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch
new file mode 100644
index 0000000..566070a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch
@@ -0,0 +1,135 @@
+From 3e7cfbe39a2a053d2a6b0d928cc172ed9d1c6da8 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Rename labels and prepare for lower EL booting
+
+Prepare for booting from lower EL. Rename *_el3 relavant labels with
+*_el_max and *_no_el3 with *_keep_el. Since the original _no_el3 means
+"We neither do init sequence at this highest EL nor drop to lower EL
+when entering to kernel", we rename it with _keep_el to make it more
+clear for lower EL initialisation.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+---
+ arch/aarch64/boot.S | 28 ++++++++++++++++++++--------
+ arch/aarch64/psci.S |  9 +++++----
+ arch/aarch64/spin.S |  4 ++--
+ 3 files changed, 27 insertions(+), 14 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 27ba449..84e1646 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -21,18 +21,30 @@ ASM_FUNC(_start)
+ 
+ 	/*
+ 	 * EL3 initialisation
++	 * Boot sequence
++	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
++	 *   lower EL before entering the kernel.
++	 * Else, no initialisation and keep the current EL before
++	 *   entering the kernel.
+ 	 */
+ 	mrs	x0, CurrentEL
+ 	cmp	x0, #CURRENTEL_EL3
+-	b.eq	1f
++	b.eq	el3_init
+ 
++	/*
++	 * We stay in the current EL for entering the kernel
++	 */
+ 	mov	w0, #1
+-	ldr	x1, =flag_no_el3
++	ldr	x1, =flag_keep_el
+ 	str	w0, [x1]
+ 
+-	b	start_no_el3
++	b	start_keep_el
+ 
+-1:	mov	x0, #0x30			// RES1
++	/*
++	 * EL3 initialisation
++	 */
++el3_init:
++	mov	x0, #0x30			// RES1
+ 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
+ 	orr	x0, x0, #(1 << 8)		// HVC enable
+ 
+@@ -124,7 +136,7 @@ ASM_FUNC(_start)
+ 
+ 	bl	gic_secure_init
+ 
+-	b	start_el3
++	b	start_el_max
+ 
+ err_invalid_id:
+ 	b	.
+@@ -151,7 +163,7 @@ ASM_FUNC(jump_kernel)
+ 	bl	find_logical_id
+ 	bl	setup_stack		// Reset stack pointer
+ 
+-	ldr	w0, flag_no_el3
++	ldr	w0, flag_keep_el
+ 	cmp	w0, #0			// Prepare Z flag
+ 
+ 	mov	x0, x20
+@@ -160,7 +172,7 @@ ASM_FUNC(jump_kernel)
+ 	mov	x3, x23
+ 
+ 	b.eq	1f
+-	br	x19			// No EL3
++	br	x19			// Keep current EL
+ 
+ 1:	mov	x4, #SPSR_KERNEL
+ 
+@@ -178,5 +190,5 @@ ASM_FUNC(jump_kernel)
+ 
+ 	.data
+ 	.align 3
+-flag_no_el3:
++flag_keep_el:
+ 	.long 0
+diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
+index 8bd224b..7b8919a 100644
+--- a/arch/aarch64/psci.S
++++ b/arch/aarch64/psci.S
+@@ -79,7 +79,7 @@ smc_exit:
+ 	ldp	x18, x19, [sp], #16
+ 	eret
+ 
+-ASM_FUNC(start_el3)
++ASM_FUNC(start_el_max)
+ 	ldr	x0, =vector
+ 	bl	setup_vector
+ 
+@@ -89,10 +89,11 @@ ASM_FUNC(start_el3)
+ 	b	psci_first_spin
+ 
+ /*
+- * This PSCI implementation requires EL3. Without EL3 we'll only boot the
+- * primary cpu, all others will be trapped in an infinite loop.
++ * This PSCI implementation requires the highest EL(EL3 or Armv8-R EL2).
++ * Without the highest EL, we'll only boot the primary cpu, all othersr
++ * will be trapped in an infinite loop.
+  */
+-ASM_FUNC(start_no_el3)
++ASM_FUNC(start_keep_el)
+ 	cpuid	x0, x1
+ 	bl	find_logical_id
+ 	cbz	x0, psci_first_spin
+diff --git a/arch/aarch64/spin.S b/arch/aarch64/spin.S
+index 1ea1c0b..bfb1d47 100644
+--- a/arch/aarch64/spin.S
++++ b/arch/aarch64/spin.S
+@@ -12,8 +12,8 @@
+ 
+ 	.text
+ 
+-ASM_FUNC(start_el3)
+-ASM_FUNC(start_no_el3)
++ASM_FUNC(start_el_max)
++ASM_FUNC(start_keep_el)
+ 	cpuid	x0, x1
+ 	bl	find_logical_id
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch
new file mode 100644
index 0000000..46447b8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch
@@ -0,0 +1,48 @@
+From 26f9b5354c2de9cc052531096ff92b04c3a3846f Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for EL1 booting
+
+When booting from EL1, add a check and skip the init of
+sctlr_el2 in jump_kernel
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S            | 6 +++++-
+ arch/aarch64/include/asm/cpu.h | 1 +
+ 2 files changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 84e1646..b589744 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -156,10 +156,14 @@ ASM_FUNC(jump_kernel)
+ 	ldr	x0, =SCTLR_EL1_KERNEL
+ 	msr	sctlr_el1, x0
+ 
++	mrs	x0, CurrentEL
++	cmp	x0, #CURRENTEL_EL2
++	b.lt	1f
++
+ 	ldr	x0, =SCTLR_EL2_KERNEL
+ 	msr	sctlr_el2, x0
+ 
+-	cpuid	x0, x1
++1:	cpuid	x0, x1
+ 	bl	find_logical_id
+ 	bl	setup_stack		// Reset stack pointer
+ 
+diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
+index 63eb1c3..b1003f4 100644
+--- a/arch/aarch64/include/asm/cpu.h
++++ b/arch/aarch64/include/asm/cpu.h
+@@ -11,6 +11,7 @@
+ 
+ #define MPIDR_ID_BITS		0xff00ffffff
+ 
++#define CURRENTEL_EL2		(2 << 2)
+ #define CURRENTEL_EL3		(3 << 2)
+ 
+ /*
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch
new file mode 100644
index 0000000..db81355
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch
@@ -0,0 +1,55 @@
+From ce628de7699dd6401ddf713efaa49872e2733619 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for lower EL booting
+
+Save SPSR_KERNEL into spsr_to_elx during el3_init.
+The jump_kernel will load spsr_to_elx into spsr_el3.
+
+This change will make it easier to control whether drop to lower EL
+before jumping to the kernel.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S | 15 +++++++++++++--
+ 1 file changed, 13 insertions(+), 2 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index b589744..6b45afc 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -130,7 +130,16 @@ el3_init:
+ 	mov	x0, #ZCR_EL3_LEN_MASK		// SVE: Enable full vector len
+ 	msr	ZCR_EL3, x0			// for EL2.
+ 
+-1:
++	/*
++	 * Save SPSR_KERNEL into spsr_to_elx.
++	 * The jump_kernel will load spsr_to_elx into spsr_el3
++	 */
++1:	mov	w0, #SPSR_KERNEL
++	ldr	x1, =spsr_to_elx
++	str	w0, [x1]
++	b	el_max_init
++
++el_max_init:
+ 	ldr	x0, =COUNTER_FREQ
+ 	msr	cntfrq_el0, x0
+ 
+@@ -178,7 +187,7 @@ ASM_FUNC(jump_kernel)
+ 	b.eq	1f
+ 	br	x19			// Keep current EL
+ 
+-1:	mov	x4, #SPSR_KERNEL
++1:	ldr	w4, spsr_to_elx
+ 
+ 	/*
+ 	 * If bit 0 of the kernel address is set, we're entering in AArch32
+@@ -196,3 +205,5 @@ ASM_FUNC(jump_kernel)
+ 	.align 3
+ flag_keep_el:
+ 	.long 0
++spsr_to_elx:
++	.long 0
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch
new file mode 100644
index 0000000..e10182e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch
@@ -0,0 +1,105 @@
+From 483d363bf825082b6db6de3c57d169e741861891 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] gic-v3: Prepare for gicv3 with EL2
+
+This is a preparation for allowing boot-wrapper configuring the gicv3
+with EL2.
+
+When confiuring with EL2, since there is no ICC_CTLR_EL2, the
+ICC_CTLR_EL3 cannot be replaced with ICC_CTLR_EL2 simply.
+See [https://developer.arm.com/documentation/ihi0069/latest/].
+
+As the caller, gic_secure_init expects the ICC_CTLR to be written,
+we change the function into gic_init_icc_ctlr(). In the GIC spec,
+the r/w bits in this register ([6:0]) either affect EL3 IRQ routing
+(not applicable since no EL3), non-secure IRQ handling (not applicable
+since only secure state in Armv8-R aarch64), or are aliased to
+ICC_CTLR_EL1 bits.
+So, based on this, the new gic_init_icc_ctlr() would be:
+When currentEL is EL3, init ICC_CTLR_EL3 as before.
+When currentEL is not EL3, init ICC_CTLR_EL1 with ICC_CTLR_EL1_RESET.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch32/include/asm/gic-v3.h |  7 +++++++
+ arch/aarch64/include/asm/gic-v3.h | 23 ++++++++++++++++++++---
+ common/gic-v3.c                   |  2 +-
+ 3 files changed, 28 insertions(+), 4 deletions(-)
+
+diff --git a/arch/aarch32/include/asm/gic-v3.h b/arch/aarch32/include/asm/gic-v3.h
+index 65f38de..11e7bc7 100644
+--- a/arch/aarch32/include/asm/gic-v3.h
++++ b/arch/aarch32/include/asm/gic-v3.h
+@@ -9,6 +9,8 @@
+ #ifndef __ASM_AARCH32_GICV3_H
+ #define __ASM_AARCH32_GICV3_H
+ 
++#define ICC_CTLR_RESET (0UL)
++
+ static inline void gic_write_icc_sre(uint32_t val)
+ {
+ 	asm volatile ("mcr p15, 6, %0, c12, c12, 5" : : "r" (val));
+@@ -19,4 +21,9 @@ static inline void gic_write_icc_ctlr(uint32_t val)
+ 	asm volatile ("mcr p15, 6, %0, c12, c12, 4" : : "r" (val));
+ }
+ 
++static inline void gic_init_icc_ctlr()
++{
++	gic_write_icc_ctlr(ICC_CTLR_RESET);
++}
++
+ #endif
+diff --git a/arch/aarch64/include/asm/gic-v3.h b/arch/aarch64/include/asm/gic-v3.h
+index 5b32380..090ab0b 100644
+--- a/arch/aarch64/include/asm/gic-v3.h
++++ b/arch/aarch64/include/asm/gic-v3.h
+@@ -15,14 +15,31 @@
+ #define ICC_CTLR_EL3	"S3_6_C12_C12_4"
+ #define ICC_PMR_EL1	"S3_0_C4_C6_0"
+ 
++#define ICC_CTLR_EL3_RESET     (0UL)
++#define ICC_CTLR_EL1_RESET     (0UL)
++
++static inline uint32_t current_el(void)
++{
++	uint32_t val;
++
++	asm volatile ("mrs %0, CurrentEL" : "=r" (val));
++	return val;
++}
++
+ static inline void gic_write_icc_sre(uint32_t val)
+ {
+-	asm volatile ("msr " ICC_SRE_EL3 ", %0" : : "r" (val));
++	if (current_el() == CURRENTEL_EL3)
++		asm volatile ("msr " ICC_SRE_EL3 ", %0" : : "r" (val));
++	else
++		asm volatile ("msr " ICC_SRE_EL2 ", %0" : : "r" (val));
+ }
+ 
+-static inline void gic_write_icc_ctlr(uint32_t val)
++static inline void gic_init_icc_ctlr()
+ {
+-	asm volatile ("msr " ICC_CTLR_EL3 ", %0" : : "r" (val));
++	if (current_el() == CURRENTEL_EL3)
++		asm volatile ("msr " ICC_CTLR_EL3 ", %0" : : "r" (ICC_CTLR_EL3_RESET));
++	else
++		asm volatile ("msr " ICC_CTLR_EL1 ", %0" : : "r" (ICC_CTLR_EL1_RESET));
+ }
+ 
+ #endif
+diff --git a/common/gic-v3.c b/common/gic-v3.c
+index 6207007..a0fe564 100644
+--- a/common/gic-v3.c
++++ b/common/gic-v3.c
+@@ -117,6 +117,6 @@ void gic_secure_init(void)
+ 	gic_write_icc_sre(ICC_SRE_Enable | ICC_SRE_DIB | ICC_SRE_DFB | ICC_SRE_SRE);
+ 	isb();
+ 
+-	gic_write_icc_ctlr(0);
++	gic_init_icc_ctlr();
+ 	isb();
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch
new file mode 100644
index 0000000..3b6f78a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch
@@ -0,0 +1,63 @@
+From be814863cdd5f61d9a16eec012d500550053c8c6 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for booting with EL2
+
+Prepare for allowing boot-wrapper to be entered in EL2.
+Detect current EL and set the corresponding EL registers.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S  |  8 ++++++++
+ arch/aarch64/utils.S | 10 +++++++++-
+ 2 files changed, 17 insertions(+), 1 deletion(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 6b45afc..908764a 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -195,10 +195,18 @@ ASM_FUNC(jump_kernel)
+ 	 */
+ 	bfi	x4, x19, #5, #1
+ 
++	mrs	x5, CurrentEL
++	cmp	x5, #CURRENTEL_EL2
++	b.eq	1f
++
+ 	msr	elr_el3, x19
+ 	msr	spsr_el3, x4
+ 	eret
+ 
++1:	msr	elr_el2, x19
++	msr	spsr_el2, x4
++	eret
++
+ 	.ltorg
+ 
+ 	.data
+diff --git a/arch/aarch64/utils.S b/arch/aarch64/utils.S
+index 85c7f8a..f02a249 100644
+--- a/arch/aarch64/utils.S
++++ b/arch/aarch64/utils.S
+@@ -34,10 +34,18 @@ ASM_FUNC(find_logical_id)
+ 	ret
+ 
+ /*
+- * Setup EL3 vectors
++ * Setup EL3/EL2 vectors
+  * x0: vector address
+  */
+ ASM_FUNC(setup_vector)
++	mrs	x1, CurrentEL
++	cmp	x1, #CURRENTEL_EL2
++	b.eq    1f
++
+ 	msr	VBAR_EL3, x0
+ 	isb
+ 	ret
++
++1:	msr	VBAR_EL2, x0
++	isb
++	ret
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch
new file mode 100644
index 0000000..aaacc72
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch
@@ -0,0 +1,182 @@
+From 81df76f8d94cb6c31c01739b078a72bdb8497441 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Introduce EL2 boot code for Armv8-R AArch64
+
+The Armv8-R AArch64 profile does not support the EL3 exception level.
+The Armv8-R AArch64 profile allows for an (optional) VMSAv8-64 MMU
+at EL1, which allows to run off-the-shelf Linux. However EL2 only
+supports a PMSA, which is not supported by Linux, so we need to drop
+into EL1 before entering the kernel.
+
+We add a new err_invalid_arch symbol as a dead loop. If we detect the
+current Armv8-R aarch64 only supports with PMSA, meaning we cannot boot
+Linux anymore, then we jump to err_invalid_arch.
+
+During Armv8-R aarch64 init, to make sure nothing unexpected traps into
+EL2, we auto-detect and config FIEN and EnSCXT in HCR_EL2.
+
+The boot sequence is:
+If CurrentEL == EL3, then goto EL3 initialisation and drop to lower EL
+  before entering the kernel.
+If CurrentEL == EL2 && id_aa64mmfr0_el1.MSA == 0xf (Armv8-R aarch64),
+  if id_aa64mmfr0_el1.MSA_frac == 0x2,
+    then goto Armv8-R AArch64 initialisation and drop to EL1 before
+    entering the kernel.
+  else, which means VMSA unsupported and cannot boot Linux,
+    goto err_invalid_arch (dead loop).
+Else, no initialisation and keep the current EL before entering the
+  kernel.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+---
+ arch/aarch64/boot.S            | 92 +++++++++++++++++++++++++++++++++-
+ arch/aarch64/include/asm/cpu.h |  2 +
+ 2 files changed, 92 insertions(+), 2 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 908764a..def9192 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -24,16 +24,24 @@ ASM_FUNC(_start)
+ 	 * Boot sequence
+ 	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
+ 	 *   lower EL before entering the kernel.
++	 * If CurrentEL == EL2 && id_aa64mmfr0_el1.MSA == 0xf, then
++	 *   If id_aa64mmfr0_el1.MSA_frac == 0x2, then goto
++	 *     Armv8-R AArch64 initialisation and drop to EL1 before
++	 *     entering the kernel.
++	 *   Else, which means VMSA unsupported and cannot boot Linux,
++	 *     goto err_invalid_arch (dead loop).
+ 	 * Else, no initialisation and keep the current EL before
+ 	 *   entering the kernel.
+ 	 */
+ 	mrs	x0, CurrentEL
+-	cmp	x0, #CURRENTEL_EL3
+-	b.eq	el3_init
++	cmp	x0, #CURRENTEL_EL2
++	bgt	el3_init
++	beq	el2_init
+ 
+ 	/*
+ 	 * We stay in the current EL for entering the kernel
+ 	 */
++keep_el:
+ 	mov	w0, #1
+ 	ldr	x1, =flag_keep_el
+ 	str	w0, [x1]
+@@ -139,6 +147,85 @@ el3_init:
+ 	str	w0, [x1]
+ 	b	el_max_init
+ 
++	/*
++	 * EL2 Armv8-R AArch64 initialisation
++	 */
++el2_init:
++	/* Detect Armv8-R AArch64 */
++	mrs	x1, id_aa64mmfr0_el1
++	/*
++	 * Check MSA, bits [51:48]:
++	 * 0xf means Armv8-R AArch64.
++	 * If not 0xf, proceed in Armv8-A EL2.
++	 */
++	ubfx	x0, x1, #48, #4			// MSA
++	cmp	x0, 0xf
++	bne	keep_el
++	/*
++	 * Check MSA_frac, bits [55:52]:
++	 * 0x2 means EL1&0 translation regime also supports VMSAv8-64.
++	 */
++	ubfx	x0, x1, #52, #4			// MSA_frac
++	cmp	x0, 0x2
++	/*
++	 * If not 0x2, no VMSA, so cannot boot Linux and dead loop.
++	 * Also, since the architecture guarantees that those CPUID
++	 * fields never lose features when the value in a field
++	 * increases, we use blt to cover it.
++	*/
++	blt	err_invalid_arch
++
++	mrs	x0, midr_el1
++	msr	vpidr_el2, x0
++
++	mrs	x0, mpidr_el1
++	msr	vmpidr_el2, x0
++
++	mov	x0, #(1 << 31)			// VTCR_MSA: VMSAv8-64 support
++	msr	vtcr_el2, x0
++
++	/* Init HCR_EL2 */
++	mov	x0, #(1 << 31)			// RES1: Armv8-R aarch64 only
++
++	mrs	x1, id_aa64pfr0_el1
++	ubfx	x2, x1, #56, 4			// ID_AA64PFR0_EL1.CSV2
++	cmp	x2, 0x2
++	b.lt	1f
++	/*
++	 * Disable trap when accessing SCTXNUM_EL0 or SCTXNUM_EL1
++	 * if FEAT_CSV2.
++	 */
++	orr	x0, x0, #(1 << 53)		// HCR_EL2.EnSCXT
++
++1:	ubfx	x2, x1, #28, 4			// ID_AA64PFR0_EL1.RAS
++	cmp	x2, 0x2
++	b.lt	1f
++	/* Disable trap when accessing ERXPFGCDN_EL1 if FEAT_RASv1p1. */
++	orr	x0, x0, #(1 << 47)		// HCR_EL2.FIEN
++
++	/* Enable pointer authentication if present */
++1:	mrs	x1, id_aa64isar1_el1
++	/*
++	 * If ID_AA64ISAR1_EL1.{GPI, GPA, API, APA} == {0000, 0000, 0000, 0000}
++	 *   then HCR_EL2.APK and HCR_EL2.API are RES 0.
++	 * Else
++	 *   set HCR_EL2.APK and HCR_EL2.API.
++	 */
++	ldr	x2, =(((0xff) << 24) | (0xff << 4))
++	and	x1, x1, x2
++	cbz	x1, 1f
++
++	orr	x0, x0, #(1 << 40)		// HCR_EL2.APK
++	orr	x0, x0, #(1 << 41)		// HCR_EL2.API
++
++1:	msr	hcr_el2, x0
++	isb
++
++	mov	w0, #SPSR_KERNEL_EL1
++	ldr	x1, =spsr_to_elx
++	str	w0, [x1]
++	// fall through
++
+ el_max_init:
+ 	ldr	x0, =COUNTER_FREQ
+ 	msr	cntfrq_el0, x0
+@@ -148,6 +235,7 @@ el_max_init:
+ 	b	start_el_max
+ 
+ err_invalid_id:
++err_invalid_arch:
+ 	b	.
+ 
+ 	/*
+diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
+index b1003f4..91f803c 100644
+--- a/arch/aarch64/include/asm/cpu.h
++++ b/arch/aarch64/include/asm/cpu.h
+@@ -25,6 +25,7 @@
+ #define SPSR_I			(1 << 7)	/* IRQ masked */
+ #define SPSR_F			(1 << 6)	/* FIQ masked */
+ #define SPSR_T			(1 << 5)	/* Thumb */
++#define SPSR_EL1H		(5 << 0)	/* EL1 Handler mode */
+ #define SPSR_EL2H		(9 << 0)	/* EL2 Handler mode */
+ #define SPSR_HYP		(0x1a << 0)	/* M[3:0] = hyp, M[4] = AArch32 */
+ 
+@@ -43,6 +44,7 @@
+ #else
+ #define SCTLR_EL1_KERNEL	SCTLR_EL1_RES1
+ #define SPSR_KERNEL		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL2H)
++#define SPSR_KERNEL_EL1		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL1H)
+ #endif
+ 
+ #ifndef __ASSEMBLY__
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch
new file mode 100644
index 0000000..b130854
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch
@@ -0,0 +1,89 @@
+From f5a31b4f4ea8daaa0d337d5a2322ddb1912083fc Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Wed, 26 May 2021 17:52:01 +0800
+Subject: [PATCH] Allow --enable-psci to choose between smc and hvc
+
+According to Armv8-R AArch64 manual [1], Armv8-R AArch64 does not
+support smc:
+
+- Pseudocode for AArch64.CheckForSMCUndefOrTrap has this snippet:
+
+      if !HaveEL(EL3) || PSTATE.EL == EL0 then
+          UNDEFINED;
+
+  And Armv8-R AArch64 does not have EL3.
+
+- In the document of HCR_EL2 TSC bit:
+  If EL3 is not implemented and HCR_EL2.NV is 0, it is IMPLEMENTATION
+  DEFINED whether this bit is:
+  - RES0.
+  - Implemented with the functionality as described in HCR_EL2.TSC.
+
+So hvc is needed in this situation. And due to the lack of libfdt, the
+psci method cannot be modified at runtime.
+
+To use smc, use --enable-psci or --enable-psci=smc.
+To use hvc, use --enable-psci=hvc.
+
+[1]: https://developer.arm.com/documentation/ddi0600/latest/
+
+Issue-Id: SCM-2654
+Upstream-Status: Pending
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+Change-Id: Ib8afabdad2d98bc37371d165bbb6f1f9b88bfc87
+
+Upstream-Status: Pending
+Signed-off-by: Huifeng Zhang <Huifeng.Zhang@arm.com>
+---
+ Makefile.am  | 10 +++++-----
+ configure.ac | 14 +++++++++-----
+ 2 files changed, 14 insertions(+), 10 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index f941b07..88a27de 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -50,11 +50,11 @@ endif
+ if PSCI
+ ARCH_OBJ	+= psci.o
+ COMMON_OBJ	+= psci.o
+-PSCI_NODE	:= psci {				\
+-			compatible = \"arm,psci\";	\
+-			method = \"smc\";		\
+-			cpu_on = <$(PSCI_CPU_ON)>;	\
+-			cpu_off = <$(PSCI_CPU_OFF)>;	\
++PSCI_NODE	:= psci {						\
++			compatible = \"arm,psci\";			\
++			method = \"$(PSCI_METHOD)\";			\
++			cpu_on = <$(PSCI_CPU_ON)>;			\
++			cpu_off = <$(PSCI_CPU_OFF)>;			\
+ 		   };
+ CPU_NODES	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/addpsci.pl $(KERNEL_DTB))
+ else
+diff --git a/configure.ac b/configure.ac
+index 9e3b722..53e51be 100644
+--- a/configure.ac
++++ b/configure.ac
+@@ -83,13 +83,17 @@ AS_IF([test "x$X_IMAGE" != "x"],
+ # Allow a user to pass --enable-psci
+ AC_ARG_ENABLE([psci],
+ 	AS_HELP_STRING([--disable-psci], [disable the psci boot method]),
+-	[USE_PSCI=$enableval], [USE_PSCI="yes"])
+-AM_CONDITIONAL([PSCI], [test "x$USE_PSCI" = "xyes"])
+-AS_IF([test "x$USE_PSCI" = "xyes"], [], [USE_PSCI=no])
+-
+-AS_IF([test "x$USE_PSCI" != "xyes" -a "x$KERNEL_ES" = "x32"],
++	[case "${enableval}" in
++		yes|smc) USE_PSCI=smc ;;
++		hvc) USE_PSCI=hvc ;;
++		*) AC_MSG_ERROR([Bad value "${enableval}" for --enable-psci. Use "smc" or "hvc"]) ;;
++	esac], [USE_PSCI="yes"])
++AM_CONDITIONAL([PSCI], [test "x$USE_PSCI" = "xyes" -o "x$USE_PSCI" = "xsmc" -o "x$USE_PSCI" = "xhvc"])
++
++AS_IF([test "x$USE_PSCI" = "xno" -a "x$KERNEL_ES" = "x32"],
+ 	[AC_MSG_ERROR([With an AArch32 kernel, boot method must be PSCI.])]
+ )
++AC_SUBST([PSCI_METHOD], [$USE_PSCI])
+ 
+ # Allow a user to pass --with-initrd
+ AC_ARG_WITH([initrd],
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch
new file mode 100644
index 0000000..2ce28b7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch
@@ -0,0 +1,48 @@
+From 3f4614e02f0f8d2522510578da2752f8e3511bb3 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Mon, 25 Oct 2021 17:09:13 +0800
+Subject: [PATCH] aarch64: Disable CNTPCT_EL0 trap for v8-R64
+
+To allow EL1 to access CNTPCT_EL0 without traping into EL2, we need to
+set CNTHCTL_EL2.EL1PCTEN to 1.
+
+For v8-R64, the CNTHCTL_EL2 register follows the v8-A architecture.
+However, as described in the v8-A architecture profile, the
+CNTHCTL_EL2's bit assignments are different according to whether the
+FEAT_VHE is implemented.
+
+Since v8-R64 does not support FEAT_VHE, we do not need to detect
+FEAT_VHE. We can simply set CNTHCTL_EL2.EL1PCTEN to 1.
+
+Issue-ID: SCM-3508
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I4147e66341c8153312021e6f2ab67d0037246da1
+---
+ arch/aarch64/boot.S | 12 ++++++++++++
+ 1 file changed, 12 insertions(+)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index def9192..6dbd5cc 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -219,6 +219,18 @@ el2_init:
+ 	orr	x0, x0, #(1 << 41)		// HCR_EL2.API
+ 
+ 1:	msr	hcr_el2, x0
++
++	/*
++	 * To disable trap when accessing CNTPCT_EL0, we need to set
++	 * CNTHCTL_EL2.EL1PCTEN to 1. However, the CNTHCTL_EL2 bit assignments
++	 * are different according to whether the FEAT_VHE is implemented.
++	 *
++	 * For Armv8-R AArch64, FEAT_VHE is not supported, so we do not need to
++	 * detect FEAT_VHE(ID_AA64MMFR1_EL1.VH) and simply set
++	 * CNTHCTL_EL2.EL1PCTEN to 1.
++	 */
++	mov	x0, #1				// CNTHCTL_EL2.EL1PCTEN
++	msr	cnthctl_el2, x0
+ 	isb
+ 
+ 	mov	w0, #SPSR_KERNEL_EL1
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch
new file mode 100644
index 0000000..0c310eb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch
@@ -0,0 +1,38 @@
+From 2851f0e6c1216894b9498d7b91256bb1ef49e544 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 15:10:28 +0800
+Subject: [PATCH] lds: Mark the mem range
+
+Add firmware_start and firmware_end, so that we can use them to
+calculate the mem range of boot-wrapper and then set the range to
+/memreserve/ of dtb.
+
+Issue-ID: SCM-3815
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Idc5a2894e193c75381049a0f359b4b2a51c567ee
+---
+ model.lds.S | 2 ++
+ 1 file changed, 2 insertions(+)
+
+diff --git a/model.lds.S b/model.lds.S
+index d4e7e13..ab98ddf 100644
+--- a/model.lds.S
++++ b/model.lds.S
+@@ -64,6 +64,7 @@ SECTIONS
+ #endif
+ 
+ 	.boot PHYS_OFFSET: {
++		PROVIDE(firmware_start = .);
+ 		*(.init)
+ 		*(.text*)
+ 		*(.data* .rodata* .bss* COMMON)
+@@ -76,6 +77,7 @@ SECTIONS
+ 		mbox = .;
+ 		QUAD(0x0)
+ 	}
++	PROVIDE(firmware_end = .);
+ 
+ 	ASSERT(etext <= (PHYS_OFFSET + TEXT_LIMIT), ".text overflow!")
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch
new file mode 100644
index 0000000..0305f8b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch
@@ -0,0 +1,6044 @@
+From fadf04f44b679d85e55b2e5f220fecbebb52ad03 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:02:17 +0800
+Subject: [PATCH] common: Introduce the libfdt
+
+We introduce libfdt (v1.6.1) [1] to boot-wrapper, so we can dynamically
+add the firmware node.
+
+According to [2]: The libfdt is GPL/BSD dual-licensed which means it can
+be used either under the terms of GPL, or under the terms of BSD.
+We choose BSD because the boot-wrapper is under BSD.
+
+[1]: https://github.com/dgibson/dtc/tree/v1.6.1/libfdt
+[2]: https://github.com/dgibson/dtc/blob/v1.6.1/README.license
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Iec2f469053c8ac0ed38838c597b21a42bdf67b38
+---
+ common/libfdt/README.license    |   56 +
+ common/libfdt/fdt.c             |  335 +++++
+ common/libfdt/fdt_addresses.c   |  101 ++
+ common/libfdt/fdt_check.c       |   93 ++
+ common/libfdt/fdt_empty_tree.c  |   38 +
+ common/libfdt/fdt_overlay.c     |  882 +++++++++++++
+ common/libfdt/fdt_ro.c          |  859 +++++++++++++
+ common/libfdt/fdt_rw.c          |  500 +++++++
+ common/libfdt/fdt_strerror.c    |   59 +
+ common/libfdt/fdt_sw.c          |  384 ++++++
+ common/libfdt/fdt_wip.c         |   94 ++
+ common/libfdt/libfdt_internal.h |  192 +++
+ include/fdt.h                   |   66 +
+ include/libfdt.h                | 2147 +++++++++++++++++++++++++++++++
+ include/libfdt_env.h            |   95 ++
+ 15 files changed, 5901 insertions(+)
+ create mode 100644 common/libfdt/README.license
+ create mode 100644 common/libfdt/fdt.c
+ create mode 100644 common/libfdt/fdt_addresses.c
+ create mode 100644 common/libfdt/fdt_check.c
+ create mode 100644 common/libfdt/fdt_empty_tree.c
+ create mode 100644 common/libfdt/fdt_overlay.c
+ create mode 100644 common/libfdt/fdt_ro.c
+ create mode 100644 common/libfdt/fdt_rw.c
+ create mode 100644 common/libfdt/fdt_strerror.c
+ create mode 100644 common/libfdt/fdt_sw.c
+ create mode 100644 common/libfdt/fdt_wip.c
+ create mode 100644 common/libfdt/libfdt_internal.h
+ create mode 100644 include/fdt.h
+ create mode 100644 include/libfdt.h
+ create mode 100644 include/libfdt_env.h
+
+diff --git a/common/libfdt/README.license b/common/libfdt/README.license
+new file mode 100644
+index 0000000..102b004
+--- /dev/null
++++ b/common/libfdt/README.license
+@@ -0,0 +1,56 @@
++Licensing and contribution policy of dtc and libfdt
++===================================================
++
++This dtc package contains two pieces of software: dtc itself, and
++libfdt which comprises the files in the libfdt/ subdirectory.  These
++two pieces of software, although closely related, are quite distinct.
++dtc does not incorporate or rely on libfdt for its operation, nor vice
++versa.  It is important that these two pieces of software have
++different license conditions.
++
++As SPDX license tags in each source file attest, dtc is licensed
++under the GNU GPL.  The full text of the GPL can be found in the file
++entitled 'GPL' which should be included in this package.  dtc code,
++therefore, may not be incorporated into works which do not have a GPL
++compatible license.
++
++libfdt, however, is GPL/BSD dual-licensed.  That is, it may be used
++either under the terms of the GPL, or under the terms of the 2-clause
++BSD license (aka the ISC license).  The full terms of that license can
++be found are in the file entitled 'BSD-2-Clause'. This is, in
++practice, equivalent to being BSD licensed, since the terms of the BSD
++license are strictly more permissive than the GPL.
++
++I made the decision to license libfdt in this way because I want to
++encourage widespread and correct usage of flattened device trees,
++including by proprietary or otherwise GPL-incompatible firmware or
++tools.  Allowing libfdt to be used under the terms of the BSD license
++makes that it easier for vendors or authors of such software to do so.
++
++This does mean that libfdt code could be "stolen" - say, included in a
++proprietary fimware and extended without contributing those extensions
++back to the libfdt mainline.  While I hope that doesn't happen, I
++believe the goal of allowing libfdt to be widely used is more
++important than avoiding that.  libfdt is quite small, and hardly
++rocket science; so the incentive for such impolite behaviour is small,
++and the inconvenience caused thereby is not dire.
++
++Licenses such as the LGPL which would allow code to be used in non-GPL
++software, but also require contributions to be returned were
++considered.  However, libfdt is designed to be used in firmwares and
++other environments with unusual technical constraints.  It's difficult
++to anticipate all possible changes which might be needed to meld
++libfdt into such environments and so difficult to suitably word a
++license that puts the boundary between what is and isn't permitted in
++the intended place.  Again, I judged encouraging widespread use of
++libfdt by keeping the license terms simple and familiar to be the more
++important goal.
++
++**IMPORTANT** It's intended that all of libfdt as released remain
++permissively licensed this way.  Therefore only contributions which
++are released under these terms can be merged into the libfdt mainline.
++
++
++David Gibson <david@gibson.dropbear.id.au>
++(principal original author of dtc and libfdt)
++2 November 2007
+diff --git a/common/libfdt/fdt.c b/common/libfdt/fdt.c
+new file mode 100644
+index 0000000..9fe7cf4
+--- /dev/null
++++ b/common/libfdt/fdt.c
+@@ -0,0 +1,335 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++/*
++ * Minimal sanity check for a read-only tree. fdt_ro_probe_() checks
++ * that the given buffer contains what appears to be a flattened
++ * device tree with sane information in its header.
++ */
++int32_t fdt_ro_probe_(const void *fdt)
++{
++	uint32_t totalsize = fdt_totalsize(fdt);
++
++	if (can_assume(VALID_DTB))
++		return totalsize;
++
++	/* The device tree must be at an 8-byte aligned address */
++	if ((uintptr_t)fdt & 7)
++		return -FDT_ERR_ALIGNMENT;
++
++	if (fdt_magic(fdt) == FDT_MAGIC) {
++		/* Complete tree */
++		if (!can_assume(LATEST)) {
++			if (fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
++				return -FDT_ERR_BADVERSION;
++			if (fdt_last_comp_version(fdt) >
++					FDT_LAST_SUPPORTED_VERSION)
++				return -FDT_ERR_BADVERSION;
++		}
++	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
++		/* Unfinished sequential-write blob */
++		if (!can_assume(VALID_INPUT) && fdt_size_dt_struct(fdt) == 0)
++			return -FDT_ERR_BADSTATE;
++	} else {
++		return -FDT_ERR_BADMAGIC;
++	}
++
++	if (totalsize < INT32_MAX)
++		return totalsize;
++	else
++		return -FDT_ERR_TRUNCATED;
++}
++
++static int check_off_(uint32_t hdrsize, uint32_t totalsize, uint32_t off)
++{
++	return (off >= hdrsize) && (off <= totalsize);
++}
++
++static int check_block_(uint32_t hdrsize, uint32_t totalsize,
++			uint32_t base, uint32_t size)
++{
++	if (!check_off_(hdrsize, totalsize, base))
++		return 0; /* block start out of bounds */
++	if ((base + size) < base)
++		return 0; /* overflow */
++	if (!check_off_(hdrsize, totalsize, base + size))
++		return 0; /* block end out of bounds */
++	return 1;
++}
++
++size_t fdt_header_size_(uint32_t version)
++{
++	if (version <= 1)
++		return FDT_V1_SIZE;
++	else if (version <= 2)
++		return FDT_V2_SIZE;
++	else if (version <= 3)
++		return FDT_V3_SIZE;
++	else if (version <= 16)
++		return FDT_V16_SIZE;
++	else
++		return FDT_V17_SIZE;
++}
++
++size_t fdt_header_size(const void *fdt)
++{
++	return can_assume(LATEST) ? FDT_V17_SIZE :
++		fdt_header_size_(fdt_version(fdt));
++}
++
++int fdt_check_header(const void *fdt)
++{
++	size_t hdrsize;
++
++	/* The device tree must be at an 8-byte aligned address */
++	if ((uintptr_t)fdt & 7)
++		return -FDT_ERR_ALIGNMENT;
++
++	if (fdt_magic(fdt) != FDT_MAGIC)
++		return -FDT_ERR_BADMAGIC;
++	if (!can_assume(LATEST)) {
++		if ((fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
++		    || (fdt_last_comp_version(fdt) >
++			FDT_LAST_SUPPORTED_VERSION))
++			return -FDT_ERR_BADVERSION;
++		if (fdt_version(fdt) < fdt_last_comp_version(fdt))
++			return -FDT_ERR_BADVERSION;
++	}
++	hdrsize = fdt_header_size(fdt);
++	if (!can_assume(VALID_DTB)) {
++
++		if ((fdt_totalsize(fdt) < hdrsize)
++		    || (fdt_totalsize(fdt) > INT_MAX))
++			return -FDT_ERR_TRUNCATED;
++
++		/* Bounds check memrsv block */
++		if (!check_off_(hdrsize, fdt_totalsize(fdt),
++				fdt_off_mem_rsvmap(fdt)))
++			return -FDT_ERR_TRUNCATED;
++	}
++
++	if (!can_assume(VALID_DTB)) {
++		/* Bounds check structure block */
++		if (!can_assume(LATEST) && fdt_version(fdt) < 17) {
++			if (!check_off_(hdrsize, fdt_totalsize(fdt),
++					fdt_off_dt_struct(fdt)))
++				return -FDT_ERR_TRUNCATED;
++		} else {
++			if (!check_block_(hdrsize, fdt_totalsize(fdt),
++					  fdt_off_dt_struct(fdt),
++					  fdt_size_dt_struct(fdt)))
++				return -FDT_ERR_TRUNCATED;
++		}
++
++		/* Bounds check strings block */
++		if (!check_block_(hdrsize, fdt_totalsize(fdt),
++				  fdt_off_dt_strings(fdt),
++				  fdt_size_dt_strings(fdt)))
++			return -FDT_ERR_TRUNCATED;
++	}
++
++	return 0;
++}
++
++const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
++{
++	unsigned int uoffset = offset;
++	unsigned int absoffset = offset + fdt_off_dt_struct(fdt);
++
++	if (offset < 0)
++		return NULL;
++
++	if (!can_assume(VALID_INPUT))
++		if ((absoffset < uoffset)
++		    || ((absoffset + len) < absoffset)
++		    || (absoffset + len) > fdt_totalsize(fdt))
++			return NULL;
++
++	if (can_assume(LATEST) || fdt_version(fdt) >= 0x11)
++		if (((uoffset + len) < uoffset)
++		    || ((offset + len) > fdt_size_dt_struct(fdt)))
++			return NULL;
++
++	return fdt_offset_ptr_(fdt, offset);
++}
++
++uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset)
++{
++	const fdt32_t *tagp, *lenp;
++	uint32_t tag;
++	int offset = startoffset;
++	const char *p;
++
++	*nextoffset = -FDT_ERR_TRUNCATED;
++	tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE);
++	if (!can_assume(VALID_DTB) && !tagp)
++		return FDT_END; /* premature end */
++	tag = fdt32_to_cpu(*tagp);
++	offset += FDT_TAGSIZE;
++
++	*nextoffset = -FDT_ERR_BADSTRUCTURE;
++	switch (tag) {
++	case FDT_BEGIN_NODE:
++		/* skip name */
++		do {
++			p = fdt_offset_ptr(fdt, offset++, 1);
++		} while (p && (*p != '\0'));
++		if (!can_assume(VALID_DTB) && !p)
++			return FDT_END; /* premature end */
++		break;
++
++	case FDT_PROP:
++		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
++		if (!can_assume(VALID_DTB) && !lenp)
++			return FDT_END; /* premature end */
++		/* skip-name offset, length and value */
++		offset += sizeof(struct fdt_property) - FDT_TAGSIZE
++			+ fdt32_to_cpu(*lenp);
++		if (!can_assume(LATEST) &&
++		    fdt_version(fdt) < 0x10 && fdt32_to_cpu(*lenp) >= 8 &&
++		    ((offset - fdt32_to_cpu(*lenp)) % 8) != 0)
++			offset += 4;
++		break;
++
++	case FDT_END:
++	case FDT_END_NODE:
++	case FDT_NOP:
++		break;
++
++	default:
++		return FDT_END;
++	}
++
++	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
++		return FDT_END; /* premature end */
++
++	*nextoffset = FDT_TAGALIGN(offset);
++	return tag;
++}
++
++int fdt_check_node_offset_(const void *fdt, int offset)
++{
++	if (!can_assume(VALID_INPUT)
++	    && ((offset < 0) || (offset % FDT_TAGSIZE)))
++		return -FDT_ERR_BADOFFSET;
++
++	if (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE)
++		return -FDT_ERR_BADOFFSET;
++
++	return offset;
++}
++
++int fdt_check_prop_offset_(const void *fdt, int offset)
++{
++	if (!can_assume(VALID_INPUT)
++	    && ((offset < 0) || (offset % FDT_TAGSIZE)))
++		return -FDT_ERR_BADOFFSET;
++
++	if (fdt_next_tag(fdt, offset, &offset) != FDT_PROP)
++		return -FDT_ERR_BADOFFSET;
++
++	return offset;
++}
++
++int fdt_next_node(const void *fdt, int offset, int *depth)
++{
++	int nextoffset = 0;
++	uint32_t tag;
++
++	if (offset >= 0)
++		if ((nextoffset = fdt_check_node_offset_(fdt, offset)) < 0)
++			return nextoffset;
++
++	do {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		switch (tag) {
++		case FDT_PROP:
++		case FDT_NOP:
++			break;
++
++		case FDT_BEGIN_NODE:
++			if (depth)
++				(*depth)++;
++			break;
++
++		case FDT_END_NODE:
++			if (depth && ((--(*depth)) < 0))
++				return nextoffset;
++			break;
++
++		case FDT_END:
++			if ((nextoffset >= 0)
++			    || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth))
++				return -FDT_ERR_NOTFOUND;
++			else
++				return nextoffset;
++		}
++	} while (tag != FDT_BEGIN_NODE);
++
++	return offset;
++}
++
++int fdt_first_subnode(const void *fdt, int offset)
++{
++	int depth = 0;
++
++	offset = fdt_next_node(fdt, offset, &depth);
++	if (offset < 0 || depth != 1)
++		return -FDT_ERR_NOTFOUND;
++
++	return offset;
++}
++
++int fdt_next_subnode(const void *fdt, int offset)
++{
++	int depth = 1;
++
++	/*
++	 * With respect to the parent, the depth of the next subnode will be
++	 * the same as the last.
++	 */
++	do {
++		offset = fdt_next_node(fdt, offset, &depth);
++		if (offset < 0 || depth < 1)
++			return -FDT_ERR_NOTFOUND;
++	} while (depth > 1);
++
++	return offset;
++}
++
++const char *fdt_find_string_(const char *strtab, int tabsize, const char *s)
++{
++	int len = strlen(s) + 1;
++	const char *last = strtab + tabsize - len;
++	const char *p;
++
++	for (p = strtab; p <= last; p++)
++		if (memcmp(p, s, len) == 0)
++			return p;
++	return NULL;
++}
++
++int fdt_move(const void *fdt, void *buf, int bufsize)
++{
++	if (!can_assume(VALID_INPUT) && bufsize < 0)
++		return -FDT_ERR_NOSPACE;
++
++	FDT_RO_PROBE(fdt);
++
++	if (fdt_totalsize(fdt) > (unsigned int)bufsize)
++		return -FDT_ERR_NOSPACE;
++
++	memmove(buf, fdt, fdt_totalsize(fdt));
++	return 0;
++}
+diff --git a/common/libfdt/fdt_addresses.c b/common/libfdt/fdt_addresses.c
+new file mode 100644
+index 0000000..9a82cd0
+--- /dev/null
++++ b/common/libfdt/fdt_addresses.c
+@@ -0,0 +1,101 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
++ * Copyright (C) 2018 embedded brains GmbH
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_cells(const void *fdt, int nodeoffset, const char *name)
++{
++	const fdt32_t *c;
++	uint32_t val;
++	int len;
++
++	c = fdt_getprop(fdt, nodeoffset, name, &len);
++	if (!c)
++		return len;
++
++	if (len != sizeof(*c))
++		return -FDT_ERR_BADNCELLS;
++
++	val = fdt32_to_cpu(*c);
++	if (val > FDT_MAX_NCELLS)
++		return -FDT_ERR_BADNCELLS;
++
++	return (int)val;
++}
++
++int fdt_address_cells(const void *fdt, int nodeoffset)
++{
++	int val;
++
++	val = fdt_cells(fdt, nodeoffset, "#address-cells");
++	if (val == 0)
++		return -FDT_ERR_BADNCELLS;
++	if (val == -FDT_ERR_NOTFOUND)
++		return 2;
++	return val;
++}
++
++int fdt_size_cells(const void *fdt, int nodeoffset)
++{
++	int val;
++
++	val = fdt_cells(fdt, nodeoffset, "#size-cells");
++	if (val == -FDT_ERR_NOTFOUND)
++		return 1;
++	return val;
++}
++
++/* This function assumes that [address|size]_cells is 1 or 2 */
++int fdt_appendprop_addrrange(void *fdt, int parent, int nodeoffset,
++			     const char *name, uint64_t addr, uint64_t size)
++{
++	int addr_cells, size_cells, ret;
++	uint8_t data[sizeof(fdt64_t) * 2], *prop;
++
++	ret = fdt_address_cells(fdt, parent);
++	if (ret < 0)
++		return ret;
++	addr_cells = ret;
++
++	ret = fdt_size_cells(fdt, parent);
++	if (ret < 0)
++		return ret;
++	size_cells = ret;
++
++	/* check validity of address */
++	prop = data;
++	if (addr_cells == 1) {
++		if ((addr > UINT32_MAX) || ((UINT32_MAX + 1 - addr) < size))
++			return -FDT_ERR_BADVALUE;
++
++		fdt32_st(prop, (uint32_t)addr);
++	} else if (addr_cells == 2) {
++		fdt64_st(prop, addr);
++	} else {
++		return -FDT_ERR_BADNCELLS;
++	}
++
++	/* check validity of size */
++	prop += addr_cells * sizeof(fdt32_t);
++	if (size_cells == 1) {
++		if (size > UINT32_MAX)
++			return -FDT_ERR_BADVALUE;
++
++		fdt32_st(prop, (uint32_t)size);
++	} else if (size_cells == 2) {
++		fdt64_st(prop, size);
++	} else {
++		return -FDT_ERR_BADNCELLS;
++	}
++
++	return fdt_appendprop(fdt, nodeoffset, name, data,
++			      (addr_cells + size_cells) * sizeof(fdt32_t));
++}
+diff --git a/common/libfdt/fdt_check.c b/common/libfdt/fdt_check.c
+new file mode 100644
+index 0000000..fa410a8
+--- /dev/null
++++ b/common/libfdt/fdt_check.c
+@@ -0,0 +1,93 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_check_full(const void *fdt, size_t bufsize)
++{
++	int err;
++	int num_memrsv;
++	int offset, nextoffset = 0;
++	uint32_t tag;
++	unsigned int depth = 0;
++	const void *prop;
++	const char *propname;
++	bool expect_end = false;
++
++	if (bufsize < FDT_V1_SIZE)
++		return -FDT_ERR_TRUNCATED;
++	if (bufsize < fdt_header_size(fdt))
++		return -FDT_ERR_TRUNCATED;
++	err = fdt_check_header(fdt);
++	if (err != 0)
++		return err;
++	if (bufsize < fdt_totalsize(fdt))
++		return -FDT_ERR_TRUNCATED;
++
++	num_memrsv = fdt_num_mem_rsv(fdt);
++	if (num_memrsv < 0)
++		return num_memrsv;
++
++	while (1) {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		if (nextoffset < 0)
++			return nextoffset;
++
++		/* If we see two root nodes, something is wrong */
++		if (expect_end && tag != FDT_END)
++			return -FDT_ERR_BADSTRUCTURE;
++
++		switch (tag) {
++		case FDT_NOP:
++			break;
++
++		case FDT_END:
++			if (depth != 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			return 0;
++
++		case FDT_BEGIN_NODE:
++			depth++;
++			if (depth > INT_MAX)
++				return -FDT_ERR_BADSTRUCTURE;
++
++			/* The root node must have an empty name */
++			if (depth == 1) {
++				const char *name;
++				int len;
++
++				name = fdt_get_name(fdt, offset, &len);
++				if (*name || len)
++					return -FDT_ERR_BADSTRUCTURE;
++			}
++			break;
++
++		case FDT_END_NODE:
++			if (depth == 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			depth--;
++			if (depth == 0)
++				expect_end = true;
++			break;
++
++		case FDT_PROP:
++			prop = fdt_getprop_by_offset(fdt, offset, &propname,
++						     &err);
++			if (!prop)
++				return err;
++			break;
++
++		default:
++			return -FDT_ERR_INTERNAL;
++		}
++	}
++}
+diff --git a/common/libfdt/fdt_empty_tree.c b/common/libfdt/fdt_empty_tree.c
+new file mode 100644
+index 0000000..49d54d4
+--- /dev/null
++++ b/common/libfdt/fdt_empty_tree.c
+@@ -0,0 +1,38 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2012 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_create_empty_tree(void *buf, int bufsize)
++{
++	int err;
++
++	err = fdt_create(buf, bufsize);
++	if (err)
++		return err;
++
++	err = fdt_finish_reservemap(buf);
++	if (err)
++		return err;
++
++	err = fdt_begin_node(buf, "");
++	if (err)
++		return err;
++
++	err =  fdt_end_node(buf);
++	if (err)
++		return err;
++
++	err = fdt_finish(buf);
++	if (err)
++		return err;
++
++	return fdt_open_into(buf, buf, bufsize);
++}
+diff --git a/common/libfdt/fdt_overlay.c b/common/libfdt/fdt_overlay.c
+new file mode 100644
+index 0000000..d217e79
+--- /dev/null
++++ b/common/libfdt/fdt_overlay.c
+@@ -0,0 +1,882 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2016 Free Electrons
++ * Copyright (C) 2016 NextThing Co.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++/**
++ * overlay_get_target_phandle - retrieves the target phandle of a fragment
++ * @fdto: pointer to the device tree overlay blob
++ * @fragment: node offset of the fragment in the overlay
++ *
++ * overlay_get_target_phandle() retrieves the target phandle of an
++ * overlay fragment when that fragment uses a phandle (target
++ * property) instead of a path (target-path property).
++ *
++ * returns:
++ *      the phandle pointed by the target property
++ *      0, if the phandle was not found
++ *	-1, if the phandle was malformed
++ */
++static uint32_t overlay_get_target_phandle(const void *fdto, int fragment)
++{
++	const fdt32_t *val;
++	int len;
++
++	val = fdt_getprop(fdto, fragment, "target", &len);
++	if (!val)
++		return 0;
++
++	if ((len != sizeof(*val)) || (fdt32_to_cpu(*val) == (uint32_t)-1))
++		return (uint32_t)-1;
++
++	return fdt32_to_cpu(*val);
++}
++
++/**
++ * overlay_get_target - retrieves the offset of a fragment's target
++ * @fdt: Base device tree blob
++ * @fdto: Device tree overlay blob
++ * @fragment: node offset of the fragment in the overlay
++ * @pathp: pointer which receives the path of the target (or NULL)
++ *
++ * overlay_get_target() retrieves the target offset in the base
++ * device tree of a fragment, no matter how the actual targeting is
++ * done (through a phandle or a path)
++ *
++ * returns:
++ *      the targeted node offset in the base device tree
++ *      Negative error code on error
++ */
++static int overlay_get_target(const void *fdt, const void *fdto,
++			      int fragment, char const **pathp)
++{
++	uint32_t phandle;
++	const char *path = NULL;
++	int path_len = 0, ret;
++
++	/* Try first to do a phandle based lookup */
++	phandle = overlay_get_target_phandle(fdto, fragment);
++	if (phandle == (uint32_t)-1)
++		return -FDT_ERR_BADPHANDLE;
++
++	/* no phandle, try path */
++	if (!phandle) {
++		/* And then a path based lookup */
++		path = fdt_getprop(fdto, fragment, "target-path", &path_len);
++		if (path)
++			ret = fdt_path_offset(fdt, path);
++		else
++			ret = path_len;
++	} else
++		ret = fdt_node_offset_by_phandle(fdt, phandle);
++
++	/*
++	* If we haven't found either a target or a
++	* target-path property in a node that contains a
++	* __overlay__ subnode (we wouldn't be called
++	* otherwise), consider it a improperly written
++	* overlay
++	*/
++	if (ret < 0 && path_len == -FDT_ERR_NOTFOUND)
++		ret = -FDT_ERR_BADOVERLAY;
++
++	/* return on error */
++	if (ret < 0)
++		return ret;
++
++	/* return pointer to path (if available) */
++	if (pathp)
++		*pathp = path ? path : NULL;
++
++	return ret;
++}
++
++/**
++ * overlay_phandle_add_offset - Increases a phandle by an offset
++ * @fdt: Base device tree blob
++ * @node: Device tree overlay blob
++ * @name: Name of the property to modify (phandle or linux,phandle)
++ * @delta: offset to apply
++ *
++ * overlay_phandle_add_offset() increments a node phandle by a given
++ * offset.
++ *
++ * returns:
++ *      0 on success.
++ *      Negative error code on error
++ */
++static int overlay_phandle_add_offset(void *fdt, int node,
++				      const char *name, uint32_t delta)
++{
++	const fdt32_t *val;
++	uint32_t adj_val;
++	int len;
++
++	val = fdt_getprop(fdt, node, name, &len);
++	if (!val)
++		return len;
++
++	if (len != sizeof(*val))
++		return -FDT_ERR_BADPHANDLE;
++
++	adj_val = fdt32_to_cpu(*val);
++	if ((adj_val + delta) < adj_val)
++		return -FDT_ERR_NOPHANDLES;
++
++	adj_val += delta;
++	if (adj_val == (uint32_t)-1)
++		return -FDT_ERR_NOPHANDLES;
++
++	return fdt_setprop_inplace_u32(fdt, node, name, adj_val);
++}
++
++/**
++ * overlay_adjust_node_phandles - Offsets the phandles of a node
++ * @fdto: Device tree overlay blob
++ * @node: Offset of the node we want to adjust
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_adjust_node_phandles() adds a constant to all the phandles
++ * of a given node. This is mainly use as part of the overlay
++ * application process, when we want to update all the overlay
++ * phandles to not conflict with the overlays of the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_adjust_node_phandles(void *fdto, int node,
++					uint32_t delta)
++{
++	int child;
++	int ret;
++
++	ret = overlay_phandle_add_offset(fdto, node, "phandle", delta);
++	if (ret && ret != -FDT_ERR_NOTFOUND)
++		return ret;
++
++	ret = overlay_phandle_add_offset(fdto, node, "linux,phandle", delta);
++	if (ret && ret != -FDT_ERR_NOTFOUND)
++		return ret;
++
++	fdt_for_each_subnode(child, fdto, node) {
++		ret = overlay_adjust_node_phandles(fdto, child, delta);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_adjust_local_phandles - Adjust the phandles of a whole overlay
++ * @fdto: Device tree overlay blob
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_adjust_local_phandles() adds a constant to all the
++ * phandles of an overlay. This is mainly use as part of the overlay
++ * application process, when we want to update all the overlay
++ * phandles to not conflict with the overlays of the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_adjust_local_phandles(void *fdto, uint32_t delta)
++{
++	/*
++	 * Start adjusting the phandles from the overlay root
++	 */
++	return overlay_adjust_node_phandles(fdto, 0, delta);
++}
++
++/**
++ * overlay_update_local_node_references - Adjust the overlay references
++ * @fdto: Device tree overlay blob
++ * @tree_node: Node offset of the node to operate on
++ * @fixup_node: Node offset of the matching local fixups node
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_update_local_nodes_references() update the phandles
++ * pointing to a node within the device tree overlay by adding a
++ * constant delta.
++ *
++ * This is mainly used as part of a device tree application process,
++ * where you want the device tree overlays phandles to not conflict
++ * with the ones from the base device tree before merging them.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_update_local_node_references(void *fdto,
++						int tree_node,
++						int fixup_node,
++						uint32_t delta)
++{
++	int fixup_prop;
++	int fixup_child;
++	int ret;
++
++	fdt_for_each_property_offset(fixup_prop, fdto, fixup_node) {
++		const fdt32_t *fixup_val;
++		const char *tree_val;
++		const char *name;
++		int fixup_len;
++		int tree_len;
++		int i;
++
++		fixup_val = fdt_getprop_by_offset(fdto, fixup_prop,
++						  &name, &fixup_len);
++		if (!fixup_val)
++			return fixup_len;
++
++		if (fixup_len % sizeof(uint32_t))
++			return -FDT_ERR_BADOVERLAY;
++		fixup_len /= sizeof(uint32_t);
++
++		tree_val = fdt_getprop(fdto, tree_node, name, &tree_len);
++		if (!tree_val) {
++			if (tree_len == -FDT_ERR_NOTFOUND)
++				return -FDT_ERR_BADOVERLAY;
++
++			return tree_len;
++		}
++
++		for (i = 0; i < fixup_len; i++) {
++			fdt32_t adj_val;
++			uint32_t poffset;
++
++			poffset = fdt32_to_cpu(fixup_val[i]);
++
++			/*
++			 * phandles to fixup can be unaligned.
++			 *
++			 * Use a memcpy for the architectures that do
++			 * not support unaligned accesses.
++			 */
++			memcpy(&adj_val, tree_val + poffset, sizeof(adj_val));
++
++			adj_val = cpu_to_fdt32(fdt32_to_cpu(adj_val) + delta);
++
++			ret = fdt_setprop_inplace_namelen_partial(fdto,
++								  tree_node,
++								  name,
++								  strlen(name),
++								  poffset,
++								  &adj_val,
++								  sizeof(adj_val));
++			if (ret == -FDT_ERR_NOSPACE)
++				return -FDT_ERR_BADOVERLAY;
++
++			if (ret)
++				return ret;
++		}
++	}
++
++	fdt_for_each_subnode(fixup_child, fdto, fixup_node) {
++		const char *fixup_child_name = fdt_get_name(fdto, fixup_child,
++							    NULL);
++		int tree_child;
++
++		tree_child = fdt_subnode_offset(fdto, tree_node,
++						fixup_child_name);
++		if (tree_child == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_BADOVERLAY;
++		if (tree_child < 0)
++			return tree_child;
++
++		ret = overlay_update_local_node_references(fdto,
++							   tree_child,
++							   fixup_child,
++							   delta);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_update_local_references - Adjust the overlay references
++ * @fdto: Device tree overlay blob
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_update_local_references() update all the phandles pointing
++ * to a node within the device tree overlay by adding a constant
++ * delta to not conflict with the base overlay.
++ *
++ * This is mainly used as part of a device tree application process,
++ * where you want the device tree overlays phandles to not conflict
++ * with the ones from the base device tree before merging them.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_update_local_references(void *fdto, uint32_t delta)
++{
++	int fixups;
++
++	fixups = fdt_path_offset(fdto, "/__local_fixups__");
++	if (fixups < 0) {
++		/* There's no local phandles to adjust, bail out */
++		if (fixups == -FDT_ERR_NOTFOUND)
++			return 0;
++
++		return fixups;
++	}
++
++	/*
++	 * Update our local references from the root of the tree
++	 */
++	return overlay_update_local_node_references(fdto, 0, fixups,
++						    delta);
++}
++
++/**
++ * overlay_fixup_one_phandle - Set an overlay phandle to the base one
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ * @symbols_off: Node offset of the symbols node in the base device tree
++ * @path: Path to a node holding a phandle in the overlay
++ * @path_len: number of path characters to consider
++ * @name: Name of the property holding the phandle reference in the overlay
++ * @name_len: number of name characters to consider
++ * @poffset: Offset within the overlay property where the phandle is stored
++ * @label: Label of the node referenced by the phandle
++ *
++ * overlay_fixup_one_phandle() resolves an overlay phandle pointing to
++ * a node in the base device tree.
++ *
++ * This is part of the device tree overlay application process, when
++ * you want all the phandles in the overlay to point to the actual
++ * base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_one_phandle(void *fdt, void *fdto,
++				     int symbols_off,
++				     const char *path, uint32_t path_len,
++				     const char *name, uint32_t name_len,
++				     int poffset, const char *label)
++{
++	const char *symbol_path;
++	uint32_t phandle;
++	fdt32_t phandle_prop;
++	int symbol_off, fixup_off;
++	int prop_len;
++
++	if (symbols_off < 0)
++		return symbols_off;
++
++	symbol_path = fdt_getprop(fdt, symbols_off, label,
++				  &prop_len);
++	if (!symbol_path)
++		return prop_len;
++
++	symbol_off = fdt_path_offset(fdt, symbol_path);
++	if (symbol_off < 0)
++		return symbol_off;
++
++	phandle = fdt_get_phandle(fdt, symbol_off);
++	if (!phandle)
++		return -FDT_ERR_NOTFOUND;
++
++	fixup_off = fdt_path_offset_namelen(fdto, path, path_len);
++	if (fixup_off == -FDT_ERR_NOTFOUND)
++		return -FDT_ERR_BADOVERLAY;
++	if (fixup_off < 0)
++		return fixup_off;
++
++	phandle_prop = cpu_to_fdt32(phandle);
++	return fdt_setprop_inplace_namelen_partial(fdto, fixup_off,
++						   name, name_len, poffset,
++						   &phandle_prop,
++						   sizeof(phandle_prop));
++};
++
++/**
++ * overlay_fixup_phandle - Set an overlay phandle to the base one
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ * @symbols_off: Node offset of the symbols node in the base device tree
++ * @property: Property offset in the overlay holding the list of fixups
++ *
++ * overlay_fixup_phandle() resolves all the overlay phandles pointed
++ * to in a __fixups__ property, and updates them to match the phandles
++ * in use in the base device tree.
++ *
++ * This is part of the device tree overlay application process, when
++ * you want all the phandles in the overlay to point to the actual
++ * base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_phandle(void *fdt, void *fdto, int symbols_off,
++				 int property)
++{
++	const char *value;
++	const char *label;
++	int len;
++
++	value = fdt_getprop_by_offset(fdto, property,
++				      &label, &len);
++	if (!value) {
++		if (len == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_INTERNAL;
++
++		return len;
++	}
++
++	do {
++		const char *path, *name, *fixup_end;
++		const char *fixup_str = value;
++		uint32_t path_len, name_len;
++		uint32_t fixup_len;
++		char *sep, *endptr;
++		int poffset, ret;
++
++		fixup_end = memchr(value, '\0', len);
++		if (!fixup_end)
++			return -FDT_ERR_BADOVERLAY;
++		fixup_len = fixup_end - fixup_str;
++
++		len -= fixup_len + 1;
++		value += fixup_len + 1;
++
++		path = fixup_str;
++		sep = memchr(fixup_str, ':', fixup_len);
++		if (!sep || *sep != ':')
++			return -FDT_ERR_BADOVERLAY;
++
++		path_len = sep - path;
++		if (path_len == (fixup_len - 1))
++			return -FDT_ERR_BADOVERLAY;
++
++		fixup_len -= path_len + 1;
++		name = sep + 1;
++		sep = memchr(name, ':', fixup_len);
++		if (!sep || *sep != ':')
++			return -FDT_ERR_BADOVERLAY;
++
++		name_len = sep - name;
++		if (!name_len)
++			return -FDT_ERR_BADOVERLAY;
++
++		poffset = strtoul(sep + 1, &endptr, 10);
++		if ((*endptr != '\0') || (endptr <= (sep + 1)))
++			return -FDT_ERR_BADOVERLAY;
++
++		ret = overlay_fixup_one_phandle(fdt, fdto, symbols_off,
++						path, path_len, name, name_len,
++						poffset, label);
++		if (ret)
++			return ret;
++	} while (len > 0);
++
++	return 0;
++}
++
++/**
++ * overlay_fixup_phandles - Resolve the overlay phandles to the base
++ *                          device tree
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_fixup_phandles() resolves all the overlay phandles pointing
++ * to nodes in the base device tree.
++ *
++ * This is one of the steps of the device tree overlay application
++ * process, when you want all the phandles in the overlay to point to
++ * the actual base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_phandles(void *fdt, void *fdto)
++{
++	int fixups_off, symbols_off;
++	int property;
++
++	/* We can have overlays without any fixups */
++	fixups_off = fdt_path_offset(fdto, "/__fixups__");
++	if (fixups_off == -FDT_ERR_NOTFOUND)
++		return 0; /* nothing to do */
++	if (fixups_off < 0)
++		return fixups_off;
++
++	/* And base DTs without symbols */
++	symbols_off = fdt_path_offset(fdt, "/__symbols__");
++	if ((symbols_off < 0 && (symbols_off != -FDT_ERR_NOTFOUND)))
++		return symbols_off;
++
++	fdt_for_each_property_offset(property, fdto, fixups_off) {
++		int ret;
++
++		ret = overlay_fixup_phandle(fdt, fdto, symbols_off, property);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_apply_node - Merges a node into the base device tree
++ * @fdt: Base Device Tree blob
++ * @target: Node offset in the base device tree to apply the fragment to
++ * @fdto: Device tree overlay blob
++ * @node: Node offset in the overlay holding the changes to merge
++ *
++ * overlay_apply_node() merges a node into a target base device tree
++ * node pointed.
++ *
++ * This is part of the final step in the device tree overlay
++ * application process, when all the phandles have been adjusted and
++ * resolved and you just have to merge overlay into the base device
++ * tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_apply_node(void *fdt, int target,
++			      void *fdto, int node)
++{
++	int property;
++	int subnode;
++
++	fdt_for_each_property_offset(property, fdto, node) {
++		const char *name;
++		const void *prop;
++		int prop_len;
++		int ret;
++
++		prop = fdt_getprop_by_offset(fdto, property, &name,
++					     &prop_len);
++		if (prop_len == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_INTERNAL;
++		if (prop_len < 0)
++			return prop_len;
++
++		ret = fdt_setprop(fdt, target, name, prop, prop_len);
++		if (ret)
++			return ret;
++	}
++
++	fdt_for_each_subnode(subnode, fdto, node) {
++		const char *name = fdt_get_name(fdto, subnode, NULL);
++		int nnode;
++		int ret;
++
++		nnode = fdt_add_subnode(fdt, target, name);
++		if (nnode == -FDT_ERR_EXISTS) {
++			nnode = fdt_subnode_offset(fdt, target, name);
++			if (nnode == -FDT_ERR_NOTFOUND)
++				return -FDT_ERR_INTERNAL;
++		}
++
++		if (nnode < 0)
++			return nnode;
++
++		ret = overlay_apply_node(fdt, nnode, fdto, subnode);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_merge - Merge an overlay into its base device tree
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_merge() merges an overlay into its base device tree.
++ *
++ * This is the next to last step in the device tree overlay application
++ * process, when all the phandles have been adjusted and resolved and
++ * you just have to merge overlay into the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_merge(void *fdt, void *fdto)
++{
++	int fragment;
++
++	fdt_for_each_subnode(fragment, fdto, 0) {
++		int overlay;
++		int target;
++		int ret;
++
++		/*
++		 * Each fragments will have an __overlay__ node. If
++		 * they don't, it's not supposed to be merged
++		 */
++		overlay = fdt_subnode_offset(fdto, fragment, "__overlay__");
++		if (overlay == -FDT_ERR_NOTFOUND)
++			continue;
++
++		if (overlay < 0)
++			return overlay;
++
++		target = overlay_get_target(fdt, fdto, fragment, NULL);
++		if (target < 0)
++			return target;
++
++		ret = overlay_apply_node(fdt, target, fdto, overlay);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++static int get_path_len(const void *fdt, int nodeoffset)
++{
++	int len = 0, namelen;
++	const char *name;
++
++	FDT_RO_PROBE(fdt);
++
++	for (;;) {
++		name = fdt_get_name(fdt, nodeoffset, &namelen);
++		if (!name)
++			return namelen;
++
++		/* root? we're done */
++		if (namelen == 0)
++			break;
++
++		nodeoffset = fdt_parent_offset(fdt, nodeoffset);
++		if (nodeoffset < 0)
++			return nodeoffset;
++		len += namelen + 1;
++	}
++
++	/* in case of root pretend it's "/" */
++	if (len == 0)
++		len++;
++	return len;
++}
++
++/**
++ * overlay_symbol_update - Update the symbols of base tree after a merge
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_symbol_update() updates the symbols of the base tree with the
++ * symbols of the applied overlay
++ *
++ * This is the last step in the device tree overlay application
++ * process, allowing the reference of overlay symbols by subsequent
++ * overlay operations.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_symbol_update(void *fdt, void *fdto)
++{
++	int root_sym, ov_sym, prop, path_len, fragment, target;
++	int len, frag_name_len, ret, rel_path_len;
++	const char *s, *e;
++	const char *path;
++	const char *name;
++	const char *frag_name;
++	const char *rel_path;
++	const char *target_path;
++	char *buf;
++	void *p;
++
++	ov_sym = fdt_subnode_offset(fdto, 0, "__symbols__");
++
++	/* if no overlay symbols exist no problem */
++	if (ov_sym < 0)
++		return 0;
++
++	root_sym = fdt_subnode_offset(fdt, 0, "__symbols__");
++
++	/* it no root symbols exist we should create them */
++	if (root_sym == -FDT_ERR_NOTFOUND)
++		root_sym = fdt_add_subnode(fdt, 0, "__symbols__");
++
++	/* any error is fatal now */
++	if (root_sym < 0)
++		return root_sym;
++
++	/* iterate over each overlay symbol */
++	fdt_for_each_property_offset(prop, fdto, ov_sym) {
++		path = fdt_getprop_by_offset(fdto, prop, &name, &path_len);
++		if (!path)
++			return path_len;
++
++		/* verify it's a string property (terminated by a single \0) */
++		if (path_len < 1 || memchr(path, '\0', path_len) != &path[path_len - 1])
++			return -FDT_ERR_BADVALUE;
++
++		/* keep end marker to avoid strlen() */
++		e = path + path_len;
++
++		if (*path != '/')
++			return -FDT_ERR_BADVALUE;
++
++		/* get fragment name first */
++		s = strchr(path + 1, '/');
++		if (!s) {
++			/* Symbol refers to something that won't end
++			 * up in the target tree */
++			continue;
++		}
++
++		frag_name = path + 1;
++		frag_name_len = s - path - 1;
++
++		/* verify format; safe since "s" lies in \0 terminated prop */
++		len = sizeof("/__overlay__/") - 1;
++		if ((e - s) > len && (memcmp(s, "/__overlay__/", len) == 0)) {
++			/* /<fragment-name>/__overlay__/<relative-subnode-path> */
++			rel_path = s + len;
++			rel_path_len = e - rel_path - 1;
++		} else if ((e - s) == len
++			   && (memcmp(s, "/__overlay__", len - 1) == 0)) {
++			/* /<fragment-name>/__overlay__ */
++			rel_path = "";
++			rel_path_len = 0;
++		} else {
++			/* Symbol refers to something that won't end
++			 * up in the target tree */
++			continue;
++		}
++
++		/* find the fragment index in which the symbol lies */
++		ret = fdt_subnode_offset_namelen(fdto, 0, frag_name,
++					       frag_name_len);
++		/* not found? */
++		if (ret < 0)
++			return -FDT_ERR_BADOVERLAY;
++		fragment = ret;
++
++		/* an __overlay__ subnode must exist */
++		ret = fdt_subnode_offset(fdto, fragment, "__overlay__");
++		if (ret < 0)
++			return -FDT_ERR_BADOVERLAY;
++
++		/* get the target of the fragment */
++		ret = overlay_get_target(fdt, fdto, fragment, &target_path);
++		if (ret < 0)
++			return ret;
++		target = ret;
++
++		/* if we have a target path use */
++		if (!target_path) {
++			ret = get_path_len(fdt, target);
++			if (ret < 0)
++				return ret;
++			len = ret;
++		} else {
++			len = strlen(target_path);
++		}
++
++		ret = fdt_setprop_placeholder(fdt, root_sym, name,
++				len + (len > 1) + rel_path_len + 1, &p);
++		if (ret < 0)
++			return ret;
++
++		if (!target_path) {
++			/* again in case setprop_placeholder changed it */
++			ret = overlay_get_target(fdt, fdto, fragment, &target_path);
++			if (ret < 0)
++				return ret;
++			target = ret;
++		}
++
++		buf = p;
++		if (len > 1) { /* target is not root */
++			if (!target_path) {
++				ret = fdt_get_path(fdt, target, buf, len + 1);
++				if (ret < 0)
++					return ret;
++			} else
++				memcpy(buf, target_path, len + 1);
++
++		} else
++			len--;
++
++		buf[len] = '/';
++		memcpy(buf + len + 1, rel_path, rel_path_len);
++		buf[len + 1 + rel_path_len] = '\0';
++	}
++
++	return 0;
++}
++
++int fdt_overlay_apply(void *fdt, void *fdto)
++{
++	uint32_t delta;
++	int ret;
++
++	FDT_RO_PROBE(fdt);
++	FDT_RO_PROBE(fdto);
++
++	ret = fdt_find_max_phandle(fdt, &delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_adjust_local_phandles(fdto, delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_update_local_references(fdto, delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_fixup_phandles(fdt, fdto);
++	if (ret)
++		goto err;
++
++	ret = overlay_merge(fdt, fdto);
++	if (ret)
++		goto err;
++
++	ret = overlay_symbol_update(fdt, fdto);
++	if (ret)
++		goto err;
++
++	/*
++	 * The overlay has been damaged, erase its magic.
++	 */
++	fdt_set_magic(fdto, ~0);
++
++	return 0;
++
++err:
++	/*
++	 * The overlay might have been damaged, erase its magic.
++	 */
++	fdt_set_magic(fdto, ~0);
++
++	/*
++	 * The base device tree might have been damaged, erase its
++	 * magic.
++	 */
++	fdt_set_magic(fdt, ~0);
++
++	return ret;
++}
+diff --git a/common/libfdt/fdt_ro.c b/common/libfdt/fdt_ro.c
+new file mode 100644
+index 0000000..17584da
+--- /dev/null
++++ b/common/libfdt/fdt_ro.c
+@@ -0,0 +1,859 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_nodename_eq_(const void *fdt, int offset,
++			    const char *s, int len)
++{
++	int olen;
++	const char *p = fdt_get_name(fdt, offset, &olen);
++
++	if (!p || olen < len)
++		/* short match */
++		return 0;
++
++	if (memcmp(p, s, len) != 0)
++		return 0;
++
++	if (p[len] == '\0')
++		return 1;
++	else if (!memchr(s, '@', len) && (p[len] == '@'))
++		return 1;
++	else
++		return 0;
++}
++
++const char *fdt_get_string(const void *fdt, int stroffset, int *lenp)
++{
++	int32_t totalsize;
++	uint32_t absoffset;
++	size_t len;
++	int err;
++	const char *s, *n;
++
++	if (can_assume(VALID_INPUT)) {
++		s = (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
++
++		if (lenp)
++			*lenp = strlen(s);
++		return s;
++	}
++	totalsize = fdt_ro_probe_(fdt);
++	err = totalsize;
++	if (totalsize < 0)
++		goto fail;
++
++	err = -FDT_ERR_BADOFFSET;
++	absoffset = stroffset + fdt_off_dt_strings(fdt);
++	if (absoffset >= (unsigned)totalsize)
++		goto fail;
++	len = totalsize - absoffset;
++
++	if (fdt_magic(fdt) == FDT_MAGIC) {
++		if (stroffset < 0)
++			goto fail;
++		if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
++			if ((unsigned)stroffset >= fdt_size_dt_strings(fdt))
++				goto fail;
++			if ((fdt_size_dt_strings(fdt) - stroffset) < len)
++				len = fdt_size_dt_strings(fdt) - stroffset;
++		}
++	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
++		unsigned int sw_stroffset = -stroffset;
++
++		if ((stroffset >= 0) ||
++		    (sw_stroffset > fdt_size_dt_strings(fdt)))
++			goto fail;
++		if (sw_stroffset < len)
++			len = sw_stroffset;
++	} else {
++		err = -FDT_ERR_INTERNAL;
++		goto fail;
++	}
++
++	s = (const char *)fdt + absoffset;
++	n = memchr(s, '\0', len);
++	if (!n) {
++		/* missing terminating NULL */
++		err = -FDT_ERR_TRUNCATED;
++		goto fail;
++	}
++
++	if (lenp)
++		*lenp = n - s;
++	return s;
++
++fail:
++	if (lenp)
++		*lenp = err;
++	return NULL;
++}
++
++const char *fdt_string(const void *fdt, int stroffset)
++{
++	return fdt_get_string(fdt, stroffset, NULL);
++}
++
++static int fdt_string_eq_(const void *fdt, int stroffset,
++			  const char *s, int len)
++{
++	int slen;
++	const char *p = fdt_get_string(fdt, stroffset, &slen);
++
++	return p && (slen == len) && (memcmp(p, s, len) == 0);
++}
++
++int fdt_find_max_phandle(const void *fdt, uint32_t *phandle)
++{
++	uint32_t max = 0;
++	int offset = -1;
++
++	while (true) {
++		uint32_t value;
++
++		offset = fdt_next_node(fdt, offset, NULL);
++		if (offset < 0) {
++			if (offset == -FDT_ERR_NOTFOUND)
++				break;
++
++			return offset;
++		}
++
++		value = fdt_get_phandle(fdt, offset);
++
++		if (value > max)
++			max = value;
++	}
++
++	if (phandle)
++		*phandle = max;
++
++	return 0;
++}
++
++int fdt_generate_phandle(const void *fdt, uint32_t *phandle)
++{
++	uint32_t max;
++	int err;
++
++	err = fdt_find_max_phandle(fdt, &max);
++	if (err < 0)
++		return err;
++
++	if (max == FDT_MAX_PHANDLE)
++		return -FDT_ERR_NOPHANDLES;
++
++	if (phandle)
++		*phandle = max + 1;
++
++	return 0;
++}
++
++static const struct fdt_reserve_entry *fdt_mem_rsv(const void *fdt, int n)
++{
++	unsigned int offset = n * sizeof(struct fdt_reserve_entry);
++	unsigned int absoffset = fdt_off_mem_rsvmap(fdt) + offset;
++
++	if (!can_assume(VALID_INPUT)) {
++		if (absoffset < fdt_off_mem_rsvmap(fdt))
++			return NULL;
++		if (absoffset > fdt_totalsize(fdt) -
++		    sizeof(struct fdt_reserve_entry))
++			return NULL;
++	}
++	return fdt_mem_rsv_(fdt, n);
++}
++
++int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
++{
++	const struct fdt_reserve_entry *re;
++
++	FDT_RO_PROBE(fdt);
++	re = fdt_mem_rsv(fdt, n);
++	if (!can_assume(VALID_INPUT) && !re)
++		return -FDT_ERR_BADOFFSET;
++
++	*address = fdt64_ld_(&re->address);
++	*size = fdt64_ld_(&re->size);
++	return 0;
++}
++
++int fdt_num_mem_rsv(const void *fdt)
++{
++	int i;
++	const struct fdt_reserve_entry *re;
++
++	for (i = 0; (re = fdt_mem_rsv(fdt, i)) != NULL; i++) {
++		if (fdt64_ld_(&re->size) == 0)
++			return i;
++	}
++	return -FDT_ERR_TRUNCATED;
++}
++
++static int nextprop_(const void *fdt, int offset)
++{
++	uint32_t tag;
++	int nextoffset;
++
++	do {
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		switch (tag) {
++		case FDT_END:
++			if (nextoffset >= 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			else
++				return nextoffset;
++
++		case FDT_PROP:
++			return offset;
++		}
++		offset = nextoffset;
++	} while (tag == FDT_NOP);
++
++	return -FDT_ERR_NOTFOUND;
++}
++
++int fdt_subnode_offset_namelen(const void *fdt, int offset,
++			       const char *name, int namelen)
++{
++	int depth;
++
++	FDT_RO_PROBE(fdt);
++
++	for (depth = 0;
++	     (offset >= 0) && (depth >= 0);
++	     offset = fdt_next_node(fdt, offset, &depth))
++		if ((depth == 1)
++		    && fdt_nodename_eq_(fdt, offset, name, namelen))
++			return offset;
++
++	if (depth < 0)
++		return -FDT_ERR_NOTFOUND;
++	return offset; /* error */
++}
++
++int fdt_subnode_offset(const void *fdt, int parentoffset,
++		       const char *name)
++{
++	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
++}
++
++int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen)
++{
++	const char *end = path + namelen;
++	const char *p = path;
++	int offset = 0;
++
++	FDT_RO_PROBE(fdt);
++
++	/* see if we have an alias */
++	if (*path != '/') {
++		const char *q = memchr(path, '/', end - p);
++
++		if (!q)
++			q = end;
++
++		p = fdt_get_alias_namelen(fdt, p, q - p);
++		if (!p)
++			return -FDT_ERR_BADPATH;
++		offset = fdt_path_offset(fdt, p);
++
++		p = q;
++	}
++
++	while (p < end) {
++		const char *q;
++
++		while (*p == '/') {
++			p++;
++			if (p == end)
++				return offset;
++		}
++		q = memchr(p, '/', end - p);
++		if (! q)
++			q = end;
++
++		offset = fdt_subnode_offset_namelen(fdt, offset, p, q-p);
++		if (offset < 0)
++			return offset;
++
++		p = q;
++	}
++
++	return offset;
++}
++
++int fdt_path_offset(const void *fdt, const char *path)
++{
++	return fdt_path_offset_namelen(fdt, path, strlen(path));
++}
++
++const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
++{
++	const struct fdt_node_header *nh = fdt_offset_ptr_(fdt, nodeoffset);
++	const char *nameptr;
++	int err;
++
++	if (((err = fdt_ro_probe_(fdt)) < 0)
++	    || ((err = fdt_check_node_offset_(fdt, nodeoffset)) < 0))
++			goto fail;
++
++	nameptr = nh->name;
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		/*
++		 * For old FDT versions, match the naming conventions of V16:
++		 * give only the leaf name (after all /). The actual tree
++		 * contents are loosely checked.
++		 */
++		const char *leaf;
++		leaf = strrchr(nameptr, '/');
++		if (leaf == NULL) {
++			err = -FDT_ERR_BADSTRUCTURE;
++			goto fail;
++		}
++		nameptr = leaf+1;
++	}
++
++	if (len)
++		*len = strlen(nameptr);
++
++	return nameptr;
++
++ fail:
++	if (len)
++		*len = err;
++	return NULL;
++}
++
++int fdt_first_property_offset(const void *fdt, int nodeoffset)
++{
++	int offset;
++
++	if ((offset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
++		return offset;
++
++	return nextprop_(fdt, offset);
++}
++
++int fdt_next_property_offset(const void *fdt, int offset)
++{
++	if ((offset = fdt_check_prop_offset_(fdt, offset)) < 0)
++		return offset;
++
++	return nextprop_(fdt, offset);
++}
++
++static const struct fdt_property *fdt_get_property_by_offset_(const void *fdt,
++						              int offset,
++						              int *lenp)
++{
++	int err;
++	const struct fdt_property *prop;
++
++	if (!can_assume(VALID_INPUT) &&
++	    (err = fdt_check_prop_offset_(fdt, offset)) < 0) {
++		if (lenp)
++			*lenp = err;
++		return NULL;
++	}
++
++	prop = fdt_offset_ptr_(fdt, offset);
++
++	if (lenp)
++		*lenp = fdt32_ld_(&prop->len);
++
++	return prop;
++}
++
++const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
++						      int offset,
++						      int *lenp)
++{
++	/* Prior to version 16, properties may need realignment
++	 * and this API does not work. fdt_getprop_*() will, however. */
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		if (lenp)
++			*lenp = -FDT_ERR_BADVERSION;
++		return NULL;
++	}
++
++	return fdt_get_property_by_offset_(fdt, offset, lenp);
++}
++
++static const struct fdt_property *fdt_get_property_namelen_(const void *fdt,
++						            int offset,
++						            const char *name,
++						            int namelen,
++							    int *lenp,
++							    int *poffset)
++{
++	for (offset = fdt_first_property_offset(fdt, offset);
++	     (offset >= 0);
++	     (offset = fdt_next_property_offset(fdt, offset))) {
++		const struct fdt_property *prop;
++
++		prop = fdt_get_property_by_offset_(fdt, offset, lenp);
++		if (!can_assume(LIBFDT_FLAWLESS) && !prop) {
++			offset = -FDT_ERR_INTERNAL;
++			break;
++		}
++		if (fdt_string_eq_(fdt, fdt32_ld_(&prop->nameoff),
++				   name, namelen)) {
++			if (poffset)
++				*poffset = offset;
++			return prop;
++		}
++	}
++
++	if (lenp)
++		*lenp = offset;
++	return NULL;
++}
++
++
++const struct fdt_property *fdt_get_property_namelen(const void *fdt,
++						    int offset,
++						    const char *name,
++						    int namelen, int *lenp)
++{
++	/* Prior to version 16, properties may need realignment
++	 * and this API does not work. fdt_getprop_*() will, however. */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		if (lenp)
++			*lenp = -FDT_ERR_BADVERSION;
++		return NULL;
++	}
++
++	return fdt_get_property_namelen_(fdt, offset, name, namelen, lenp,
++					 NULL);
++}
++
++
++const struct fdt_property *fdt_get_property(const void *fdt,
++					    int nodeoffset,
++					    const char *name, int *lenp)
++{
++	return fdt_get_property_namelen(fdt, nodeoffset, name,
++					strlen(name), lenp);
++}
++
++const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
++				const char *name, int namelen, int *lenp)
++{
++	int poffset;
++	const struct fdt_property *prop;
++
++	prop = fdt_get_property_namelen_(fdt, nodeoffset, name, namelen, lenp,
++					 &poffset);
++	if (!prop)
++		return NULL;
++
++	/* Handle realignment */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
++	    (poffset + sizeof(*prop)) % 8 && fdt32_ld_(&prop->len) >= 8)
++		return prop->data + 4;
++	return prop->data;
++}
++
++const void *fdt_getprop_by_offset(const void *fdt, int offset,
++				  const char **namep, int *lenp)
++{
++	const struct fdt_property *prop;
++
++	prop = fdt_get_property_by_offset_(fdt, offset, lenp);
++	if (!prop)
++		return NULL;
++	if (namep) {
++		const char *name;
++		int namelen;
++
++		if (!can_assume(VALID_INPUT)) {
++			name = fdt_get_string(fdt, fdt32_ld_(&prop->nameoff),
++					      &namelen);
++			if (!name) {
++				if (lenp)
++					*lenp = namelen;
++				return NULL;
++			}
++			*namep = name;
++		} else {
++			*namep = fdt_string(fdt, fdt32_ld_(&prop->nameoff));
++		}
++	}
++
++	/* Handle realignment */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
++	    (offset + sizeof(*prop)) % 8 && fdt32_ld_(&prop->len) >= 8)
++		return prop->data + 4;
++	return prop->data;
++}
++
++const void *fdt_getprop(const void *fdt, int nodeoffset,
++			const char *name, int *lenp)
++{
++	return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp);
++}
++
++uint32_t fdt_get_phandle(const void *fdt, int nodeoffset)
++{
++	const fdt32_t *php;
++	int len;
++
++	/* FIXME: This is a bit sub-optimal, since we potentially scan
++	 * over all the properties twice. */
++	php = fdt_getprop(fdt, nodeoffset, "phandle", &len);
++	if (!php || (len != sizeof(*php))) {
++		php = fdt_getprop(fdt, nodeoffset, "linux,phandle", &len);
++		if (!php || (len != sizeof(*php)))
++			return 0;
++	}
++
++	return fdt32_ld_(php);
++}
++
++const char *fdt_get_alias_namelen(const void *fdt,
++				  const char *name, int namelen)
++{
++	int aliasoffset;
++
++	aliasoffset = fdt_path_offset(fdt, "/aliases");
++	if (aliasoffset < 0)
++		return NULL;
++
++	return fdt_getprop_namelen(fdt, aliasoffset, name, namelen, NULL);
++}
++
++const char *fdt_get_alias(const void *fdt, const char *name)
++{
++	return fdt_get_alias_namelen(fdt, name, strlen(name));
++}
++
++int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen)
++{
++	int pdepth = 0, p = 0;
++	int offset, depth, namelen;
++	const char *name;
++
++	FDT_RO_PROBE(fdt);
++
++	if (buflen < 2)
++		return -FDT_ERR_NOSPACE;
++
++	for (offset = 0, depth = 0;
++	     (offset >= 0) && (offset <= nodeoffset);
++	     offset = fdt_next_node(fdt, offset, &depth)) {
++		while (pdepth > depth) {
++			do {
++				p--;
++			} while (buf[p-1] != '/');
++			pdepth--;
++		}
++
++		if (pdepth >= depth) {
++			name = fdt_get_name(fdt, offset, &namelen);
++			if (!name)
++				return namelen;
++			if ((p + namelen + 1) <= buflen) {
++				memcpy(buf + p, name, namelen);
++				p += namelen;
++				buf[p++] = '/';
++				pdepth++;
++			}
++		}
++
++		if (offset == nodeoffset) {
++			if (pdepth < (depth + 1))
++				return -FDT_ERR_NOSPACE;
++
++			if (p > 1) /* special case so that root path is "/", not "" */
++				p--;
++			buf[p] = '\0';
++			return 0;
++		}
++	}
++
++	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
++		return -FDT_ERR_BADOFFSET;
++	else if (offset == -FDT_ERR_BADOFFSET)
++		return -FDT_ERR_BADSTRUCTURE;
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
++				 int supernodedepth, int *nodedepth)
++{
++	int offset, depth;
++	int supernodeoffset = -FDT_ERR_INTERNAL;
++
++	FDT_RO_PROBE(fdt);
++
++	if (supernodedepth < 0)
++		return -FDT_ERR_NOTFOUND;
++
++	for (offset = 0, depth = 0;
++	     (offset >= 0) && (offset <= nodeoffset);
++	     offset = fdt_next_node(fdt, offset, &depth)) {
++		if (depth == supernodedepth)
++			supernodeoffset = offset;
++
++		if (offset == nodeoffset) {
++			if (nodedepth)
++				*nodedepth = depth;
++
++			if (supernodedepth > depth)
++				return -FDT_ERR_NOTFOUND;
++			else
++				return supernodeoffset;
++		}
++	}
++
++	if (!can_assume(VALID_INPUT)) {
++		if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
++			return -FDT_ERR_BADOFFSET;
++		else if (offset == -FDT_ERR_BADOFFSET)
++			return -FDT_ERR_BADSTRUCTURE;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_node_depth(const void *fdt, int nodeoffset)
++{
++	int nodedepth;
++	int err;
++
++	err = fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, &nodedepth);
++	if (err)
++		return (can_assume(LIBFDT_FLAWLESS) || err < 0) ? err :
++			-FDT_ERR_INTERNAL;
++	return nodedepth;
++}
++
++int fdt_parent_offset(const void *fdt, int nodeoffset)
++{
++	int nodedepth = fdt_node_depth(fdt, nodeoffset);
++
++	if (nodedepth < 0)
++		return nodedepth;
++	return fdt_supernode_atdepth_offset(fdt, nodeoffset,
++					    nodedepth - 1, NULL);
++}
++
++int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
++				  const char *propname,
++				  const void *propval, int proplen)
++{
++	int offset;
++	const void *val;
++	int len;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we scan each
++	 * property of a node in fdt_getprop(), then if that didn't
++	 * find what we want, we scan over them again making our way
++	 * to the next node.  Still it's the easiest to implement
++	 * approach; performance can come later. */
++	for (offset = fdt_next_node(fdt, startoffset, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		val = fdt_getprop(fdt, offset, propname, &len);
++		if (val && (len == proplen)
++		    && (memcmp(val, propval, len) == 0))
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle)
++{
++	int offset;
++
++	if ((phandle == 0) || (phandle == ~0U))
++		return -FDT_ERR_BADPHANDLE;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we
++	 * potentially scan each property of a node in
++	 * fdt_get_phandle(), then if that didn't find what
++	 * we want, we scan over them again making our way to the next
++	 * node.  Still it's the easiest to implement approach;
++	 * performance can come later. */
++	for (offset = fdt_next_node(fdt, -1, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		if (fdt_get_phandle(fdt, offset) == phandle)
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
++{
++	int len = strlen(str);
++	const char *p;
++
++	while (listlen >= len) {
++		if (memcmp(str, strlist, len+1) == 0)
++			return 1;
++		p = memchr(strlist, '\0', listlen);
++		if (!p)
++			return 0; /* malformed strlist.. */
++		listlen -= (p-strlist) + 1;
++		strlist = p + 1;
++	}
++	return 0;
++}
++
++int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property)
++{
++	const char *list, *end;
++	int length, count = 0;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list)
++		return length;
++
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end)
++			return -FDT_ERR_BADVALUE;
++
++		list += length;
++		count++;
++	}
++
++	return count;
++}
++
++int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
++			  const char *string)
++{
++	int length, len, idx = 0;
++	const char *list, *end;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list)
++		return length;
++
++	len = strlen(string) + 1;
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end)
++			return -FDT_ERR_BADVALUE;
++
++		if (length == len && memcmp(list, string, length) == 0)
++			return idx;
++
++		list += length;
++		idx++;
++	}
++
++	return -FDT_ERR_NOTFOUND;
++}
++
++const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
++			       const char *property, int idx,
++			       int *lenp)
++{
++	const char *list, *end;
++	int length;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list) {
++		if (lenp)
++			*lenp = length;
++
++		return NULL;
++	}
++
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end) {
++			if (lenp)
++				*lenp = -FDT_ERR_BADVALUE;
++
++			return NULL;
++		}
++
++		if (idx == 0) {
++			if (lenp)
++				*lenp = length - 1;
++
++			return list;
++		}
++
++		list += length;
++		idx--;
++	}
++
++	if (lenp)
++		*lenp = -FDT_ERR_NOTFOUND;
++
++	return NULL;
++}
++
++int fdt_node_check_compatible(const void *fdt, int nodeoffset,
++			      const char *compatible)
++{
++	const void *prop;
++	int len;
++
++	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
++	if (!prop)
++		return len;
++
++	return !fdt_stringlist_contains(prop, len, compatible);
++}
++
++int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
++				  const char *compatible)
++{
++	int offset, err;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we scan each
++	 * property of a node in fdt_node_check_compatible(), then if
++	 * that didn't find what we want, we scan over them again
++	 * making our way to the next node.  Still it's the easiest to
++	 * implement approach; performance can come later. */
++	for (offset = fdt_next_node(fdt, startoffset, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		err = fdt_node_check_compatible(fdt, offset, compatible);
++		if ((err < 0) && (err != -FDT_ERR_NOTFOUND))
++			return err;
++		else if (err == 0)
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
+diff --git a/common/libfdt/fdt_rw.c b/common/libfdt/fdt_rw.c
+new file mode 100644
+index 0000000..3621d36
+--- /dev/null
++++ b/common/libfdt/fdt_rw.c
+@@ -0,0 +1,500 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_blocks_misordered_(const void *fdt,
++				  int mem_rsv_size, int struct_size)
++{
++	return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
++		|| (fdt_off_dt_struct(fdt) <
++		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
++		|| (fdt_off_dt_strings(fdt) <
++		    (fdt_off_dt_struct(fdt) + struct_size))
++		|| (fdt_totalsize(fdt) <
++		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
++}
++
++static int fdt_rw_probe_(void *fdt)
++{
++	if (can_assume(VALID_DTB))
++		return 0;
++	FDT_RO_PROBE(fdt);
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 17)
++		return -FDT_ERR_BADVERSION;
++	if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry),
++				   fdt_size_dt_struct(fdt)))
++		return -FDT_ERR_BADLAYOUT;
++	if (!can_assume(LATEST) && fdt_version(fdt) > 17)
++		fdt_set_version(fdt, 17);
++
++	return 0;
++}
++
++#define FDT_RW_PROBE(fdt) \
++	{ \
++		int err_; \
++		if ((err_ = fdt_rw_probe_(fdt)) != 0) \
++			return err_; \
++	}
++
++static inline unsigned int fdt_data_size_(void *fdt)
++{
++	return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
++}
++
++static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen)
++{
++	char *p = splicepoint;
++	unsigned int dsize = fdt_data_size_(fdt);
++	size_t soff = p - (char *)fdt;
++
++	if ((oldlen < 0) || (soff + oldlen < soff) || (soff + oldlen > dsize))
++		return -FDT_ERR_BADOFFSET;
++	if ((p < (char *)fdt) || (dsize + newlen < (unsigned)oldlen))
++		return -FDT_ERR_BADOFFSET;
++	if (dsize - oldlen + newlen > fdt_totalsize(fdt))
++		return -FDT_ERR_NOSPACE;
++	memmove(p + newlen, p + oldlen, ((char *)fdt + dsize) - (p + oldlen));
++	return 0;
++}
++
++static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p,
++			       int oldn, int newn)
++{
++	int delta = (newn - oldn) * sizeof(*p);
++	int err;
++	err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
++	if (err)
++		return err;
++	fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
++	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
++	return 0;
++}
++
++static int fdt_splice_struct_(void *fdt, void *p,
++			      int oldlen, int newlen)
++{
++	int delta = newlen - oldlen;
++	int err;
++
++	if ((err = fdt_splice_(fdt, p, oldlen, newlen)))
++		return err;
++
++	fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
++	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
++	return 0;
++}
++
++/* Must only be used to roll back in case of error */
++static void fdt_del_last_string_(void *fdt, const char *s)
++{
++	int newlen = strlen(s) + 1;
++
++	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen);
++}
++
++static int fdt_splice_string_(void *fdt, int newlen)
++{
++	void *p = (char *)fdt
++		+ fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
++	int err;
++
++	if ((err = fdt_splice_(fdt, p, 0, newlen)))
++		return err;
++
++	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
++	return 0;
++}
++
++/**
++ * fdt_find_add_string_() - Find or allocate a string
++ *
++ * @fdt: pointer to the device tree to check/adjust
++ * @s: string to find/add
++ * @allocated: Set to 0 if the string was found, 1 if not found and so
++ *	allocated. Ignored if can_assume(NO_ROLLBACK)
++ * @return offset of string in the string table (whether found or added)
++ */
++static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
++{
++	char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
++	const char *p;
++	char *new;
++	int len = strlen(s) + 1;
++	int err;
++
++	if (!can_assume(NO_ROLLBACK))
++		*allocated = 0;
++
++	p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
++	if (p)
++		/* found it */
++		return (p - strtab);
++
++	new = strtab + fdt_size_dt_strings(fdt);
++	err = fdt_splice_string_(fdt, len);
++	if (err)
++		return err;
++
++	if (!can_assume(NO_ROLLBACK))
++		*allocated = 1;
++
++	memcpy(new, s, len);
++	return (new - strtab);
++}
++
++int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
++{
++	struct fdt_reserve_entry *re;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
++	err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
++	if (err)
++		return err;
++
++	re->address = cpu_to_fdt64(address);
++	re->size = cpu_to_fdt64(size);
++	return 0;
++}
++
++int fdt_del_mem_rsv(void *fdt, int n)
++{
++	struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
++
++	FDT_RW_PROBE(fdt);
++
++	if (n >= fdt_num_mem_rsv(fdt))
++		return -FDT_ERR_NOTFOUND;
++
++	return fdt_splice_mem_rsv_(fdt, re, 1, 0);
++}
++
++static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
++				int len, struct fdt_property **prop)
++{
++	int oldlen;
++	int err;
++
++	*prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
++	if (!*prop)
++		return oldlen;
++
++	if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
++				      FDT_TAGALIGN(len))))
++		return err;
++
++	(*prop)->len = cpu_to_fdt32(len);
++	return 0;
++}
++
++static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
++			     int len, struct fdt_property **prop)
++{
++	int proplen;
++	int nextoffset;
++	int namestroff;
++	int err;
++	int allocated;
++
++	if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
++		return nextoffset;
++
++	namestroff = fdt_find_add_string_(fdt, name, &allocated);
++	if (namestroff < 0)
++		return namestroff;
++
++	*prop = fdt_offset_ptr_w_(fdt, nextoffset);
++	proplen = sizeof(**prop) + FDT_TAGALIGN(len);
++
++	err = fdt_splice_struct_(fdt, *prop, 0, proplen);
++	if (err) {
++		/* Delete the string if we failed to add it */
++		if (!can_assume(NO_ROLLBACK) && allocated)
++			fdt_del_last_string_(fdt, name);
++		return err;
++	}
++
++	(*prop)->tag = cpu_to_fdt32(FDT_PROP);
++	(*prop)->nameoff = cpu_to_fdt32(namestroff);
++	(*prop)->len = cpu_to_fdt32(len);
++	return 0;
++}
++
++int fdt_set_name(void *fdt, int nodeoffset, const char *name)
++{
++	char *namep;
++	int oldlen, newlen;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
++	if (!namep)
++		return oldlen;
++
++	newlen = strlen(name);
++
++	err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
++				 FDT_TAGALIGN(newlen+1));
++	if (err)
++		return err;
++
++	memcpy(namep, name, newlen+1);
++	return 0;
++}
++
++int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
++			    int len, void **prop_data)
++{
++	struct fdt_property *prop;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
++	if (err == -FDT_ERR_NOTFOUND)
++		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
++	if (err)
++		return err;
++
++	*prop_data = prop->data;
++	return 0;
++}
++
++int fdt_setprop(void *fdt, int nodeoffset, const char *name,
++		const void *val, int len)
++{
++	void *prop_data;
++	int err;
++
++	err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
++	if (err)
++		return err;
++
++	if (len)
++		memcpy(prop_data, val, len);
++	return 0;
++}
++
++int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
++		   const void *val, int len)
++{
++	struct fdt_property *prop;
++	int err, oldlen, newlen;
++
++	FDT_RW_PROBE(fdt);
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
++	if (prop) {
++		newlen = len + oldlen;
++		err = fdt_splice_struct_(fdt, prop->data,
++					 FDT_TAGALIGN(oldlen),
++					 FDT_TAGALIGN(newlen));
++		if (err)
++			return err;
++		prop->len = cpu_to_fdt32(newlen);
++		memcpy(prop->data + oldlen, val, len);
++	} else {
++		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
++		if (err)
++			return err;
++		memcpy(prop->data, val, len);
++	}
++	return 0;
++}
++
++int fdt_delprop(void *fdt, int nodeoffset, const char *name)
++{
++	struct fdt_property *prop;
++	int len, proplen;
++
++	FDT_RW_PROBE(fdt);
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
++	if (!prop)
++		return len;
++
++	proplen = sizeof(*prop) + FDT_TAGALIGN(len);
++	return fdt_splice_struct_(fdt, prop, proplen, 0);
++}
++
++int fdt_add_subnode_namelen(void *fdt, int parentoffset,
++			    const char *name, int namelen)
++{
++	struct fdt_node_header *nh;
++	int offset, nextoffset;
++	int nodelen;
++	int err;
++	uint32_t tag;
++	fdt32_t *endtag;
++
++	FDT_RW_PROBE(fdt);
++
++	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
++	if (offset >= 0)
++		return -FDT_ERR_EXISTS;
++	else if (offset != -FDT_ERR_NOTFOUND)
++		return offset;
++
++	/* Try to place the new node after the parent's properties */
++	tag = fdt_next_tag(fdt, parentoffset, &nextoffset);
++	/* the fdt_subnode_offset_namelen() should ensure this never hits */
++	if (!can_assume(LIBFDT_FLAWLESS) && (tag != FDT_BEGIN_NODE))
++		return -FDT_ERR_INTERNAL;
++	do {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++	} while ((tag == FDT_PROP) || (tag == FDT_NOP));
++
++	nh = fdt_offset_ptr_w_(fdt, offset);
++	nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
++
++	err = fdt_splice_struct_(fdt, nh, 0, nodelen);
++	if (err)
++		return err;
++
++	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
++	memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
++	memcpy(nh->name, name, namelen);
++	endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
++	*endtag = cpu_to_fdt32(FDT_END_NODE);
++
++	return offset;
++}
++
++int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
++{
++	return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
++}
++
++int fdt_del_node(void *fdt, int nodeoffset)
++{
++	int endoffset;
++
++	FDT_RW_PROBE(fdt);
++
++	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
++	if (endoffset < 0)
++		return endoffset;
++
++	return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
++				  endoffset - nodeoffset, 0);
++}
++
++static void fdt_packblocks_(const char *old, char *new,
++			    int mem_rsv_size,
++			    int struct_size,
++			    int strings_size)
++{
++	int mem_rsv_off, struct_off, strings_off;
++
++	mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
++	struct_off = mem_rsv_off + mem_rsv_size;
++	strings_off = struct_off + struct_size;
++
++	memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
++	fdt_set_off_mem_rsvmap(new, mem_rsv_off);
++
++	memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
++	fdt_set_off_dt_struct(new, struct_off);
++	fdt_set_size_dt_struct(new, struct_size);
++
++	memmove(new + strings_off, old + fdt_off_dt_strings(old), strings_size);
++	fdt_set_off_dt_strings(new, strings_off);
++	fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
++}
++
++int fdt_open_into(const void *fdt, void *buf, int bufsize)
++{
++	int err;
++	int mem_rsv_size, struct_size;
++	int newsize;
++	const char *fdtstart = fdt;
++	const char *fdtend = fdtstart + fdt_totalsize(fdt);
++	char *tmp;
++
++	FDT_RO_PROBE(fdt);
++
++	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
++		* sizeof(struct fdt_reserve_entry);
++
++	if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
++		struct_size = fdt_size_dt_struct(fdt);
++	} else if (fdt_version(fdt) == 16) {
++		struct_size = 0;
++		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
++			;
++		if (struct_size < 0)
++			return struct_size;
++	} else {
++		return -FDT_ERR_BADVERSION;
++	}
++
++	if (can_assume(LIBFDT_ORDER) ||
++	    !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
++		/* no further work necessary */
++		err = fdt_move(fdt, buf, bufsize);
++		if (err)
++			return err;
++		fdt_set_version(buf, 17);
++		fdt_set_size_dt_struct(buf, struct_size);
++		fdt_set_totalsize(buf, bufsize);
++		return 0;
++	}
++
++	/* Need to reorder */
++	newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
++		+ struct_size + fdt_size_dt_strings(fdt);
++
++	if (bufsize < newsize)
++		return -FDT_ERR_NOSPACE;
++
++	/* First attempt to build converted tree at beginning of buffer */
++	tmp = buf;
++	/* But if that overlaps with the old tree... */
++	if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
++		/* Try right after the old tree instead */
++		tmp = (char *)(uintptr_t)fdtend;
++		if ((tmp + newsize) > ((char *)buf + bufsize))
++			return -FDT_ERR_NOSPACE;
++	}
++
++	fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size,
++			fdt_size_dt_strings(fdt));
++	memmove(buf, tmp, newsize);
++
++	fdt_set_magic(buf, FDT_MAGIC);
++	fdt_set_totalsize(buf, bufsize);
++	fdt_set_version(buf, 17);
++	fdt_set_last_comp_version(buf, 16);
++	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
++
++	return 0;
++}
++
++int fdt_pack(void *fdt)
++{
++	int mem_rsv_size;
++
++	FDT_RW_PROBE(fdt);
++
++	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
++		* sizeof(struct fdt_reserve_entry);
++	fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt),
++			fdt_size_dt_strings(fdt));
++	fdt_set_totalsize(fdt, fdt_data_size_(fdt));
++
++	return 0;
++}
+diff --git a/common/libfdt/fdt_strerror.c b/common/libfdt/fdt_strerror.c
+new file mode 100644
+index 0000000..b435693
+--- /dev/null
++++ b/common/libfdt/fdt_strerror.c
+@@ -0,0 +1,59 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++struct fdt_errtabent {
++	const char *str;
++};
++
++#define FDT_ERRTABENT(val) \
++	[(val)] = { .str = #val, }
++
++static struct fdt_errtabent fdt_errtable[] = {
++	FDT_ERRTABENT(FDT_ERR_NOTFOUND),
++	FDT_ERRTABENT(FDT_ERR_EXISTS),
++	FDT_ERRTABENT(FDT_ERR_NOSPACE),
++
++	FDT_ERRTABENT(FDT_ERR_BADOFFSET),
++	FDT_ERRTABENT(FDT_ERR_BADPATH),
++	FDT_ERRTABENT(FDT_ERR_BADPHANDLE),
++	FDT_ERRTABENT(FDT_ERR_BADSTATE),
++
++	FDT_ERRTABENT(FDT_ERR_TRUNCATED),
++	FDT_ERRTABENT(FDT_ERR_BADMAGIC),
++	FDT_ERRTABENT(FDT_ERR_BADVERSION),
++	FDT_ERRTABENT(FDT_ERR_BADSTRUCTURE),
++	FDT_ERRTABENT(FDT_ERR_BADLAYOUT),
++	FDT_ERRTABENT(FDT_ERR_INTERNAL),
++	FDT_ERRTABENT(FDT_ERR_BADNCELLS),
++	FDT_ERRTABENT(FDT_ERR_BADVALUE),
++	FDT_ERRTABENT(FDT_ERR_BADOVERLAY),
++	FDT_ERRTABENT(FDT_ERR_NOPHANDLES),
++	FDT_ERRTABENT(FDT_ERR_BADFLAGS),
++};
++#define FDT_ERRTABSIZE	((int)(sizeof(fdt_errtable) / sizeof(fdt_errtable[0])))
++
++const char *fdt_strerror(int errval)
++{
++	if (errval > 0)
++		return "<valid offset/length>";
++	else if (errval == 0)
++		return "<no error>";
++	else if (-errval < FDT_ERRTABSIZE) {
++		const char *s = fdt_errtable[-errval].str;
++
++		if (s)
++			return s;
++	}
++
++	return "<unknown error>";
++}
+diff --git a/common/libfdt/fdt_sw.c b/common/libfdt/fdt_sw.c
+new file mode 100644
+index 0000000..4c569ee
+--- /dev/null
++++ b/common/libfdt/fdt_sw.c
+@@ -0,0 +1,384 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_sw_probe_(void *fdt)
++{
++	if (!can_assume(VALID_INPUT)) {
++		if (fdt_magic(fdt) == FDT_MAGIC)
++			return -FDT_ERR_BADSTATE;
++		else if (fdt_magic(fdt) != FDT_SW_MAGIC)
++			return -FDT_ERR_BADMAGIC;
++	}
++
++	return 0;
++}
++
++#define FDT_SW_PROBE(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_(fdt)) != 0) \
++			return err; \
++	}
++
++/* 'memrsv' state:	Initial state after fdt_create()
++ *
++ * Allowed functions:
++ *	fdt_add_reservemap_entry()
++ *	fdt_finish_reservemap()		[moves to 'struct' state]
++ */
++static int fdt_sw_probe_memrsv_(void *fdt)
++{
++	int err = fdt_sw_probe_(fdt);
++	if (err)
++		return err;
++
++	if (!can_assume(VALID_INPUT) && fdt_off_dt_strings(fdt) != 0)
++		return -FDT_ERR_BADSTATE;
++	return 0;
++}
++
++#define FDT_SW_PROBE_MEMRSV(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
++			return err; \
++	}
++
++/* 'struct' state:	Enter this state after fdt_finish_reservemap()
++ *
++ * Allowed functions:
++ *	fdt_begin_node()
++ *	fdt_end_node()
++ *	fdt_property*()
++ *	fdt_finish()			[moves to 'complete' state]
++ */
++static int fdt_sw_probe_struct_(void *fdt)
++{
++	int err = fdt_sw_probe_(fdt);
++	if (err)
++		return err;
++
++	if (!can_assume(VALID_INPUT) &&
++	    fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
++		return -FDT_ERR_BADSTATE;
++	return 0;
++}
++
++#define FDT_SW_PROBE_STRUCT(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
++			return err; \
++	}
++
++static inline uint32_t sw_flags(void *fdt)
++{
++	/* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
++	return fdt_last_comp_version(fdt);
++}
++
++/* 'complete' state:	Enter this state after fdt_finish()
++ *
++ * Allowed functions: none
++ */
++
++static void *fdt_grab_space_(void *fdt, size_t len)
++{
++	unsigned int offset = fdt_size_dt_struct(fdt);
++	unsigned int spaceleft;
++
++	spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
++		- fdt_size_dt_strings(fdt);
++
++	if ((offset + len < offset) || (offset + len > spaceleft))
++		return NULL;
++
++	fdt_set_size_dt_struct(fdt, offset + len);
++	return fdt_offset_ptr_w_(fdt, offset);
++}
++
++int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
++{
++	const int hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
++				      sizeof(struct fdt_reserve_entry));
++	void *fdt = buf;
++
++	if (bufsize < hdrsize)
++		return -FDT_ERR_NOSPACE;
++
++	if (flags & ~FDT_CREATE_FLAGS_ALL)
++		return -FDT_ERR_BADFLAGS;
++
++	memset(buf, 0, bufsize);
++
++	/*
++	 * magic and last_comp_version keep intermediate state during the fdt
++	 * creation process, which is replaced with the proper FDT format by
++	 * fdt_finish().
++	 *
++	 * flags should be accessed with sw_flags().
++	 */
++	fdt_set_magic(fdt, FDT_SW_MAGIC);
++	fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
++	fdt_set_last_comp_version(fdt, flags);
++
++	fdt_set_totalsize(fdt,  bufsize);
++
++	fdt_set_off_mem_rsvmap(fdt, hdrsize);
++	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
++	fdt_set_off_dt_strings(fdt, 0);
++
++	return 0;
++}
++
++int fdt_create(void *buf, int bufsize)
++{
++	return fdt_create_with_flags(buf, bufsize, 0);
++}
++
++int fdt_resize(void *fdt, void *buf, int bufsize)
++{
++	size_t headsize, tailsize;
++	char *oldtail, *newtail;
++
++	FDT_SW_PROBE(fdt);
++
++	if (bufsize < 0)
++		return -FDT_ERR_NOSPACE;
++
++	headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	tailsize = fdt_size_dt_strings(fdt);
++
++	if (!can_assume(VALID_DTB) &&
++	    headsize + tailsize > fdt_totalsize(fdt))
++		return -FDT_ERR_INTERNAL;
++
++	if ((headsize + tailsize) > (unsigned)bufsize)
++		return -FDT_ERR_NOSPACE;
++
++	oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
++	newtail = (char *)buf + bufsize - tailsize;
++
++	/* Two cases to avoid clobbering data if the old and new
++	 * buffers partially overlap */
++	if (buf <= fdt) {
++		memmove(buf, fdt, headsize);
++		memmove(newtail, oldtail, tailsize);
++	} else {
++		memmove(newtail, oldtail, tailsize);
++		memmove(buf, fdt, headsize);
++	}
++
++	fdt_set_totalsize(buf, bufsize);
++	if (fdt_off_dt_strings(buf))
++		fdt_set_off_dt_strings(buf, bufsize);
++
++	return 0;
++}
++
++int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
++{
++	struct fdt_reserve_entry *re;
++	int offset;
++
++	FDT_SW_PROBE_MEMRSV(fdt);
++
++	offset = fdt_off_dt_struct(fdt);
++	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
++		return -FDT_ERR_NOSPACE;
++
++	re = (struct fdt_reserve_entry *)((char *)fdt + offset);
++	re->address = cpu_to_fdt64(addr);
++	re->size = cpu_to_fdt64(size);
++
++	fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
++
++	return 0;
++}
++
++int fdt_finish_reservemap(void *fdt)
++{
++	int err = fdt_add_reservemap_entry(fdt, 0, 0);
++
++	if (err)
++		return err;
++
++	fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
++	return 0;
++}
++
++int fdt_begin_node(void *fdt, const char *name)
++{
++	struct fdt_node_header *nh;
++	int namelen;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	namelen = strlen(name) + 1;
++	nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
++	if (! nh)
++		return -FDT_ERR_NOSPACE;
++
++	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
++	memcpy(nh->name, name, namelen);
++	return 0;
++}
++
++int fdt_end_node(void *fdt)
++{
++	fdt32_t *en;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	en = fdt_grab_space_(fdt, FDT_TAGSIZE);
++	if (! en)
++		return -FDT_ERR_NOSPACE;
++
++	*en = cpu_to_fdt32(FDT_END_NODE);
++	return 0;
++}
++
++static int fdt_add_string_(void *fdt, const char *s)
++{
++	char *strtab = (char *)fdt + fdt_totalsize(fdt);
++	unsigned int strtabsize = fdt_size_dt_strings(fdt);
++	unsigned int len = strlen(s) + 1;
++	unsigned int struct_top, offset;
++
++	offset = strtabsize + len;
++	struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	if (fdt_totalsize(fdt) - offset < struct_top)
++		return 0; /* no more room :( */
++
++	memcpy(strtab - offset, s, len);
++	fdt_set_size_dt_strings(fdt, strtabsize + len);
++	return -offset;
++}
++
++/* Must only be used to roll back in case of error */
++static void fdt_del_last_string_(void *fdt, const char *s)
++{
++	int strtabsize = fdt_size_dt_strings(fdt);
++	int len = strlen(s) + 1;
++
++	fdt_set_size_dt_strings(fdt, strtabsize - len);
++}
++
++static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
++{
++	char *strtab = (char *)fdt + fdt_totalsize(fdt);
++	int strtabsize = fdt_size_dt_strings(fdt);
++	const char *p;
++
++	*allocated = 0;
++
++	p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
++	if (p)
++		return p - strtab;
++
++	*allocated = 1;
++
++	return fdt_add_string_(fdt, s);
++}
++
++int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
++{
++	struct fdt_property *prop;
++	int nameoff;
++	int allocated;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	/* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
++	if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
++		allocated = 1;
++		nameoff = fdt_add_string_(fdt, name);
++	} else {
++		nameoff = fdt_find_add_string_(fdt, name, &allocated);
++	}
++	if (nameoff == 0)
++		return -FDT_ERR_NOSPACE;
++
++	prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
++	if (! prop) {
++		if (allocated)
++			fdt_del_last_string_(fdt, name);
++		return -FDT_ERR_NOSPACE;
++	}
++
++	prop->tag = cpu_to_fdt32(FDT_PROP);
++	prop->nameoff = cpu_to_fdt32(nameoff);
++	prop->len = cpu_to_fdt32(len);
++	*valp = prop->data;
++	return 0;
++}
++
++int fdt_property(void *fdt, const char *name, const void *val, int len)
++{
++	void *ptr;
++	int ret;
++
++	ret = fdt_property_placeholder(fdt, name, len, &ptr);
++	if (ret)
++		return ret;
++	memcpy(ptr, val, len);
++	return 0;
++}
++
++int fdt_finish(void *fdt)
++{
++	char *p = (char *)fdt;
++	fdt32_t *end;
++	int oldstroffset, newstroffset;
++	uint32_t tag;
++	int offset, nextoffset;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	/* Add terminator */
++	end = fdt_grab_space_(fdt, sizeof(*end));
++	if (! end)
++		return -FDT_ERR_NOSPACE;
++	*end = cpu_to_fdt32(FDT_END);
++
++	/* Relocate the string table */
++	oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
++	newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
++	fdt_set_off_dt_strings(fdt, newstroffset);
++
++	/* Walk the structure, correcting string offsets */
++	offset = 0;
++	while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
++		if (tag == FDT_PROP) {
++			struct fdt_property *prop =
++				fdt_offset_ptr_w_(fdt, offset);
++			int nameoff;
++
++			nameoff = fdt32_to_cpu(prop->nameoff);
++			nameoff += fdt_size_dt_strings(fdt);
++			prop->nameoff = cpu_to_fdt32(nameoff);
++		}
++		offset = nextoffset;
++	}
++	if (nextoffset < 0)
++		return nextoffset;
++
++	/* Finally, adjust the header */
++	fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
++
++	/* And fix up fields that were keeping intermediate state. */
++	fdt_set_last_comp_version(fdt, FDT_LAST_COMPATIBLE_VERSION);
++	fdt_set_magic(fdt, FDT_MAGIC);
++
++	return 0;
++}
+diff --git a/common/libfdt/fdt_wip.c b/common/libfdt/fdt_wip.c
+new file mode 100644
+index 0000000..c2d7566
+--- /dev/null
++++ b/common/libfdt/fdt_wip.c
+@@ -0,0 +1,94 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
++					const char *name, int namelen,
++					uint32_t idx, const void *val,
++					int len)
++{
++	void *propval;
++	int proplen;
++
++	propval = fdt_getprop_namelen_w(fdt, nodeoffset, name, namelen,
++					&proplen);
++	if (!propval)
++		return proplen;
++
++	if ((unsigned)proplen < (len + idx))
++		return -FDT_ERR_NOSPACE;
++
++	memcpy((char *)propval + idx, val, len);
++	return 0;
++}
++
++int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
++			const void *val, int len)
++{
++	const void *propval;
++	int proplen;
++
++	propval = fdt_getprop(fdt, nodeoffset, name, &proplen);
++	if (!propval)
++		return proplen;
++
++	if (proplen != len)
++		return -FDT_ERR_NOSPACE;
++
++	return fdt_setprop_inplace_namelen_partial(fdt, nodeoffset, name,
++						   strlen(name), 0,
++						   val, len);
++}
++
++static void fdt_nop_region_(void *start, int len)
++{
++	fdt32_t *p;
++
++	for (p = start; (char *)p < ((char *)start + len); p++)
++		*p = cpu_to_fdt32(FDT_NOP);
++}
++
++int fdt_nop_property(void *fdt, int nodeoffset, const char *name)
++{
++	struct fdt_property *prop;
++	int len;
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
++	if (!prop)
++		return len;
++
++	fdt_nop_region_(prop, len + sizeof(*prop));
++
++	return 0;
++}
++
++int fdt_node_end_offset_(void *fdt, int offset)
++{
++	int depth = 0;
++
++	while ((offset >= 0) && (depth >= 0))
++		offset = fdt_next_node(fdt, offset, &depth);
++
++	return offset;
++}
++
++int fdt_nop_node(void *fdt, int nodeoffset)
++{
++	int endoffset;
++
++	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
++	if (endoffset < 0)
++		return endoffset;
++
++	fdt_nop_region_(fdt_offset_ptr_w(fdt, nodeoffset, 0),
++			endoffset - nodeoffset);
++	return 0;
++}
+diff --git a/common/libfdt/libfdt_internal.h b/common/libfdt/libfdt_internal.h
+new file mode 100644
+index 0000000..16bda19
+--- /dev/null
++++ b/common/libfdt/libfdt_internal.h
+@@ -0,0 +1,192 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_INTERNAL_H
++#define LIBFDT_INTERNAL_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include <fdt.h>
++
++#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
++#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
++
++int32_t fdt_ro_probe_(const void *fdt);
++#define FDT_RO_PROBE(fdt)					\
++	{							\
++		int32_t totalsize_;				\
++		if ((totalsize_ = fdt_ro_probe_(fdt)) < 0)	\
++			return totalsize_;			\
++	}
++
++int fdt_check_node_offset_(const void *fdt, int offset);
++int fdt_check_prop_offset_(const void *fdt, int offset);
++const char *fdt_find_string_(const char *strtab, int tabsize, const char *s);
++int fdt_node_end_offset_(void *fdt, int nodeoffset);
++
++static inline const void *fdt_offset_ptr_(const void *fdt, int offset)
++{
++	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
++}
++
++static inline void *fdt_offset_ptr_w_(void *fdt, int offset)
++{
++	return (void *)(uintptr_t)fdt_offset_ptr_(fdt, offset);
++}
++
++static inline const struct fdt_reserve_entry *fdt_mem_rsv_(const void *fdt, int n)
++{
++	const struct fdt_reserve_entry *rsv_table =
++		(const struct fdt_reserve_entry *)
++		((const char *)fdt + fdt_off_mem_rsvmap(fdt));
++
++	return rsv_table + n;
++}
++static inline struct fdt_reserve_entry *fdt_mem_rsv_w_(void *fdt, int n)
++{
++	return (void *)(uintptr_t)fdt_mem_rsv_(fdt, n);
++}
++
++/*
++ * Internal helpers to access tructural elements of the device tree
++ * blob (rather than for exaple reading integers from within property
++ * values).  We assume that we are either given a naturally aligned
++ * address for the platform or if we are not, we are on a platform
++ * where unaligned memory reads will be handled in a graceful manner.
++ * If not the external helpers fdtXX_ld() from libfdt.h can be used
++ * instead.
++ */
++static inline uint32_t fdt32_ld_(const fdt32_t *p)
++{
++	return fdt32_to_cpu(*p);
++}
++
++static inline uint64_t fdt64_ld_(const fdt64_t *p)
++{
++	return fdt64_to_cpu(*p);
++}
++
++#define FDT_SW_MAGIC		(~FDT_MAGIC)
++
++/**********************************************************************/
++/* Checking controls                                                  */
++/**********************************************************************/
++
++#ifndef FDT_ASSUME_MASK
++#define FDT_ASSUME_MASK 0
++#endif
++
++/*
++ * Defines assumptions which can be enabled. Each of these can be enabled
++ * individually. For maximum safety, don't enable any assumptions!
++ *
++ * For minimal code size and no safety, use ASSUME_PERFECT at your own risk.
++ * You should have another method of validating the device tree, such as a
++ * signature or hash check before using libfdt.
++ *
++ * For situations where security is not a concern it may be safe to enable
++ * ASSUME_SANE.
++ */
++enum {
++	/*
++	 * This does essentially no checks. Only the latest device-tree
++	 * version is correctly handled. Inconsistencies or errors in the device
++	 * tree may cause undefined behaviour or crashes. Invalid parameters
++	 * passed to libfdt may do the same.
++	 *
++	 * If an error occurs when modifying the tree it may leave the tree in
++	 * an intermediate (but valid) state. As an example, adding a property
++	 * where there is insufficient space may result in the property name
++	 * being added to the string table even though the property itself is
++	 * not added to the struct section.
++	 *
++	 * Only use this if you have a fully validated device tree with
++	 * the latest supported version and wish to minimise code size.
++	 */
++	ASSUME_PERFECT		= 0xff,
++
++	/*
++	 * This assumes that the device tree is sane. i.e. header metadata
++	 * and basic hierarchy are correct.
++	 *
++	 * With this assumption enabled, normal device trees produced by libfdt
++	 * and the compiler should be handled safely. Malicious device trees and
++	 * complete garbage may cause libfdt to behave badly or crash. Truncated
++	 * device trees (e.g. those only partially loaded) can also cause
++	 * problems.
++	 *
++	 * Note: Only checks that relate exclusively to the device tree itself
++	 * (not the parameters passed to libfdt) are disabled by this
++	 * assumption. This includes checking headers, tags and the like.
++	 */
++	ASSUME_VALID_DTB	= 1 << 0,
++
++	/*
++	 * This builds on ASSUME_VALID_DTB and further assumes that libfdt
++	 * functions are called with valid parameters, i.e. not trigger
++	 * FDT_ERR_BADOFFSET or offsets that are out of bounds. It disables any
++	 * extensive checking of parameters and the device tree, making various
++	 * assumptions about correctness.
++	 *
++	 * It doesn't make sense to enable this assumption unless
++	 * ASSUME_VALID_DTB is also enabled.
++	 */
++	ASSUME_VALID_INPUT	= 1 << 1,
++
++	/*
++	 * This disables checks for device-tree version and removes all code
++	 * which handles older versions.
++	 *
++	 * Only enable this if you know you have a device tree with the latest
++	 * version.
++	 */
++	ASSUME_LATEST		= 1 << 2,
++
++	/*
++	 * This assumes that it is OK for a failed addition to the device tree,
++	 * due to lack of space or some other problem, to skip any rollback
++	 * steps (such as dropping the property name from the string table).
++	 * This is safe to enable in most circumstances, even though it may
++	 * leave the tree in a sub-optimal state.
++	 */
++	ASSUME_NO_ROLLBACK	= 1 << 3,
++
++	/*
++	 * This assumes that the device tree components appear in a 'convenient'
++	 * order, i.e. the memory reservation block first, then the structure
++	 * block and finally the string block.
++	 *
++	 * This order is not specified by the device-tree specification,
++	 * but is expected by libfdt. The device-tree compiler always created
++	 * device trees with this order.
++	 *
++	 * This assumption disables a check in fdt_open_into() and removes the
++	 * ability to fix the problem there. This is safe if you know that the
++	 * device tree is correctly ordered. See fdt_blocks_misordered_().
++	 */
++	ASSUME_LIBFDT_ORDER	= 1 << 4,
++
++	/*
++	 * This assumes that libfdt itself does not have any internal bugs. It
++	 * drops certain checks that should never be needed unless libfdt has an
++	 * undiscovered bug.
++	 *
++	 * This can generally be considered safe to enable.
++	 */
++	ASSUME_LIBFDT_FLAWLESS	= 1 << 5,
++};
++
++/**
++ * can_assume_() - check if a particular assumption is enabled
++ *
++ * @mask: Mask to check (ASSUME_...)
++ * @return true if that assumption is enabled, else false
++ */
++static inline bool can_assume_(int mask)
++{
++	return FDT_ASSUME_MASK & mask;
++}
++
++/** helper macros for checking assumptions */
++#define can_assume(_assume)	can_assume_(ASSUME_ ## _assume)
++
++#endif /* LIBFDT_INTERNAL_H */
+diff --git a/include/fdt.h b/include/fdt.h
+new file mode 100644
+index 0000000..f2e6880
+--- /dev/null
++++ b/include/fdt.h
+@@ -0,0 +1,66 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef FDT_H
++#define FDT_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
++ */
++
++#ifndef __ASSEMBLY__
++
++struct fdt_header {
++	fdt32_t magic;			 /* magic word FDT_MAGIC */
++	fdt32_t totalsize;		 /* total size of DT block */
++	fdt32_t off_dt_struct;		 /* offset to structure */
++	fdt32_t off_dt_strings;		 /* offset to strings */
++	fdt32_t off_mem_rsvmap;		 /* offset to memory reserve map */
++	fdt32_t version;		 /* format version */
++	fdt32_t last_comp_version;	 /* last compatible version */
++
++	/* version 2 fields below */
++	fdt32_t boot_cpuid_phys;	 /* Which physical CPU id we're
++					    booting on */
++	/* version 3 fields below */
++	fdt32_t size_dt_strings;	 /* size of the strings block */
++
++	/* version 17 fields below */
++	fdt32_t size_dt_struct;		 /* size of the structure block */
++};
++
++struct fdt_reserve_entry {
++	fdt64_t address;
++	fdt64_t size;
++};
++
++struct fdt_node_header {
++	fdt32_t tag;
++	char name[0];
++};
++
++struct fdt_property {
++	fdt32_t tag;
++	fdt32_t len;
++	fdt32_t nameoff;
++	char data[0];
++};
++
++#endif /* !__ASSEMBLY */
++
++#define FDT_MAGIC	0xd00dfeed	/* 4: version, 4: total size */
++#define FDT_TAGSIZE	sizeof(fdt32_t)
++
++#define FDT_BEGIN_NODE	0x1		/* Start node: full name */
++#define FDT_END_NODE	0x2		/* End node */
++#define FDT_PROP	0x3		/* Property: name off,
++					   size, content */
++#define FDT_NOP		0x4		/* nop */
++#define FDT_END		0x9
++
++#define FDT_V1_SIZE	(7*sizeof(fdt32_t))
++#define FDT_V2_SIZE	(FDT_V1_SIZE + sizeof(fdt32_t))
++#define FDT_V3_SIZE	(FDT_V2_SIZE + sizeof(fdt32_t))
++#define FDT_V16_SIZE	FDT_V3_SIZE
++#define FDT_V17_SIZE	(FDT_V16_SIZE + sizeof(fdt32_t))
++
++#endif /* FDT_H */
+diff --git a/include/libfdt.h b/include/libfdt.h
+new file mode 100644
+index 0000000..a7f432c
+--- /dev/null
++++ b/include/libfdt.h
+@@ -0,0 +1,2147 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_H
++#define LIBFDT_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++
++#include <libfdt_env.h>
++#include <fdt.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++#define FDT_FIRST_SUPPORTED_VERSION	0x02
++#define FDT_LAST_COMPATIBLE_VERSION 0x10
++#define FDT_LAST_SUPPORTED_VERSION	0x11
++
++/* Error codes: informative error codes */
++#define FDT_ERR_NOTFOUND	1
++	/* FDT_ERR_NOTFOUND: The requested node or property does not exist */
++#define FDT_ERR_EXISTS		2
++	/* FDT_ERR_EXISTS: Attempted to create a node or property which
++	 * already exists */
++#define FDT_ERR_NOSPACE		3
++	/* FDT_ERR_NOSPACE: Operation needed to expand the device
++	 * tree, but its buffer did not have sufficient space to
++	 * contain the expanded tree. Use fdt_open_into() to move the
++	 * device tree to a buffer with more space. */
++
++/* Error codes: codes for bad parameters */
++#define FDT_ERR_BADOFFSET	4
++	/* FDT_ERR_BADOFFSET: Function was passed a structure block
++	 * offset which is out-of-bounds, or which points to an
++	 * unsuitable part of the structure for the operation. */
++#define FDT_ERR_BADPATH		5
++	/* FDT_ERR_BADPATH: Function was passed a badly formatted path
++	 * (e.g. missing a leading / for a function which requires an
++	 * absolute path) */
++#define FDT_ERR_BADPHANDLE	6
++	/* FDT_ERR_BADPHANDLE: Function was passed an invalid phandle.
++	 * This can be caused either by an invalid phandle property
++	 * length, or the phandle value was either 0 or -1, which are
++	 * not permitted. */
++#define FDT_ERR_BADSTATE	7
++	/* FDT_ERR_BADSTATE: Function was passed an incomplete device
++	 * tree created by the sequential-write functions, which is
++	 * not sufficiently complete for the requested operation. */
++
++/* Error codes: codes for bad device tree blobs */
++#define FDT_ERR_TRUNCATED	8
++	/* FDT_ERR_TRUNCATED: FDT or a sub-block is improperly
++	 * terminated (overflows, goes outside allowed bounds, or
++	 * isn't properly terminated).  */
++#define FDT_ERR_BADMAGIC	9
++	/* FDT_ERR_BADMAGIC: Given "device tree" appears not to be a
++	 * device tree at all - it is missing the flattened device
++	 * tree magic number. */
++#define FDT_ERR_BADVERSION	10
++	/* FDT_ERR_BADVERSION: Given device tree has a version which
++	 * can't be handled by the requested operation.  For
++	 * read-write functions, this may mean that fdt_open_into() is
++	 * required to convert the tree to the expected version. */
++#define FDT_ERR_BADSTRUCTURE	11
++	/* FDT_ERR_BADSTRUCTURE: Given device tree has a corrupt
++	 * structure block or other serious error (e.g. misnested
++	 * nodes, or subnodes preceding properties). */
++#define FDT_ERR_BADLAYOUT	12
++	/* FDT_ERR_BADLAYOUT: For read-write functions, the given
++	 * device tree has it's sub-blocks in an order that the
++	 * function can't handle (memory reserve map, then structure,
++	 * then strings).  Use fdt_open_into() to reorganize the tree
++	 * into a form suitable for the read-write operations. */
++
++/* "Can't happen" error indicating a bug in libfdt */
++#define FDT_ERR_INTERNAL	13
++	/* FDT_ERR_INTERNAL: libfdt has failed an internal assertion.
++	 * Should never be returned, if it is, it indicates a bug in
++	 * libfdt itself. */
++
++/* Errors in device tree content */
++#define FDT_ERR_BADNCELLS	14
++	/* FDT_ERR_BADNCELLS: Device tree has a #address-cells, #size-cells
++	 * or similar property with a bad format or value */
++
++#define FDT_ERR_BADVALUE	15
++	/* FDT_ERR_BADVALUE: Device tree has a property with an unexpected
++	 * value. For example: a property expected to contain a string list
++	 * is not NUL-terminated within the length of its value. */
++
++#define FDT_ERR_BADOVERLAY	16
++	/* FDT_ERR_BADOVERLAY: The device tree overlay, while
++	 * correctly structured, cannot be applied due to some
++	 * unexpected or missing value, property or node. */
++
++#define FDT_ERR_NOPHANDLES	17
++	/* FDT_ERR_NOPHANDLES: The device tree doesn't have any
++	 * phandle available anymore without causing an overflow */
++
++#define FDT_ERR_BADFLAGS	18
++	/* FDT_ERR_BADFLAGS: The function was passed a flags field that
++	 * contains invalid flags or an invalid combination of flags. */
++
++#define FDT_ERR_ALIGNMENT	19
++	/* FDT_ERR_ALIGNMENT: The device tree base address is not 8-byte
++	 * aligned. */
++
++#define FDT_ERR_MAX		19
++
++/* constants */
++#define FDT_MAX_PHANDLE 0xfffffffe
++	/* Valid values for phandles range from 1 to 2^32-2. */
++
++/**********************************************************************/
++/* Low-level functions (you probably don't need these)                */
++/**********************************************************************/
++
++#ifndef SWIG /* This function is not useful in Python */
++const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int checklen);
++#endif
++static inline void *fdt_offset_ptr_w(void *fdt, int offset, int checklen)
++{
++	return (void *)(uintptr_t)fdt_offset_ptr(fdt, offset, checklen);
++}
++
++uint32_t fdt_next_tag(const void *fdt, int offset, int *nextoffset);
++
++/*
++ * External helpers to access words from a device tree blob. They're built
++ * to work even with unaligned pointers on platforms (such as ARMv5) that don't
++ * like unaligned loads and stores.
++ */
++static inline uint16_t fdt16_ld(const fdt16_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint16_t)bp[0] << 8) | bp[1];
++}
++
++static inline uint32_t fdt32_ld(const fdt32_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint32_t)bp[0] << 24)
++		| ((uint32_t)bp[1] << 16)
++		| ((uint32_t)bp[2] << 8)
++		| bp[3];
++}
++
++static inline void fdt32_st(void *property, uint32_t value)
++{
++	uint8_t *bp = (uint8_t *)property;
++
++	bp[0] = value >> 24;
++	bp[1] = (value >> 16) & 0xff;
++	bp[2] = (value >> 8) & 0xff;
++	bp[3] = value & 0xff;
++}
++
++static inline uint64_t fdt64_ld(const fdt64_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint64_t)bp[0] << 56)
++		| ((uint64_t)bp[1] << 48)
++		| ((uint64_t)bp[2] << 40)
++		| ((uint64_t)bp[3] << 32)
++		| ((uint64_t)bp[4] << 24)
++		| ((uint64_t)bp[5] << 16)
++		| ((uint64_t)bp[6] << 8)
++		| bp[7];
++}
++
++static inline void fdt64_st(void *property, uint64_t value)
++{
++	uint8_t *bp = (uint8_t *)property;
++
++	bp[0] = value >> 56;
++	bp[1] = (value >> 48) & 0xff;
++	bp[2] = (value >> 40) & 0xff;
++	bp[3] = (value >> 32) & 0xff;
++	bp[4] = (value >> 24) & 0xff;
++	bp[5] = (value >> 16) & 0xff;
++	bp[6] = (value >> 8) & 0xff;
++	bp[7] = value & 0xff;
++}
++
++/**********************************************************************/
++/* Traversal functions                                                */
++/**********************************************************************/
++
++int fdt_next_node(const void *fdt, int offset, int *depth);
++
++/**
++ * fdt_first_subnode() - get offset of first direct subnode
++ * @fdt:	FDT blob
++ * @offset:	Offset of node to check
++ *
++ * Return: offset of first subnode, or -FDT_ERR_NOTFOUND if there is none
++ */
++int fdt_first_subnode(const void *fdt, int offset);
++
++/**
++ * fdt_next_subnode() - get offset of next direct subnode
++ * @fdt:	FDT blob
++ * @offset:	Offset of previous subnode
++ *
++ * After first calling fdt_first_subnode(), call this function repeatedly to
++ * get direct subnodes of a parent node.
++ *
++ * Return: offset of next subnode, or -FDT_ERR_NOTFOUND if there are no more
++ *         subnodes
++ */
++int fdt_next_subnode(const void *fdt, int offset);
++
++/**
++ * fdt_for_each_subnode - iterate over all subnodes of a parent
++ *
++ * @node:	child node (int, lvalue)
++ * @fdt:	FDT blob (const void *)
++ * @parent:	parent node (int)
++ *
++ * This is actually a wrapper around a for loop and would be used like so:
++ *
++ *	fdt_for_each_subnode(node, fdt, parent) {
++ *		Use node
++ *		...
++ *	}
++ *
++ *	if ((node < 0) && (node != -FDT_ERR_NOTFOUND)) {
++ *		Error handling
++ *	}
++ *
++ * Note that this is implemented as a macro and @node is used as
++ * iterator in the loop. The parent variable be constant or even a
++ * literal.
++ */
++#define fdt_for_each_subnode(node, fdt, parent)		\
++	for (node = fdt_first_subnode(fdt, parent);	\
++	     node >= 0;					\
++	     node = fdt_next_subnode(fdt, node))
++
++/**********************************************************************/
++/* General functions                                                  */
++/**********************************************************************/
++#define fdt_get_header(fdt, field) \
++	(fdt32_ld(&((const struct fdt_header *)(fdt))->field))
++#define fdt_magic(fdt)			(fdt_get_header(fdt, magic))
++#define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
++#define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
++#define fdt_off_dt_strings(fdt)		(fdt_get_header(fdt, off_dt_strings))
++#define fdt_off_mem_rsvmap(fdt)		(fdt_get_header(fdt, off_mem_rsvmap))
++#define fdt_version(fdt)		(fdt_get_header(fdt, version))
++#define fdt_last_comp_version(fdt)	(fdt_get_header(fdt, last_comp_version))
++#define fdt_boot_cpuid_phys(fdt)	(fdt_get_header(fdt, boot_cpuid_phys))
++#define fdt_size_dt_strings(fdt)	(fdt_get_header(fdt, size_dt_strings))
++#define fdt_size_dt_struct(fdt)		(fdt_get_header(fdt, size_dt_struct))
++
++#define fdt_set_hdr_(name) \
++	static inline void fdt_set_##name(void *fdt, uint32_t val) \
++	{ \
++		struct fdt_header *fdth = (struct fdt_header *)fdt; \
++		fdth->name = cpu_to_fdt32(val); \
++	}
++fdt_set_hdr_(magic);
++fdt_set_hdr_(totalsize);
++fdt_set_hdr_(off_dt_struct);
++fdt_set_hdr_(off_dt_strings);
++fdt_set_hdr_(off_mem_rsvmap);
++fdt_set_hdr_(version);
++fdt_set_hdr_(last_comp_version);
++fdt_set_hdr_(boot_cpuid_phys);
++fdt_set_hdr_(size_dt_strings);
++fdt_set_hdr_(size_dt_struct);
++#undef fdt_set_hdr_
++
++/**
++ * fdt_header_size - return the size of the tree's header
++ * @fdt: pointer to a flattened device tree
++ *
++ * Return: size of DTB header in bytes
++ */
++size_t fdt_header_size(const void *fdt);
++
++/**
++ * fdt_header_size_ - internal function to get header size from a version number
++ * @version: devicetree version number
++ *
++ * Return: size of DTB header in bytes
++ */
++size_t fdt_header_size_(uint32_t version);
++
++/**
++ * fdt_check_header - sanity check a device tree header
++ * @fdt: pointer to data which might be a flattened device tree
++ *
++ * fdt_check_header() checks that the given buffer contains what
++ * appears to be a flattened device tree, and that the header contains
++ * valid information (to the extent that can be determined from the
++ * header alone).
++ *
++ * returns:
++ *     0, if the buffer appears to contain a valid device tree
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE,
++ *     -FDT_ERR_TRUNCATED, standard meanings, as above
++ */
++int fdt_check_header(const void *fdt);
++
++/**
++ * fdt_move - move a device tree around in memory
++ * @fdt: pointer to the device tree to move
++ * @buf: pointer to memory where the device is to be moved
++ * @bufsize: size of the memory space at buf
++ *
++ * fdt_move() relocates, if possible, the device tree blob located at
++ * fdt to the buffer at buf of size bufsize.  The buffer may overlap
++ * with the existing device tree blob at fdt.  Therefore,
++ *     fdt_move(fdt, fdt, fdt_totalsize(fdt))
++ * should always succeed.
++ *
++ * returns:
++ *     0, on success
++ *     -FDT_ERR_NOSPACE, bufsize is insufficient to contain the device tree
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_move(const void *fdt, void *buf, int bufsize);
++
++/**********************************************************************/
++/* Read-only functions                                                */
++/**********************************************************************/
++
++int fdt_check_full(const void *fdt, size_t bufsize);
++
++/**
++ * fdt_get_string - retrieve a string from the strings block of a device tree
++ * @fdt: pointer to the device tree blob
++ * @stroffset: offset of the string within the strings block (native endian)
++ * @lenp: optional pointer to return the string's length
++ *
++ * fdt_get_string() retrieves a pointer to a single string from the
++ * strings block of the device tree blob at fdt, and optionally also
++ * returns the string's length in *lenp.
++ *
++ * returns:
++ *     a pointer to the string, on success
++ *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
++ */
++const char *fdt_get_string(const void *fdt, int stroffset, int *lenp);
++
++/**
++ * fdt_string - retrieve a string from the strings block of a device tree
++ * @fdt: pointer to the device tree blob
++ * @stroffset: offset of the string within the strings block (native endian)
++ *
++ * fdt_string() retrieves a pointer to a single string from the
++ * strings block of the device tree blob at fdt.
++ *
++ * returns:
++ *     a pointer to the string, on success
++ *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
++ */
++const char *fdt_string(const void *fdt, int stroffset);
++
++/**
++ * fdt_find_max_phandle - find and return the highest phandle in a tree
++ * @fdt: pointer to the device tree blob
++ * @phandle: return location for the highest phandle value found in the tree
++ *
++ * fdt_find_max_phandle() finds the highest phandle value in the given device
++ * tree. The value returned in @phandle is only valid if the function returns
++ * success.
++ *
++ * returns:
++ *     0 on success or a negative error code on failure
++ */
++int fdt_find_max_phandle(const void *fdt, uint32_t *phandle);
++
++/**
++ * fdt_get_max_phandle - retrieves the highest phandle in a tree
++ * @fdt: pointer to the device tree blob
++ *
++ * fdt_get_max_phandle retrieves the highest phandle in the given
++ * device tree. This will ignore badly formatted phandles, or phandles
++ * with a value of 0 or -1.
++ *
++ * This function is deprecated in favour of fdt_find_max_phandle().
++ *
++ * returns:
++ *      the highest phandle on success
++ *      0, if no phandle was found in the device tree
++ *      -1, if an error occurred
++ */
++static inline uint32_t fdt_get_max_phandle(const void *fdt)
++{
++	uint32_t phandle;
++	int err;
++
++	err = fdt_find_max_phandle(fdt, &phandle);
++	if (err < 0)
++		return (uint32_t)-1;
++
++	return phandle;
++}
++
++/**
++ * fdt_generate_phandle - return a new, unused phandle for a device tree blob
++ * @fdt: pointer to the device tree blob
++ * @phandle: return location for the new phandle
++ *
++ * Walks the device tree blob and looks for the highest phandle value. On
++ * success, the new, unused phandle value (one higher than the previously
++ * highest phandle value in the device tree blob) will be returned in the
++ * @phandle parameter.
++ *
++ * Return: 0 on success or a negative error-code on failure
++ */
++int fdt_generate_phandle(const void *fdt, uint32_t *phandle);
++
++/**
++ * fdt_num_mem_rsv - retrieve the number of memory reserve map entries
++ * @fdt: pointer to the device tree blob
++ *
++ * Returns the number of entries in the device tree blob's memory
++ * reservation map.  This does not include the terminating 0,0 entry
++ * or any other (0,0) entries reserved for expansion.
++ *
++ * returns:
++ *     the number of entries
++ */
++int fdt_num_mem_rsv(const void *fdt);
++
++/**
++ * fdt_get_mem_rsv - retrieve one memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @n: index of reserve map entry
++ * @address: pointer to 64-bit variable to hold the start address
++ * @size: pointer to 64-bit variable to hold the size of the entry
++ *
++ * On success, @address and @size will contain the address and size of
++ * the n-th reserve map entry from the device tree blob, in
++ * native-endian format.
++ *
++ * returns:
++ *     0, on success
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size);
++
++/**
++ * fdt_subnode_offset_namelen - find a subnode based on substring
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_subnode_offset(), but only examine the first
++ * namelen characters of name for matching the subnode name.  This is
++ * useful for finding subnodes based on a portion of a larger string,
++ * such as a full path.
++ *
++ * Return: offset of the subnode or -FDT_ERR_NOTFOUND if name not found.
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
++			       const char *name, int namelen);
++#endif
++/**
++ * fdt_subnode_offset - find a subnode of a given node
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ *
++ * fdt_subnode_offset() finds a subnode of the node at structure block
++ * offset parentoffset with the given name.  name may include a unit
++ * address, in which case fdt_subnode_offset() will find the subnode
++ * with that unit address, or the unit address may be omitted, in
++ * which case fdt_subnode_offset() will find an arbitrary subnode
++ * whose name excluding unit address matches the given name.
++ *
++ * returns:
++ *	structure block offset of the requested subnode (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
++ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
++ *		tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
++
++/**
++ * fdt_path_offset_namelen - find a tree node by its full path
++ * @fdt: pointer to the device tree blob
++ * @path: full path of the node to locate
++ * @namelen: number of characters of path to consider
++ *
++ * Identical to fdt_path_offset(), but only consider the first namelen
++ * characters of path as the path name.
++ *
++ * Return: offset of the node or negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen);
++#endif
++
++/**
++ * fdt_path_offset - find a tree node by its full path
++ * @fdt: pointer to the device tree blob
++ * @path: full path of the node to locate
++ *
++ * fdt_path_offset() finds a node of a given path in the device tree.
++ * Each path component may omit the unit address portion, but the
++ * results of this are undefined if any such path component is
++ * ambiguous (that is if there are multiple nodes at the relevant
++ * level matching the given component, differentiated only by unit
++ * address).
++ *
++ * returns:
++ *	structure block offset of the node with the requested path (>=0), on
++ *		success
++ *	-FDT_ERR_BADPATH, given path does not begin with '/' or is invalid
++ *	-FDT_ERR_NOTFOUND, if the requested node does not exist
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_path_offset(const void *fdt, const char *path);
++
++/**
++ * fdt_get_name - retrieve the name of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of the starting node
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_name() retrieves the name (including unit address) of the
++ * device tree node at structure block offset nodeoffset.  If lenp is
++ * non-NULL, the length of this name is also returned, in the integer
++ * pointed to by lenp.
++ *
++ * returns:
++ *	pointer to the node's name, on success
++ *		If lenp is non-NULL, *lenp contains the length of that name
++ *			(>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE, standard meanings
++ */
++const char *fdt_get_name(const void *fdt, int nodeoffset, int *lenp);
++
++/**
++ * fdt_first_property_offset - find the offset of a node's first property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of a node
++ *
++ * fdt_first_property_offset() finds the first property of the node at
++ * the given structure block offset.
++ *
++ * returns:
++ *	structure block offset of the property (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the requested node has no properties
++ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_BEGIN_NODE tag
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_first_property_offset(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_next_property_offset - step through a node's properties
++ * @fdt: pointer to the device tree blob
++ * @offset: structure block offset of a property
++ *
++ * fdt_next_property_offset() finds the property immediately after the
++ * one at the given structure block offset.  This will be a property
++ * of the same node as the given property.
++ *
++ * returns:
++ *	structure block offset of the next property (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the given property is the last in its node
++ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_PROP tag
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_next_property_offset(const void *fdt, int offset);
++
++/**
++ * fdt_for_each_property_offset - iterate over all properties of a node
++ *
++ * @property:	property offset (int, lvalue)
++ * @fdt:	FDT blob (const void *)
++ * @node:	node offset (int)
++ *
++ * This is actually a wrapper around a for loop and would be used like so:
++ *
++ *	fdt_for_each_property_offset(property, fdt, node) {
++ *		Use property
++ *		...
++ *	}
++ *
++ *	if ((property < 0) && (property != -FDT_ERR_NOTFOUND)) {
++ *		Error handling
++ *	}
++ *
++ * Note that this is implemented as a macro and property is used as
++ * iterator in the loop. The node variable can be constant or even a
++ * literal.
++ */
++#define fdt_for_each_property_offset(property, fdt, node)	\
++	for (property = fdt_first_property_offset(fdt, node);	\
++	     property >= 0;					\
++	     property = fdt_next_property_offset(fdt, property))
++
++/**
++ * fdt_get_property_by_offset - retrieve the property at a given offset
++ * @fdt: pointer to the device tree blob
++ * @offset: offset of the property to retrieve
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_property_by_offset() retrieves a pointer to the
++ * fdt_property structure within the device tree blob at the given
++ * offset.  If lenp is non-NULL, the length of the property value is
++ * also returned, in the integer pointed to by lenp.
++ *
++ * Note that this code only works on device tree versions >= 16. fdt_getprop()
++ * works on all versions.
++ *
++ * returns:
++ *	pointer to the structure representing the property
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
++						      int offset,
++						      int *lenp);
++
++/**
++ * fdt_get_property_namelen - find a property based on substring
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @namelen: number of characters of name to consider
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * Identical to fdt_get_property(), but only examine the first namelen
++ * characters of name for matching the property name.
++ *
++ * Return: pointer to the structure representing the property, or NULL
++ *         if not found
++ */
++#ifndef SWIG /* Not available in Python */
++const struct fdt_property *fdt_get_property_namelen(const void *fdt,
++						    int nodeoffset,
++						    const char *name,
++						    int namelen, int *lenp);
++#endif
++
++/**
++ * fdt_get_property - find a given property in a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_property() retrieves a pointer to the fdt_property
++ * structure within the device tree blob corresponding to the property
++ * named 'name' of the node at offset nodeoffset.  If lenp is
++ * non-NULL, the length of the property value is also returned, in the
++ * integer pointed to by lenp.
++ *
++ * returns:
++ *	pointer to the structure representing the property
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_NOTFOUND, node does not have named property
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset,
++					    const char *name, int *lenp);
++static inline struct fdt_property *fdt_get_property_w(void *fdt, int nodeoffset,
++						      const char *name,
++						      int *lenp)
++{
++	return (struct fdt_property *)(uintptr_t)
++		fdt_get_property(fdt, nodeoffset, name, lenp);
++}
++
++/**
++ * fdt_getprop_by_offset - retrieve the value of a property at a given offset
++ * @fdt: pointer to the device tree blob
++ * @offset: offset of the property to read
++ * @namep: pointer to a string variable (will be overwritten) or NULL
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_getprop_by_offset() retrieves a pointer to the value of the
++ * property at structure block offset 'offset' (this will be a pointer
++ * to within the device blob itself, not a copy of the value).  If
++ * lenp is non-NULL, the length of the property value is also
++ * returned, in the integer pointed to by lenp.  If namep is non-NULL,
++ * the property's namne will also be returned in the char * pointed to
++ * by namep (this will be a pointer to within the device tree's string
++ * block, not a new copy of the name).
++ *
++ * returns:
++ *	pointer to the property's value
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *		if namep is non-NULL *namep contiains a pointer to the property
++ *		name.
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++#ifndef SWIG /* This function is not useful in Python */
++const void *fdt_getprop_by_offset(const void *fdt, int offset,
++				  const char **namep, int *lenp);
++#endif
++
++/**
++ * fdt_getprop_namelen - get property value based on substring
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @namelen: number of characters of name to consider
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * Identical to fdt_getprop(), but only examine the first namelen
++ * characters of name for matching the property name.
++ *
++ * Return: pointer to the property's value or NULL on error
++ */
++#ifndef SWIG /* Not available in Python */
++const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
++				const char *name, int namelen, int *lenp);
++static inline void *fdt_getprop_namelen_w(void *fdt, int nodeoffset,
++					  const char *name, int namelen,
++					  int *lenp)
++{
++	return (void *)(uintptr_t)fdt_getprop_namelen(fdt, nodeoffset, name,
++						      namelen, lenp);
++}
++#endif
++
++/**
++ * fdt_getprop - retrieve the value of a given property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_getprop() retrieves a pointer to the value of the property
++ * named @name of the node at offset @nodeoffset (this will be a
++ * pointer to within the device blob itself, not a copy of the value).
++ * If @lenp is non-NULL, the length of the property value is also
++ * returned, in the integer pointed to by @lenp.
++ *
++ * returns:
++ *	pointer to the property's value
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_NOTFOUND, node does not have named property
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const void *fdt_getprop(const void *fdt, int nodeoffset,
++			const char *name, int *lenp);
++static inline void *fdt_getprop_w(void *fdt, int nodeoffset,
++				  const char *name, int *lenp)
++{
++	return (void *)(uintptr_t)fdt_getprop(fdt, nodeoffset, name, lenp);
++}
++
++/**
++ * fdt_get_phandle - retrieve the phandle of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of the node
++ *
++ * fdt_get_phandle() retrieves the phandle of the device tree node at
++ * structure block offset nodeoffset.
++ *
++ * returns:
++ *	the phandle of the node at nodeoffset, on success (!= 0, != -1)
++ *	0, if the node has no phandle, or another error occurs
++ */
++uint32_t fdt_get_phandle(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_get_alias_namelen - get alias based on substring
++ * @fdt: pointer to the device tree blob
++ * @name: name of the alias th look up
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_get_alias(), but only examine the first @namelen
++ * characters of @name for matching the alias name.
++ *
++ * Return: a pointer to the expansion of the alias named @name, if it exists,
++ *	   NULL otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++const char *fdt_get_alias_namelen(const void *fdt,
++				  const char *name, int namelen);
++#endif
++
++/**
++ * fdt_get_alias - retrieve the path referenced by a given alias
++ * @fdt: pointer to the device tree blob
++ * @name: name of the alias th look up
++ *
++ * fdt_get_alias() retrieves the value of a given alias.  That is, the
++ * value of the property named @name in the node /aliases.
++ *
++ * returns:
++ *	a pointer to the expansion of the alias named 'name', if it exists
++ *	NULL, if the given alias or the /aliases node does not exist
++ */
++const char *fdt_get_alias(const void *fdt, const char *name);
++
++/**
++ * fdt_get_path - determine the full path of a node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose path to find
++ * @buf: character buffer to contain the returned path (will be overwritten)
++ * @buflen: size of the character buffer at buf
++ *
++ * fdt_get_path() computes the full path of the node at offset
++ * nodeoffset, and records that path in the buffer at buf.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	0, on success
++ *		buf contains the absolute path of the node at
++ *		nodeoffset, as a NUL-terminated string.
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_NOSPACE, the path of the given node is longer than (bufsize-1)
++ *		characters and will not fit in the given buffer.
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen);
++
++/**
++ * fdt_supernode_atdepth_offset - find a specific ancestor of a node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ * @supernodedepth: depth of the ancestor to find
++ * @nodedepth: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_supernode_atdepth_offset() finds an ancestor of the given node
++ * at a specific depth from the root (where the root itself has depth
++ * 0, its immediate subnodes depth 1 and so forth).  So
++ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, NULL);
++ * will always return 0, the offset of the root node.  If the node at
++ * nodeoffset has depth D, then:
++ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, D, NULL);
++ * will return nodeoffset itself.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	structure block offset of the node at node offset's ancestor
++ *		of depth supernodedepth (>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of
++ *		nodeoffset
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
++				 int supernodedepth, int *nodedepth);
++
++/**
++ * fdt_node_depth - find the depth of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ *
++ * fdt_node_depth() finds the depth of a given node.  The root node
++ * has depth 0, its immediate subnodes depth 1 and so forth.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	depth of the node at nodeoffset (>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_depth(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_parent_offset - find the parent of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ *
++ * fdt_parent_offset() locates the parent node of a given node (that
++ * is, it finds the offset of the node which contains the node at
++ * nodeoffset as a subnode).
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset, *twice*.
++ *
++ * returns:
++ *	structure block offset of the parent of the node at nodeoffset
++ *		(>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_parent_offset(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_node_offset_by_prop_value - find nodes with a given property value
++ * @fdt: pointer to the device tree blob
++ * @startoffset: only find nodes after this offset
++ * @propname: property name to check
++ * @propval: property value to search for
++ * @proplen: length of the value in propval
++ *
++ * fdt_node_offset_by_prop_value() returns the offset of the first
++ * node after startoffset, which has a property named propname whose
++ * value is of length proplen and has value equal to propval; or if
++ * startoffset is -1, the very first such node in the tree.
++ *
++ * To iterate through all nodes matching the criterion, the following
++ * idiom can be used:
++ *	offset = fdt_node_offset_by_prop_value(fdt, -1, propname,
++ *					       propval, proplen);
++ *	while (offset != -FDT_ERR_NOTFOUND) {
++ *		// other code here
++ *		offset = fdt_node_offset_by_prop_value(fdt, offset, propname,
++ *						       propval, proplen);
++ *	}
++ *
++ * Note the -1 in the first call to the function, if 0 is used here
++ * instead, the function will never locate the root node, even if it
++ * matches the criterion.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0, >startoffset),
++ *		 on success
++ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
++ *		tree after startoffset
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
++				  const char *propname,
++				  const void *propval, int proplen);
++
++/**
++ * fdt_node_offset_by_phandle - find the node with a given phandle
++ * @fdt: pointer to the device tree blob
++ * @phandle: phandle value
++ *
++ * fdt_node_offset_by_phandle() returns the offset of the node
++ * which has the given phandle value.  If there is more than one node
++ * in the tree with the given phandle (an invalid tree), results are
++ * undefined.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0), on success
++ *	-FDT_ERR_NOTFOUND, no node with that phandle exists
++ *	-FDT_ERR_BADPHANDLE, given phandle value was invalid (0 or -1)
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle);
++
++/**
++ * fdt_node_check_compatible - check a node's compatible property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @compatible: string to match against
++ *
++ * fdt_node_check_compatible() returns 0 if the given node contains a
++ * @compatible property with the given string as one of its elements,
++ * it returns non-zero otherwise, or on error.
++ *
++ * returns:
++ *	0, if the node has a 'compatible' property listing the given string
++ *	1, if the node has a 'compatible' property, but it does not list
++ *		the given string
++ *	-FDT_ERR_NOTFOUND, if the given node has no 'compatible' property
++ *	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_check_compatible(const void *fdt, int nodeoffset,
++			      const char *compatible);
++
++/**
++ * fdt_node_offset_by_compatible - find nodes with a given 'compatible' value
++ * @fdt: pointer to the device tree blob
++ * @startoffset: only find nodes after this offset
++ * @compatible: 'compatible' string to match against
++ *
++ * fdt_node_offset_by_compatible() returns the offset of the first
++ * node after startoffset, which has a 'compatible' property which
++ * lists the given compatible string; or if startoffset is -1, the
++ * very first such node in the tree.
++ *
++ * To iterate through all nodes matching the criterion, the following
++ * idiom can be used:
++ *	offset = fdt_node_offset_by_compatible(fdt, -1, compatible);
++ *	while (offset != -FDT_ERR_NOTFOUND) {
++ *		// other code here
++ *		offset = fdt_node_offset_by_compatible(fdt, offset, compatible);
++ *	}
++ *
++ * Note the -1 in the first call to the function, if 0 is used here
++ * instead, the function will never locate the root node, even if it
++ * matches the criterion.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0, >startoffset),
++ *		 on success
++ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
++ *		tree after startoffset
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
++				  const char *compatible);
++
++/**
++ * fdt_stringlist_contains - check a string list property for a string
++ * @strlist: Property containing a list of strings to check
++ * @listlen: Length of property
++ * @str: String to search for
++ *
++ * This is a utility function provided for convenience. The list contains
++ * one or more strings, each terminated by \0, as is found in a device tree
++ * "compatible" property.
++ *
++ * Return: 1 if the string is found in the list, 0 not found, or invalid list
++ */
++int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
++
++/**
++ * fdt_stringlist_count - count the number of strings in a string list
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ *
++ * Return:
++ *   the number of strings in the given property
++ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *   -FDT_ERR_NOTFOUND if the property does not exist
++ */
++int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property);
++
++/**
++ * fdt_stringlist_search - find a string in a string list and return its index
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ * @string: string to look up in the string list
++ *
++ * Note that it is possible for this function to succeed on property values
++ * that are not NUL-terminated. That's because the function will stop after
++ * finding the first occurrence of @string. This can for example happen with
++ * small-valued cell properties, such as #address-cells, when searching for
++ * the empty string.
++ *
++ * return:
++ *   the index of the string in the list of strings
++ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *   -FDT_ERR_NOTFOUND if the property does not exist or does not contain
++ *                     the given string
++ */
++int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
++			  const char *string);
++
++/**
++ * fdt_stringlist_get() - obtain the string at a given index in a string list
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ * @index: index of the string to return
++ * @lenp: return location for the string length or an error code on failure
++ *
++ * Note that this will successfully extract strings from properties with
++ * non-NUL-terminated values. For example on small-valued cell properties
++ * this function will return the empty string.
++ *
++ * If non-NULL, the length of the string (on success) or a negative error-code
++ * (on failure) will be stored in the integer pointer to by lenp.
++ *
++ * Return:
++ *   A pointer to the string at the given index in the string list or NULL on
++ *   failure. On success the length of the string will be stored in the memory
++ *   location pointed to by the lenp parameter, if non-NULL. On failure one of
++ *   the following negative error codes will be returned in the lenp parameter
++ *   (if non-NULL):
++ *     -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *     -FDT_ERR_NOTFOUND if the property does not exist
++ */
++const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
++			       const char *property, int index,
++			       int *lenp);
++
++/**********************************************************************/
++/* Read-only functions (addressing related)                           */
++/**********************************************************************/
++
++/**
++ * FDT_MAX_NCELLS - maximum value for #address-cells and #size-cells
++ *
++ * This is the maximum value for #address-cells, #size-cells and
++ * similar properties that will be processed by libfdt.  IEE1275
++ * requires that OF implementations handle values up to 4.
++ * Implementations may support larger values, but in practice higher
++ * values aren't used.
++ */
++#define FDT_MAX_NCELLS		4
++
++/**
++ * fdt_address_cells - retrieve address size for a bus represented in the tree
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to find the address size for
++ *
++ * When the node has a valid #address-cells property, returns its value.
++ *
++ * returns:
++ *	0 <= n < FDT_MAX_NCELLS, on success
++ *      2, if the node has no #address-cells property
++ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#address-cells property
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_address_cells(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_size_cells - retrieve address range size for a bus represented in the
++ *                  tree
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to find the address range size for
++ *
++ * When the node has a valid #size-cells property, returns its value.
++ *
++ * returns:
++ *	0 <= n < FDT_MAX_NCELLS, on success
++ *      1, if the node has no #size-cells property
++ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#size-cells property
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_size_cells(const void *fdt, int nodeoffset);
++
++
++/**********************************************************************/
++/* Write-in-place functions                                           */
++/**********************************************************************/
++
++/**
++ * fdt_setprop_inplace_namelen_partial - change a property's value,
++ *                                       but not its size
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @namelen: number of characters of name to consider
++ * @idx: index of the property to change in the array
++ * @val: pointer to data to replace the property value with
++ * @len: length of the property value
++ *
++ * Identical to fdt_setprop_inplace(), but modifies the given property
++ * starting from the given index, and using only the first characters
++ * of the name. It is useful when you want to manipulate only one value of
++ * an array and you have a string that doesn't end with \0.
++ *
++ * Return: 0 on success, negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
++					const char *name, int namelen,
++					uint32_t idx, const void *val,
++					int len);
++#endif
++
++/**
++ * fdt_setprop_inplace - change a property's value, but not its size
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: pointer to data to replace the property value with
++ * @len: length of the property value
++ *
++ * fdt_setprop_inplace() replaces the value of a given property with
++ * the data in val, of length len.  This function cannot change the
++ * size of a property, and so will only work if len is equal to the
++ * current length of the property.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if len is not equal to the property's current length
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
++			const void *val, int len);
++#endif
++
++/**
++ * fdt_setprop_inplace_u32 - change the value of a 32-bit integer property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to replace the property with
++ *
++ * fdt_setprop_inplace_u32() replaces the value of a given property
++ * with the 32-bit integer value in val, converting val to big-endian
++ * if necessary.  This function cannot change the size of a property,
++ * and so will only work if the property already exists and has length
++ * 4.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 4
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_inplace_u32(void *fdt, int nodeoffset,
++					  const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_inplace_u64 - change the value of a 64-bit integer property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value to replace the property with
++ *
++ * fdt_setprop_inplace_u64() replaces the value of a given property
++ * with the 64-bit integer value in val, converting val to big-endian
++ * if necessary.  This function cannot change the size of a property,
++ * and so will only work if the property already exists and has length
++ * 8.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 8
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_inplace_u64(void *fdt, int nodeoffset,
++					  const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_inplace_cell - change the value of a single-cell property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node containing the property
++ * @name: name of the property to change the value of
++ * @val: new value of the 32-bit cell
++ *
++ * This is an alternative name for fdt_setprop_inplace_u32()
++ * Return: 0 on success, negative libfdt error number otherwise.
++ */
++static inline int fdt_setprop_inplace_cell(void *fdt, int nodeoffset,
++					   const char *name, uint32_t val)
++{
++	return fdt_setprop_inplace_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_nop_property - replace a property with nop tags
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to nop
++ * @name: name of the property to nop
++ *
++ * fdt_nop_property() will replace a given property's representation
++ * in the blob with FDT_NOP tags, effectively removing it from the
++ * tree.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the property, and will not alter or move any other part of the
++ * tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_nop_property(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_nop_node - replace a node (subtree) with nop tags
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to nop
++ *
++ * fdt_nop_node() will replace a given node's representation in the
++ * blob, including all its subnodes, if any, with FDT_NOP tags,
++ * effectively removing it from the tree.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the node and its properties and subnodes, and will not alter or
++ * move any other part of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_nop_node(void *fdt, int nodeoffset);
++
++/**********************************************************************/
++/* Sequential write functions                                         */
++/**********************************************************************/
++
++/* fdt_create_with_flags flags */
++#define FDT_CREATE_FLAG_NO_NAME_DEDUP 0x1
++	/* FDT_CREATE_FLAG_NO_NAME_DEDUP: Do not try to de-duplicate property
++	 * names in the fdt. This can result in faster creation times, but
++	 * a larger fdt. */
++
++#define FDT_CREATE_FLAGS_ALL	(FDT_CREATE_FLAG_NO_NAME_DEDUP)
++
++/**
++ * fdt_create_with_flags - begin creation of a new fdt
++ * @buf: pointer to memory allocated where fdt will be created
++ * @bufsize: size of the memory space at fdt
++ * @flags: a valid combination of FDT_CREATE_FLAG_ flags, or 0.
++ *
++ * fdt_create_with_flags() begins the process of creating a new fdt with
++ * the sequential write interface.
++ *
++ * fdt creation process must end with fdt_finished() to produce a valid fdt.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, bufsize is insufficient for a minimal fdt
++ *	-FDT_ERR_BADFLAGS, flags is not valid
++ */
++int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags);
++
++/**
++ * fdt_create - begin creation of a new fdt
++ * @buf: pointer to memory allocated where fdt will be created
++ * @bufsize: size of the memory space at fdt
++ *
++ * fdt_create() is equivalent to fdt_create_with_flags() with flags=0.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, bufsize is insufficient for a minimal fdt
++ */
++int fdt_create(void *buf, int bufsize);
++
++int fdt_resize(void *fdt, void *buf, int bufsize);
++int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size);
++int fdt_finish_reservemap(void *fdt);
++int fdt_begin_node(void *fdt, const char *name);
++int fdt_property(void *fdt, const char *name, const void *val, int len);
++static inline int fdt_property_u32(void *fdt, const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_property(fdt, name, &tmp, sizeof(tmp));
++}
++static inline int fdt_property_u64(void *fdt, const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_property(fdt, name, &tmp, sizeof(tmp));
++}
++
++#ifndef SWIG /* Not available in Python */
++static inline int fdt_property_cell(void *fdt, const char *name, uint32_t val)
++{
++	return fdt_property_u32(fdt, name, val);
++}
++#endif
++
++/**
++ * fdt_property_placeholder - add a new property and return a ptr to its value
++ *
++ * @fdt: pointer to the device tree blob
++ * @name: name of property to add
++ * @len: length of property value in bytes
++ * @valp: returns a pointer to where where the value should be placed
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_NOSPACE, standard meanings
++ */
++int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp);
++
++#define fdt_property_string(fdt, name, str) \
++	fdt_property(fdt, name, str, strlen(str)+1)
++int fdt_end_node(void *fdt);
++int fdt_finish(void *fdt);
++
++/**********************************************************************/
++/* Read-write functions                                               */
++/**********************************************************************/
++
++int fdt_create_empty_tree(void *buf, int bufsize);
++int fdt_open_into(const void *fdt, void *buf, int bufsize);
++int fdt_pack(void *fdt);
++
++/**
++ * fdt_add_mem_rsv - add one memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @address: 64-bit start address of the reserve map entry
++ * @size: 64-bit size of the reserved region
++ *
++ * Adds a reserve map entry to the given blob reserving a region at
++ * address address of length size.
++ *
++ * This function will insert data into the reserve map and will
++ * therefore change the indexes of some entries in the table.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new reservation entry
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size);
++
++/**
++ * fdt_del_mem_rsv - remove a memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @n: entry to remove
++ *
++ * fdt_del_mem_rsv() removes the n-th memory reserve map entry from
++ * the blob.
++ *
++ * This function will delete data from the reservation table and will
++ * therefore change the indexes of some entries in the table.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, there is no entry of the given index (i.e. there
++ *		are less than n+1 reserve map entries)
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_del_mem_rsv(void *fdt, int n);
++
++/**
++ * fdt_set_name - change the name of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of a node
++ * @name: name to give the node
++ *
++ * fdt_set_name() replaces the name (including unit address, if any)
++ * of the given node with the given string.  NOTE: this function can't
++ * efficiently check if the new name is unique amongst the given
++ * node's siblings; results are undefined if this function is invoked
++ * with a name equal to one of the given node's siblings.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob
++ *		to contain the new name
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_set_name(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_setprop - create or change a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: pointer to data to set the property value to
++ * @len: length of the property value
++ *
++ * fdt_setprop() sets the value of the named property in the given
++ * node to the given value and length, creating the property if it
++ * does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_setprop(void *fdt, int nodeoffset, const char *name,
++		const void *val, int len);
++
++/**
++ * fdt_setprop_placeholder - allocate space for a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @len: length of the property value
++ * @prop_data: return pointer to property data
++ *
++ * fdt_setprop_placeholer() allocates the named property in the given node.
++ * If the property exists it is resized. In either case a pointer to the
++ * property data is returned.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
++			    int len, void **prop_data);
++
++/**
++ * fdt_setprop_u32 - set a property to a 32-bit integer
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value for the property (native endian)
++ *
++ * fdt_setprop_u32() sets the value of the named property in the given
++ * node to the given 32-bit integer value (converting to big-endian if
++ * necessary), or creates a new property with that value if it does
++ * not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_u32(void *fdt, int nodeoffset, const char *name,
++				  uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_u64 - set a property to a 64-bit integer
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value for the property (native endian)
++ *
++ * fdt_setprop_u64() sets the value of the named property in the given
++ * node to the given 64-bit integer value (converting to big-endian if
++ * necessary), or creates a new property with that value if it does
++ * not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_u64(void *fdt, int nodeoffset, const char *name,
++				  uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_cell - set a property to a single cell value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value for the property (native endian)
++ *
++ * This is an alternative name for fdt_setprop_u32()
++ *
++ * Return: 0 on success, negative libfdt error value otherwise.
++ */
++static inline int fdt_setprop_cell(void *fdt, int nodeoffset, const char *name,
++				   uint32_t val)
++{
++	return fdt_setprop_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_setprop_string - set a property to a string value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @str: string value for the property
++ *
++ * fdt_setprop_string() sets the value of the named property in the
++ * given node to the given string value (using the length of the
++ * string to determine the new length of the property), or creates a
++ * new property with that value if it does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_setprop_string(fdt, nodeoffset, name, str) \
++	fdt_setprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
++
++
++/**
++ * fdt_setprop_empty - set a property to an empty value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ *
++ * fdt_setprop_empty() sets the value of the named property in the
++ * given node to an empty (zero length) value, or creates a new empty
++ * property if it does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_setprop_empty(fdt, nodeoffset, name) \
++	fdt_setprop((fdt), (nodeoffset), (name), NULL, 0)
++
++/**
++ * fdt_appendprop - append to or create a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to append to
++ * @val: pointer to data to append to the property value
++ * @len: length of the data to append to the property value
++ *
++ * fdt_appendprop() appends the value to the named property in the
++ * given node, creating the property if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
++		   const void *val, int len);
++
++/**
++ * fdt_appendprop_u32 - append a 32-bit integer value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to append to the property (native endian)
++ *
++ * fdt_appendprop_u32() appends the given 32-bit integer value
++ * (converting to big-endian if necessary) to the value of the named
++ * property in the given node, or creates a new property with that
++ * value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_appendprop_u32(void *fdt, int nodeoffset,
++				     const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_appendprop_u64 - append a 64-bit integer value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value to append to the property (native endian)
++ *
++ * fdt_appendprop_u64() appends the given 64-bit integer value
++ * (converting to big-endian if necessary) to the value of the named
++ * property in the given node, or creates a new property with that
++ * value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_appendprop_u64(void *fdt, int nodeoffset,
++				     const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_appendprop_cell - append a single cell value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to append to the property (native endian)
++ *
++ * This is an alternative name for fdt_appendprop_u32()
++ *
++ * Return: 0 on success, negative libfdt error value otherwise.
++ */
++static inline int fdt_appendprop_cell(void *fdt, int nodeoffset,
++				      const char *name, uint32_t val)
++{
++	return fdt_appendprop_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_appendprop_string - append a string to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @str: string value to append to the property
++ *
++ * fdt_appendprop_string() appends the given string to the value of
++ * the named property in the given node, or creates a new property
++ * with that value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_appendprop_string(fdt, nodeoffset, name, str) \
++	fdt_appendprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
++
++/**
++ * fdt_appendprop_addrrange - append a address range property
++ * @fdt: pointer to the device tree blob
++ * @parent: offset of the parent node
++ * @nodeoffset: offset of the node to add a property at
++ * @name: name of property
++ * @addr: start address of a given range
++ * @size: size of a given range
++ *
++ * fdt_appendprop_addrrange() appends an address range value (start
++ * address and size) to the value of the named property in the given
++ * node, or creates a new property with that value if it does not
++ * already exist.
++ * If "name" is not specified, a default "reg" is used.
++ * Cell sizes are determined by parent's #address-cells and #size-cells.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#address-cells property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADVALUE, addr or size doesn't fit to respective cells size
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain a new property
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_appendprop_addrrange(void *fdt, int parent, int nodeoffset,
++			     const char *name, uint64_t addr, uint64_t size);
++
++/**
++ * fdt_delprop - delete a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to nop
++ * @name: name of the property to nop
++ *
++ * fdt_del_property() will delete the given property.
++ *
++ * This function will delete data from the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_delprop(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_add_subnode_namelen - creates a new node based on substring
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to create
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_add_subnode(), but use only the first @namelen
++ * characters of @name as the name of the new node.  This is useful for
++ * creating subnodes based on a portion of a larger string, such as a
++ * full path.
++ *
++ * Return: structure block offset of the created subnode (>=0),
++ *	   negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_add_subnode_namelen(void *fdt, int parentoffset,
++			    const char *name, int namelen);
++#endif
++
++/**
++ * fdt_add_subnode - creates a new node
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ *
++ * fdt_add_subnode() creates a new node as a subnode of the node at
++ * structure block offset parentoffset, with the given name (which
++ * should include the unit address, if any).
++ *
++ * This function will insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	structure block offset of the created nodeequested subnode (>=0), on
++ *		success
++ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
++ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
++ *		tag
++ *	-FDT_ERR_EXISTS, if the node at parentoffset already has a subnode of
++ *		the given name
++ *	-FDT_ERR_NOSPACE, if there is insufficient free space in the
++ *		blob to contain the new node
++ *	-FDT_ERR_NOSPACE
++ *	-FDT_ERR_BADLAYOUT
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_add_subnode(void *fdt, int parentoffset, const char *name);
++
++/**
++ * fdt_del_node - delete a node (subtree)
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to nop
++ *
++ * fdt_del_node() will remove the given node, including all its
++ * subnodes if any, from the blob.
++ *
++ * This function will delete data from the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_del_node(void *fdt, int nodeoffset);
++
++/**
++ * fdt_overlay_apply - Applies a DT overlay on a base DT
++ * @fdt: pointer to the base device tree blob
++ * @fdto: pointer to the device tree overlay blob
++ *
++ * fdt_overlay_apply() will apply the given device tree overlay on the
++ * given base device tree.
++ *
++ * Expect the base device tree to be modified, even if the function
++ * returns an error.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there's not enough space in the base device tree
++ *	-FDT_ERR_NOTFOUND, the overlay points to some inexistant nodes or
++ *		properties in the base DT
++ *	-FDT_ERR_BADPHANDLE,
++ *	-FDT_ERR_BADOVERLAY,
++ *	-FDT_ERR_NOPHANDLES,
++ *	-FDT_ERR_INTERNAL,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADOFFSET,
++ *	-FDT_ERR_BADPATH,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_overlay_apply(void *fdt, void *fdto);
++
++/**
++ * fdt_overlay_target_offset - retrieves the offset of a fragment's target
++ * @fdt: Base device tree blob
++ * @fdto: Device tree overlay blob
++ * @fragment_offset: node offset of the fragment in the overlay
++ * @pathp: pointer which receives the path of the target (or NULL)
++ *
++ * fdt_overlay_target_offset() retrieves the target offset in the base
++ * device tree of a fragment, no matter how the actual targeting is
++ * done (through a phandle or a path)
++ *
++ * returns:
++ *      the targeted node offset in the base device tree
++ *      Negative error code on error
++ */
++int fdt_overlay_target_offset(const void *fdt, const void *fdto,
++			      int fragment_offset, char const **pathp);
++
++/**********************************************************************/
++/* Debugging / informational functions                                */
++/**********************************************************************/
++
++const char *fdt_strerror(int errval);
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* LIBFDT_H */
+diff --git a/include/libfdt_env.h b/include/libfdt_env.h
+new file mode 100644
+index 0000000..51b31d1
+--- /dev/null
++++ b/include/libfdt_env.h
+@@ -0,0 +1,95 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_ENV_H
++#define LIBFDT_ENV_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
++ */
++
++#include <stdbool.h>
++#include <stddef.h>
++#include <stdint.h>
++#include <limits.h>
++#include <string.h>
++
++#ifdef __CHECKER__
++#define FDT_FORCE __attribute__((force))
++#define FDT_BITWISE __attribute__((bitwise))
++#else
++#define FDT_FORCE
++#define FDT_BITWISE
++#endif
++
++typedef uint16_t FDT_BITWISE fdt16_t;
++typedef uint32_t FDT_BITWISE fdt32_t;
++typedef uint64_t FDT_BITWISE fdt64_t;
++
++#define EXTRACT_BYTE(x, n)	((unsigned long long)((uint8_t *)&x)[n])
++#define CPU_TO_FDT16(x) ((EXTRACT_BYTE(x, 0) << 8) | EXTRACT_BYTE(x, 1))
++#define CPU_TO_FDT32(x) ((EXTRACT_BYTE(x, 0) << 24) | (EXTRACT_BYTE(x, 1) << 16) | \
++			 (EXTRACT_BYTE(x, 2) << 8) | EXTRACT_BYTE(x, 3))
++#define CPU_TO_FDT64(x) ((EXTRACT_BYTE(x, 0) << 56) | (EXTRACT_BYTE(x, 1) << 48) | \
++			 (EXTRACT_BYTE(x, 2) << 40) | (EXTRACT_BYTE(x, 3) << 32) | \
++			 (EXTRACT_BYTE(x, 4) << 24) | (EXTRACT_BYTE(x, 5) << 16) | \
++			 (EXTRACT_BYTE(x, 6) << 8) | EXTRACT_BYTE(x, 7))
++
++static inline uint16_t fdt16_to_cpu(fdt16_t x)
++{
++	return (FDT_FORCE uint16_t)CPU_TO_FDT16(x);
++}
++static inline fdt16_t cpu_to_fdt16(uint16_t x)
++{
++	return (FDT_FORCE fdt16_t)CPU_TO_FDT16(x);
++}
++
++static inline uint32_t fdt32_to_cpu(fdt32_t x)
++{
++	return (FDT_FORCE uint32_t)CPU_TO_FDT32(x);
++}
++static inline fdt32_t cpu_to_fdt32(uint32_t x)
++{
++	return (FDT_FORCE fdt32_t)CPU_TO_FDT32(x);
++}
++
++static inline uint64_t fdt64_to_cpu(fdt64_t x)
++{
++	return (FDT_FORCE uint64_t)CPU_TO_FDT64(x);
++}
++static inline fdt64_t cpu_to_fdt64(uint64_t x)
++{
++	return (FDT_FORCE fdt64_t)CPU_TO_FDT64(x);
++}
++#undef CPU_TO_FDT64
++#undef CPU_TO_FDT32
++#undef CPU_TO_FDT16
++#undef EXTRACT_BYTE
++
++#ifdef __APPLE__
++#include <AvailabilityMacros.h>
++
++/* strnlen() is not available on Mac OS < 10.7 */
++# if !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED < \
++                                         MAC_OS_X_VERSION_10_7)
++
++#define strnlen fdt_strnlen
++
++/*
++ * fdt_strnlen: returns the length of a string or max_count - which ever is
++ * smallest.
++ * Input 1 string: the string whose size is to be determined
++ * Input 2 max_count: the maximum value returned by this function
++ * Output: length of the string or max_count (the smallest of the two)
++ */
++static inline size_t fdt_strnlen(const char *string, size_t max_count)
++{
++    const char *p = memchr(string, 0, max_count);
++    return p ? p - string : max_count;
++}
++
++#endif /* !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED <
++          MAC_OS_X_VERSION_10_7) */
++
++#endif /* __APPLE__ */
++
++#endif /* LIBFDT_ENV_H */
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch
new file mode 100644
index 0000000..871a178
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch
@@ -0,0 +1,101 @@
+From 0f2c7ca446063be6b193fbf870d38c0af19e15c5 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:28:25 +0800
+Subject: [PATCH] common: Add essential libc functions
+
+The libfdt uses some of the libc functions, e.g. memcmp, memmove,
+strlen .etc. Add them in lib.c.
+
+The code is copied from TF-A (v2.5) [1] project, which is under the
+terms of BSD license. It is the same with boot-wrapper.
+
+[1]: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: If3b55b00afa8694c7522df989a41e0b38eda1d38
+---
+ common/lib.c | 71 +++++++++++++++++++++++++++++++++++++++++++++++++++-
+ 1 file changed, 70 insertions(+), 1 deletion(-)
+
+diff --git a/common/lib.c b/common/lib.c
+index fcf5f69..0be1c4a 100644
+--- a/common/lib.c
++++ b/common/lib.c
+@@ -32,4 +32,73 @@ void *memset(void *s, int c, size_t n)
+ 	return s;
+ }
+ 
+-/* TODO: memmove and memcmp could also be called */
++int memcmp(const void *s1, const void *s2, size_t len)
++{
++	const unsigned char *s = s1;
++	const unsigned char *d = s2;
++	unsigned char sc;
++	unsigned char dc;
++
++	while (len--) {
++		sc = *s++;
++		dc = *d++;
++		if (sc - dc)
++			return (sc - dc);
++	}
++
++	return 0;
++}
++
++void *memmove(void *dst, const void *src, size_t len)
++{
++	if ((size_t)dst - (size_t)src >= len) {
++		/* destination not in source data, so can safely use memcpy */
++		return memcpy(dst, src, len);
++	} else {
++		/* copy backwards... */
++		const char *end = dst;
++		const char *s = (const char *)src + len;
++		char *d = (char *)dst + len;
++		while (d != end)
++			*--d = *--s;
++	}
++	return dst;
++}
++
++void *memchr(const void *src, int c, size_t len)
++{
++	const unsigned char *s = src;
++
++	while (len--) {
++		if (*s == (unsigned char)c)
++			return (void *) s;
++		s++;
++	}
++
++	return NULL;
++}
++
++char *strrchr(const char *p, int ch)
++{
++	char *save;
++	char c;
++
++	c = ch;
++	for (save = NULL;; ++p) {
++		if (*p == c)
++			save = (char *)p;
++		if (*p == '\0')
++			return (save);
++	}
++	/* NOTREACHED */
++}
++
++size_t strlen(const char *s)
++{
++	const char *cursor = s;
++
++	while (*cursor)
++		cursor++;
++
++	return cursor - s;
++}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch
new file mode 100644
index 0000000..5917ef2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch
@@ -0,0 +1,61 @@
+From de5d2b6c200ae5dd8113751e58bf7cf5844eec5a Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:42:48 +0800
+Subject: [PATCH] Makefile: Add the libfdt to the Makefile system
+
+Add the libfdt into Makefile system. The libfdt uses const value and
+thus gcc will enable the stack guard. The stack guard will fail the
+compile. Add -fno-stack-protector to fix it.
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I472bc28cdc5cde3b22461a4b7d7a3752ae382b4b
+---
+ Makefile.am | 11 +++++++++--
+ 1 file changed, 9 insertions(+), 2 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 88a27de..5e8668a 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -36,6 +36,9 @@ PSCI_CPU_OFF	:= 0x84000002
+ COMMON_SRC	:= common/
+ COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o
+ 
++LIBFDT_SRC	:= common/libfdt/
++LIBFDT_OBJS	:= fdt.o fdt_ro.o fdt_rw.o
++
+ ARCH_OBJ	:= boot.o stack.o utils.o
+ 
+ if BOOTWRAPPER_32
+@@ -125,11 +128,12 @@ CHOSEN_NODE	:= chosen {						\
+ CPPFLAGS	+= $(INITRD_FLAGS)
+ CFLAGS		+= -I$(top_srcdir)/include/ -I$(top_srcdir)/$(ARCH_SRC)/include/
+ CFLAGS		+= -Wall -fomit-frame-pointer
++CFLAGS 		+= -fno-stack-protector
+ CFLAGS		+= -ffunction-sections -fdata-sections
+ CFLAGS		+= -fno-pic -fno-pie
+ LDFLAGS		+= --gc-sections
+ 
+-OBJ		:= $(addprefix $(ARCH_SRC),$(ARCH_OBJ)) $(addprefix $(COMMON_SRC),$(COMMON_OBJ))
++OBJ		:= $(addprefix $(ARCH_SRC),$(ARCH_OBJ)) $(addprefix $(COMMON_SRC),$(COMMON_OBJ)) $(addprefix $(LIBFDT_SRC),$(LIBFDT_OBJS))
+ 
+ # Don't lookup all prerequisites in $(top_srcdir), only the source files. When
+ # building outside the source tree $(ARCH_SRC) needs to be created.
+@@ -150,10 +154,13 @@ $(ARCH_SRC):
+ $(COMMON_SRC):
+ 	$(MKDIR_P) $@
+ 
++$(LIBFDT_SRC):
++	$(MKDIR_P) $@
++
+ %.o: %.S Makefile | $(ARCH_SRC)
+ 	$(CC) $(CPPFLAGS) -D__ASSEMBLY__ $(CFLAGS) $(DEFINES) -c -o $@ $<
+ 
+-%.o: %.c Makefile | $(COMMON_SRC)
++%.o: %.c Makefile | $(COMMON_SRC) $(LIBFDT_SRC)
+ 	$(CC) $(CPPFLAGS) $(CFLAGS) $(DEFINES) -c -o $@ $<
+ 
+ model.lds: $(LD_SCRIPT) Makefile
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch
new file mode 100644
index 0000000..136e18e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch
@@ -0,0 +1,67 @@
+From 5b8cb5192dbd0332e027e8999c3afe4433983291 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 10:50:21 +0800
+Subject: [PATCH] platform: Add print_hex func
+
+Refine the print functions, and add a new print_hex func to print hex
+numbers.
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ic960345d9ef0b41d81d30c4a4dbd9c31139907c4
+---
+ common/platform.c | 33 +++++++++++++++++++++++++--------
+ 1 file changed, 25 insertions(+), 8 deletions(-)
+
+diff --git a/common/platform.c b/common/platform.c
+index d11f568..8269392 100644
+--- a/common/platform.c
++++ b/common/platform.c
+@@ -30,20 +30,37 @@
+ #define V2M_SYS(reg)	((void *)SYSREGS_BASE + V2M_SYS_##reg)
+ #endif
+ 
+-static void print_string(const char *str)
++static void print_char(const char c)
+ {
+ 	uint32_t flags;
++	do {
++		flags = raw_readl(PL011(UARTFR));
++	} while (flags & PL011_UARTFR_FIFO_FULL);
+ 
++	raw_writel(c, PL011(UARTDR));
++
++	do {
++		flags = raw_readl(PL011(UARTFR));
++	} while (flags & PL011_UARTFR_BUSY);
++}
++
++void print_string(const char *str)
++{
+ 	while (*str) {
+-		do
+-			flags = raw_readl(PL011(UARTFR));
+-		while (flags & PL011_UARTFR_FIFO_FULL);
++		print_char(*str++);
++	}
++}
+ 
+-		raw_writel(*str++, PL011(UARTDR));
++#define HEX_CHARS_PER_INT (2 * sizeof(int))
++
++void print_hex(unsigned int val)
++{
+ 
+-		do
+-			flags = raw_readl(PL011(UARTFR));
+-		while (flags & PL011_UARTFR_BUSY);
++	const char hex_chars[16] = "0123456789abcdef";
++	int i;
++	for (i = HEX_CHARS_PER_INT - 1; i >= 0; i--) {
++		int v = (val >> (4 * i)) & 0xf;
++		print_char(hex_chars[v]);
+ 	}
+ }
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch
new file mode 100644
index 0000000..ea51816
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch
@@ -0,0 +1,96 @@
+From b447242cd2457bec20d47fe6a8a5758d97a3bde3 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 19 Jan 2022 16:19:02 +0800
+Subject: [PATCH] common: Add mem usage to /memreserve/
+
+Set /memreserve/ to prevent next boot stages from overrding PSCI
+services with libfdt.
+
+Issue-Id: SCM-3815
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I2ea80cdf736a910fa2c3deb622e21d50f04be960
+---
+ Makefile.am          |  2 +-
+ common/boot.c        |  1 +
+ common/device_tree.c | 34 ++++++++++++++++++++++++++++++++++
+ include/boot.h       |  1 +
+ 4 files changed, 37 insertions(+), 1 deletion(-)
+ create mode 100644 common/device_tree.c
+
+diff --git a/Makefile.am b/Makefile.am
+index 5e8668a..734de92 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -34,7 +34,7 @@ endif
+ PSCI_CPU_OFF	:= 0x84000002
+ 
+ COMMON_SRC	:= common/
+-COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o
++COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o device_tree.o
+ 
+ LIBFDT_SRC	:= common/libfdt/
+ LIBFDT_OBJS	:= fdt.o fdt_ro.o fdt_rw.o
+diff --git a/common/boot.c b/common/boot.c
+index c74d34c..ee2bea0 100644
+--- a/common/boot.c
++++ b/common/boot.c
+@@ -63,6 +63,7 @@ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ {
+ 	if (cpu == 0) {
+ 		init_platform();
++		dt_add_memreserve();
+ 
+ 		*mbox = (unsigned long)&entrypoint;
+ 		sevl();
+diff --git a/common/device_tree.c b/common/device_tree.c
+new file mode 100644
+index 0000000..4d0876c
+--- /dev/null
++++ b/common/device_tree.c
+@@ -0,0 +1,34 @@
++/*
++ * device_tree.c - Basic device tree node handler
++ *
++ * Copyright (C) 2021 ARM Limited. All rights reserved.
++ *
++ * Use of this source code is governed by a BSD-style license that can be
++ * found in the LICENSE.txt file.
++ */
++#include <libfdt.h>
++
++extern unsigned long dtb;
++extern char firmware_start[], firmware_end[];
++
++extern void print_string(const char *str);
++
++static void *blob;
++
++
++void dt_add_memreserve(void)
++{
++	int ret;
++
++	blob = (void*)&dtb;
++	print_string("Add /memreserve/\n\r");
++
++	fdt_open_into(blob, blob, fdt_totalsize(blob) +
++		      sizeof(struct fdt_reserve_entry));
++	ret = fdt_add_mem_rsv(blob, (uint64_t)firmware_start,
++			      (uint64_t)(firmware_end - firmware_start));
++
++	if(ret < 0) {
++		print_string("reserve mem add err\n\r");
++	}
++}
+diff --git a/include/boot.h b/include/boot.h
+index d75e013..c3e2ec1 100644
+--- a/include/boot.h
++++ b/include/boot.h
+@@ -16,4 +16,5 @@ void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry);
+ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ 			   unsigned long invalid_addr);
+ 
++void dt_add_memreserve(void);
+ #endif
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch
new file mode 100644
index 0000000..0411ef0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch
@@ -0,0 +1,102 @@
+From 8271c21bcff260295203214b7b8c87cdb8236453 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 4 Jan 2022 17:01:55 +0800
+Subject: [PATCH] boot: Add the --enable-keep-el compile option
+
+Add --enable-keep-el compile option to enable boot-wrapper booting next
+stage at EL2.
+The Armv8R AArch64 boots at EL2. If the next stage requires EL2 booting,
+the boot-wrapper should not drop to EL1.
+Currently, this option only works for Armv8R AArch64. Also, to work with
+Linux PSCI, this option will cause secondary cores booting at EL1.
+
+Issue-Id: SCM-3813
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I3ba9c87cf0b59d163ca433f74c9e3a46e5ca2c63
+---
+ Makefile.am         | 4 ++++
+ arch/aarch64/boot.S | 6 +++++-
+ common/psci.c       | 6 ++++++
+ configure.ac        | 5 +++++
+ 4 files changed, 20 insertions(+), 1 deletion(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 734de92..054becd 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -33,6 +33,10 @@ PSCI_CPU_ON	:= 0xc4000003
+ endif
+ PSCI_CPU_OFF	:= 0x84000002
+ 
++if KEEP_EL
++DEFINES		+= -DKEEP_EL
++endif
++
+ COMMON_SRC	:= common/
+ COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o device_tree.o
+ 
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 6dbd5cc..157c097 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -233,7 +233,11 @@ el2_init:
+ 	msr	cnthctl_el2, x0
+ 	isb
+ 
++#ifdef KEEP_EL
++	mov	w0, #SPSR_KERNEL
++#else
+ 	mov	w0, #SPSR_KERNEL_EL1
++#endif
+ 	ldr	x1, =spsr_to_elx
+ 	str	w0, [x1]
+ 	// fall through
+@@ -313,5 +317,5 @@ ASM_FUNC(jump_kernel)
+ 	.align 3
+ flag_keep_el:
+ 	.long 0
+-spsr_to_elx:
++ASM_DATA(spsr_to_elx)
+ 	.long 0
+diff --git a/common/psci.c b/common/psci.c
+index a0e8700..945780b 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -18,6 +18,8 @@
+ #error "No MPIDRs provided"
+ #endif
+ 
++extern unsigned int spsr_to_elx;
++
+ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+@@ -44,6 +46,10 @@ static int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+ 	ret = psci_store_address(cpu, address);
+ 	bakery_unlock(branch_table_lock, this_cpu);
+ 
++#ifdef KEEP_EL
++	spsr_to_elx = SPSR_KERNEL_EL1;
++#endif
++
+ 	return ret;
+ }
+ 
+diff --git a/configure.ac b/configure.ac
+index 53e51be..0e07db3 100644
+--- a/configure.ac
++++ b/configure.ac
+@@ -25,6 +25,11 @@ AS_IF([test "x$BOOTWRAPPER_ES" = x32 -a "x$KERNEL_ES" != x32],
+ 	[AC_MSG_ERROR([a 32-bit boot-wrapper cannot launch a 64-bit kernel])]
+ )
+ 
++AC_ARG_ENABLE([keep-el],
++	AC_HELP_STRING([--enable-keep-el], [keep exception level when start kernel]),
++	[KEEP_EL=yes], [KEEP_EL=no])
++AM_CONDITIONAL([KEEP_EL], [test "x$KEEP_EL" = xyes])
++
+ # Allow a user to pass --with-kernel-dir
+ AC_ARG_WITH([kernel-dir],
+ 	AS_HELP_STRING([--with-kernel-dir], [specify the root Linux kernel build directory (required)]),
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch
new file mode 100644
index 0000000..a6b16e4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch
@@ -0,0 +1,34 @@
+From dd3e3f414d0e6ed1643c2e2ccac676b7fc1dc7a9 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Tue, 1 Feb 2022 11:28:46 +0000
+Subject: [PATCH] Makefile: Change COUNTER_FREQ to 100 MHz
+
+Older Arm Fast Models (AEM < RevC) had a base frequency of 24 MHz. but
+the RevC base models use 100 MHz. There is not a robust method of
+determining the configured base frequency at runtime, so update
+COUNTER_FREQ to be 100 MHz.
+
+Issue-Id: SCM-3871
+Upstream-Status: Pending
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: Ia9ad0f8ee488d1a887791f1fa1d8f3bf9c5887fd
+---
+ Makefile.am | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 40bc5d6..b48173c 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -13,7 +13,7 @@ SCRIPT_DIR	:= $(top_srcdir)/scripts
+ PHYS_OFFSET	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findmem.pl $(KERNEL_DTB))
+ UART_BASE	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findbase.pl $(KERNEL_DTB) 0 'arm,pl011')
+ SYSREGS_BASE	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findbase.pl $(KERNEL_DTB) 0 'arm,vexpress-sysreg' 2> /dev/null)
+-COUNTER_FREQ	:= 24000000
++COUNTER_FREQ	:= 100000000
+ 
+ CPU_IDS		:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findcpuids.pl $(KERNEL_DTB))
+ NR_CPUS         := $(shell echo $(CPU_IDS) | tr ',' ' ' | wc -w)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch
new file mode 100644
index 0000000..8d981f5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch
@@ -0,0 +1,52 @@
+From 6923f2a0c59cf92ba5ad50ec1d658a357b4ba5d7 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 10:48:39 +0800
+Subject: [PATCH] PSCI: Apply flush cache after setting branch_data
+
+For v8-R64, Hypervisor calls boot-wrapper's PSCI service using simple
+function call (instead of hvc).
+
+In this case, hypervisor's main core has enabled MPU and cache, but
+the secondary cores which are spinning have not enabled cache.
+That means if the main core set the branch_data to 1 to boot other
+cores, the secondary cores cannot see the change of branch_data and
+also cannot break the spin.
+
+Thus, the PSCI service in boot-wrapper needs a cache flush after
+setting branch_data in order to let other cores see the change.
+
+Issue-ID: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ifc282091c54d8fb2ffdb8cfa7fd3ffc1f4be717e
+---
+ common/psci.c | 6 ++++++
+ 1 file changed, 6 insertions(+)
+
+diff --git a/common/psci.c b/common/psci.c
+index 945780b..6efc695 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -24,12 +24,18 @@ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+ 
++static inline void flush_per_cpu_data(void *data)
++{
++	asm volatile ("dc cvac, %0" : : "r" (data));
++}
++
+ static int psci_store_address(unsigned int cpu, unsigned long address)
+ {
+ 	if (branch_table[cpu] != PSCI_ADDR_INVALID)
+ 		return PSCI_RET_ALREADY_ON;
+ 
+ 	branch_table[cpu] = address;
++	flush_per_cpu_data((void*)&(branch_table[cpu]));
+ 	return PSCI_RET_SUCCESS;
+ }
+ 
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch
new file mode 100644
index 0000000..97cd3cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch
@@ -0,0 +1,74 @@
+From ed46e83df2400b1b3f3364169aacf787bd91bd45 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 Jan 2022 14:56:36 +0800
+Subject: [PATCH] PSCI: Add function call entry point
+
+The max exception level of Armv8R AArch64 is EL2, which means it has no
+exclusive EL for firmware. That is, firmware and hypervisors have to share
+the EL2. Also, hypervisors cannot call firmware services via a 'smc'
+instruction. Thus, boot-wrapper has to provide a function entry point
+for Armv8R AArch64.
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I06ec8e50298603155c6d8ae2330e71db2f111182
+---
+ common/psci.c | 24 ++++++++++++++++++++----
+ 1 file changed, 20 insertions(+), 4 deletions(-)
+
+diff --git a/common/psci.c b/common/psci.c
+index 6efc695..8fdefb5 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -20,6 +20,8 @@
+ 
+ extern unsigned int spsr_to_elx;
+ 
++unsigned long flag_from_smc_fn[NR_CPUS];
++
+ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+@@ -49,12 +51,14 @@ static int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+ 		return PSCI_RET_INVALID_PARAMETERS;
+ 
+ 	bakery_lock(branch_table_lock, this_cpu);
+-	ret = psci_store_address(cpu, address);
+-	bakery_unlock(branch_table_lock, this_cpu);
+-
+ #ifdef KEEP_EL
+-	spsr_to_elx = SPSR_KERNEL_EL1;
++	if (!flag_from_smc_fn[this_cpu]) {
++		spsr_to_elx = SPSR_KERNEL_EL1;
++		flush_per_cpu_data((void*)&(spsr_to_elx));
++	}
+ #endif
++	ret = psci_store_address(cpu, address);
++	bakery_unlock(branch_table_lock, this_cpu);
+ 
+ 	return ret;
+ }
+@@ -90,6 +94,18 @@ long psci_call(unsigned long fid, unsigned long arg1, unsigned long arg2)
+ 	}
+ }
+ 
++long smc_fn_entry(unsigned long fid, unsigned long arg1, unsigned long arg2)
++{
++	long ret;
++	unsigned int this_cpu = this_cpu_logical_id();
++
++	flag_from_smc_fn[this_cpu] = 1;
++	ret = psci_call(fid, arg1, arg2);
++	flag_from_smc_fn[this_cpu] = 0;
++
++	return ret;
++}
++
+ void __noreturn psci_first_spin(unsigned int cpu)
+ {
+ 	if (cpu == MPIDR_INVALID)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch
new file mode 100644
index 0000000..1f10209
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch
@@ -0,0 +1,61 @@
+From 36b5fa3f4db49ac7aef42ff1d58a895226c7e96c Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 15:10:28 +0800
+Subject: [PATCH] lds: Rearrange and mark the sections
+
+To make it possible for the next stage to protect sections with MPU,
+boot-wrapper needs to provide the text and data section information.
+By rearranging the .data .rodata and .vector sections, all sections
+can be split into 2 big sections:
+ - RO and Executable
+ - RW and Non-Executable
+Add firmware_data to mark the boundry, thus:
+firmware_start to firmware_data - 1 indicates RO and Executable section,
+firmware_data to firmware_end - 1 indicates RW and Non-Executable
+section.
+
+Also, the firmware_data and firmware_end should align with 64 bytes,
+since Armv8R AArch64 MPU requires it.
+
+Issue-ID: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I55342aa7492f2c7b5c16ab9a6472c8cb45cff8fd
+---
+ model.lds.S | 7 ++++++-
+ 1 file changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/model.lds.S b/model.lds.S
+index ab98ddf..85451f9 100644
+--- a/model.lds.S
++++ b/model.lds.S
+@@ -63,12 +63,16 @@ SECTIONS
+ 	}
+ #endif
+ 
++#define FIRMWARE_ALIGN . = ALIGN(1 << 6)
+ 	.boot PHYS_OFFSET: {
+ 		PROVIDE(firmware_start = .);
+ 		*(.init)
+ 		*(.text*)
+-		*(.data* .rodata* .bss* COMMON)
+ 		*(.vectors)
++		*(.rodata*)
++		FIRMWARE_ALIGN;
++		PROVIDE(firmware_data = .);
++		*(.data* .bss* COMMON)
+ 		*(.stack)
+ 		PROVIDE(etext = .);
+ 	}
+@@ -77,6 +81,7 @@ SECTIONS
+ 		mbox = .;
+ 		QUAD(0x0)
+ 	}
++	FIRMWARE_ALIGN;
+ 	PROVIDE(firmware_end = .);
+ 
+ 	ASSERT(etext <= (PHYS_OFFSET + TEXT_LIMIT), ".text overflow!")
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch
new file mode 100644
index 0000000..cafcc09
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch
@@ -0,0 +1,345 @@
+From 8bdbb64d13f14d40546b71dbcfee2b2a8ea002a5 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 15:17:38 +0800
+Subject: [PATCH] common: Provide firmware info using libfdt
+
+Boot-wrapper uses libfdt to provide more info in device tree.
+We add a new node to include those new firmware relevant infomation.
+The new node defined as follows:
+  fw-shared-info {
+        compatible = "firmware,shared_info";
+
+        #address-cells = <0x02>;
+        #size-cells = <0x02>;
+
+        version = "1.0";
+        regions = <START_ADDR_HIGH START_ADDR_LOW SIZE_HIGH SIZE_LOW
+                   0x0 0x80000000 0x0 0x400000
+                   0x0 0x90000000 0x0 0x400000
+                   0x0 0xA0000000 0x0 0x400000>;
+        regions-permission = "RX", "R", "RWX", "RW";
+        regions-cache = "Cache", "NCache", "Cache", "Device"
+
+        function_entry = <ENTRY_ADDR_HIGH ENRTY_ADDR_LOW>;
+  };
+The node path is /fw-shared-info.
+For boot-wrapper, in real case, it will be:
+  fw-shared-info {
+        compatible = "firmware,shared_info";
+
+        #address-cells = <0x02>;
+        #size-cells = <0x02>;
+
+        version = "1.0";
+        regions = <0x0 firmware_start 0x0 firmware_code_size
+                   0x0 firmware_data 0x0 firmware_data_size>;
+        regions-permission = "RX", "RW";
+        regions-cache = "Cache", "Cache";
+
+        function_entry = <0x0 smc_fn_entry>;
+  };
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I6ebc59ce2bd3939b0fe066720d57821eaa1bed27
+---
+ common/device_tree.c | 271 ++++++++++++++++++++++++++++++++++++++++++-
+ 1 file changed, 270 insertions(+), 1 deletion(-)
+
+diff --git a/common/device_tree.c b/common/device_tree.c
+index 4d0876c..7f7befc 100644
+--- a/common/device_tree.c
++++ b/common/device_tree.c
+@@ -8,13 +8,225 @@
+  */
+ #include <libfdt.h>
+ 
++#define DEVICE_TREE_DEBUG 1
++
++#define FW_NODE_NAME	"/fw-shared-info"
++#define FW_COMPAT	"firmware,shared_info"
++#define FW_INFO_VER	"1.0"
++
++#ifdef BOOTWRAPPER_32
++#define CELL_NUM	1
++#define VAL_TYPE	uint32_t
++#else
++#define CELL_NUM	2
++#define VAL_TYPE	uint64_t
++#endif
++
++#define ALIGN(x)	(((x) + (FDT_TAGSIZE) - 1) & ~((FDT_TAGSIZE) - 1))
++
+ extern unsigned long dtb;
+-extern char firmware_start[], firmware_end[];
++extern char firmware_start[], firmware_data[], firmware_end[];
++
++extern long smc_fn_entry(unsigned long, unsigned long, unsigned long);
+ 
+ extern void print_string(const char *str);
++extern void print_hex(unsigned int val);
+ 
+ static void *blob;
+ 
++static char *realloc_node(char *fdt, const char *name)
++{
++	int delta;
++	int new_sz;
++	/* FDT_BEGIN_NODE, node name in off_struct and FDT_END_NODE */
++	delta = sizeof(struct fdt_node_header) + ALIGN(strlen(name) + 1)
++			+ FDT_TAGSIZE;
++	new_sz = fdt_totalsize(fdt) + delta;
++	fdt_open_into(fdt, fdt, new_sz);
++	return fdt;
++}
++
++static int create_node(const char *node_name)
++{
++	int node = 0;
++	char *p;
++
++	p = strrchr(node_name, '/');
++	if (!p) {
++		print_string("node name without '/'\r\n");
++		return -1;
++	}
++	*p = '\0';
++
++	blob = realloc_node(blob, p + 1);
++
++	if (p > node_name) {
++		node = fdt_path_offset(blob, node_name);
++		if (node < 0) {
++			print_string("no node name\r\n");
++			return -1;
++		}
++	}
++
++	node = fdt_add_subnode(blob, node, p + 1);
++	if (node < 0) {
++		print_string("add subnode err\r\n");
++		return -1;
++	}
++
++	return node;
++}
++
++static int dt_create_fw_node(void) {
++	int fw_node;
++
++	fw_node = fdt_path_offset(blob, FW_NODE_NAME);
++
++	if(fw_node < 0) {
++		fw_node = create_node(FW_NODE_NAME);
++	}
++
++	return fw_node;
++}
++
++static char *realloc_property(char *fdt, int nodeoffset, const char *name,
++			      int newlen)
++{
++	int delta = 0;
++	int oldlen = 0;
++	int new_sz;
++
++	if (!fdt_get_property(fdt, nodeoffset, name, &oldlen))
++		delta = sizeof(struct fdt_property) + strlen(name) + 1;
++
++	if (newlen > oldlen)
++		delta += ALIGN(newlen) - ALIGN(oldlen);
++
++	new_sz = fdt_totalsize(fdt) + delta;
++	fdt_open_into(fdt, fdt, new_sz);
++	return fdt;
++}
++
++static void dt_set_prop(int node, char *property, void *buf, int len)
++{
++	int err;
++
++	err = fdt_setprop(blob, node, property, buf, len);
++	if (err == -FDT_ERR_NOSPACE) {
++		blob = realloc_property(blob, node, property, len);
++		err = fdt_setprop(blob, node, property, buf, len);
++	}
++	if (err) {
++		print_string("fdt error\n\r");
++	}
++}
++
++static void dt_set_prop_u32(int node, char *property, uint32_t val)
++{
++	fdt32_t fdt_val = cpu_to_fdt32(val);
++	int len = sizeof(fdt32_t);
++
++	dt_set_prop(node, property, (void*)&fdt_val, len);
++}
++
++static void dt_set_prop_u64(int node, char *property, uint64_t val)
++{
++	fdt64_t fdt_val = cpu_to_fdt64(val);
++	int len = sizeof(fdt64_t);
++
++	dt_set_prop(node, property, (void*)&fdt_val, len);
++}
++
++/* This dt_set_prop_u32_array maybe unused according to the BOOTWRAPPER_32 */
++__attribute__((unused))
++static void dt_set_prop_u32_array(int node, char *property, uint32_t *vals,
++				       int size)
++{
++	fdt32_t *fdt_vals = (fdt32_t*)vals;
++	int len = sizeof(fdt32_t) * size;
++
++	for (int i = 0; i < size; i++) {
++		fdt_vals[i] = cpu_to_fdt32(vals[i]);
++	}
++
++	dt_set_prop(node, property, (void*)fdt_vals, len);
++}
++
++static void dt_set_prop_u64_array(int node, char *property, uint64_t *vals,
++				       int size)
++{
++	fdt64_t *fdt_vals = (fdt64_t*)vals;
++	int len = sizeof(fdt64_t) * size;
++
++	for (int i = 0; i < size; i++) {
++		fdt_vals[i] = cpu_to_fdt64(vals[i]);
++	}
++
++	dt_set_prop(node, property, (void*)fdt_vals, len);
++}
++
++#if DEVICE_TREE_DEBUG
++static void dt_dump_string(const void *s, int len)
++{
++	char *sub = (char*)s;
++	int sublen;
++	while(*sub && ((uint64_t)sub - (uint64_t)s) < len) {
++		sublen = strlen(sub) + 1;
++		print_string(sub);
++		print_string(" ");
++		sub += sublen;
++	}
++	print_string("\n\r");
++}
++
++static void dt_dump_fdt32_array(const void *vals, int len)
++{
++	fdt32_t *fdt_vals = (fdt32_t*)vals;
++	len = len / sizeof(fdt32_t);
++	for (int i = 0; i < len; i++) {
++		print_hex(fdt32_to_cpu(fdt_vals[i]));
++		print_string(" ");
++	}
++	print_string("\n\r");
++}
++
++static void dt_dump(int node, char *property, char type)
++{
++	const void *val;
++	int len;
++
++	val = fdt_getprop(blob, node, property, &len);
++	print_string(property);
++	print_string(": ");
++
++	if (type == 's') {
++		/* string type */
++		dt_dump_string(val, len);
++		return;
++	}
++
++	/* uint type */
++	dt_dump_fdt32_array(val, len);
++}
++
++void dt_dump_all(int node)
++{
++	if (node >= 0) {
++		print_string(FW_NODE_NAME" info:\r\n");
++		dt_dump(node, "compatible", 's');
++		dt_dump(node, "version", 's');
++		dt_dump(node, "function_entry", 'i');
++		dt_dump(node, "address-cells", 'i');
++		dt_dump(node, "size-cells", 'i');
++		dt_dump(node, "regions", 'i');
++		dt_dump(node, "regions-permission", 's');
++		dt_dump(node, "regions-cache", 's');
++		print_string("\r\n");
++	}
++}
++#else
++void dt_dump_all(int node) { (void*)node; return; }
++#endif
+ 
+ void dt_add_memreserve(void)
+ {
+@@ -32,3 +244,60 @@ void dt_add_memreserve(void)
+ 		print_string("reserve mem add err\n\r");
+ 	}
+ }
++
++void dt_fw_node_init(int enable)
++{
++	int fw_node;
++
++	VAL_TYPE regions[] = {
++		/* code region: start, end, ro, x, cachable */
++		(VAL_TYPE)firmware_start,
++		(VAL_TYPE)(firmware_data - firmware_start),
++		/* data region: start, end, rw, xn, cachable */
++		(VAL_TYPE)firmware_data,
++		(VAL_TYPE)(firmware_end - firmware_data),
++	};
++	int regions_num = sizeof(regions) / sizeof(VAL_TYPE);
++	char regions_permission[] = "RX\0RW";
++	char regions_cache[] = "Cache\0Cache";
++
++	if (!enable)
++		return;
++
++	print_string("Prepare "FW_NODE_NAME" node\n\r");
++
++	blob = (void*)&dtb;
++
++	if(fdt_path_offset(blob, "/psci") < 0) {
++		print_string("/psci node not found\n\r");
++		return;
++	}
++
++	fw_node = dt_create_fw_node();
++
++	if(fw_node < 0) {
++		print_string(FW_NODE_NAME" node create err\n\r");
++	}
++
++	dt_set_prop(fw_node, "compatible", FW_COMPAT, sizeof(FW_COMPAT));
++	dt_set_prop(fw_node, "version", FW_INFO_VER, sizeof(FW_INFO_VER));
++
++	dt_set_prop_u32(fw_node, "address-cells", CELL_NUM);
++	dt_set_prop_u32(fw_node, "size-cells", CELL_NUM);
++	dt_set_prop(fw_node, "regions-permission", regions_permission,
++		    sizeof(regions_permission));
++	dt_set_prop(fw_node, "regions-cache", regions_cache,
++		    sizeof(regions_cache));
++
++#ifdef BOOTWRAPPER_32
++	dt_set_prop_u32_array(fw_node, "regions", regions, regions_num);
++	dt_set_prop_u32(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry);
++#else
++	dt_set_prop_u64_array(fw_node, "regions", regions, regions_num);
++	dt_set_prop_u64(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry);
++#endif
++
++	fdt_pack(blob);
++
++	dt_dump_all(fw_node);
++}
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch
new file mode 100644
index 0000000..943afde
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch
@@ -0,0 +1,98 @@
+From 6dfc937d1ae54d2ae9f8c60ca29ba73ca14dc8c4 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 15:33:17 +0800
+Subject: [PATCH] boot: Enable firmware node initialization
+
+Enable the firmware node initialization, so that the next stage
+(hypervisor) could share the EL2 with firmware (boot-wrapper). The next
+stage (hypervisor) get the smccc entry point, code/data sections, the
+sections attrs and firmware node version and so on.
+It is worth noting that this EL2 sharing mechanism is only for Armv8R
+AArch64, thus add flag_v8r to record if the arch is Armv8R AArch64.
+Enable the firmware node initialization only if it is Armv8R AArch64.
+Also, we increase the stack size to 1024 to fix the stack overflow issue
+when using the libfdt.
+
+Add -fno-builtin options to CFLAGS to avoid the issue that the 'memset'
+in common/lib.c conflicts with builtin 'memset' function. GCC version
+>= 10 will have an incorrect compilation without -fno-builtin;
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ib274485a34d26215595fd0cd737be86610289817
+---
+ Makefile.am         | 4 ++--
+ arch/aarch64/boot.S | 6 ++++++
+ common/boot.c       | 4 ++++
+ 3 files changed, 12 insertions(+), 2 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 054becd..b01809c 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -23,7 +23,7 @@ DEFINES		+= -DCPU_IDS=$(CPU_IDS)
+ DEFINES		+= -DNR_CPUS=$(NR_CPUS)
+ DEFINES		+= $(if $(SYSREGS_BASE), -DSYSREGS_BASE=$(SYSREGS_BASE), )
+ DEFINES		+= -DUART_BASE=$(UART_BASE)
+-DEFINES		+= -DSTACK_SIZE=256
++DEFINES		+= -DSTACK_SIZE=1024
+ 
+ if KERNEL_32
+ DEFINES		+= -DKERNEL_32
+@@ -132,7 +132,7 @@ CHOSEN_NODE	:= chosen {						\
+ CPPFLAGS	+= $(INITRD_FLAGS)
+ CFLAGS		+= -I$(top_srcdir)/include/ -I$(top_srcdir)/$(ARCH_SRC)/include/
+ CFLAGS		+= -Wall -fomit-frame-pointer
+-CFLAGS 		+= -fno-stack-protector
++CFLAGS 		+= -fno-stack-protector -fno-builtin
+ CFLAGS		+= -ffunction-sections -fdata-sections
+ CFLAGS		+= -fno-pic -fno-pie
+ LDFLAGS		+= --gc-sections
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 157c097..f310387 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -240,6 +240,10 @@ el2_init:
+ #endif
+ 	ldr	x1, =spsr_to_elx
+ 	str	w0, [x1]
++
++	mov	w0, #1
++	ldr	x1, =flag_v8r
++	str	w0, [x1]
+ 	// fall through
+ 
+ el_max_init:
+@@ -319,3 +323,5 @@ flag_keep_el:
+ 	.long 0
+ ASM_DATA(spsr_to_elx)
+ 	.long 0
++ASM_DATA(flag_v8r)
++	.long 0
+diff --git a/common/boot.c b/common/boot.c
+index ee2bea0..38b2dca 100644
+--- a/common/boot.c
++++ b/common/boot.c
+@@ -11,6 +11,9 @@
+ 
+ extern unsigned long entrypoint;
+ extern unsigned long dtb;
++extern unsigned int  flag_v8r;
++
++extern void dt_fw_node_init(int enable);
+ 
+ void init_platform(void);
+ 
+@@ -64,6 +67,7 @@ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ 	if (cpu == 0) {
+ 		init_platform();
+ 		dt_add_memreserve();
++		dt_fw_node_init(flag_v8r == 1);
+ 
+ 		*mbox = (unsigned long)&entrypoint;
+ 		sevl();
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch
new file mode 100644
index 0000000..7094c8b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch
@@ -0,0 +1,252 @@
+From c8bd941579fb062359b683b184b851eea2ddb761 Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Fri, 4 Mar 2022 16:48:14 +0000
+Subject: [PATCH 1/5] feat: emulate cntp timer register accesses using cnthps
+
+Upstream-Status: Inappropriate [Experimental feature]
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Change-Id: I67508203273baf3bd8e6be2d99717028db945715
+---
+ Makefile                                |   3 +-
+ src/arch/aarch64/hypervisor/BUILD.gn    |   1 +
+ src/arch/aarch64/hypervisor/cpu.c       |  11 ++-
+ src/arch/aarch64/hypervisor/handler.c   |   6 ++
+ src/arch/aarch64/hypervisor/timer_el1.c | 104 ++++++++++++++++++++++++
+ src/arch/aarch64/hypervisor/timer_el1.h |  20 +++++
+ src/arch/aarch64/msr.h                  |   8 ++
+ 7 files changed, 150 insertions(+), 3 deletions(-)
+ create mode 100644 src/arch/aarch64/hypervisor/timer_el1.c
+ create mode 100644 src/arch/aarch64/hypervisor/timer_el1.h
+
+diff --git a/Makefile b/Makefile
+index c9fb16f..6371a8a 100644
+--- a/Makefile
++++ b/Makefile
+@@ -59,7 +59,8 @@ CHECKPATCH := $(CURDIR)/third_party/linux/scripts/checkpatch.pl \
+ # debug_el1.c : uses XMACROS, which checkpatch doesn't understand.
+ # perfmon.c : uses XMACROS, which checkpatch doesn't understand.
+ # feature_id.c : uses XMACROS, which checkpatch doesn't understand.
+-CHECKPATCH_IGNORE := "src/arch/aarch64/hypervisor/debug_el1.c\|src/arch/aarch64/hypervisor/perfmon.c\|src/arch/aarch64/hypervisor/feature_id.c"
++# timer_el1.c : uses XMACROS, which checkpatch doesn't understand.
++CHECKPATCH_IGNORE := "src/arch/aarch64/hypervisor/debug_el1.c\|src/arch/aarch64/hypervisor/perfmon.c\|src/arch/aarch64/hypervisor/feature_id.c\|src/arch/aarch64/hypervisor/timer_el1.c"
+ 
+ OUT ?= out/$(PROJECT)
+ OUT_DIR = out/$(PROJECT)
+diff --git a/src/arch/aarch64/hypervisor/BUILD.gn b/src/arch/aarch64/hypervisor/BUILD.gn
+index 6068d1e..de1a414 100644
+--- a/src/arch/aarch64/hypervisor/BUILD.gn
++++ b/src/arch/aarch64/hypervisor/BUILD.gn
+@@ -45,6 +45,7 @@ source_set("hypervisor") {
+     "handler.c",
+     "perfmon.c",
+     "psci_handler.c",
++    "timer_el1.c",
+     "vm.c",
+   ]
+ 
+diff --git a/src/arch/aarch64/hypervisor/cpu.c b/src/arch/aarch64/hypervisor/cpu.c
+index c6cebdd..cb41e6e 100644
+--- a/src/arch/aarch64/hypervisor/cpu.c
++++ b/src/arch/aarch64/hypervisor/cpu.c
+@@ -91,13 +91,20 @@ void arch_regs_reset(struct vcpu *vcpu)
+ 	if (is_primary) {
+ 		/*
+ 		 * cnthctl_el2 is redefined when VHE is enabled.
+-		 * EL1PCTEN, don't trap phys cnt access.
+-		 * EL1PCEN, don't trap phys timer access.
++		 * EL1PCTEN, don't trap phys cnt access. Except when in
++		 * secure world without vhe.
++		 * EL1PCEN, don't trap phys timer access. Except when in
++		 * secure world without vhe.
+ 		 */
+ 		if (has_vhe_support()) {
+ 			cnthctl |= (1U << 10) | (1U << 11);
+ 		} else {
++#if SECURE_WORLD == 1
++			cnthctl &= ~(1U << 0);
++			cnthctl &= ~(1U << 1);
++#else
+ 			cnthctl |= (1U << 0) | (1U << 1);
++#endif
+ 		}
+ 	}
+ 
+diff --git a/src/arch/aarch64/hypervisor/handler.c b/src/arch/aarch64/hypervisor/handler.c
+index cd64d68..c9068c5 100644
+--- a/src/arch/aarch64/hypervisor/handler.c
++++ b/src/arch/aarch64/hypervisor/handler.c
+@@ -34,6 +34,7 @@
+ #include "psci_handler.h"
+ #include "smc.h"
+ #include "sysregs.h"
++#include "timer_el1.h"
+ 
+ /**
+  * Hypervisor Fault Address Register Non-Secure.
+@@ -1276,6 +1277,11 @@ void handle_system_register_access(uintreg_t esr_el2)
+ 			inject_el1_unknown_exception(vcpu, esr_el2);
+ 			return;
+ 		}
++	} else if (timer_el1_is_register_access(esr_el2)) {
++		if (!timer_el1_process_access(vcpu, vm_id, esr_el2)) {
++			inject_el1_unknown_exception(vcpu, esr_el2);
++			return;
++		}
+ 	} else {
+ 		inject_el1_unknown_exception(vcpu, esr_el2);
+ 		return;
+diff --git a/src/arch/aarch64/hypervisor/timer_el1.c b/src/arch/aarch64/hypervisor/timer_el1.c
+new file mode 100644
+index 0000000..c30e554
+--- /dev/null
++++ b/src/arch/aarch64/hypervisor/timer_el1.c
+@@ -0,0 +1,104 @@
++/*
++ * Copyright 2022 The Hafnium Authors.
++ *
++ * Use of this source code is governed by a BSD-style
++ * license that can be found in the LICENSE file or at
++ * https://opensource.org/licenses/BSD-3-Clause.
++ */
++
++#include "timer_el1.h"
++
++#include "hf/dlog.h"
++
++#include "msr.h"
++#include "sysregs.h"
++
++/*
++ * Physical timer (CNTP) register encodings as defined in
++ * table D13-8 of the ARMv8 ARM (DDI0487F).
++ * TYPE, op0, op1, crn, crm, op2
++ * The register names are the concatenation of
++ * "CNTP_", TYPE and "_EL2".
++ */
++#define CNTP_REGISTERS          \
++	X(CTL,  3, 3, 14, 2, 1) \
++	X(CVAL, 3, 3, 14, 2, 2) \
++	X(TVAL, 3, 3, 14, 2, 0) \
++
++bool timer_el1_is_register_access(uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	bool is_timer_access;
++	switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                  \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		is_timer_access = true;                   \
++		break;
++			CNTP_REGISTERS
++#undef X
++	case (GET_ISS_ENCODING(3, 3, 14, 0, 1)):
++		is_timer_access = true;
++		break;
++	default:
++		is_timer_access = false;
++	}
++
++	return is_timer_access;
++}
++
++/* Accesses to CNTP timer emulated with CNTHPS */
++bool timer_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++			      uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	uintreg_t rt_register = GET_ISS_RT(esr);
++	uintreg_t value;
++
++	if (ISS_IS_READ(esr)) {
++		switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                           \
++		case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)):  \
++			value = read_msr(MSR_CNTHPS_##type##_EL2); \
++			vcpu->regs.r[rt_register] = value;         \
++			break;
++				CNTP_REGISTERS
++#undef X
++		case (GET_ISS_ENCODING(3, 3, 14, 0, 1)):
++			value = read_msr(cntpct_el0);
++			vcpu->regs.r[rt_register] = value;
++			break;
++		default:
++			dlog_notice(
++				"Unsupported timer register "
++				"read: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++	} else {
++		value = vcpu->regs.r[rt_register];
++		switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                           \
++		case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)):  \
++			write_msr(MSR_CNTHPS_##type##_EL2, value); \
++			break;
++				CNTP_REGISTERS
++#undef X
++		default:
++			dlog_notice(
++				"Unsupported timer register "
++				"write: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d, value=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr), value);
++			break;
++		}
++	}
++
++	return true;
++}
+diff --git a/src/arch/aarch64/hypervisor/timer_el1.h b/src/arch/aarch64/hypervisor/timer_el1.h
+new file mode 100644
+index 0000000..04a43b6
+--- /dev/null
++++ b/src/arch/aarch64/hypervisor/timer_el1.h
+@@ -0,0 +1,20 @@
++/*
++ * Copyright 2022 The Hafnium Authors.
++ *
++ * Use of this source code is governed by a BSD-style
++ * license that can be found in the LICENSE file or at
++ * https://opensource.org/licenses/BSD-3-Clause.
++ */
++
++#pragma once
++
++#include "hf/arch/types.h"
++
++#include "hf/cpu.h"
++
++#include "vmapi/hf/ffa.h"
++
++bool timer_el1_is_register_access(uintreg_t esr);
++
++bool timer_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++			      uintreg_t esr);
+diff --git a/src/arch/aarch64/msr.h b/src/arch/aarch64/msr.h
+index cd6778b..55e7833 100644
+--- a/src/arch/aarch64/msr.h
++++ b/src/arch/aarch64/msr.h
+@@ -126,3 +126,11 @@
+ #define MSR_ELR_EL12 S3_5_C4_C0_1
+ 
+ #endif
++
++/*
++ * Secure EL2 Physical timer (CNTHPS) register encodings as defined in
++ * table D13-8 of the ARMv8 ARM (DDI0487F).
++ */
++#define MSR_CNTHPS_CTL_EL2 S3_4_C14_C5_1
++#define MSR_CNTHPS_CVAL_EL2 S3_4_C14_C5_2
++#define MSR_CNTHPS_TVAL_EL2 S3_4_C14_C5_0
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch
new file mode 100644
index 0000000..2b57b23
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch
@@ -0,0 +1,152 @@
+From e6bcc390749f0560b3bc92507ecbaaabc7145200 Mon Sep 17 00:00:00 2001
+From: Lucian Paul-Trifu <lucian.paul-trifu@arm.com>
+Date: Wed, 10 Mar 2021 11:31:02 +0000
+Subject: [PATCH 2/5] fix(ff-a): Use FFA_INTERRUPT to signal an interrupted
+ FFA_MSG_WAIT
+
+Rather than FFA_ERROR(INTERRUPTED).
+
+Change-Id: I6b23a442714852b6183e0e46af6f0504ec0ee8f4
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ src/api.c                                                  | 2 +-
+ test/inc/test/vmapi/ffa.h                                  | 7 +++++++
+ test/vmapi/arch/aarch64/gicv3/services/common.c            | 3 +--
+ test/vmapi/arch/aarch64/gicv3/services/timer.c             | 2 +-
+ test/vmapi/el0_partitions/services/interruptible.c         | 3 +--
+ test/vmapi/el0_partitions/services/interruptible_echo.c    | 3 +--
+ test/vmapi/el0_partitions/services/receive_block.c         | 2 +-
+ .../primary_with_secondaries/services/interruptible.c      | 3 +--
+ .../primary_with_secondaries/services/receive_block.c      | 2 +-
+ 9 files changed, 15 insertions(+), 12 deletions(-)
+
+diff --git a/src/api.c b/src/api.c
+index b713b7c..00c4d44 100644
+--- a/src/api.c
++++ b/src/api.c
+@@ -1588,7 +1588,7 @@ struct ffa_value api_ffa_msg_recv(bool block, struct vcpu *current,
+ 	 * received. If a message is received the return value will be set at
+ 	 * that time to FFA_SUCCESS.
+ 	 */
+-	return_code = ffa_error(FFA_INTERRUPTED);
++	return_code = (struct ffa_value){.func = FFA_INTERRUPT_32};
+ 	if (api_ffa_msg_recv_block_interrupted(current)) {
+ 		goto out;
+ 	}
+diff --git a/test/inc/test/vmapi/ffa.h b/test/inc/test/vmapi/ffa.h
+index 8fc1223..f0f3e75 100644
+--- a/test/inc/test/vmapi/ffa.h
++++ b/test/inc/test/vmapi/ffa.h
+@@ -24,6 +24,13 @@
+ 		EXPECT_EQ(ffa_error_code(v), (ffa_error)); \
+ 	} while (0)
+ 
++#define EXPECT_FFA_INTERRUPT(value)                        \
++	do {                                               \
++		struct ffa_value v = (value);              \
++		EXPECT_EQ(v.func, FFA_INTERRUPT_32);       \
++	} while (0)
++
++
+ /*
+  * The bit 15 of the FF-A ID indicates whether the partition is executing
+  * in the normal world, in case it is a Virtual Machine (VM); or in the
+diff --git a/test/vmapi/arch/aarch64/gicv3/services/common.c b/test/vmapi/arch/aarch64/gicv3/services/common.c
+index 06df28d..4ada9e2 100644
+--- a/test/vmapi/arch/aarch64/gicv3/services/common.c
++++ b/test/vmapi/arch/aarch64/gicv3/services/common.c
+@@ -22,8 +22,7 @@ struct ffa_value mailbox_receive_retry(void)
+ 
+ 	do {
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 received.arg2 == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/arch/aarch64/gicv3/services/timer.c b/test/vmapi/arch/aarch64/gicv3/services/timer.c
+index 156f160..d5d2816 100644
+--- a/test/vmapi/arch/aarch64/gicv3/services/timer.c
++++ b/test/vmapi/arch/aarch64/gicv3/services/timer.c
+@@ -104,7 +104,7 @@ TEST_SERVICE(timer)
+ 		} else if (receive) {
+ 			struct ffa_value res = ffa_msg_wait();
+ 
+-			EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++			EXPECT_FFA_INTERRUPT(res);
+ 		} else {
+ 			/* Busy wait until the timer fires. */
+ 			while (!timer_fired) {
+diff --git a/test/vmapi/el0_partitions/services/interruptible.c b/test/vmapi/el0_partitions/services/interruptible.c
+index 0d00b16..4c9f099 100644
+--- a/test/vmapi/el0_partitions/services/interruptible.c
++++ b/test/vmapi/el0_partitions/services/interruptible.c
+@@ -50,8 +50,7 @@ static struct ffa_value mailbox_receive_retry()
+ 	do {
+ 		irq();
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 ffa_error_code(received) == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/el0_partitions/services/interruptible_echo.c b/test/vmapi/el0_partitions/services/interruptible_echo.c
+index b618cf2..a857783 100644
+--- a/test/vmapi/el0_partitions/services/interruptible_echo.c
++++ b/test/vmapi/el0_partitions/services/interruptible_echo.c
+@@ -39,8 +39,7 @@ static struct ffa_value mailbox_receive_retry()
+ 	do {
+ 		irq();
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 received.arg2 == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/el0_partitions/services/receive_block.c b/test/vmapi/el0_partitions/services/receive_block.c
+index 05a22f3..60da28b 100644
+--- a/test/vmapi/el0_partitions/services/receive_block.c
++++ b/test/vmapi/el0_partitions/services/receive_block.c
+@@ -27,7 +27,7 @@ TEST_SERVICE(receive_block)
+ 
+ 	for (i = 0; i < 10; ++i) {
+ 		struct ffa_value res = ffa_msg_wait();
+-		EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++		EXPECT_FFA_INTERRUPT(res);
+ 	}
+ 
+ 	memcpy_s(SERVICE_SEND_BUFFER(), FFA_MSG_PAYLOAD_MAX, message,
+diff --git a/test/vmapi/primary_with_secondaries/services/interruptible.c b/test/vmapi/primary_with_secondaries/services/interruptible.c
+index cc1c1f9..005d1ff 100644
+--- a/test/vmapi/primary_with_secondaries/services/interruptible.c
++++ b/test/vmapi/primary_with_secondaries/services/interruptible.c
+@@ -46,8 +46,7 @@ struct ffa_value mailbox_receive_retry()
+ 
+ 	do {
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 ffa_error_code(received) == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/primary_with_secondaries/services/receive_block.c b/test/vmapi/primary_with_secondaries/services/receive_block.c
+index edb4e3c..a6805ae 100644
+--- a/test/vmapi/primary_with_secondaries/services/receive_block.c
++++ b/test/vmapi/primary_with_secondaries/services/receive_block.c
+@@ -40,7 +40,7 @@ TEST_SERVICE(receive_block)
+ 
+ 	for (i = 0; i < 10; ++i) {
+ 		struct ffa_value res = ffa_msg_wait();
+-		EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++		EXPECT_FFA_INTERRUPT(res);
+ 	}
+ 
+ 	memcpy_s(SERVICE_SEND_BUFFER(), FFA_MSG_PAYLOAD_MAX, message,
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch
new file mode 100644
index 0000000..8d2cc13
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch
@@ -0,0 +1,28 @@
+From a6f466c2594b2f56d34fee72494fbd29ea9c7d21 Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Tue, 26 Apr 2022 12:59:42 +0000
+Subject: [PATCH 3/5] fix(ff-a): Add FFA_SECONDARY_EP_REGISTER_64 to list of
+ features
+
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Change-Id: Ic1344eb2c982c195210dc2c86aa6845f3e037077
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ src/api.c | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/src/api.c b/src/api.c
+index 00c4d44..33a9b42 100644
+--- a/src/api.c
++++ b/src/api.c
+@@ -2021,6 +2021,7 @@ struct ffa_value api_ffa_features(uint32_t feature_function_id)
+ 	case FFA_MEM_PERM_SET_32:
+ 	case FFA_MEM_PERM_GET_64:
+ 	case FFA_MEM_PERM_SET_64:
++	case FFA_SECONDARY_EP_REGISTER_64:
+ #endif
+ 		return (struct ffa_value){.func = FFA_SUCCESS_32};
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch
new file mode 100644
index 0000000..95f1651
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch
@@ -0,0 +1,159 @@
+From 380f2cf944dd5db36c168a11d31a46ad14cdcb6d Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 26 Apr 2022 14:43:58 +0100
+Subject: [PATCH 4/5] feat: emulate interrupt controller register access
+
+This emulates ICC_SGI1R_EL1 and ICC_IGRPEN1_EL1 register
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I0c11f034f3676067597461a183a341c809adcaa4
+Upstream-Status: Inappropriate [Experimental feature]
+---
+ src/arch/aarch64/hypervisor/handler.c |  5 ++
+ src/arch/aarch64/hypervisor/perfmon.c | 84 +++++++++++++++++++++++++++
+ src/arch/aarch64/hypervisor/perfmon.h |  5 ++
+ src/arch/aarch64/msr.h                |  3 +
+ 4 files changed, 97 insertions(+)
+
+diff --git a/src/arch/aarch64/hypervisor/handler.c b/src/arch/aarch64/hypervisor/handler.c
+index c9068c5..b9aa5d8 100644
+--- a/src/arch/aarch64/hypervisor/handler.c
++++ b/src/arch/aarch64/hypervisor/handler.c
+@@ -1282,6 +1282,11 @@ void handle_system_register_access(uintreg_t esr_el2)
+ 			inject_el1_unknown_exception(vcpu, esr_el2);
+ 			return;
+ 		}
++	} else if (intr_ctrl_is_register_access(esr_el2)) {
++		if (!intr_ctrl_el1_process_access(vcpu, vm_id, esr_el2)) {
++			inject_el1_unknown_exception(vcpu, esr_el2);
++			return;
++		}
+ 	} else {
+ 		inject_el1_unknown_exception(vcpu, esr_el2);
+ 		return;
+diff --git a/src/arch/aarch64/hypervisor/perfmon.c b/src/arch/aarch64/hypervisor/perfmon.c
+index f13b035..05e216c 100644
+--- a/src/arch/aarch64/hypervisor/perfmon.c
++++ b/src/arch/aarch64/hypervisor/perfmon.c
+@@ -116,6 +116,10 @@
+ 	X(PMEVTYPER30_EL0   , 3, 3, 14, 15, 6) \
+ 	X(PMCCFILTR_EL0     , 3, 3, 14, 15, 7)
+ 
++#define INTR_CTRL_REGISTERS                    \
++	X(ICC_IGRPEN1_EL1   , 3, 0, 12, 12, 7) \
++	X(ICC_SGI1R_EL1     , 3, 0, 12, 11, 5) \
++
+ /* clang-format on */
+ 
+ /**
+@@ -232,3 +236,83 @@ uintreg_t perfmon_get_pmccfiltr_el0_init_value(ffa_vm_id_t vm_id)
+ 
+ 	return 0;
+ }
++
++bool intr_ctrl_is_register_access(uintreg_t esr)
++{
++	uintreg_t op0 = GET_ISS_OP0(esr);
++	uintreg_t op1 = GET_ISS_OP1(esr);
++	uintreg_t crn = GET_ISS_CRN(esr);
++	uintreg_t crm = GET_ISS_CRM(esr);
++
++	if (op0 == 3 && op1 == 0 && crn == 12 && crm == 12) {
++		return true;
++	}
++
++	if (op0 == 3 && op1 == 0 && crn == 12 && crm == 11) {
++		return true;
++	}
++
++	return false;
++}
++
++bool intr_ctrl_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++				  uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	uintreg_t rt_register = GET_ISS_RT(esr);
++	uintreg_t value;
++
++	/* +1 because Rt can access register XZR */
++	CHECK(rt_register < NUM_GP_REGS + 1);
++
++	if (ISS_IS_READ(esr)) {
++		switch (sys_register) {
++#define X(reg_name, op0, op1, crn, crm, op2)              \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		value = read_msr(reg_name);               \
++		break;
++			INTR_CTRL_REGISTERS
++#undef X
++		default:
++			value = vcpu->regs.r[rt_register];
++			dlog_notice(
++				"Unsupported interrupt control register "
++				"read: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++		if (rt_register != RT_REG_XZR) {
++			vcpu->regs.r[rt_register] = value;
++		}
++	} else {
++		if (rt_register != RT_REG_XZR) {
++			value = vcpu->regs.r[rt_register];
++		} else {
++			value = 0;
++		}
++		switch (sys_register) {
++#define X(reg_name, op0, op1, crn, crm, op2)              \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		write_msr(reg_name, value);               \
++		break;
++			INTR_CTRL_REGISTERS
++#undef X
++		default:
++			dlog_notice(
++				"Unsupported interrupt control register "
++				"write: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++	}
++
++	return true;
++}
+diff --git a/src/arch/aarch64/hypervisor/perfmon.h b/src/arch/aarch64/hypervisor/perfmon.h
+index 81669ba..c90d45b 100644
+--- a/src/arch/aarch64/hypervisor/perfmon.h
++++ b/src/arch/aarch64/hypervisor/perfmon.h
+@@ -70,3 +70,8 @@ bool perfmon_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
+ 			    uintreg_t esr_el2);
+ 
+ uintreg_t perfmon_get_pmccfiltr_el0_init_value(ffa_vm_id_t vm_id);
++
++bool intr_ctrl_is_register_access(uintreg_t esr);
++
++bool intr_ctrl_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++				  uintreg_t esr);
+diff --git a/src/arch/aarch64/msr.h b/src/arch/aarch64/msr.h
+index 55e7833..82aa884 100644
+--- a/src/arch/aarch64/msr.h
++++ b/src/arch/aarch64/msr.h
+@@ -134,3 +134,6 @@
+ #define MSR_CNTHPS_CTL_EL2 S3_4_C14_C5_1
+ #define MSR_CNTHPS_CVAL_EL2 S3_4_C14_C5_2
+ #define MSR_CNTHPS_TVAL_EL2 S3_4_C14_C5_0
++
++#define ICC_IGRPEN1_EL1 S3_0_C12_C12_7
++#define ICC_SGI1R_EL1 S3_0_C12_C11_5
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch
new file mode 100644
index 0000000..e5f9489
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch
@@ -0,0 +1,318 @@
+From 64d5628c8439e4649e9c1da9b9e02ebd5c7fb8cf Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Thu, 28 Apr 2022 15:53:31 +0000
+Subject: [PATCH 5/5] Revert "fix(ff-a): check receiver's attributes on memory
+ retrieve"
+
+This reverts commit a98603aa965e3ff3ca5383249213e2fd1a96d850.
+
+Change-Id: Ia71ce3ac52e9b2e85578372c24eb8d593b62435f
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+---
+ src/ffa_memory.c                              | 76 ++++++-----------
+ test/vmapi/el0_partitions/memory_sharing.c    | 81 -------------------
+ .../primary_with_secondaries/memory_sharing.c | 81 -------------------
+ 3 files changed, 25 insertions(+), 213 deletions(-)
+
+diff --git a/src/ffa_memory.c b/src/ffa_memory.c
+index ab47929..2fcc386 100644
+--- a/src/ffa_memory.c
++++ b/src/ffa_memory.c
+@@ -1344,42 +1344,6 @@ static struct ffa_value ffa_memory_send_complete(
+ 	return ffa_mem_success(share_state->memory_region->handle);
+ }
+ 
+-/**
+- * Check that the memory attributes match Hafnium expectations:
+- * Normal Memory, Inner shareable, Write-Back Read-Allocate
+- * Write-Allocate Cacheable.
+- */
+-static struct ffa_value ffa_memory_attributes_validate(
+-	ffa_memory_access_permissions_t attributes)
+-{
+-	enum ffa_memory_type memory_type;
+-	enum ffa_memory_cacheability cacheability;
+-	enum ffa_memory_shareability shareability;
+-
+-	memory_type = ffa_get_memory_type_attr(attributes);
+-	if (memory_type != FFA_MEMORY_NORMAL_MEM) {
+-		dlog_verbose("Invalid memory type %#x, expected %#x.\n",
+-			     memory_type, FFA_MEMORY_NORMAL_MEM);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	cacheability = ffa_get_memory_cacheability_attr(attributes);
+-	if (cacheability != FFA_MEMORY_CACHE_WRITE_BACK) {
+-		dlog_verbose("Invalid cacheability %#x, expected %#x.\n",
+-			     cacheability, FFA_MEMORY_CACHE_WRITE_BACK);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	shareability = ffa_get_memory_shareability_attr(attributes);
+-	if (shareability != FFA_MEMORY_INNER_SHAREABLE) {
+-		dlog_verbose("Invalid shareability %#x, expected #%x.\n",
+-			     shareability, FFA_MEMORY_INNER_SHAREABLE);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	return (struct ffa_value){.func = FFA_SUCCESS_32};
+-}
+-
+ /**
+  * Check that the given `memory_region` represents a valid memory send request
+  * of the given `share_func` type, return the clear flag and permissions via the
+@@ -1400,7 +1364,10 @@ static struct ffa_value ffa_memory_send_validate(
+ 	uint32_t constituents_length;
+ 	enum ffa_data_access data_access;
+ 	enum ffa_instruction_access instruction_access;
+-	struct ffa_value ret;
++	ffa_memory_access_permissions_t attributes;
++	enum ffa_memory_type memory_type;
++	enum ffa_memory_cacheability memory_cacheability;
++	enum ffa_memory_shareability memory_shareability;
+ 
+ 	assert(permissions != NULL);
+ 
+@@ -1536,9 +1503,26 @@ static struct ffa_value ffa_memory_send_validate(
+ 	 * Normal Memory, Inner shareable, Write-Back Read-Allocate
+ 	 * Write-Allocate Cacheable.
+ 	 */
+-	ret = ffa_memory_attributes_validate(memory_region->attributes);
+-	if (ret.func != FFA_SUCCESS_32) {
+-		return ret;
++	attributes = memory_region->attributes;
++	memory_type = ffa_get_memory_type_attr(attributes);
++	if (memory_type != FFA_MEMORY_NORMAL_MEM) {
++		dlog_verbose("Invalid memory type %#x, expected %#x.\n",
++			     memory_type, FFA_MEMORY_NORMAL_MEM);
++		return ffa_error(FFA_INVALID_PARAMETERS);
++	}
++
++	memory_cacheability = ffa_get_memory_cacheability_attr(attributes);
++	if (memory_cacheability != FFA_MEMORY_CACHE_WRITE_BACK) {
++		dlog_verbose("Invalid cacheability %#x, expected %#x.\n",
++			     memory_cacheability, FFA_MEMORY_CACHE_WRITE_BACK);
++		return ffa_error(FFA_INVALID_PARAMETERS);
++	}
++
++	memory_shareability = ffa_get_memory_shareability_attr(attributes);
++	if (memory_shareability != FFA_MEMORY_INNER_SHAREABLE) {
++		dlog_verbose("Invalid shareability %#x, expected %#x.\n",
++			     memory_shareability, FFA_MEMORY_INNER_SHAREABLE);
++		return ffa_error(FFA_INVALID_PARAMETERS);
+ 	}
+ 
+ 	return (struct ffa_value){.func = FFA_SUCCESS_32};
+@@ -2376,6 +2360,7 @@ struct ffa_value ffa_memory_retrieve(struct vm_locked to_locked,
+ 	 * Check permissions from sender against permissions requested by
+ 	 * receiver.
+ 	 */
++	/* TODO: Check attributes too. */
+ 	sent_permissions =
+ 		memory_region->receivers[0].receiver_permissions.permissions;
+ 	sent_data_access = ffa_get_data_access_attr(sent_permissions);
+@@ -2453,17 +2438,6 @@ struct ffa_value ffa_memory_retrieve(struct vm_locked to_locked,
+ 		panic("Got unexpected FFA_INSTRUCTION_ACCESS_RESERVED. Should "
+ 		      "be checked before this point.");
+ 	}
+-
+-	/*
+-	 * Ensure receiver's attributes are compatible with how Hafnium maps
+-	 * memory: Normal Memory, Inner shareable, Write-Back Read-Allocate
+-	 * Write-Allocate Cacheable.
+-	 */
+-	ret = ffa_memory_attributes_validate(retrieve_request->attributes);
+-	if (ret.func != FFA_SUCCESS_32) {
+-		goto out;
+-	}
+-
+ 	memory_to_attributes = ffa_memory_permissions_to_mode(
+ 		permissions, share_state->sender_orig_mode);
+ 	ret = ffa_retrieve_check_update(
+diff --git a/test/vmapi/el0_partitions/memory_sharing.c b/test/vmapi/el0_partitions/memory_sharing.c
+index 3756d7d..c29f029 100644
+--- a/test/vmapi/el0_partitions/memory_sharing.c
++++ b/test/vmapi/el0_partitions/memory_sharing.c
+@@ -2160,87 +2160,6 @@ TEST(memory_sharing, ffa_validate_retrieve_req_mbz)
+ 	}
+ }
+ 
+-/**
+- * Memory can't be shared with arbitrary attributes because Hafnium maps pages
+- * with hardcoded values and doesn't support custom mappings.
+- */
+-TEST(memory_sharing, ffa_validate_retrieve_req_attributes)
+-{
+-	struct ffa_value ret;
+-	struct mailbox_buffers mb = set_up_mailbox();
+-	uint32_t msg_size;
+-	ffa_memory_handle_t handle;
+-
+-	struct ffa_value (*send_function[])(uint32_t, uint32_t) = {
+-		ffa_mem_share,
+-		ffa_mem_lend,
+-	};
+-
+-	struct ffa_memory_region_constituent constituents[] = {
+-		{.address = (uint64_t)pages, .page_count = 2},
+-		{.address = (uint64_t)pages + PAGE_SIZE * 3, .page_count = 1},
+-	};
+-
+-	SERVICE_SELECT(SERVICE_VM1, "ffa_memory_share_fail", mb.send);
+-
+-	struct {
+-		enum ffa_memory_type memory_type;
+-		enum ffa_memory_cacheability memory_cacheability;
+-		enum ffa_memory_shareability memory_shareability;
+-	} invalid_attributes[] = {
+-		/* Invalid memory type */
+-		{FFA_MEMORY_DEVICE_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid cacheability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_NON_CACHEABLE,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid shareability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_SHARE_NON_SHAREABLE},
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_OUTER_SHAREABLE}};
+-
+-	for (uint32_t i = 0; i < ARRAY_SIZE(send_function); i++) {
+-		/* Prepare memory region, and set all flags */
+-		EXPECT_EQ(ffa_memory_region_init(
+-				  mb.send, HF_MAILBOX_SIZE, HF_PRIMARY_VM_ID,
+-				  SERVICE_VM1, constituents,
+-				  ARRAY_SIZE(constituents), 0, 0,
+-				  FFA_DATA_ACCESS_RW,
+-				  FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				  FFA_MEMORY_NORMAL_MEM,
+-				  FFA_MEMORY_CACHE_WRITE_BACK,
+-				  FFA_MEMORY_INNER_SHAREABLE, NULL, &msg_size),
+-			  0);
+-
+-		ret = send_function[i](msg_size, msg_size);
+-		EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+-
+-		handle = ffa_mem_success_handle(ret);
+-
+-		for (uint32_t j = 0; j < ARRAY_SIZE(invalid_attributes); ++j) {
+-			msg_size = ffa_memory_retrieve_request_init(
+-				mb.send, handle, HF_PRIMARY_VM_ID, SERVICE_VM1,
+-				0, 0, FFA_DATA_ACCESS_RW,
+-				FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				invalid_attributes[j].memory_type,
+-				invalid_attributes[j].memory_cacheability,
+-				invalid_attributes[j].memory_shareability);
+-
+-			EXPECT_LE(msg_size, HF_MAILBOX_SIZE);
+-
+-			EXPECT_EQ(ffa_msg_send(HF_PRIMARY_VM_ID, SERVICE_VM1,
+-					       msg_size, 0)
+-					  .func,
+-				  FFA_SUCCESS_32);
+-
+-			ffa_run(SERVICE_VM1, 0);
+-		}
+-
+-		EXPECT_EQ(ffa_mem_reclaim(handle, 0).func, FFA_SUCCESS_32);
+-	}
+-}
+-
+ /**
+  * If memory is shared can't request zeroing of memory at both send and
+  * relinquish.
+diff --git a/test/vmapi/primary_with_secondaries/memory_sharing.c b/test/vmapi/primary_with_secondaries/memory_sharing.c
+index 6080709..4bcf252 100644
+--- a/test/vmapi/primary_with_secondaries/memory_sharing.c
++++ b/test/vmapi/primary_with_secondaries/memory_sharing.c
+@@ -2307,87 +2307,6 @@ TEST(memory_sharing, ffa_validate_retrieve_req_mbz)
+ 	}
+ }
+ 
+-/**
+- * Memory can't be shared with arbitrary attributes because Hafnium maps pages
+- * with hardcoded values and doesn't support custom mappings.
+- */
+-TEST(memory_sharing, ffa_validate_retrieve_req_attributes)
+-{
+-	struct ffa_value ret;
+-	struct mailbox_buffers mb = set_up_mailbox();
+-	uint32_t msg_size;
+-	ffa_memory_handle_t handle;
+-
+-	struct ffa_value (*send_function[])(uint32_t, uint32_t) = {
+-		ffa_mem_share,
+-		ffa_mem_lend,
+-	};
+-
+-	struct ffa_memory_region_constituent constituents[] = {
+-		{.address = (uint64_t)pages, .page_count = 2},
+-		{.address = (uint64_t)pages + PAGE_SIZE * 3, .page_count = 1},
+-	};
+-
+-	SERVICE_SELECT(SERVICE_VM1, "ffa_memory_share_fail_denied", mb.send);
+-
+-	struct {
+-		enum ffa_memory_type memory_type;
+-		enum ffa_memory_cacheability memory_cacheability;
+-		enum ffa_memory_shareability memory_shareability;
+-	} invalid_attributes[] = {
+-		/* Invalid memory type */
+-		{FFA_MEMORY_DEVICE_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid cacheability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_NON_CACHEABLE,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid shareability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_SHARE_NON_SHAREABLE},
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_OUTER_SHAREABLE}};
+-
+-	for (uint32_t i = 0; i < ARRAY_SIZE(send_function); i++) {
+-		/* Prepare memory region, and set all flags */
+-		EXPECT_EQ(ffa_memory_region_init(
+-				  mb.send, HF_MAILBOX_SIZE, HF_PRIMARY_VM_ID,
+-				  SERVICE_VM1, constituents,
+-				  ARRAY_SIZE(constituents), 0, 0,
+-				  FFA_DATA_ACCESS_RW,
+-				  FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				  FFA_MEMORY_NORMAL_MEM,
+-				  FFA_MEMORY_CACHE_WRITE_BACK,
+-				  FFA_MEMORY_INNER_SHAREABLE, NULL, &msg_size),
+-			  0);
+-
+-		ret = send_function[i](msg_size, msg_size);
+-		EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+-
+-		handle = ffa_mem_success_handle(ret);
+-
+-		for (uint32_t j = 0; j < ARRAY_SIZE(invalid_attributes); ++j) {
+-			msg_size = ffa_memory_retrieve_request_init(
+-				mb.send, handle, HF_PRIMARY_VM_ID, SERVICE_VM1,
+-				0, 0, FFA_DATA_ACCESS_RW,
+-				FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				invalid_attributes[j].memory_type,
+-				invalid_attributes[j].memory_cacheability,
+-				invalid_attributes[j].memory_shareability);
+-
+-			EXPECT_LE(msg_size, HF_MAILBOX_SIZE);
+-
+-			EXPECT_EQ(ffa_msg_send(HF_PRIMARY_VM_ID, SERVICE_VM1,
+-					       msg_size, 0)
+-					  .func,
+-				  FFA_SUCCESS_32);
+-
+-			ffa_run(SERVICE_VM1, 0);
+-		}
+-
+-		EXPECT_EQ(ffa_mem_reclaim(handle, 0).func, FFA_SUCCESS_32);
+-	}
+-}
+-
+ /**
+  * If memory is shared can't request zeroing of memory at both send and
+  * relinquish.
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch
new file mode 100644
index 0000000..671f6a5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch
@@ -0,0 +1,26 @@
+From e918cc5179241e1d35ba4b465b035b74b88e55d2 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 29 Apr 2022 20:07:50 +0100
+Subject: [PATCH] tc: increase heap pages
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ project/reference/BUILD.gn | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/project/reference/BUILD.gn b/project/reference/BUILD.gn
+index 5d84d13..4ea0890 100644
+--- a/project/reference/BUILD.gn
++++ b/project/reference/BUILD.gn
+@@ -233,7 +233,7 @@ aarch64_toolchains("secure_tc") {
+   gicd_base_address = "0x30000000"
+   gicr_base_address = "0x30080000"
+   gicr_frames = 8
+-  heap_pages = 60
++  heap_pages = 120
+   max_cpus = 8
+   max_vms = 16
+   branch_protection = "standard"
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc
new file mode 100644
index 0000000..c8f77dc
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc
@@ -0,0 +1,24 @@
+# TC specific configuration
+
+COMPATIBLE_MACHINE = "(tc?)"
+HAFNIUM_PLATFORM = "secure_tc"
+
+SRCREV = "4eb7b54348234d7f4bbac64bac28f683d6574ba9"
+FILESEXTRAPATHS:prepend:tc := "${THISDIR}/files/tc:"
+
+SRC_URI:remove = " \
+    file://host-ld.patch \
+    file://native-dtc.patch"
+
+SRC_URI:append = " \
+        file://0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch \
+        file://0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch \
+        file://0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch \
+        file://0004-feat-emulate-interrupt-controller-register-access.patch \
+        file://0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch \
+        file://0006-tc-increase-heap-pages.patch \
+        "
+
+do_compile() {
+    PATH="${S}/prebuilts/linux-x64/clang/bin:$PATH" oe_runmake -C ${S}
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend
new file mode 100644
index 0000000..bfc2c46
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_HAFNIUM_REQUIRE ?= ""
+MACHINE_HAFNIUM_REQUIRE:tc = "hafnium-tc.inc"
+
+require ${MACHINE_HAFNIUM_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb
new file mode 100644
index 0000000..ad5ec95
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb
@@ -0,0 +1,13 @@
+SUMARY = "Corstone1000 platform Image"
+DESCRIPTION = "This is the main image which is the container of all the binaries \
+               generated for the Corstone1000 platform."
+LICENSE = "MIT"
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+inherit image
+inherit wic_nopt
+
+PACKAGE_INSTALL = ""
+
+IMAGE_FSTYPES += "wic wic.nopt"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb
new file mode 100644
index 0000000..b778a00
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb
@@ -0,0 +1,30 @@
+SUMARY = "Corstone1000 platform Initramfs Image"
+DESCRIPTION = "This is the main Linux image which includes an initramfs kernel/rootfs bundle."
+
+LICENSE = "MIT"
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+IMAGE_FSTYPES = "${INITRAMFS_FSTYPES}"
+
+inherit core-image
+
+# By default all basic packages required for a bootable system are installed
+# by core-image . These packages are: packagegroup-core-boot and
+# packagegroup-base-extended
+
+inherit image-buildinfo
+
+IMAGE_FEATURES += "debug-tweaks"
+
+#package management is not supported in corstone1000
+IMAGE_FEATURES:remove = "package-management"
+
+# all optee packages
+IMAGE_INSTALL += "optee-client"
+
+# FF-A Debugfs driver
+IMAGE_INSTALL += "ffa-debugfs-mod"
+
+# psa-arch-tests linux userspace application
+IMAGE_INSTALL += "secure-partitions-psa-api-tests"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb
new file mode 100644
index 0000000..80565af
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb
@@ -0,0 +1,79 @@
+DESCRIPTION = "Firmware Image for Juno to be copied to the Configuration \
+microSD card"
+
+LICENSE = "BSD-3-Clause"
+SECTION = "firmware"
+LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/BSD-3-Clause;md5=550794465ba0ec5312d6919e203a55f9"
+
+INHIBIT_DEFAULT_DEPS = "1"
+DEPENDS = "trusted-firmware-a virtual/kernel virtual/control-processor-firmware"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+COMPATIBLE_MACHINE = "juno"
+
+LINARO_RELEASE = "19.06"
+
+SRC_URI = "http://releases.linaro.org/members/arm/platforms/${LINARO_RELEASE}/juno-latest-oe-uboot.zip;subdir=${UNPACK_DIR} \
+    file://images-r0.txt \
+    file://images-r1.txt \
+    file://images-r2.txt \
+    file://uEnv.txt \
+"
+SRC_URI[md5sum] = "01b662b81fa409d55ff298238ad24003"
+SRC_URI[sha256sum] = "b8a3909bb3bc4350a8771b863193a3e33b358e2a727624a77c9ecf13516cec82"
+
+UNPACK_DIR = "juno-firmware-${LINARO_RELEASE}"
+
+inherit deploy nopackages
+
+do_configure[noexec] = "1"
+do_compile[noexec] = "1"
+
+# The ${D} is used as a temporary directory and we don't generate any
+# packages for this recipe.
+do_install() {
+    cp -a ${WORKDIR}/${UNPACK_DIR} ${D}
+    cp -f ${RECIPE_SYSROOT}/firmware/bl1-juno.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/bl1.bin
+
+    cp -f ${RECIPE_SYSROOT}/firmware/fip-juno.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/fip.bin
+
+    cp -f ${RECIPE_SYSROOT}/firmware/scp_romfw_bypass.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/scp_bl1.bin
+
+    # u-boot environment file
+    cp -f ${WORKDIR}/uEnv.txt ${D}/${UNPACK_DIR}/SOFTWARE/
+
+    # Juno images list file
+    cp -f ${WORKDIR}/images-r0.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262B/images.txt
+    cp -f ${WORKDIR}/images-r1.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262C/images.txt
+    cp -f ${WORKDIR}/images-r2.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262D/images.txt
+}
+
+do_deploy() {
+    # To avoid dependency loop between firmware-image-juno:do_install
+    # and virtual/kernel:do_deploy when INITRAMFS_IMAGE_BUNDLE = "1",
+    # we need to handle the kernel binaries copying in the do_deploy
+    # task.
+    for f in ${KERNEL_DEVICETREE}; do
+        install -m 755 -c ${DEPLOY_DIR_IMAGE}/$(basename $f) \
+            ${D}/${UNPACK_DIR}/SOFTWARE/.
+    done
+
+    if [ "${INITRAMFS_IMAGE_BUNDLE}" -eq 1 ]; then
+        cp -L -f ${DEPLOY_DIR_IMAGE}/Image-initramfs-juno.bin \
+            ${D}/${UNPACK_DIR}/SOFTWARE/Image
+    else
+        cp -L -f ${DEPLOY_DIR_IMAGE}/Image ${D}/${UNPACK_DIR}/SOFTWARE/
+    fi
+
+    # Compress the files
+    tar -C ${D}/${UNPACK_DIR} -zcvf ${WORKDIR}/${PN}.tar.gz ./
+
+    # Deploy the compressed archive to the deploy folder
+    install -D -p -m0644 ${WORKDIR}/${PN}.tar.gz ${DEPLOYDIR}/${PN}.tar.gz
+}
+do_deploy[depends] += "virtual/kernel:do_deploy"
+addtask deploy after do_install
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt
new file mode 100644
index 0000000..3b36ed1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno.dtb     ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt
new file mode 100644
index 0000000..5db13af
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno-r1.dtb  ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt
new file mode 100644
index 0000000..7c499bf
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno-r2.dtb  ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt
new file mode 100644
index 0000000..77c02e3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt
@@ -0,0 +1,11 @@
+uenvcmd=run mybootcmd
+mybootcmd=echo Loading custom boot command; \
+echo Loading kernel; \
+afs load ${kernel_name} ${kernel_addr_r} ; \
+if test $? -eq 1; then echo Loading ${kernel_alt_name} instead of ${kernel_name}; afs load ${kernel_alt_name} ${kernel_addr_r}; fi; \
+echo Loading device tree; \
+afs load  ${fdtfile} ${fdt_addr_r}; \
+if test $? -eq 1; then echo Loading ${fdt_alt_name} instead of ${fdtfile}; \
+afs load ${fdt_alt_name} ${fdt_addr_r}; fi; fdt addr ${fdt_addr_r}; fdt resize; \
+booti ${kernel_addr_r} - ${fdt_addr_r};
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb
new file mode 100644
index 0000000..dda99e7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb
@@ -0,0 +1,35 @@
+SUMMARY = "Board Firmware binaries for N1SDP"
+SECTION = "firmware"
+
+LICENSE = "STM-SLA0044-Rev5"
+LIC_FILES_CHKSUM = "file://LICENSES/STM.TXT;md5=cd18335eff80d0a690a650f0e6748baf"
+
+inherit deploy
+
+INHIBIT_DEFAULT_DEPS = "1"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+COMPATIBLE_MACHINE = "n1sdp"
+
+SRC_URI = "git://git.gitlab.arm.com/arm-reference-solutions/board-firmware.git;protocol=https;branch=n1sdp"
+
+SRCREV = "e6cd91c7a9733e501bc3b57ff6f9eb2461ffee54"
+
+S = "${WORKDIR}/git"
+
+INSTALL_DIR = "/n1sdp-board-firmware_source"
+
+do_install() {
+    rm -rf ${S}/SOFTWARE
+    install -d ${D}${INSTALL_DIR}
+    cp -Rp --no-preserve=ownership ${S}/* ${D}${INSTALL_DIR}
+}
+
+FILES:${PN} = "${INSTALL_DIR}"
+SYSROOT_DIRS += "${INSTALL_DIR}"
+
+do_deploy() {
+    install -d ${DEPLOYDIR}${INSTALL_DIR}
+    cp -Rp --no-preserve=ownership ${S}/* ${DEPLOYDIR}${INSTALL_DIR}
+}
+addtask deploy after do_install before do_build
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb
new file mode 100644
index 0000000..3ed71c5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb
@@ -0,0 +1,85 @@
+SUMMARY = "Firmware image recipe for generating SD-Card artifacts."
+
+inherit deploy nopackages
+
+DEPENDS = "trusted-firmware-a \
+           virtual/control-processor-firmware \
+           n1sdp-board-firmware"
+
+LICENSE = "MIT"
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+COMPATIBLE_MACHINE = "n1sdp"
+RM_WORK_EXCLUDE += "${PN}"
+do_configure[noexec] = "1"
+do_compile[noexec] = "1"
+do_install[noexec] = "1"
+
+FIRMWARE_DIR = "n1sdp-board-firmware_source"
+PRIMARY_DIR = "${WORKDIR}/n1sdp-board-firmware_primary"
+SECONDARY_DIR = "${WORKDIR}/n1sdp-board-firmware_secondary"
+
+SOC_BINARIES = "mcp_fw.bin scp_fw.bin mcp_rom.bin scp_rom.bin"
+
+prepare_package() {
+    cd ${WORKDIR}
+
+    # Master/Primary
+    cp -av ${RECIPE_SYSROOT}/${FIRMWARE_DIR}/* ${PRIMARY_DIR}
+    mkdir -p ${PRIMARY_DIR}/SOFTWARE/
+
+    # Copy FIP binary
+    cp -v ${RECIPE_SYSROOT}/firmware/fip.bin ${PRIMARY_DIR}/SOFTWARE/
+
+    # Copy SOC binaries
+    for f in ${SOC_BINARIES}; do
+        cp -v ${RECIPE_SYSROOT}/firmware/${f} ${PRIMARY_DIR}/SOFTWARE/
+    done
+
+    sed -i -e 's|^C2C_ENABLE.*|C2C_ENABLE: TRUE            ;C2C enable TRUE/FALSE|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|^C2C_SIDE.*|C2C_SIDE: MASTER            ;C2C side SLAVE/MASTER|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|.*SOCCON: 0x1170.*PLATFORM_CTRL.*|SOCCON: 0x1170 0x00000100   ;SoC SCC PLATFORM_CTRL|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+
+    # Update load address for trusted boot
+    sed -i -e '/^IMAGE4ADDRESS:/ s|0x60200000|0x64200000|' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e '/^IMAGE4UPDATE:/ s|FORCE   |SCP_AUTO|' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e '/^IMAGE4FILE: \\SOFTWARE\\/s|uefi.bin|fip.bin |' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+
+    # Slave/Secondary
+    cp -av ${RECIPE_SYSROOT}/${FIRMWARE_DIR}/* ${SECONDARY_DIR}
+    mkdir -p ${SECONDARY_DIR}/SOFTWARE/
+
+    # Copy SOC binaries
+    for f in ${SOC_BINARIES}; do
+        cp -v ${RECIPE_SYSROOT}/firmware/${f} ${SECONDARY_DIR}/SOFTWARE/
+    done
+
+    sed -i -e 's|^C2C_ENABLE.*|C2C_ENABLE: TRUE            ;C2C enable TRUE/FALSE|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|^C2C_SIDE.*|C2C_SIDE: SLAVE             ;C2C side SLAVE/MASTER|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|.*SOCCON: 0x1170.*PLATFORM_CTRL.*|SOCCON: 0x1170 0x00000101   ;SoC SCC PLATFORM_CTRL|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e '/^TOTALIMAGES:/ s|5|4|' ${SECONDARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e 's|^IMAGE4|;&|' ${SECONDARY_DIR}/MB/HBI0316A/images.txt
+}
+
+do_deploy() {
+    # prepare Master & Slave packages
+    prepare_package
+
+    for dir in ${PRIMARY_DIR} ${SECONDARY_DIR}; do
+        dir_name=$(basename ${dir})
+        mkdir -p ${D}/${dir_name}
+        cp -av ${dir} ${D}
+
+        # Compress the files
+        tar -C ${D}/${dir_name} -zcvf ${DEPLOYDIR}/${dir_name}.tar.gz ./
+    done
+}
+do_deploy[dirs] += "${PRIMARY_DIR} ${SECONDARY_DIR}"
+do_deploy[cleandirs] += "${PRIMARY_DIR} ${SECONDARY_DIR}"
+do_deploy[umask] = "022"
+addtask deploy after do_prepare_recipe_sysroot
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb
new file mode 100644
index 0000000..ded7404
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2020 Arm Limited
+#
+SUMMARY = "Total Compute Images"
+DESCRIPTION = "Build all the images required for Total Compute platform"
+LICENSE = "Apache-2.0"
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+inherit nopackages
+
+# The last image to be built is trusted-firmware-a
+DEPENDS += " trusted-firmware-a"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch
new file mode 100644
index 0000000..f2044a9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch
@@ -0,0 +1,92 @@
+From 736bd8aeceefd474c15a97e4a4ec99f07ef9a82c Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 11 Feb 2022 18:28:43 +0000
+Subject: [PATCH 2/4] tc0: fix mpmm config
+
+Do not enable MPMM in standard features set.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I7b273a2055452e2e8cd78a0d932514a6f2947ec5
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ product/tc0/scp_ramfw/config_mpmm.c | 15 ---------------
+ 1 file changed, 15 deletions(-)
+
+diff --git a/product/tc0/scp_ramfw/config_mpmm.c b/product/tc0/scp_ramfw/config_mpmm.c
+index 3bfe99d3..13d866a5 100644
+--- a/product/tc0/scp_ramfw/config_mpmm.c
++++ b/product/tc0/scp_ramfw/config_mpmm.c
+@@ -27,7 +27,6 @@ enum core_pd_idx {
+     CORE7_IDX
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static struct mod_mpmm_pct_table k_pct[] = {
+     { .cores_online = 4,
+       .default_perf_limit = 1153 * 1000000UL,
+@@ -115,7 +114,6 @@ static struct mod_mpmm_pct_table m_pct[] = {
+                           },
+                         } },
+ };
+-#endif
+ 
+ static struct mod_mpmm_pct_table m_elp_pct[] = {
+     { .cores_online = 1,
+@@ -132,7 +130,6 @@ static struct mod_mpmm_pct_table m_elp_pct[] = {
+                         } },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static const struct mod_mpmm_core_config k_core_config[] = {
+     [0] = {
+         .pd_id = FWK_ID_ELEMENT_INIT(FWK_MODULE_IDX_POWER_DOMAIN, CORE0_IDX),
+@@ -180,7 +177,6 @@ static const struct mod_mpmm_core_config m_core_config[] = {
+         .core_starts_online = false,
+         },
+ };
+-#endif
+ 
+ static const struct mod_mpmm_core_config m_elp_core_config[] = {
+     [0] = {
+@@ -191,7 +187,6 @@ static const struct mod_mpmm_core_config m_elp_core_config[] = {
+         },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static const struct mod_mpmm_domain_config k_domain_conf[] = {
+     [0] = {
+         .perf_id = FWK_ID_ELEMENT_INIT(
+@@ -219,7 +214,6 @@ static const struct mod_mpmm_domain_config m_domain_conf[] = {
+     },
+     [1] = {0},
+ };
+-#endif
+ 
+ static const struct mod_mpmm_domain_config m_elp_domain_conf[] = {
+     [0] = {
+@@ -236,14 +230,6 @@ static const struct mod_mpmm_domain_config m_elp_domain_conf[] = {
+ };
+ 
+ static const struct fwk_element element_table[] = {
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
+-    [0] = {
+-        .name = "MPMM_MATTERHORN_ELP_ARM_ELEM",
+-        .sub_element_count = 1,
+-        .data = m_elp_domain_conf,
+-    },
+-    [1] = { 0 },
+-#else
+     [0] = {
+         .name = "MPMM_KLEIN_ELEM",
+         .sub_element_count = 4,
+@@ -260,7 +246,6 @@ static const struct fwk_element element_table[] = {
+         .data = m_elp_domain_conf,
+     },
+     [3] = { 0 },
+-#endif
+ };
+ 
+ static const struct fwk_element *mpmm_get_element_table(fwk_id_t module_id)
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch
new file mode 100644
index 0000000..87dfbfa
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch
@@ -0,0 +1,203 @@
+From 50e63f11762348bcd95d809af248f620f03d9ce4 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 11 Feb 2022 18:16:54 +0000
+Subject: [PATCH 3/4] tc0: rename platform variant to platform feature set
+
+THe PLATFORM_VARIANT flag was added to differentiate the software
+features enabled in SCP firmware. But this flag misleads to a new
+variant of same platform. This commits renames PLATFORM_VARIANT to
+PLATFORM_FEATURE_SET
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I93c0bc3e11fe18192bb8246df851345bdc473974
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ product/tc0/doc/{variants.md => features.md} | 28 +++++++++-----------
+ product/tc0/scp_ramfw/CMakeLists.txt         | 26 +++---------------
+ product/tc0/scp_ramfw/Firmware.cmake         |  2 +-
+ product/tc0/scp_ramfw/config_scmi_perf.c     |  8 +++---
+ product/tc0/scp_romfw/CMakeLists.txt         |  6 ++---
+ product/tc0/scp_romfw/Firmware.cmake         |  2 +-
+ 6 files changed, 25 insertions(+), 47 deletions(-)
+ rename product/tc0/doc/{variants.md => features.md} (77%)
+
+diff --git a/product/tc0/doc/variants.md b/product/tc0/doc/features.md
+similarity index 77%
+rename from product/tc0/doc/variants.md
+rename to product/tc0/doc/features.md
+index fbf616db..3ef520e2 100644
+--- a/product/tc0/doc/variants.md
++++ b/product/tc0/doc/features.md
+@@ -1,4 +1,4 @@
+-# TC0 Platform Variants
++# TC0 Platform Features
+ 
+ Copyright (c) 2022, Arm Limited. All rights reserved.
+ 
+@@ -7,30 +7,27 @@ Copyright (c) 2022, Arm Limited. All rights reserved.
+ 
+ Documentation for TC0 platform can be found at [1].
+ 
++### Standard
++
++The standard build provides all the features described in [1].
++For this default features, it's not required to provide any extra parameters in
++the build commands.
++
++### MPMM/Power/Performance (Experimental)
++
+ For the purpose of experimenting some of the software features that have been
+-introduced in SCP-firmware a new variant of TC0 has been created.
+-The variant(s) can be chosen at build time by adding:
++introduced in SCP-firmware of TC0. This can be enabled at build time, by adding:
+ 
+ ```sh
+ 
+ make -f Makefile.cmake \
+     PRODUCT=tc0 \
+     MODE=<debug,release> \
+-    PLATFORM_VARIANT=<0,1>
++    EXTRA_CONFIG_ARGS+=-DSCP_PLATFORM_FEATURE_SET=1
+ 
+ ```
+ 
+-
+-### Variant 0 (Standard build)
+-
+-The standard build provides all the features described in [1].
+-For this default variant, it's not required to provide any extra parameters in
+-the build commands.
+-
+-
+-### Variant 1 (Power/Performance testing)
+-
+-This variant adds support for the following software features:
++This adds support for the following software features:
+ - Traffic Cop
+ - MPMM (Maximum Power Mitigation Mechanism)
+ - Thermal Management
+@@ -63,7 +60,6 @@ Once built, the features above will act as:
+ 
+ ## Limitations
+ 
+-- The "variant" option is available only with the CMake build.
+ - The Thermal functionality is limited at this time cause the constant
+   temperature being sampled.
+ 
+diff --git a/product/tc0/scp_ramfw/CMakeLists.txt b/product/tc0/scp_ramfw/CMakeLists.txt
+index 96310320..ce3178ee 100644
+--- a/product/tc0/scp_ramfw/CMakeLists.txt
++++ b/product/tc0/scp_ramfw/CMakeLists.txt
+@@ -11,25 +11,13 @@
+ 
+ add_executable(tc0-bl2)
+ 
++set(SCP_PLATFORM_FEATURE_SET ${SCP_PLATFORM_FEATURE_SET_INIT} CACHE STRING "1")
+ 
+-# SCP_PLATFORM_VARIANT options:
+-# - 'TC0_VARIANT_STD' for TC0 standard build
+-# - 'TC0_VAR_EXPERIMENT_POWER' for TC0 with power/performance plugins used for
+-#   evaluation purposes
+-
+-
+-target_compile_definitions(tc0-bl2 PUBLIC -DTC0_VARIANT_STD=0)
+-target_compile_definitions(tc0-bl2 PUBLIC -DTC0_VAR_EXPERIMENT_POWER=1)
+-
+-
+-set(SCP_PLATFORM_VARIANT ${SCP_PLATFORM_VARIANT_INIT} CACHE STRING "1")
+-
+-
+-if (SCP_PLATFORM_VARIANT STREQUAL "1")
+-    message(NOTICE "SCP_PLATFORM_VARIANT set to EXPERIMENT_POWER (tc0-bl2)\n")
++if (SCP_PLATFORM_FEATURE_SET STREQUAL "1")
++    message(NOTICE "TC0 platform features MPMM/POWER/PERFORMANCE is experimental (tc0-bl2)\n")
+ 
+     target_compile_definitions(tc0-bl2
+-        PUBLIC -DPLATFORM_VARIANT=TC0_VAR_EXPERIMENT_POWER)
++	PUBLIC -DTC0_FEATURES_MPMM_POWER_PERF)
+ 
+     set(SCP_ENABLE_PLUGIN_HANDLER TRUE PARENT_SCOPE)
+     set(SCP_ENABLE_FAST_CHANNELS TRUE PARENT_SCOPE)
+@@ -56,12 +44,6 @@ if (SCP_PLATFORM_VARIANT STREQUAL "1")
+     list(PREPEND SCP_MODULE_PATHS
+         "${CMAKE_CURRENT_LIST_DIR}/../module/tc0_power_model")
+     target_sources(tc0-bl2 PRIVATE "config_tc0_power_model.c")
+-
+-else()
+-    message(NOTICE "SCP_PLATFORM_VARIANT set to STANDARD (tc0-bl2)\n")
+-
+-    target_compile_definitions(tc0-bl2
+-        PUBLIC -DPLATFORM_VARIANT=TC0_VARIANT_STD)
+ endif()
+ 
+ 
+diff --git a/product/tc0/scp_ramfw/Firmware.cmake b/product/tc0/scp_ramfw/Firmware.cmake
+index 11d8eaab..4a555296 100644
+--- a/product/tc0/scp_ramfw/Firmware.cmake
++++ b/product/tc0/scp_ramfw/Firmware.cmake
+@@ -27,7 +27,7 @@ set(SCP_ENABLE_FAST_CHANNELS_INIT FALSE)
+ 
+ set(SCP_ENABLE_PLUGIN_HANDLER_INIT FALSE)
+ 
+-set(SCP_PLATFORM_VARIANT_INIT 0)
++set(SCP_PLATFORM_FEATURE_SET_INIT 0)
+ 
+ set(SCP_ARCHITECTURE "armv7-m")
+ 
+diff --git a/product/tc0/scp_ramfw/config_scmi_perf.c b/product/tc0/scp_ramfw/config_scmi_perf.c
+index a4a47b3a..3e91939a 100644
+--- a/product/tc0/scp_ramfw/config_scmi_perf.c
++++ b/product/tc0/scp_ramfw/config_scmi_perf.c
+@@ -129,7 +129,7 @@ static const struct mod_scmi_perf_domain_config domains[] = {
+     },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
++#ifdef TC0_FEATURES_MPMM_POWER_PERF
+ static const struct mod_scmi_plugin_config plugins_table[] = {
+     [0] = {
+         .id = FWK_ID_MODULE_INIT(FWK_MODULE_IDX_TRAFFIC_COP),
+@@ -156,9 +156,9 @@ const struct fwk_module_config config_scmi_perf = {
+ #else
+         .fast_channels_alarm_id = FWK_ID_NONE_INIT,
+ #endif
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
+-        .plugins = plugins_table,
+-        .plugins_count = FWK_ARRAY_SIZE(plugins_table),
++#ifdef TC0_FEATURES_MPMM_POWER_PERF
++	.plugins = plugins_table,
++	.plugins_count = FWK_ARRAY_SIZE(plugins_table),
+ #endif
+     })
+ };
+diff --git a/product/tc0/scp_romfw/CMakeLists.txt b/product/tc0/scp_romfw/CMakeLists.txt
+index f9f40ad3..09cd2f5d 100644
+--- a/product/tc0/scp_romfw/CMakeLists.txt
++++ b/product/tc0/scp_romfw/CMakeLists.txt
+@@ -48,6 +48,6 @@ target_include_directories(tc0-bl1
+     PUBLIC $<TARGET_PROPERTY:cmsis::core-m,INTERFACE_INCLUDE_DIRECTORIES>)
+ 
+ cmake_dependent_option(
+-    SCP_PLATFORM_VARIANT "Choose platform software variant?"
+-    "${SCP_PLATFORM_VARIANT_INIT}" "DEFINED SCP_PLATFORM_VARIANT_INIT"
+-    "${SCP_PLATFORM_VARIANT}")
++    SCP_PLATFORM_FEATURE_SET "Choose platform software features?"
++    "${SCP_PLATFORM_FEATURE_SET_INIT}" "DEFINED SCP_PLATFORM_FEATURE_SET_INIT"
++    "${SCP_PLATFORM_FEATURE_SET}")
+diff --git a/product/tc0/scp_romfw/Firmware.cmake b/product/tc0/scp_romfw/Firmware.cmake
+index ab4468be..e1360159 100644
+--- a/product/tc0/scp_romfw/Firmware.cmake
++++ b/product/tc0/scp_romfw/Firmware.cmake
+@@ -21,7 +21,7 @@ set(SCP_ENABLE_NOTIFICATIONS_INIT TRUE)
+ 
+ set(SCP_ENABLE_IPO_INIT FALSE)
+ 
+-set(SCP_PLATFORM_VARIANT_INIT 0)
++set(SCP_PLATFORM_FEATURE_SET_INIT 0)
+ 
+ set(SCP_ARCHITECTURE "armv7-m")
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch
new file mode 100644
index 0000000..aa83332
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch
@@ -0,0 +1,114 @@
+From 3e737dd47b228bdeffb06e39bffec7a4a436b244 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 9 Feb 2022 16:02:10 +0000
+Subject: [PATCH 4/4] tc0: support platform feature set options in firmware.mk
+
+Support existing platform feature set options that is in cmake to
+firmware.mk. Two feature set for TC0 are
+0. Standard
+1. MPMM/Power/Performance (Experimental)
+
+Build option to select the feature set is using:
+make PRODUCT=tc0 MODE=<debug,release> SCP_PLATFORM_FEATURE_SET=<0,1>
+
+The default value is set to 0 (Standard).
+Refer product/tc0/doc/features.md for more details.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I4028686a8f8461e0e2c29e15d5e52eb1d37ca60a
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ product/tc0/scp_ramfw/firmware.mk | 41 +++++++++++++++++++++++++++++--
+ product/tc0/scp_romfw/firmware.mk | 12 +++++++++
+ 2 files changed, 51 insertions(+), 2 deletions(-)
+
+diff --git a/product/tc0/scp_ramfw/firmware.mk b/product/tc0/scp_ramfw/firmware.mk
+index ec6e6679..d7515f5b 100644
+--- a/product/tc0/scp_ramfw/firmware.mk
++++ b/product/tc0/scp_ramfw/firmware.mk
+@@ -9,8 +9,24 @@ BS_FIRMWARE_CPU := cortex-m3
+ BS_FIRMWARE_HAS_NOTIFICATION := yes
+ BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS := yes
+ BS_FIRMWARE_USE_NEWLIB_NANO_SPECS := yes
+-BS_FIRMWARE_HAS_FAST_CHANNELS := no
+-BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := no
++
++DEFAULT_SCP_PLATFORM_FEATURE_SET := 0
++
++export SCP_PLATFORM_FEATURE_SET ?= $(DEFAULT_SCP_PLATFORM_FEATURE_SET)
++ifneq ($(filter-out 0 1, $(SCP_PLATFORM_FEATURE_SET)),)
++    $(error "Invalid for SCP_PLATFORM_FEATURE_SET parameter. Valid options are \
++      0 or 1. Aborting...")
++endif
++
++ifeq ($(SCP_PLATFORM_FEATURE_SET),0)
++    BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := no
++    BS_FIRMWARE_HAS_FAST_CHANNELS := no
++else
++    DEFINES += TC0_FEATURES_MPMM_POWER_PERF
++    BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := yes
++    BS_FIRMWARE_HAS_FAST_CHANNELS := yes
++    $(info "TC0 platform features POWER/PERFORMANCE is experimental")
++endif
+ 
+ BS_FIRMWARE_MODULES := \
+     armv7m_mpu \
+@@ -44,6 +60,16 @@ ifeq ($(BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS),yes)
+     BS_FIRMWARE_MODULES += resource_perms
+ endif
+ 
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++BS_FIRMWARE_MODULES += \
++        traffic_cop \
++        mpmm \
++        sensor \
++        reg_sensor \
++        thermal_mgmt \
++        tc0_power_model
++endif
++
+ BS_FIRMWARE_SOURCES := \
+     config_system_power.c \
+     config_armv7m_mpu.c \
+@@ -75,4 +101,15 @@ ifeq ($(BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS),yes)
+     BS_FIRMWARE_SOURCES += config_resource_perms.c
+ endif
+ 
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++    BS_FIRMWARE_SOURCES += \
++        config_traffic_cop.c \
++        config_mpmm.c \
++        config_sensor.c \
++        config_reg_sensor.c \
++        config_thermal_mgmt.c \
++        config_tc0_power_model.c
++endif
++
++
+ include $(BS_DIR)/firmware.mk
+diff --git a/product/tc0/scp_romfw/firmware.mk b/product/tc0/scp_romfw/firmware.mk
+index 9977712f..0012b9fa 100644
+--- a/product/tc0/scp_romfw/firmware.mk
++++ b/product/tc0/scp_romfw/firmware.mk
+@@ -9,6 +9,18 @@ BS_FIRMWARE_CPU := cortex-m3
+ BS_FIRMWARE_HAS_NOTIFICATION := yes
+ BS_FIRMWARE_USE_NEWLIB_NANO_SPECS := yes
+ 
++DEFAULT_SCP_PLATFORM_FEATURE_SET := 0
++
++export SCP_PLATFORM_FEATURE_SET ?= $(DEFAULT_SCP_PLATFORM_FEATURE_SET)
++ifneq ($(filter-out 0 1, $(SCP_PLATFORM_FEATURE_SET)),)
++    $(error "Invalid for SCP_PLATFORM_FEATURE_SET parameter. Valid options are \
++      0 or 1. Aborting...")
++endif
++
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++    $(info "TC0 platform features POWER/PERFORMANCE is experimental")
++endif
++
+ BS_FIRMWARE_MODULE_HEADERS_ONLY := \
+     power_domain \
+     timer
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc
new file mode 100644
index 0000000..ea2face
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc
@@ -0,0 +1,16 @@
+# juno specific SCP configuration
+
+COMPATIBLE_MACHINE = "juno"
+
+SCP_PLATFORM = "juno"
+FW_TARGETS = "scp"
+FW_INSTALL:append = " romfw_bypass"
+
+do_install:append() {
+    for TYPE in ${FW_INSTALL}; do
+        if [ "$TYPE" = "romfw_bypass" ]; then
+            install -D "${B}/${TYPE}/${FW_TARGETS}/bin/${SCP_PLATFORM}-bl1-bypass.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+            install -D "${B}/${TYPE}/${FW_TARGETS}/bin/${SCP_PLATFORM}-bl1-bypass" "${D}/firmware/${FW}_${TYPE}.elf"
+        fi
+    done
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc
new file mode 100644
index 0000000..e66469c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc
@@ -0,0 +1,33 @@
+# N1SDP specific SCP configurations and build instructions
+
+SCP_PLATFORM  = "n1sdp"
+SCP_LOG_LEVEL = "INFO"
+
+# master branch at n1sdp: Introduce trusted board boot
+SRCREV  = "3e4c34ceccc1c960eb3a4adaa922f2a0c6b36be3"
+PV .= "+git${SRCPV}"
+
+COMPATIBLE_MACHINE:n1sdp = "n1sdp"
+
+DEPENDS += "fiptool-native"
+DEPENDS += "trusted-firmware-a"
+
+do_install:append() {
+   fiptool \
+       create \
+       --scp-fw "${D}/firmware/scp_ramfw.bin" \
+       --blob uuid=cfacc2c4-15e8-4668-82be-430a38fad705,file="${RECIPE_SYSROOT}/firmware/bl1.bin" \
+       "scp_fw.bin"
+
+   # This UUID is FIP_UUID_MCP_BL2 in SCP-Firmware.
+   fiptool \
+       create \
+       --blob uuid=54464222-a4cf-4bf8-b1b6-cee7dade539e,file="${D}/firmware/mcp_ramfw.bin" \
+       "mcp_fw.bin"
+
+   install "scp_fw.bin" "${D}/firmware/scp_fw.bin"
+   install "mcp_fw.bin" "${D}/firmware/mcp_fw.bin"
+
+   ln -sf "scp_romfw.bin" "${D}/firmware/scp_rom.bin"
+   ln -sf "mcp_romfw.bin" "${D}/firmware/mcp_rom.bin"
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc
new file mode 100644
index 0000000..e1b0a85
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc
@@ -0,0 +1,6 @@
+# SGI575 specific SCP configurations and build instructions
+
+SCP_PLATFORM  = "sgi575"
+SCP_LOG_LEVEL = "INFO"
+
+COMPATIBLE_MACHINE:sgi575 = "sgi575"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc
new file mode 100644
index 0000000..a6a005c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc
@@ -0,0 +1,14 @@
+# TC specific SCP configuration
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/tc:"
+SRC_URI:append:tc = " \
+    file://0002-tc0-fix-mpmm-config.patch \
+    file://0003-tc0-rename-platform-variant-to-platform-feature-set.patch \
+    file://0004-tc0-support-platform-feature-set-options-in-firmware.patch \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+SCP_PLATFORM:tc0 = "tc0"
+SCP_PLATFORM:tc1 = "tc1"
+FW_TARGETS = "scp"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend
new file mode 100644
index 0000000..bb1a48c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend
@@ -0,0 +1,10 @@
+# Include machine specific SCP configurations
+
+MACHINE_SCP_REQUIRE ?= ""
+
+MACHINE_SCP_REQUIRE:juno = "scp-firmware-juno.inc"
+MACHINE_SCP_REQUIRE:n1sdp = "scp-firmware-n1sdp.inc"
+MACHINE_SCP_REQUIRE:sgi575 = "scp-firmware-sgi575.inc"
+MACHINE_SCP_REQUIRE:tc = "scp-firmware-tc.inc"
+
+require ${MACHINE_SCP_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch
new file mode 100644
index 0000000..016de8d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch
@@ -0,0 +1,34 @@
+Upstream-Status: Inappropriate
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+From a31aee0988ef64724ec5866f10709f51f8cb3237 Mon Sep 17 00:00:00 2001
+From: emeara01 <emekcan.aras@arm.com>
+Date: Wed, 11 May 2022 14:37:06 +0100
+Subject: [PATCH] Fix FF-A version in SPMC manifest
+
+OPTEE does not support FF-A version 1.1 in SPMC at the moment.
+This commit corrects the FF-A version in corstone1000_spmc_manifest.dts.
+This patch will not be upstreamed and will be dropped once
+OPTEE version is updated for Corstone1000.
+
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+---
+ .../corstone1000/common/fdts/corstone1000_spmc_manifest.dts     | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts b/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
+index 8e49ab83f..5baa1b115 100644
+--- a/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
++++ b/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
+@@ -20,7 +20,7 @@
+ 	attribute {
+ 		spmc_id = <0x8000>;
+ 		maj_ver = <0x1>;
+-		min_ver = <0x1>;
++		min_ver = <0x0>;
+ 		exec_state = <0x0>;
+ 		load_address = <0x0 0x2002000>;
+ 		entrypoint = <0x0 0x2002000>;
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch
new file mode 100644
index 0000000..a5b3019
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch
@@ -0,0 +1,40 @@
+From 80b1efa92486a87f9e82dbf665ef612291148de8 Mon Sep 17 00:00:00 2001
+From: Adam Johnston <adam.johnston@arm.com>
+Date: Tue, 14 Jun 2022 11:19:30 +0000
+Subject: [PATCH] arm-bsp/trusted-firmware-a: N1SDP trusted boot
+
+Increase max size of BL2 on N1SDP by 4KB to enable trusted boot
+Decrease max size of BL1 on N1SDP by 8KB so BL1/BL2 fits above BL31 progbits
+
+Signed-off-by: Adam Johnston <adam.johnston@arm.com>
+Upstream-Status: Pending [Flagged to upstream]
+
+---
+ plat/arm/board/n1sdp/include/platform_def.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
+index c9b81bafa..7468a31ed 100644
+--- a/plat/arm/board/n1sdp/include/platform_def.h
++++ b/plat/arm/board/n1sdp/include/platform_def.h
+@@ -91,7 +91,7 @@
+  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+  * plus a little space for growth.
+  */
+-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xE000
++#define PLAT_ARM_MAX_BL1_RW_SIZE	0xC000
+ 
+ /*
+  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
+@@ -110,7 +110,7 @@
+  * little space for growth.
+  */
+ #if TRUSTED_BOARD_BOOT
+-# define PLAT_ARM_MAX_BL2_SIZE		0x20000
++# define PLAT_ARM_MAX_BL2_SIZE		0x21000
+ #else
+ # define PLAT_ARM_MAX_BL2_SIZE		0x14000
+ #endif
+-- 
+2.35.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch
new file mode 100644
index 0000000..74ab361
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch
@@ -0,0 +1,43 @@
+From 008cfc6457c239466ca62610d59aaf1a78f6b2f6 Mon Sep 17 00:00:00 2001
+From: Tudor Cretu <tudor.cretu@arm.com>
+Date: Fri, 21 May 2021 14:56:37 +0000
+Subject: [PATCH 1/7] plat: tc: Increase maximum BL2 size.
+
+BL2 size gets increased due to the firmware update changes.
+Increase the MAX_BL2_SIZE by 8Kb.
+
+Signed-off-by: Tudor Cretu <tudor.cretu@arm.com>
+Change-Id: I1cb28b0eb7f834426873ff9f4c40bd496413806f
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ plat/arm/board/tc/include/platform_def.h | 6 +++---
+ 1 file changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
+index 745d91cab..cd77773aa 100644
+--- a/plat/arm/board/tc/include/platform_def.h
++++ b/plat/arm/board/tc/include/platform_def.h
+@@ -120,9 +120,9 @@
+  * little space for growth.
+  */
+ #if TRUSTED_BOARD_BOOT
+-# define PLAT_ARM_MAX_BL2_SIZE		0x20000
++# define PLAT_ARM_MAX_BL2_SIZE		0x25000
+ #else
+-# define PLAT_ARM_MAX_BL2_SIZE		0x14000
++# define PLAT_ARM_MAX_BL2_SIZE		0x19000
+ #endif
+ 
+ /*
+@@ -130,7 +130,7 @@
+  * calculated using the current BL31 PROGBITS debug size plus the sizes of
+  * BL2 and BL1-RW
+  */
+-#define PLAT_ARM_MAX_BL31_SIZE		0x3F000
++#define PLAT_ARM_MAX_BL31_SIZE		0x4F000
+ 
+ /*
+  * Size of cacheable stacks
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch
new file mode 100644
index 0000000..75cabdd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch
@@ -0,0 +1,46 @@
+From 2f8b0cc6be3787717247d1c02a45181a5ac6f125 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 14:36:54 +0100
+Subject: [PATCH 2/7] Makefile: add trusty_sp_fw_config build option
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ief90ae9113d32265ee2200f35f3e517b7b9a4bea
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ Makefile                            | 4 ++++
+ docs/plat/arm/arm-build-options.rst | 4 ++++
+ 2 files changed, 8 insertions(+)
+
+diff --git a/Makefile b/Makefile
+index 3941f8698..a20d647a2 100644
+--- a/Makefile
++++ b/Makefile
+@@ -531,6 +531,10 @@ ifneq (${SPD},none)
+             DTC_CPPFLAGS	+=	-DOPTEE_SP_FW_CONFIG
+         endif
+ 
++        ifeq ($(findstring trusty_sp,$(ARM_SPMC_MANIFEST_DTS)),trusty_sp)
++            DTC_CPPFLAGS	+=	-DTRUSTY_SP_FW_CONFIG
++        endif
++
+         ifeq ($(TS_SP_FW_CONFIG),1)
+             DTC_CPPFLAGS	+=	-DTS_SP_FW_CONFIG
+         endif
+diff --git a/docs/plat/arm/arm-build-options.rst b/docs/plat/arm/arm-build-options.rst
+index 339ebbe33..3c9a41fb8 100644
+--- a/docs/plat/arm/arm-build-options.rst
++++ b/docs/plat/arm/arm-build-options.rst
+@@ -107,6 +107,10 @@ Arm Platform Build Options
+    device tree. This flag is defined only when ``ARM_SPMC_MANIFEST_DTS`` manifest
+    file name contains pattern optee_sp.
+ 
++-  ``TRUSTY_SP_FW_CONFIG``: DTC build flag to include Trusty as SP in
++   tb_fw_config device tree. This flag is defined only when
++   ``ARM_SPMC_MANIFEST_DTS`` manifest file name contains pattern trusty_sp.
++
+ -  ``TS_SP_FW_CONFIG``: DTC build flag to include Trusted Services (Crypto and
+    internal-trusted-storage) as SP in tb_fw_config device tree.
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch
new file mode 100644
index 0000000..6807191
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch
@@ -0,0 +1,30 @@
+From 0060b1a4fbe3bc9992f59a2d4cb986821f7bcf13 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 18:31:01 +0100
+Subject: [PATCH 3/7] fix(plat/arm): increase sp max image size
+
+Increase ARM_SP_MAX_SIZE to support Trusty image.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I9ef9e755769445aee998062a7fba508fad50b33e
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/plat/arm/common/fconf_arm_sp_getter.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/include/plat/arm/common/fconf_arm_sp_getter.h b/include/plat/arm/common/fconf_arm_sp_getter.h
+index aa628dfd3..3ed953d1c 100644
+--- a/include/plat/arm/common/fconf_arm_sp_getter.h
++++ b/include/plat/arm/common/fconf_arm_sp_getter.h
+@@ -13,7 +13,7 @@
+ /* arm_sp getter */
+ #define arm__sp_getter(prop)	arm_sp.prop
+ 
+-#define ARM_SP_MAX_SIZE		U(0xb0000)
++#define ARM_SP_MAX_SIZE		U(0x2000000)
+ #define ARM_SP_OWNER_NAME_LEN	U(8)
+ 
+ struct arm_sp_t {
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch
new file mode 100644
index 0000000..aec8be0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch
@@ -0,0 +1,69 @@
+From 000e19d360a5ad9abd7d823af86a364bac2afc58 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 17:38:17 +0100
+Subject: [PATCH 4/7] fix(plat/tc): increase tc_tzc_dram1_size
+
+Increase TC_TZC_DRAM1_SIZE for Trusty image and its memory size.
+Update OP-TEE reserved memory range in DTS
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Iad433c3c155f28860b15bde2398df653487189dd
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts                              |  4 ++--
+ plat/arm/board/tc/include/platform_def.h | 10 ++++++----
+ 2 files changed, 8 insertions(+), 6 deletions(-)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index 20992294b..af64504a4 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -213,8 +213,8 @@
+ 			linux,cma-default;
+ 		};
+ 
+-		optee@0xfce00000 {
+-			reg = <0x00000000 0xfce00000 0 0x00200000>;
++		optee@0xf8e00000 {
++			reg = <0x00000000 0xf8e00000 0 0x00200000>;
+ 			no-map;
+ 		};
+ 	};
+diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
+index cd77773aa..35d8fd24e 100644
+--- a/plat/arm/board/tc/include/platform_def.h
++++ b/plat/arm/board/tc/include/platform_def.h
+@@ -31,7 +31,7 @@
+  */
+ #define TC_TZC_DRAM1_BASE		(ARM_AP_TZC_DRAM1_BASE -	\
+ 					 TC_TZC_DRAM1_SIZE)
+-#define TC_TZC_DRAM1_SIZE		UL(0x02000000)	/* 32 MB */
++#define TC_TZC_DRAM1_SIZE		UL(0x06000000)	/* 96 MB */
+ #define TC_TZC_DRAM1_END		(TC_TZC_DRAM1_BASE +		\
+ 					 TC_TZC_DRAM1_SIZE - 1)
+ 
+@@ -68,7 +68,9 @@
+  * max size of BL32 image.
+  */
+ #if defined(SPD_spmd)
+-#define PLAT_ARM_SPMC_BASE		TC_TZC_DRAM1_BASE
++#define TC_EL2SPMC_LOAD_ADDR		(TC_TZC_DRAM1_BASE + 0x04000000)
++
++#define PLAT_ARM_SPMC_BASE		TC_EL2SPMC_LOAD_ADDR
+ #define PLAT_ARM_SPMC_SIZE		UL(0x200000)  /* 2 MB */
+ #endif
+ 
+@@ -259,8 +261,8 @@
+ 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_DEFAULT))
+ 
+ /*
+- * The first region below, TC_TZC_DRAM1_BASE (0xfd000000) to
+- * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 48 MB of DRAM as
++ * The first region below, TC_TZC_DRAM1_BASE (0xf9000000) to
++ * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 112 MB of DRAM as
+  * secure. The second and third regions gives non secure access to rest of DRAM.
+  */
+ #define TC_TZC_REGIONS_DEF	\
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch
new file mode 100644
index 0000000..0b34683
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch
@@ -0,0 +1,169 @@
+From a04466ceb81a04c5179e8064837c34a89c2b11bd Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 14:43:15 +0100
+Subject: [PATCH 5/7] feat(plat/tc): add spmc manifest with trusty sp
+
+Add SPMC manifest with Trusty SP. Define Trusty's load address,
+vcpu count, memory size.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: If4363580a478776d233f7f391a30e1cb345453c2
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ .../tc/fdts/tc_spmc_trusty_sp_manifest.dts    | 120 ++++++++++++++++++
+ plat/arm/board/tc/fdts/tc_tb_fw_config.dts    |   7 +-
+ 2 files changed, 126 insertions(+), 1 deletion(-)
+ create mode 100644 plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+
+diff --git a/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts b/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+new file mode 100644
+index 000000000..e2ea7b811
+--- /dev/null
++++ b/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+@@ -0,0 +1,120 @@
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++/dts-v1/;
++
++/ {
++	compatible = "arm,ffa-core-manifest-1.0";
++	#address-cells = <2>;
++	#size-cells = <1>;
++
++	attribute {
++		spmc_id = <0x8000>;
++		maj_ver = <0x1>;
++		min_ver = <0x1>;
++		exec_state = <0x0>;
++		load_address = <0x0 0xfd000000>;
++		entrypoint = <0x0 0xfd000000>;
++		binary_size = <0x80000>;
++	};
++
++	hypervisor {
++		compatible = "hafnium,hafnium";
++		vm1 {
++			is_ffa_partition;
++			debug_name = "trusty";
++			load_address = <0xf901f000>;
++			vcpu_count = <8>;
++			mem_size = <0x3f00000>; /* 64MB TZC DRAM - 1MB align */
++		};
++#ifdef TS_SP_FW_CONFIG
++		vm2 {
++			is_ffa_partition;
++			debug_name = "internal-trusted-storage";
++			load_address = <0xfee00000>;
++			vcpu_count = <1>;
++			mem_size = <2097152>; /* 2MB TZC DRAM */
++		};
++		vm3 {
++			is_ffa_partition;
++			debug_name = "crypto";
++			load_address = <0xfec00000>;
++			vcpu_count = <1>;
++			mem_size = <2097152>; /* 2MB TZC DRAM */
++		};
++#endif
++	};
++
++	cpus {
++		#address-cells = <0x2>;
++		#size-cells = <0x0>;
++
++		CPU0:cpu@0 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x0>;
++			enable-method = "psci";
++		};
++
++		/*
++		 * SPMC (Hafnium) requires secondary cpu nodes are declared in
++		 * descending order
++		 */
++		CPU7:cpu@700 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x700>;
++			enable-method = "psci";
++		};
++
++		CPU6:cpu@600 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x600>;
++			enable-method = "psci";
++		};
++
++		CPU5:cpu@500 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x500>;
++			enable-method = "psci";
++		};
++
++		CPU4:cpu@400 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x400>;
++			enable-method = "psci";
++		};
++
++		CPU3:cpu@300 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x300>;
++			enable-method = "psci";
++		};
++
++		CPU2:cpu@200 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x200>;
++			enable-method = "psci";
++		};
++
++		CPU1:cpu@100 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x100>;
++			enable-method = "psci";
++		};
++	};
++
++	/* 96MB of TC_TZC_DRAM1_BASE */
++	memory@f9000000 {
++		device_type = "memory";
++		reg = <0x0 0xf9000000 0x6000000>;
++	};
++};
+diff --git a/plat/arm/board/tc/fdts/tc_tb_fw_config.dts b/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
+index 4c6ccef25..a72772fb3 100644
+--- a/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
++++ b/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -47,6 +47,11 @@
+ 		       uuid = "486178e0-e7f8-11e3-bc5e-0002a5d5c51b";
+ 		       load-address = <0xfd280000>;
+ 		};
++#elif TRUSTY_SP_FW_CONFIG
++		trusty {
++		       uuid = "40ee25f0-a2bc-304c-8c4c-a173c57d8af1";
++		       load-address = <0xf901f000>;
++		};
+ #else
+ 		cactus-primary {
+ 			uuid = "b4b5671e-4a90-4fe1-b81f-fb13dae1dacb";
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch
new file mode 100644
index 0000000..e2bfb2c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch
@@ -0,0 +1,50 @@
+From 96151af7eed28d63fdaa1ac6de2d14a9c71f1d4a Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 30 Mar 2022 12:14:49 +0000
+Subject: [PATCH 6/7] feat(plat/tc): update dts with trusty compatible string
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ic6661df479e114bf3f464165c14df5fa02dc0139
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts | 26 ++++++++++++++++++++++++++
+ 1 file changed, 26 insertions(+)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index af64504a4..dc86958bf 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -555,4 +555,30 @@
+ 		compatible = "arm,trace-buffer-extension";
+ 		interrupts = <1 2 4>;
+ 	};
++
++	trusty {
++		#size-cells = <0x02>;
++		#address-cells = <0x02>;
++		ranges = <0x00>;
++		compatible = "android,trusty-v1";
++
++		virtio {
++			compatible = "android,trusty-virtio-v1";
++		};
++
++		test {
++			compatible = "android,trusty-test-v1";
++		};
++
++		log {
++			compatible = "android,trusty-log-v1";
++		};
++
++		irq {
++			ipi-range = <0x08 0x0f 0x08>;
++			interrupt-ranges = <0x00 0x0f 0x00 0x10 0x1f 0x01 0x20 0x3f 0x02>;
++			interrupt-templates = <0x01 0x00 0x8001 0x01 0x01 0x04 0x8001 0x01 0x00 0x04>;
++			compatible = "android,trusty-irq-v1";
++		};
++	};
+ };
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch
new file mode 100644
index 0000000..91f5a9e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch
@@ -0,0 +1,64 @@
+From 1a051bef6c63f6871edd8d87e969460f073820a7 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 27 Apr 2022 18:15:47 +0100
+Subject: [PATCH 7/7] fix(plat/tc): disable smmu
+
+Reserve static shared-dma-pool below 4GB. This removes dependency on
+SMMU driver. As there are stability issues in SMMU driver, it is
+disabled. This change is temporary and will be reverted upon proper
+fix and testing.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I6b1b4c2a0acdf62df8c26007c7ca596774e13710
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts | 16 +++-------------
+ 1 file changed, 3 insertions(+), 13 deletions(-)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index dc86958bf..fbae3e3e8 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -209,12 +209,12 @@
+ 		linux,cma {
+ 			compatible = "shared-dma-pool";
+ 			reusable;
+-			size = <0x0 0x8000000>;
++			reg = <0x0 0xf1000000 0x0 0x8000000>;
+ 			linux,cma-default;
+ 		};
+ 
+-		optee@0xf8e00000 {
+-			reg = <0x00000000 0xf8e00000 0 0x00200000>;
++		optee@0xf0e00000 {
++			reg = <0x0 0xf0e00000 0 0x00200000>;
+ 			no-map;
+ 		};
+ 	};
+@@ -460,13 +460,6 @@
+ 		>;
+ 	};
+ 
+-	smmu: smmu@2ce00000 {
+-		#iommu-cells = <1>;
+-		compatible = "arm,smmu-v3";
+-		reg = <0x0 0x2ce00000 0x0 0x20000>;
+-		status = "okay";
+-	};
+-
+ 	dp0: display@2cc00000 {
+ 		#address-cells = <1>;
+ 		#size-cells = <0>;
+@@ -476,9 +469,6 @@
+ 		interrupt-names = "DPU";
+ 		clocks = <&scmi_clk 0>;
+ 		clock-names = "aclk";
+-		iommus = <&smmu 0>, <&smmu 1>, <&smmu 2>, <&smmu 3>,
+-			<&smmu 4>, <&smmu 5>, <&smmu 6>, <&smmu 7>,
+-			<&smmu 8>, <&smmu 9>;
+ 		pl0: pipeline@0 {
+ 			reg = <0>;
+ 			clocks = <&scmi_clk 1>;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py
new file mode 100644
index 0000000..f3670ce
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import argparse
+import uuid
+import zlib
+
+def main(metadata_file, img_type_uuids, location_uuids, img_uuids):
+    def add_field_to_metadata(value):
+        # Write the integer values to file in little endian representation
+        with open(metadata_file, "ab") as fp:
+            fp.write(value.to_bytes(4, byteorder='little'))
+
+    def add_uuid_to_metadata(uuid_str):
+        # Validate UUID string and write to file in little endian representation
+        uuid_val = uuid.UUID(uuid_str)
+        with open(metadata_file, "ab") as fp:
+            fp.write(uuid_val.bytes_le)
+
+    # Fill metadata preamble
+    add_field_to_metadata(1) #version=1
+    add_field_to_metadata(0) #active_index=0
+    add_field_to_metadata(0) #previous_active_index=0
+
+    for img_type_uuid, location_uuid in zip(img_type_uuids, location_uuids):
+        # Fill metadata image entry
+        add_uuid_to_metadata(img_type_uuid) # img_type_uuid
+        add_uuid_to_metadata(location_uuid) # location_uuid
+
+        for img_uuid in img_uuids:
+            # Fill metadata bank image info
+            add_uuid_to_metadata(img_uuid) # image unique bank_uuid
+            add_field_to_metadata(1)       # accepted=1
+            add_field_to_metadata(0)       # reserved (MBZ)
+
+    # Prepend CRC32
+    with open(metadata_file, 'rb+') as fp:
+        content = fp.read()
+        crc = zlib.crc32(content)
+        fp.seek(0)
+        fp.write(crc.to_bytes(4, byteorder='little') + content)
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--metadata_file', required=True,
+                        help='Output binary file to store the metadata')
+    parser.add_argument('--img_type_uuids', type=str, nargs='+', required=True,
+                        help='A list of UUIDs identifying the image types')
+    parser.add_argument('--location_uuids', type=str, nargs='+', required=True,
+                        help='A list of UUIDs of the storage volumes where the images are located. '
+                             'Must have the same length as img_type_uuids.')
+    parser.add_argument('--img_uuids', type=str, nargs='+', required=True,
+                        help='A list UUIDs of the images in a firmware bank')
+
+    args = parser.parse_args()
+
+    if len(args.img_type_uuids) != len(args.location_uuids):
+        parser.print_help()
+        raise argparse.ArgumentError(None, 'Arguments img_type_uuids and location_uuids must have the same length.')
+
+    main(args.metadata_file, args.img_type_uuids, args.location_uuids, args.img_uuids)
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend
new file mode 100644
index 0000000..ff22ff1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend
@@ -0,0 +1,3 @@
+# Machine specific TFAs
+
+COMPATIBLE_MACHINE:corstone1000 = "corstone1000"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc
new file mode 100644
index 0000000..341c8a2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc
@@ -0,0 +1,42 @@
+# Corstone1000 64-bit machines specific TFA support
+
+COMPATIBLE_MACHINE = "(corstone1000)"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/corstone1000:"
+
+SRC_URI:append = " \ 
+	file://0001-Fix-FF-A-version-in-SPMC-manifest.patch \
+	"
+
+TFA_DEBUG = "1"
+TFA_UBOOT ?= "1"
+TFA_MBEDTLS = "1"
+TFA_BUILD_TARGET = "bl2 bl31 fip"
+
+# Enabling Secure-EL1 Payload Dispatcher (SPD)
+TFA_SPD = "spmd"
+# Cortex-A35 supports Armv8.0-A (no S-EL2 execution state).
+# So, the SPD SPMC component should run at the S-EL1 execution state
+TFA_SPMD_SPM_AT_SEL2 = "0"
+
+# BL2 loads BL32 (optee). So, optee needs to be built first:
+DEPENDS += "optee-os"
+
+EXTRA_OEMAKE:append = " \
+                        ARCH=aarch64 \
+                        TARGET_PLATFORM=${TFA_TARGET_PLATFORM} \
+                        ENABLE_STACK_PROTECTOR=strong \
+                        ENABLE_PIE=1 \
+                        BL2_AT_EL3=1 \
+                        CREATE_KEYS=1 \
+                        GENERATE_COT=1 \
+                        TRUSTED_BOARD_BOOT=1 \
+                        COT=tbbr \
+                        ARM_ROTPK_LOCATION=devel_rsa  \
+                        ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem \
+                        BL32=${RECIPE_SYSROOT}/lib/firmware/tee-pager_v2.bin \
+                        LOG_LEVEL=50 \
+                        "
+
+# trigger TF-M build so TF-A binaries get signed
+do_deploy[depends]+= "virtual/trusted-firmware-m:do_prepare_recipe_sysroot"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc
new file mode 100644
index 0000000..acd9e3d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc
@@ -0,0 +1,17 @@
+# Corstone-500 specific TFA support
+
+COMPATIBLE_MACHINE = "corstone500"
+TFA_PLATFORM = "a5ds"
+TFA_DEBUG = "1"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "all fip"
+TFA_INSTALL_TARGET = "bl1.bin fip.bin"
+
+EXTRA_OEMAKE:append = " \
+                    ARCH=aarch32 \
+                    FVP_HW_CONFIG_DTS=fdts/a5ds.dts \
+                    ARM_ARCH_MAJOR=7 \
+                    AARCH32_SP=sp_min \
+                    ARM_CORTEX_A5=yes \
+                    ARM_XLAT_TABLES_LIB_V1=1 \
+                    "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc
new file mode 100644
index 0000000..fdaadb9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc
@@ -0,0 +1,12 @@
+# Armv7-A FVP specific TFA parameters
+
+COMPATIBLE_MACHINE = "fvp-base-arm32"
+TFA_PLATFORM = "fvp"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "dtbs bl1 bl32 fip"
+
+EXTRA_OEMAKE:append = " \
+    ARCH=aarch32 \
+    AARCH32_SP=sp_min \
+    "
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc
new file mode 100644
index 0000000..43340cd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc
@@ -0,0 +1,12 @@
+# FVP specific TFA parameters
+
+#
+# Armv8-A Base Platform FVP
+#
+
+COMPATIBLE_MACHINE = "fvp-base"
+TFA_PLATFORM = "fvp"
+TFA_DEBUG = "1"
+TFA_MBEDTLS = "1"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "bl1 bl2 bl31 dtbs fip"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc
new file mode 100644
index 0000000..3ddd8cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc
@@ -0,0 +1,13 @@
+# Juno specific TFA support
+
+COMPATIBLE_MACHINE = "juno"
+TFA_PLATFORM = "juno"
+TFA_DEBUG = "1"
+TFA_MBEDTLS = "1"
+TFA_UBOOT ?= "1"
+TFA_BUILD_TARGET = "bl1 bl2 bl31 dtbs fip"
+
+# Juno needs the System Control Processor Firmware
+DEPENDS += "virtual/control-processor-firmware"
+
+EXTRA_OEMAKE:append = " SCP_BL2=${RECIPE_SYSROOT}/firmware/scp_ramfw.bin"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc
new file mode 100644
index 0000000..f8a0b8d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc
@@ -0,0 +1,24 @@
+# N1SDP specific TFA support
+
+COMPATIBLE_MACHINE = "n1sdp"
+TFA_PLATFORM       = "n1sdp"
+TFA_BUILD_TARGET   = "all fip"
+TFA_INSTALL_TARGET = "bl1 bl2 bl31 n1sdp-multi-chip n1sdp-single-chip n1sdp_fw_config n1sdp_tb_fw_config fip"
+TFA_DEBUG          = "1"
+TFA_MBEDTLS        = "1"
+TFA_UBOOT          = "0"
+TFA_UEFI           = "1"
+
+SRC_URI:append = " file://bl_size.patch"
+
+TFA_ROT_KEY= "plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
+
+EXTRA_OEMAKE:append = "\
+                    TRUSTED_BOARD_BOOT=1 \
+                    GENERATE_COT=1 \
+                    CREATE_KEYS=1 \
+                    ENABLE_PIE=0 \
+                    ARM_ROTPK_LOCATION="devel_rsa" \
+                    ROT_KEY="${TFA_ROT_KEY}" \
+                    BL33=${RECIPE_SYSROOT}/firmware/uefi.bin \
+                    "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc
new file mode 100644
index 0000000..20904b3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc
@@ -0,0 +1,13 @@
+# SGI575 specific TFA support
+
+COMPATIBLE_MACHINE = "sgi575"
+TFA_PLATFORM       = "sgi575"
+TFA_BUILD_TARGET   = "all fip"
+TFA_INSTALL_TARGET = "bl1 fip"
+TFA_DEBUG          = "1"
+TFA_MBEDTLS        = "1"
+TFA_UBOOT          = "0"
+TFA_UEFI           = "1"
+
+EXTRA_OEMAKE += "TRUSTED_BOARD_BOOT=1 GENERATE_COT=1 ARM_ROTPK_LOCATION=devel_rsa \
+                     ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc
new file mode 100644
index 0000000..8cb53de
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc
@@ -0,0 +1,140 @@
+# TC0 specific TFA configuration
+
+DEPENDS += "scp-firmware util-linux-native gptfdisk-native"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/tc:"
+SRC_URI:append = " \
+    file://0001-plat-tc-Increase-maximum-BL2-size.patch \
+    file://0002-Makefile-add-trusty_sp_fw_config-build-option.patch \
+    file://0003-fix-plat-arm-increase-sp-max-image-size.patch \
+    file://0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch \
+    file://0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch \
+    file://0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch \
+    file://0007-fix-plat-tc-disable-smmu.patch \
+    file://generate_metadata.py \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+TFA_PLATFORM = "tc"
+TFA_BUILD_TARGET = "all fip"
+TFA_UBOOT = "1"
+TFA_INSTALL_TARGET = "bl1 fip"
+TFA_MBEDTLS = "1"
+TFA_DEBUG = "1"
+
+TFA_SPD = "spmd"
+TFA_SPMD_SPM_AT_SEL2 = "1"
+
+TFA_TARGET_PLATFORM:tc0 = "0"
+TFA_TARGET_PLATFORM:tc1 = "1"
+
+EXTRA_OEMAKE += "TARGET_PLATFORM=${TFA_TARGET_PLATFORM}"
+
+# Set optee as SP. Set spmc manifest and sp layout file to optee
+DEPENDS += "optee-os"
+
+TFA_SP_LAYOUT_FILE = "${RECIPE_SYSROOT}/lib/firmware/sp_layout.json"
+TFA_ARM_SPMC_MANIFEST_DTS = "plat/arm/board/tc/fdts/tc_spmc_optee_sp_manifest.dts"
+
+EXTRA_OEMAKE += "SCP_BL2=${RECIPE_SYSROOT}/firmware/scp_ramfw.bin"
+EXTRA_OEMAKE += "TRUSTED_BOARD_BOOT=1 GENERATE_COT=1 ARM_ROTPK_LOCATION=devel_rsa \
+                     ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
+EXTRA_OEMAKE += "PSA_FWU_SUPPORT=1 ARM_GPT_SUPPORT=1"
+
+do_generate_gpt() {
+    gpt_image="${BUILD_DIR}/fip_gpt.bin"
+    fip_bin="${BUILD_DIR}/fip.bin"
+    # the FIP partition type is not standardized, so generate one
+    fip_type_uuid=`uuidgen --sha1 --namespace @dns --name "fip_type_uuid"`
+    # metadata partition type UUID, specified by the document:
+    # Platform Security Firmware Update for the A-profile Arm Architecture
+    # version: 1.0BET0
+    metadata_type_uuid="8a7a84a0-8387-40f6-ab41-a8b9a5a60d23"
+    location_uuid=`uuidgen`
+    FIP_A_uuid=`uuidgen`
+    FIP_B_uuid=`uuidgen`
+
+    # maximum FIP size 4MB. This is the current size of the FIP rounded up to an integer number of MB.
+    fip_max_size=4194304
+    fip_bin_size=$(stat -c %s $fip_bin)
+    if [ $fip_max_size -lt $fip_bin_size ]; then
+        bberror "FIP binary ($fip_bin_size bytes) is larger than the GPT partition ($fip_max_size bytes)"
+    fi
+
+    # maximum metadata size 512B. This is the current size of the metadata rounded up to an integer number of sectors.
+    metadata_max_size=512
+    metadata_file="${BUILD_DIR}/metadata.bin"
+    python3 ${WORKDIR}/generate_metadata.py --metadata_file $metadata_file \
+                                            --img_type_uuids $fip_type_uuid \
+                                            --location_uuids $location_uuid \
+                                            --img_uuids $FIP_A_uuid $FIP_B_uuid
+
+    # create GPT image. The GPT contains 2 FIP partitions: FIP_A and FIP_B, and 2 metadata partitions: FWU-Metadata and Bkup-FWU-Metadata.
+    # the GPT layout is the following:
+    # -----------------------
+    # Protective MBR
+    # -----------------------
+    # Primary GPT Header
+    # -----------------------
+    # FIP_A
+    # -----------------------
+    # FIP_B
+    # -----------------------
+    # FWU-Metadata
+    # -----------------------
+    # Bkup-FWU-Metadata
+    # -----------------------
+    # Secondary GPT Header
+    # -----------------------
+
+    sector_size=512
+    gpt_header_size=33 # valid only for 512-byte sectors
+    num_sectors_fip=`expr $fip_max_size / $sector_size`
+    num_sectors_metadata=`expr $metadata_max_size / $sector_size`
+    start_sector_1=`expr 1 + $gpt_header_size` # size of MBR is 1 sector
+    start_sector_2=`expr $start_sector_1 + $num_sectors_fip`
+    start_sector_3=`expr $start_sector_2 + $num_sectors_fip`
+    start_sector_4=`expr $start_sector_3 + $num_sectors_metadata`
+    num_sectors_gpt=`expr $start_sector_4 + $num_sectors_metadata + $gpt_header_size`
+    gpt_size=`expr $num_sectors_gpt \* $sector_size`
+
+    # create raw image
+    dd if=/dev/zero of=$gpt_image bs=$gpt_size count=1
+
+    # create the GPT layout
+    sgdisk $gpt_image \
+           --set-alignment 1 \
+           --disk-guid $location_uuid \
+           \
+           --new 1:$start_sector_1:+$num_sectors_fip \
+           --change-name 1:FIP_A \
+           --typecode 1:$fip_type_uuid \
+           --partition-guid 1:$FIP_A_uuid \
+           \
+           --new 2:$start_sector_2:+$num_sectors_fip \
+           --change-name 2:FIP_B \
+           --typecode 2:$fip_type_uuid \
+           --partition-guid 2:$FIP_B_uuid \
+           \
+           --new 3:$start_sector_3:+$num_sectors_metadata \
+           --change-name 3:FWU-Metadata \
+           --typecode 3:$metadata_type_uuid \
+           \
+           --new 4:$start_sector_4:+$num_sectors_metadata \
+           --change-name 4:Bkup-FWU-Metadata \
+           --typecode 4:$metadata_type_uuid
+
+    # populate the GPT partitions
+    dd if=$fip_bin of=$gpt_image bs=$sector_size seek=$start_sector_1 count=$num_sectors_fip conv=notrunc
+    dd if=$fip_bin of=$gpt_image bs=$sector_size seek=$start_sector_2 count=$num_sectors_fip conv=notrunc
+    dd if=$metadata_file of=$gpt_image bs=$sector_size seek=$start_sector_3 count=$num_sectors_metadata conv=notrunc
+    dd if=$metadata_file of=$gpt_image bs=$sector_size seek=$start_sector_4 count=$num_sectors_metadata conv=notrunc
+}
+
+addtask do_generate_gpt after do_compile before do_install
+
+do_install:append() {
+    install -m 0644 ${BUILD_DIR}/fip_gpt.bin ${D}/firmware/fip_gpt-tc.bin
+    ln -sf fip_gpt-tc.bin ${D}/firmware/fip_gpt.bin
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend
new file mode 100644
index 0000000..09ed3f7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend
@@ -0,0 +1,15 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/:"
+
+# Machine specific TFAs
+
+MACHINE_TFA_REQUIRE ?= ""
+MACHINE_TFA_REQUIRE:corstone500 = "trusted-firmware-a-corstone500.inc"
+MACHINE_TFA_REQUIRE:corstone1000 = "trusted-firmware-a-corstone1000.inc"
+MACHINE_TFA_REQUIRE:fvp-base = "trusted-firmware-a-fvp.inc"
+MACHINE_TFA_REQUIRE:fvp-base-arm32 = "trusted-firmware-a-fvp-arm32.inc"
+MACHINE_TFA_REQUIRE:juno = "trusted-firmware-a-juno.inc"
+MACHINE_TFA_REQUIRE:n1sdp = "trusted-firmware-a-n1sdp.inc"
+MACHINE_TFA_REQUIRE:sgi575 = "trusted-firmware-a-sgi575.inc"
+MACHINE_TFA_REQUIRE:tc = "trusted-firmware-a-tc.inc"
+
+require ${MACHINE_TFA_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc
new file mode 100644
index 0000000..eb400e5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc
@@ -0,0 +1,49 @@
+# Corstone1000 machines specific TFM support
+
+COMPATIBLE_MACHINE = "(corstone1000)"
+
+TFM_PLATFORM = "arm/corstone1000"
+
+TFM_DEBUG = "1"
+
+## Default is the MPS3 board
+TFM_PLATFORM_IS_FVP ?= "FALSE"
+EXTRA_OECMAKE += "-DPLATFORM_IS_FVP=${TFM_PLATFORM_IS_FVP}"
+EXTRA_OECMAKE += "-DCC312_LEGACY_DRIVER_API_ENABLED=OFF"
+
+# libmetal
+LICENSE += "& BSD-3-Clause"
+LIC_FILES_CHKSUM += "file://../libmetal/LICENSE.md;md5=fe0b8a4beea8f0813b606d15a3df3d3c"
+SRC_URI += "git://github.com/OpenAMP/libmetal.git;protocol=https;branch=main;name=libmetal;destsuffix=git/libmetal"
+SRCREV_libmetal = "f252f0e007fbfb8b3a52b1d5901250ddac96baad"
+EXTRA_OECMAKE += "-DLIBMETAL_SRC_PATH=${WORKDIR}/git/libmetal -DLIBMETAL_BIN_PATH=${B}/libmetal-build"
+
+# OpenAMP
+LICENSE += "& BSD-2-Clause & BSD-3-Clause"
+LIC_FILES_CHKSUM += "file://../openamp/LICENSE.md;md5=a8d8cf662ef6bf9936a1e1413585ecbf"
+SRC_URI += "git://github.com/OpenAMP/open-amp.git;protocol=https;branch=main;name=openamp;destsuffix=git/openamp"
+SRCREV_openamp = "347397decaa43372fc4d00f965640ebde042966d"
+EXTRA_OECMAKE += "-DLIBOPENAMP_SRC_PATH=${WORKDIR}/git/openamp -DLIBOPENAMP_BIN_PATH=${B}/libopenamp-build"
+
+DEPENDS += "trusted-firmware-a"
+
+# adding host images signing support
+require trusted-firmware-m-sign-host-images.inc
+
+do_install() {
+  install -D -p -m 0644 ${B}/install/outputs/tfm_s_signed.bin ${D}/firmware/tfm_s_signed.bin
+  install -D -p -m 0644 ${B}/install/outputs/bl2_signed.bin ${D}/firmware/bl2_signed.bin
+  install -D -p -m 0644 ${B}/install/outputs/bl1.bin ${D}/firmware/bl1.bin
+
+  #
+  # Signing TF-A BL2 and the FIP image
+  #
+
+  sign_host_image ${TFA_BL2_BINARY} ${RECIPE_SYSROOT}/firmware ${TFA_BL2_RE_IMAGE_LOAD_ADDRESS} ${TFA_BL2_RE_SIGN_BIN_SIZE}
+
+  fiptool update \
+      --tb-fw ${D}/firmware/signed_${TFA_BL2_BINARY} \
+      ${RECIPE_SYSROOT}/firmware/${TFA_FIP_BINARY}
+
+  sign_host_image ${TFA_FIP_BINARY} ${RECIPE_SYSROOT}/firmware ${TFA_FIP_RE_IMAGE_LOAD_ADDRESS} ${TFA_FIP_RE_SIGN_BIN_SIZE}
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc
new file mode 100644
index 0000000..49af356
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc
@@ -0,0 +1,50 @@
+# Signing host images using TF-M tools
+
+DEPENDS += "python3-imgtool-native fiptool-native"
+
+#
+# sign_host_image
+#
+# Description:
+#
+# A generic function that signs a host image
+# using MCUBOOT format
+#
+# Arguments:
+#
+# $1 ... host binary to sign
+# $2 ... host binary path
+# $3 ... load address of the given binary
+# $4 ... signed binary size
+#
+# Note: The signed binary is copied to ${D}/firmware
+#
+sign_host_image() {
+
+    host_binary_filename="`basename -s .bin ${1}`"
+    host_binary_layout="${host_binary_filename}_ns"
+
+    cat << EOF > ${B}/${host_binary_layout}
+enum image_attributes {
+    RE_IMAGE_LOAD_ADDRESS = ${3},
+    RE_SIGN_BIN_SIZE = ${4},
+};
+EOF
+
+    host_binary="${2}/`basename ${1}`"
+    host_binary_signed="${D}/firmware/signed_`basename ${1}`"
+
+    ${PYTHON} ${S}/bl2/ext/mcuboot/scripts/wrapper/wrapper.py \
+            -v ${RE_LAYOUT_WRAPPER_VERSION} \
+            --layout ${B}/${host_binary_layout} \
+            -k  ${TFM_SIGN_PRIVATE_KEY} \
+            --public-key-format full \
+            --align 1 \
+            --pad \
+            --pad-header \
+            -H ${RE_IMAGE_OFFSET} \
+            -s auto \
+            ${host_binary} \
+            ${host_binary_signed}
+
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend
new file mode 100644
index 0000000..da70bc7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_TFM_REQUIRE ?= ""
+MACHINE_TFM_REQUIRE:corstone1000 = "trusted-firmware-m-corstone1000.inc"
+
+require ${MACHINE_TFM_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch
new file mode 100644
index 0000000..d0ae964
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch
@@ -0,0 +1,185 @@
+From c047b15fba9da36616bc9a346d9fe4d76e7ca4b2 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Thu, 24 Jun 2021 09:25:00 +0100
+Subject: [PATCH 01/27] cmd: load: add load command for memory mapped
+
+cp.b is used a lot as a way to load binaries to memory and execute
+them, however we may need to integrate this with the efi subsystem to
+set it up as a bootdev.
+
+So, introduce a loadm command that will be consistent with the other
+loadX commands and will call the efi API's.
+
+ex: loadm $kernel_addr $kernel_addr_r $kernel_size
+
+with this a kernel with CONFIG_EFI_STUB enabled will be loaded and
+then subsequently booted with bootefi command.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ README                           |  1 +
+ cmd/Kconfig                      |  6 ++++
+ cmd/bootefi.c                    | 12 ++++++++
+ cmd/load.c                       | 48 ++++++++++++++++++++++++++++++++
+ include/efi_loader.h             |  2 ++
+ lib/efi_loader/efi_device_path.c |  9 ++++++
+ 6 files changed, 78 insertions(+)
+
+diff --git a/README b/README
+index f51f392111f9..049bcd108980 100644
+--- a/README
++++ b/README
+@@ -2760,6 +2760,7 @@ rarpboot- boot image via network using RARP/TFTP protocol
+ diskboot- boot from IDE devicebootd   - boot default, i.e., run 'bootcmd'
+ loads	- load S-Record file over serial line
+ loadb	- load binary file over serial line (kermit mode)
++loadm   - load binary blob from source address to destination address
+ md	- memory display
+ mm	- memory modify (auto-incrementing)
+ nm	- memory modify (constant address)
+diff --git a/cmd/Kconfig b/cmd/Kconfig
+index 5e25e45fd288..ff50102a89c7 100644
+--- a/cmd/Kconfig
++++ b/cmd/Kconfig
+@@ -1076,6 +1076,12 @@ config CMD_LOADB
+ 	help
+ 	  Load a binary file over serial line.
+ 
++config CMD_LOADM
++	bool "loadm"
++	default y
++	help
++	  Load a binary over memory mapped.
++
+ config CMD_LOADS
+ 	bool "loads"
+ 	default y
+diff --git a/cmd/bootefi.c b/cmd/bootefi.c
+index 53d9f0e0dcca..8d492ea9e70c 100644
+--- a/cmd/bootefi.c
++++ b/cmd/bootefi.c
+@@ -34,6 +34,18 @@ static struct efi_device_path *bootefi_device_path;
+ static void *image_addr;
+ static size_t image_size;
+ 
++/**
++ * efi_get_image_parameters() - return image parameters
++ *
++ * @img_addr:		address of loaded image in memory
++ * @img_size:		size of loaded image
++ */
++void efi_get_image_parameters(void **img_addr, size_t *img_size)
++{
++	*img_addr = image_addr;
++	*img_size = image_size;
++}
++
+ /**
+  * efi_clear_bootdev() - clear boot device
+  */
+diff --git a/cmd/load.c b/cmd/load.c
+index 7e4a552d90ef..1224a7f85bb3 100644
+--- a/cmd/load.c
++++ b/cmd/load.c
+@@ -1063,6 +1063,44 @@ static ulong load_serial_ymodem(ulong offset, int mode)
+ 
+ #endif
+ 
++#if defined(CONFIG_CMD_LOADM)
++static int do_load_memory_bin(struct cmd_tbl *cmdtp, int flag, int argc,
++			      char *const argv[])
++{
++	ulong	addr, dest, size;
++	void	*src, *dst;
++
++	if (argc != 4)
++		return CMD_RET_USAGE;
++
++	addr = simple_strtoul(argv[1], NULL, 16);
++
++	dest = simple_strtoul(argv[2], NULL, 16);
++
++	size = simple_strtoul(argv[3], NULL, 16);
++
++	if (!size) {
++		printf("loadm: can not load zero bytes\n");
++		return 1;
++	}
++
++	src = map_sysmem(addr, size);
++	dst = map_sysmem(dest, size);
++
++	memcpy(dst, src, size);
++
++	unmap_sysmem(src);
++	unmap_sysmem(dst);
++
++	if (IS_ENABLED(CONFIG_CMD_BOOTEFI))
++		efi_set_bootdev("Mem", "", "", map_sysmem(dest, 0), size);
++
++	printf("loaded bin to memory: size: %lu\n", size);
++
++	return 0;
++}
++#endif
++
+ /* -------------------------------------------------------------------- */
+ 
+ #if defined(CONFIG_CMD_LOADS)
+@@ -1137,3 +1175,13 @@ U_BOOT_CMD(
+ );
+ 
+ #endif	/* CONFIG_CMD_LOADB */
++
++#if defined(CONFIG_CMD_LOADM)
++U_BOOT_CMD(
++	loadm, 4, 0,	do_load_memory_bin,
++	"load binary blob from source address to destination address",
++	"[src_addr] [dst_addr] [size]\n"
++	"     - load a binary blob from one memory location to other"
++	" from src_addr to dst_addr by size bytes"
++);
++#endif /* CONFIG_CMD_LOADM */
+diff --git a/include/efi_loader.h b/include/efi_loader.h
+index af36639ec6a7..126db279dd3e 100644
+--- a/include/efi_loader.h
++++ b/include/efi_loader.h
+@@ -585,6 +585,8 @@ efi_status_t efi_load_pe(struct efi_loaded_image_obj *handle,
+ void efi_save_gd(void);
+ /* Call this to relocate the runtime section to an address space */
+ void efi_runtime_relocate(ulong offset, struct efi_mem_desc *map);
++/* Call this to get image parameters */
++void efi_get_image_parameters(void **img_addr, size_t *img_size);
+ /* Add a new object to the object list. */
+ void efi_add_handle(efi_handle_t obj);
+ /* Create handle */
+diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
+index 0542aaae16c7..d8dc59b2c95c 100644
+--- a/lib/efi_loader/efi_device_path.c
++++ b/lib/efi_loader/efi_device_path.c
+@@ -1138,6 +1138,8 @@ efi_status_t efi_dp_from_name(const char *dev, const char *devnr,
+ {
+ 	struct blk_desc *desc = NULL;
+ 	struct disk_partition fs_partition;
++	size_t image_size;
++	void *image_addr;
+ 	int part = 0;
+ 	char *filename;
+ 	char *s;
+@@ -1153,6 +1155,13 @@ efi_status_t efi_dp_from_name(const char *dev, const char *devnr,
+ 	} else if (!strcmp(dev, "Uart")) {
+ 		if (device)
+ 			*device = efi_dp_from_uart();
++	} else if (!strcmp(dev, "Mem")) {
++		efi_get_image_parameters(&image_addr, &image_size);
++
++		if (device)
++			*device = efi_dp_from_mem(EFI_RESERVED_MEMORY_TYPE,
++						  (uintptr_t)image_addr,
++						  image_size);
+ 	} else {
+ 		part = blk_get_device_part_str(dev, devnr, &desc, &fs_partition,
+ 					       1);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch
new file mode 100644
index 0000000..e0caaa4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch
@@ -0,0 +1,677 @@
+From 5cc889db3279ef4944ab64e36db3dbab1bf9ffa5 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Tue, 15 Feb 2022 09:44:10 +0000
+Subject: [PATCH 02/27] arm: add support to corstone1000 platform
+
+Corstone1000 is a platform from arm, which includes pre
+verified Corstone SSE710 sub-system that combines Cortex-A and
+Cortex-M processors [0].
+
+This code adds the support for the Cortex-A35 implementation
+at host side, it contains also the necessary bits to support
+the Corstone 1000 FVP (Fixed Virtual Platform) [1] and also the
+FPGA MPS3 board implementation of this platform. [2]
+
+0: https://documentation-service.arm.com/static/619e02b1f45f0b1fbf3a8f16
+1: https://developer.arm.com/tools-and-software/open-source-software/arm-platforms-software/arm-ecosystem-fvps
+2: https://documentation-service.arm.com/static/61f3f4d7fa8173727a1b71bf
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ arch/arm/Kconfig                         |   8 ++
+ arch/arm/dts/Makefile                    |   3 +
+ arch/arm/dts/corstone1000-fvp.dts        |  23 +++
+ arch/arm/dts/corstone1000-mps3.dts       |  32 +++++
+ arch/arm/dts/corstone1000.dtsi           | 169 +++++++++++++++++++++++
+ board/armltd/corstone1000/Kconfig        |  12 ++
+ board/armltd/corstone1000/MAINTAINERS    |   7 +
+ board/armltd/corstone1000/Makefile       |   7 +
+ board/armltd/corstone1000/corstone1000.c | 121 ++++++++++++++++
+ configs/corstone1000_defconfig           |  80 +++++++++++
+ include/configs/corstone1000.h           |  86 ++++++++++++
+ 11 files changed, 548 insertions(+)
+ create mode 100644 arch/arm/dts/corstone1000-fvp.dts
+ create mode 100644 arch/arm/dts/corstone1000-mps3.dts
+ create mode 100644 arch/arm/dts/corstone1000.dtsi
+ create mode 100644 board/armltd/corstone1000/Kconfig
+ create mode 100644 board/armltd/corstone1000/MAINTAINERS
+ create mode 100644 board/armltd/corstone1000/Makefile
+ create mode 100644 board/armltd/corstone1000/corstone1000.c
+ create mode 100644 configs/corstone1000_defconfig
+ create mode 100644 include/configs/corstone1000.h
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb84..d64051b533a7 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -1266,6 +1266,12 @@ config TARGET_VEXPRESS64_JUNO
+ 	select USB
+ 	imply OF_HAS_PRIOR_STAGE
+ 
++config TARGET_CORSTONE1000
++	bool "Support Corstone1000 Platform"
++	select ARM64
++	select PL01X_SERIAL
++	select DM
++
+ config TARGET_TOTAL_COMPUTE
+ 	bool "Support Total Compute Platform"
+ 	select ARM64
+@@ -2198,6 +2204,8 @@ source "arch/arm/mach-nexell/Kconfig"
+ 
+ source "board/armltd/total_compute/Kconfig"
+ 
++source "board/armltd/corstone1000/Kconfig"
++
+ source "board/bosch/shc/Kconfig"
+ source "board/bosch/guardian/Kconfig"
+ source "board/Marvell/octeontx/Kconfig"
+diff --git a/arch/arm/dts/Makefile b/arch/arm/dts/Makefile
+index 644ba961a223..7de25d09c9fe 100644
+--- a/arch/arm/dts/Makefile
++++ b/arch/arm/dts/Makefile
+@@ -1215,6 +1215,9 @@ dtb-$(CONFIG_TARGET_EA_LPC3250DEVKITV2) += lpc3250-ea3250.dtb
+ 
+ dtb-$(CONFIG_ARCH_QEMU) += qemu-arm.dtb qemu-arm64.dtb
+ 
++dtb-$(CONFIG_TARGET_CORSTONE1000) += corstone1000-mps3.dtb \
++				     corstone1000-fvp.dtb
++
+ include $(srctree)/scripts/Makefile.dts
+ 
+ targets += $(dtb-y)
+diff --git a/arch/arm/dts/corstone1000-fvp.dts b/arch/arm/dts/corstone1000-fvp.dts
+new file mode 100644
+index 000000000000..1fcc137a493c
+--- /dev/null
++++ b/arch/arm/dts/corstone1000-fvp.dts
+@@ -0,0 +1,23 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++/dts-v1/;
++
++#include "corstone1000.dtsi"
++
++/ {
++	model = "ARM Corstone1000 FVP (Fixed Virtual Platform)";
++	compatible = "arm,corstone1000-fvp";
++
++	smsc: ethernet@4010000 {
++		compatible = "smsc,lan91c111";
++		reg = <0x40100000 0x10000>;
++		phy-mode = "mii";
++		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
++		reg-io-width = <2>;
++	};
++};
+diff --git a/arch/arm/dts/corstone1000-mps3.dts b/arch/arm/dts/corstone1000-mps3.dts
+new file mode 100644
+index 000000000000..e3146747c2d9
+--- /dev/null
++++ b/arch/arm/dts/corstone1000-mps3.dts
+@@ -0,0 +1,32 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++/dts-v1/;
++
++#include "corstone1000.dtsi"
++
++/ {
++	model = "ARM Corstone1000 FPGA MPS3 board";
++	compatible = "arm,corstone1000-mps3";
++
++	smsc: ethernet@4010000 {
++		compatible = "smsc,lan9220", "smsc,lan9115";
++		reg = <0x40100000 0x10000>;
++		phy-mode = "mii";
++		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
++		reg-io-width = <2>;
++		smsc,irq-push-pull;
++	};
++
++	usb_host: usb@40200000 {
++		compatible = "nxp,usb-isp1763";
++		reg = <0x40200000 0x100000>;
++		interrupts = <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
++		bus-width = <16>;
++		dr_mode = "host";
++	};
++};
+diff --git a/arch/arm/dts/corstone1000.dtsi b/arch/arm/dts/corstone1000.dtsi
+new file mode 100644
+index 000000000000..d0194aa893f2
+--- /dev/null
++++ b/arch/arm/dts/corstone1000.dtsi
+@@ -0,0 +1,169 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++#include <dt-bindings/interrupt-controller/arm-gic.h>
++
++/ {
++	interrupt-parent = <&gic>;
++	#address-cells = <1>;
++	#size-cells = <1>;
++
++	aliases {
++		serial0 = &uart0;
++		serial1 = &uart1;
++	};
++
++	chosen {
++		stdout-path = "serial0:115200n8";
++	};
++
++	cpus {
++		#address-cells = <1>;
++		#size-cells = <0>;
++
++		cpu: cpu@0 {
++			device_type = "cpu";
++			compatible = "arm,cortex-a35";
++			reg = <0>;
++			next-level-cache = <&L2_0>;
++		};
++	};
++
++	memory@88200000 {
++		device_type = "memory";
++		reg = <0x88200000 0x77e00000>;
++	};
++
++	gic: interrupt-controller@1c000000 {
++		compatible = "arm,gic-400";
++		#interrupt-cells = <3>;
++		#address-cells = <0>;
++		interrupt-controller;
++		reg =	<0x1c010000 0x1000>,
++			<0x1c02f000 0x2000>,
++			<0x1c04f000 0x1000>,
++			<0x1c06f000 0x2000>;
++		interrupts = <GIC_PPI 9 (GIC_CPU_MASK_SIMPLE(1) |
++			      IRQ_TYPE_LEVEL_LOW)>;
++	};
++
++	L2_0: l2-cache0 {
++		compatible = "cache";
++		cache-level = <2>;
++		cache-size = <0x80000>;
++		cache-line-size = <64>;
++		cache-sets = <1024>;
++	};
++
++	refclk100mhz: refclk100mhz {
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <100000000>;
++		clock-output-names = "apb_pclk";
++	};
++
++	smbclk: refclk24mhzx2 {
++		/* Reference 24MHz clock x 2 */
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <48000000>;
++		clock-output-names = "smclk";
++	};
++
++	timer {
++		compatible = "arm,armv8-timer";
++		interrupts =	<GIC_PPI 13 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 14 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 11 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 10 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>;
++	};
++
++	uartclk: uartclk {
++		/* UART clock - 50MHz */
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <50000000>;
++		clock-output-names = "uartclk";
++	};
++
++	psci {
++		compatible = "arm,psci-1.0", "arm,psci-0.2";
++		method = "smc";
++	};
++
++	soc {
++		compatible = "simple-bus";
++		#address-cells = <1>;
++		#size-cells = <1>;
++		interrupt-parent = <&gic>;
++		ranges;
++
++		timer@1a220000 {
++			compatible = "arm,armv7-timer-mem";
++			reg = <0x1a220000 0x1000>;
++			#address-cells = <1>;
++			#size-cells = <1>;
++			clock-frequency = <50000000>;
++			ranges;
++
++			frame@1a230000 {
++				frame-number = <0>;
++				interrupts = <GIC_SPI 2 IRQ_TYPE_LEVEL_HIGH>;
++				reg = <0x1a230000 0x1000>;
++			};
++		};
++
++		uart0: serial@1a510000 {
++			compatible = "arm,pl011", "arm,primecell";
++			reg = <0x1a510000 0x1000>;
++			interrupts = <GIC_SPI 19 IRQ_TYPE_LEVEL_HIGH>;
++			clocks = <&uartclk>, <&refclk100mhz>;
++			clock-names = "uartclk", "apb_pclk";
++		};
++
++		uart1: serial@1a520000 {
++			compatible = "arm,pl011", "arm,primecell";
++			reg = <0x1a520000 0x1000>;
++			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
++			clocks = <&uartclk>, <&refclk100mhz>;
++			clock-names = "uartclk", "apb_pclk";
++		};
++
++		mhu_hse1: mailbox@1b820000 {
++			compatible = "arm,mhuv2-tx", "arm,primecell";
++			reg = <0x1b820000 0x1000>;
++			clocks = <&refclk100mhz>;
++			clock-names = "apb_pclk";
++			interrupts = <GIC_SPI 45 IRQ_TYPE_LEVEL_HIGH>;
++			#mbox-cells = <2>;
++			arm,mhuv2-protocols = <0 0>;
++			secure-status = "okay";     /* secure-world-only */
++			status = "disabled";
++		};
++
++		mhu_seh1: mailbox@1b830000 {
++			compatible = "arm,mhuv2-rx", "arm,primecell";
++			reg = <0x1b830000 0x1000>;
++			clocks = <&refclk100mhz>;
++			clock-names = "apb_pclk";
++			interrupts = <GIC_SPI 46 IRQ_TYPE_LEVEL_HIGH>;
++			#mbox-cells = <2>;
++			arm,mhuv2-protocols = <0 0>;
++			secure-status = "okay";     /* secure-world-only */
++			status = "disabled";
++		};
++	};
++
++	arm_ffa: arm_ffa {
++		compatible = "arm,ffa";
++		method = "smc";
++	};
++};
+diff --git a/board/armltd/corstone1000/Kconfig b/board/armltd/corstone1000/Kconfig
+new file mode 100644
+index 000000000000..709674d4cf7d
+--- /dev/null
++++ b/board/armltd/corstone1000/Kconfig
+@@ -0,0 +1,12 @@
++if TARGET_CORSTONE1000
++
++config SYS_BOARD
++	default "corstone1000"
++
++config SYS_VENDOR
++	default "armltd"
++
++config SYS_CONFIG_NAME
++	default "corstone1000"
++
++endif
+diff --git a/board/armltd/corstone1000/MAINTAINERS b/board/armltd/corstone1000/MAINTAINERS
+new file mode 100644
+index 000000000000..8c905686de76
+--- /dev/null
++++ b/board/armltd/corstone1000/MAINTAINERS
+@@ -0,0 +1,7 @@
++CORSTONE1000 BOARD
++M:	Rui Miguel Silva <rui.silva@linaro.org>
++M:	Vishnu Banavath <vishnu.banavath@arm.com>
++S:	Maintained
++F:	board/armltd/corstone1000/
++F:	include/configs/corstone1000.h
++F:	configs/corstone1000_defconfig
+diff --git a/board/armltd/corstone1000/Makefile b/board/armltd/corstone1000/Makefile
+new file mode 100644
+index 000000000000..77a82c28929b
+--- /dev/null
++++ b/board/armltd/corstone1000/Makefile
+@@ -0,0 +1,7 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2022 Arm Limited
++# (C) Copyright 2022 Linaro
++# Rui Miguel Silva <rui.silva@linaro.org>
++
++obj-y	:= corstone1000.o
+diff --git a/board/armltd/corstone1000/corstone1000.c b/board/armltd/corstone1000/corstone1000.c
+new file mode 100644
+index 000000000000..eff1739f0b02
+--- /dev/null
++++ b/board/armltd/corstone1000/corstone1000.c
+@@ -0,0 +1,121 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/platform_data/serial_pl01x.h>
++#include <asm/armv8/mmu.h>
++#include <asm/global_data.h>
++
++
++static const struct pl01x_serial_plat serial_plat = {
++	.base = V2M_UART0,
++	.type = TYPE_PL011,
++	.clock = CONFIG_PL011_CLOCK,
++};
++
++U_BOOT_DRVINFO(corstone1000_serials) = {
++	.name = "serial_pl01x",
++	.plat = &serial_plat,
++};
++
++static struct mm_region corstone1000_mem_map[] = {
++	{
++		/* CVM */
++		.virt = 0x02000000UL,
++		.phys = 0x02000000UL,
++		.size = 0x02000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* QSPI */
++		.virt = 0x08000000UL,
++		.phys = 0x08000000UL,
++		.size = 0x08000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* Host Peripherals */
++		.virt = 0x1A000000UL,
++		.phys = 0x1A000000UL,
++		.size = 0x26000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++			 PTE_BLOCK_NON_SHARE |
++			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
++        }, {
++                /* USB */
++                .virt = 0x40200000UL,
++                .phys = 0x40200000UL,
++                .size = 0x00100000UL,
++                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++                         PTE_BLOCK_NON_SHARE |
++                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++                 /* ethernet */
++                .virt = 0x40100000UL,
++                .phys = 0x40100000UL,
++                .size = 0x00100000UL,
++                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++                         PTE_BLOCK_NON_SHARE |
++                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* OCVM */
++		.virt = 0x80000000UL,
++		.phys = 0x80000000UL,
++		.size = 0x80000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* List terminator */
++		0,
++	}
++};
++
++struct mm_region *mem_map = corstone1000_mem_map;
++
++int board_init(void)
++{
++	return 0;
++}
++
++int dram_init(void)
++{
++	gd->ram_size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++int dram_init_banksize(void)
++{
++	gd->bd->bi_dram[0].start = PHYS_SDRAM_1;
++	gd->bd->bi_dram[0].size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++/*
++ * Board specific ethernet initialization routine.
++ * */
++int board_eth_init(struct bd_info *bis)
++{
++	int rc = 0;
++
++#ifndef CONFIG_DM_ETH
++#ifdef CONFIG_SMC91111
++	rc = smc91111_initialize(0, CONFIG_SMC91111_BASE);
++#endif
++#ifdef CONFIG_SMC911X
++	rc = smc911x_initialize(0, CONFIG_SMC911X_BASE);
++#endif
++#endif
++
++	return rc;
++}
++
++void reset_cpu(ulong addr)
++{
++}
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+new file mode 100644
+index 000000000000..02f931b0d469
+--- /dev/null
++++ b/configs/corstone1000_defconfig
+@@ -0,0 +1,80 @@
++CONFIG_ARM=y
++CONFIG_TARGET_CORSTONE1000=y
++CONFIG_SYS_TEXT_BASE=0x80000000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_SYS_MALLOC_LEN=0x2000000
++CONFIG_SYS_LOAD_ADDR=0x82100000
++CONFIG_NR_DRAM_BANKS=1
++CONFIG_IDENT_STRING=" corstone1000 aarch64 "
++CONFIG_FIT=y
++CONFIG_BOOTDELAY=3
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 loglevel=9 ip=dhcp earlyprintk"
++CONFIG_LOGLEVEL=7
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="corstone1000# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++CONFIG_CMD_BOOTM=y
++CONFIG_CMD_LOADM=y
++CONFIG_CMD_BOOTEFI=y
++CONFIG_EFI_LOADER=y
++CONFIG_EFI_PARTITION=y
++CONFIG_CMD_BOOTEFI_HELLO_COMPILE=y
++CONFIG_CMD_BOOTEFI_HELLO=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_NVEDIT_EFI=y
++# CONFIG_CMD_LOADS is not set
++CONFIG_CMD_USB=y
++CONFIG_CMD_ITEST=y
++# CONFIG_CMD_SETEXPR is not set
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_EFIDEBUG=y
++CONFIG_CMD_FAT=y
++CONFIG_OF_CONTROL=y
++CONFIG_REGMAP=y
++# CONFIG_MMC is not set
++CONFIG_DM_SERIAL=y
++CONFIG_USB=y
++CONFIG_DM_USB=y
++CONFIG_USB_STORAGE=y
++CONFIG_EFI_MM_COMM_TEE=y
++# CONFIG_OPTEE is not set
++# CONFIG_GENERATE_SMBIOS_TABLE is not set
++# CONFIG_HEXDUMP is not set
++CONFIG_EFI_RUNTIME_UPDATE_CAPSULE=y
++CONFIG_EFI_CAPSULE_ON_DISK=y
++# CONFIG_EFI_CAPSULE_ON_DISK_EARLY is not set
++# CONFIG_EFI_CAPSULE_AUTHENTICATE is not set
++CONFIG_EFI_HAVE_CAPSULE_SUPPORT=y
++CONFIG_EFI_CAPSULE_FIRMWARE_FIT=y
++CONFIG_EFI_CAPSULE_FIRMWARE_RAW=y
++CONFIG_EFI_SECURE_BOOT=y
++CONFIG_DM_RTC=y
++CONFIG_CMD_RTC=y
++CONFIG_EFI_GET_TIME=y
++CONFIG_EFI_SET_TIME=y
++CONFIG_RTC_EMULATION=y
++CONFIG_PSCI_RESET=y
++CONFIG_DISTRO_DEFAULTS=y
++CONFIG_CMD_DHCP=y
++CONFIG_SMC911X=y
++CONFIG_SMC911X_BASE=0x40100000
++CONFIG_DM_ETH=y
++CONFIG_PHY_SMSC=y
++CONFIG_CMD_BOOTEFI_SELFTEST=y
++CONFIG_CMD_TIME=y
++CONFIG_CMD_GETTIME=y
++CONFIG_NET_RANDOM_ETHADDR=y
++CONFIG_VERSION_VARIABLE=y
++CONFIG_PHYLIB=y
++CONFIG_PHY=y
++CONFIG_RAM=y
++CONFIG_ERRNO_STR=y
++CONFIG_CMD_EDITENV=y
++CONFIG_MISC=y
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+new file mode 100644
+index 000000000000..cf166f107efd
+--- /dev/null
++++ b/include/configs/corstone1000.h
+@@ -0,0 +1,86 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ *
++ * Configuration for Corstone1000. Parts were derived from other ARM
++ * configurations.
++ */
++
++#ifndef __CORSTONE1000_H
++#define __CORSTONE1000_H
++
++#include <linux/sizes.h>
++
++#define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
++#define CONFIG_SKIP_LOWLEVEL_INIT
++
++#define CONFIG_SYS_HZ		1000
++
++#define V2M_SRAM0		0x02000000
++#define V2M_QSPI		0x08000000
++
++#define V2M_DEBUG		0x10000000
++#define V2M_BASE_PERIPH		0x1A000000
++
++#define V2M_BASE		0x80000000
++
++#define V2M_PERIPH_OFFSET(x)	(x << 16)
++
++#define V2M_SYSID		(V2M_BASE_PERIPH)
++#define V2M_SYSCTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(1))
++
++#define V2M_COUNTER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(32))
++#define V2M_COUNTER_READ	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(33))
++
++#define V2M_TIMER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(34))
++#define V2M_TIMER_BASE0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(35))
++
++#define V2M_UART0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(81))
++#define V2M_UART1		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(82))
++
++#define CONFIG_PL011_CLOCK	50000000
++
++/* Physical Memory Map */
++#define PHYS_SDRAM_1		(V2M_BASE)
++#define PHYS_SDRAM_1_SIZE	0x80000000
++
++#define CONFIG_ENV_SECT_SIZE	SZ_64K
++
++#define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
++
++/* Monitor Command Prompt */
++#define CONFIG_SYS_CBSIZE	512	/* Console I/O Buffer Size */
++#define CONFIG_SYS_MAXARGS	64	/* max command args */
++
++#define CONFIG_EXTRA_ENV_SETTINGS							\
++				"usb_pgood_delay=250\0"					\
++				"boot_bank_flag=0x08002000\0"				\
++				"kernel_addr_bank_0=0x083EE000\0"			\
++				"kernel_addr_bank_1=0x0936E000\0"			\
++				"retrieve_kernel_load_addr="				\
++					"if itest.l *${boot_bank_flag} == 0; then "	\
++					    "setenv kernel_addr $kernel_addr_bank_0;"	\
++					"else "						\
++					    "setenv kernel_addr $kernel_addr_bank_1;"	\
++					"fi;"						\
++					"\0"						\
++				"kernel_addr_r=0x88200000\0"				\
++				"fdt_high=0xffffffff\0"
++
++/*
++ * config_distro_bootcmd define the boot command to distro_bootcmd, but we here
++ * want to first try to load a kernel if exists, override that config then
++ */
++#undef CONFIG_BOOTCOMMAND
++
++#define CONFIG_BOOTCOMMAND								\
++				"run retrieve_kernel_load_addr;"			\
++				"echo Loading kernel from $kernel_addr to memory ... ;"	\
++				"loadm $kernel_addr $kernel_addr_r 0xc00000;"		\
++				"usb start; usb reset;"					\
++				"run distro_bootcmd;"					\
++				"bootefi $kernel_addr_r $fdtcontroladdr;"
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch
new file mode 100644
index 0000000..9974915
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch
@@ -0,0 +1,497 @@
+From b8f4f76e135ff2061d0032292f5209ac911dba16 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Mon, 28 Jun 2021 23:20:55 +0100
+Subject: [PATCH 03/27] usb: common: move urb code to common
+
+Move urb code from musb only use to a more common scope, so other
+drivers in the future can use the handling of urb in usb.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ drivers/usb/common/Makefile                   |   2 +
+ drivers/usb/common/usb_urb.c                  | 160 ++++++++++++++++++
+ drivers/usb/host/r8a66597-hcd.c               |  30 +---
+ drivers/usb/musb-new/musb_core.c              |   2 +-
+ drivers/usb/musb-new/musb_host.c              |   2 +-
+ drivers/usb/musb-new/musb_host.h              |   2 +-
+ drivers/usb/musb-new/musb_uboot.c             |  38 +----
+ drivers/usb/musb-new/musb_uboot.h             |   2 +-
+ .../linux/usb/usb_urb_compat.h                |  46 ++++-
+ include/usb_defs.h                            |  32 ++++
+ 10 files changed, 240 insertions(+), 76 deletions(-)
+ create mode 100644 drivers/usb/common/usb_urb.c
+ rename drivers/usb/musb-new/usb-compat.h => include/linux/usb/usb_urb_compat.h (60%)
+
+diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
+index 3bedbf213f47..dc05cb0a5077 100644
+--- a/drivers/usb/common/Makefile
++++ b/drivers/usb/common/Makefile
+@@ -4,5 +4,7 @@
+ #
+ 
+ obj-$(CONFIG_$(SPL_)DM_USB) += common.o
++obj-$(CONFIG_USB_MUSB_HCD) += usb_urb.o
++obj-$(CONFIG_USB_MUSB_UDC) += usb_urb.o
+ obj-$(CONFIG_USB_EHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+ obj-$(CONFIG_USB_XHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+diff --git a/drivers/usb/common/usb_urb.c b/drivers/usb/common/usb_urb.c
+new file mode 100644
+index 000000000000..be3b6b9f32e8
+--- /dev/null
++++ b/drivers/usb/common/usb_urb.c
+@@ -0,0 +1,160 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Common code for usb urb handling, based on the musb-new code
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <dm/device.h>
++#include <dm/device_compat.h>
++#include <linux/usb/usb_urb_compat.h>
++
++#include <time.h>
++#include <usb.h>
++
++#if CONFIG_IS_ENABLED(DM_USB)
++struct usb_device *usb_dev_get_parent(struct usb_device *udev)
++{
++	struct udevice *parent = udev->dev->parent;
++
++	/*
++	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
++	 * to the parent udevice, not the actual udevice belonging to the
++	 * udev as the device is not instantiated yet.
++	 *
++	 * If dev is an usb-bus, then we are called from usb_scan_device() for
++	 * an usb-device plugged directly into the root port, return NULL.
++	 */
++	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
++		return NULL;
++
++	/*
++	 * If these 2 are not the same we are being called from
++	 * usb_scan_device() and udev itself is the parent.
++	 */
++	if (dev_get_parent_priv(udev->dev) != udev)
++		return udev;
++
++	/* We are being called normally, use the parent pointer */
++	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
++		return dev_get_parent_priv(parent);
++
++	return NULL;
++}
++#else
++struct usb_device *usb_dev_get_parent(struct usb_device *udev)
++{
++	return udev->parent;
++}
++#endif
++
++static void usb_urb_complete(struct urb *urb)
++{
++	urb->dev->status &= ~USB_ST_NOT_PROC;
++	urb->dev->act_len = urb->actual_length;
++
++	if (urb->status == -EINPROGRESS)
++		urb->status = 0;
++}
++
++void usb_urb_fill(struct urb *urb, struct usb_host_endpoint *hep,
++		  struct usb_device *dev, int endpoint_type,
++		  unsigned long pipe, void *buffer, int len,
++		  struct devrequest *setup, int interval)
++{
++	int epnum = usb_pipeendpoint(pipe);
++	int is_in = usb_pipein(pipe);
++	u16 maxpacketsize = is_in ? dev->epmaxpacketin[epnum] :
++					dev->epmaxpacketout[epnum];
++
++	memset(urb, 0, sizeof(struct urb));
++	memset(hep, 0, sizeof(struct usb_host_endpoint));
++	INIT_LIST_HEAD(&hep->urb_list);
++	INIT_LIST_HEAD(&urb->urb_list);
++	urb->ep = hep;
++	urb->complete = usb_urb_complete;
++	urb->status = -EINPROGRESS;
++	urb->dev = dev;
++	urb->pipe = pipe;
++	urb->transfer_buffer = buffer;
++	urb->transfer_dma = (unsigned long)buffer;
++	urb->transfer_buffer_length = len;
++	urb->setup_packet = (unsigned char *)setup;
++
++	urb->ep->desc.wMaxPacketSize = __cpu_to_le16(maxpacketsize);
++	urb->ep->desc.bmAttributes = endpoint_type;
++	urb->ep->desc.bEndpointAddress = ((is_in ? USB_DIR_IN : USB_DIR_OUT) |
++					  epnum);
++	urb->ep->desc.bInterval = interval;
++}
++
++int usb_urb_submit(struct usb_hcd *hcd, struct urb *urb)
++{
++	const struct usb_urb_ops *ops = hcd->urb_ops;
++	unsigned long timeout;
++	int ret;
++
++	if (!ops)
++		return -EINVAL;
++
++	ret = ops->urb_enqueue(hcd, urb, 0);
++	if (ret < 0) {
++		printf("Failed to enqueue URB to controller\n");
++		return ret;
++	}
++
++	timeout = get_timer(0) + USB_TIMEOUT_MS(urb->pipe);
++	do {
++		if (ctrlc())
++			return -EIO;
++		ops->isr(0, hcd);
++	} while (urb->status == -EINPROGRESS && get_timer(0) < timeout);
++
++	if (urb->status == -EINPROGRESS)
++		ops->urb_dequeue(hcd, urb, -ETIME);
++
++	return urb->status;
++}
++
++int usb_urb_submit_control(struct usb_hcd *hcd, struct urb *urb,
++			   struct usb_host_endpoint *hep,
++			   struct usb_device *dev, unsigned long pipe,
++			   void *buffer, int len, struct devrequest *setup,
++			   int interval, enum usb_device_speed speed)
++{
++	const struct usb_urb_ops *ops = hcd->urb_ops;
++
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_CONTROL, pipe, buffer,
++		     len, setup, 0);
++
++	/* Fix speed for non hub-attached devices */
++	if (!usb_dev_get_parent(dev)) {
++		dev->speed = speed;
++		if (ops->hub_control)
++			return ops->hub_control(hcd, dev, pipe, buffer, len,
++						setup);
++	}
++
++	return usb_urb_submit(hcd, urb);
++}
++
++int usb_urb_submit_bulk(struct usb_hcd *hcd, struct urb *urb,
++			struct usb_host_endpoint *hep, struct usb_device *dev,
++			unsigned long pipe, void *buffer, int len)
++{
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_BULK, pipe, buffer, len,
++		     NULL, 0);
++
++	return usb_urb_submit(hcd, urb);
++}
++
++int usb_urb_submit_irq(struct usb_hcd *hcd, struct urb *urb,
++		       struct usb_host_endpoint *hep, struct usb_device *dev,
++		       unsigned long pipe, void *buffer, int len, int interval)
++{
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_INT, pipe, buffer, len,
++		     NULL, interval);
++
++	return usb_urb_submit(hcd, urb);
++}
+diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
+index f1fc93f3d403..3ccbc16da379 100644
+--- a/drivers/usb/host/r8a66597-hcd.c
++++ b/drivers/usb/host/r8a66597-hcd.c
+@@ -14,6 +14,7 @@
+ #include <dm/device_compat.h>
+ #include <linux/delay.h>
+ #include <linux/iopoll.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include <power/regulator.h>
+ 
+ #include "r8a66597.h"
+@@ -24,35 +25,6 @@
+ #define R8A66597_DPRINT(...)
+ #endif
+ 
+-static inline struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	struct udevice *parent = udev->dev->parent;
+-
+-	/*
+-	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
+-	 * to the parent udevice, not the actual udevice belonging to the
+-	 * udev as the device is not instantiated yet.
+-	 *
+-	 * If dev is an usb-bus, then we are called from usb_scan_device() for
+-	 * an usb-device plugged directly into the root port, return NULL.
+-	 */
+-	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
+-		return NULL;
+-
+-	/*
+-	 * If these 2 are not the same we are being called from
+-	 * usb_scan_device() and udev itself is the parent.
+-	 */
+-	if (dev_get_parent_priv(udev->dev) != udev)
+-		return udev;
+-
+-	/* We are being called normally, use the parent pointer */
+-	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
+-		return dev_get_parent_priv(parent);
+-
+-	return NULL;
+-}
+-
+ static void get_hub_data(struct usb_device *dev, u16 *hub_devnum, u16 *hubport)
+ {
+ 	struct usb_device *parent = usb_dev_get_parent(dev);
+diff --git a/drivers/usb/musb-new/musb_core.c b/drivers/usb/musb-new/musb_core.c
+index 18d9bc805f8a..fc7af7484e4c 100644
+--- a/drivers/usb/musb-new/musb_core.c
++++ b/drivers/usb/musb-new/musb_core.c
+@@ -89,9 +89,9 @@
+ #include <linux/usb/ch9.h>
+ #include <linux/usb/gadget.h>
+ #include <linux/usb/musb.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include <asm/io.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #endif
+ 
+ #include "musb_core.h"
+diff --git a/drivers/usb/musb-new/musb_host.c b/drivers/usb/musb-new/musb_host.c
+index acb2d40f3b5a..e5905d90d66f 100644
+--- a/drivers/usb/musb-new/musb_host.c
++++ b/drivers/usb/musb-new/musb_host.c
+@@ -26,8 +26,8 @@
+ #include <dm/device_compat.h>
+ #include <usb.h>
+ #include <linux/bug.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #endif
+ 
+ #include "musb_core.h"
+diff --git a/drivers/usb/musb-new/musb_host.h b/drivers/usb/musb-new/musb_host.h
+index afc8fa35a738..5a604bdb0cf2 100644
+--- a/drivers/usb/musb-new/musb_host.h
++++ b/drivers/usb/musb-new/musb_host.h
+@@ -10,7 +10,7 @@
+ #ifndef _MUSB_HOST_H
+ #define _MUSB_HOST_H
+ #ifdef __UBOOT__
+-#include "usb-compat.h"
++#include <linux/usb/usb_urb_compat.h>
+ #endif
+ 
+ static inline struct usb_hcd *musb_to_hcd(struct musb *musb)
+diff --git a/drivers/usb/musb-new/musb_uboot.c b/drivers/usb/musb-new/musb_uboot.c
+index 61ff68def2fa..d186facc7e02 100644
+--- a/drivers/usb/musb-new/musb_uboot.c
++++ b/drivers/usb/musb-new/musb_uboot.c
+@@ -8,10 +8,10 @@
+ #include <linux/errno.h>
+ #include <linux/usb/ch9.h>
+ #include <linux/usb/gadget.h>
++#include <linux/usb/usb_urb_compat.h>
+ 
+ #include <usb.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #include "musb_core.h"
+ #include "musb_host.h"
+ #include "musb_gadget.h"
+@@ -453,39 +453,3 @@ struct musb *musb_register(struct musb_hdrc_platform_data *plat, void *bdata,
+ 
+ 	return *musbp;
+ }
+-
+-#if CONFIG_IS_ENABLED(DM_USB)
+-struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	struct udevice *parent = udev->dev->parent;
+-
+-	/*
+-	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
+-	 * to the parent udevice, not the actual udevice belonging to the
+-	 * udev as the device is not instantiated yet.
+-	 *
+-	 * If dev is an usb-bus, then we are called from usb_scan_device() for
+-	 * an usb-device plugged directly into the root port, return NULL.
+-	 */
+-	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
+-		return NULL;
+-
+-	/*
+-	 * If these 2 are not the same we are being called from
+-	 * usb_scan_device() and udev itself is the parent.
+-	 */
+-	if (dev_get_parent_priv(udev->dev) != udev)
+-		return udev;
+-
+-	/* We are being called normally, use the parent pointer */
+-	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
+-		return dev_get_parent_priv(parent);
+-
+-	return NULL;
+-}
+-#else
+-struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	return udev->parent;
+-}
+-#endif
+diff --git a/drivers/usb/musb-new/musb_uboot.h b/drivers/usb/musb-new/musb_uboot.h
+index 18282efccc9d..6b162f03b19e 100644
+--- a/drivers/usb/musb-new/musb_uboot.h
++++ b/drivers/usb/musb-new/musb_uboot.h
+@@ -8,8 +8,8 @@
+ #define __MUSB_UBOOT_H__
+ 
+ #include <usb.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #include "musb_core.h"
+ 
+ struct musb_host_data {
+diff --git a/drivers/usb/musb-new/usb-compat.h b/include/linux/usb/usb_urb_compat.h
+similarity index 60%
+rename from drivers/usb/musb-new/usb-compat.h
+rename to include/linux/usb/usb_urb_compat.h
+index df68c9220a7a..5ed96fa64e96 100644
+--- a/drivers/usb/musb-new/usb-compat.h
++++ b/include/linux/usb/usb_urb_compat.h
+@@ -1,16 +1,31 @@
+-#ifndef __USB_COMPAT_H__
+-#define __USB_COMPAT_H__
++#ifndef __USB_URB_COMPAT_H__
++#define __USB_URB_COMPAT_H__
+ 
+-#include "usb.h"
++#include <linux/compat.h>
++#include <usb.h>
+ 
+ struct udevice;
++struct urb;
++struct usb_hcd;
++
++
++struct usb_urb_ops {
++	int (*urb_enqueue)(struct usb_hcd *hcd, struct urb *urb,
++			   gfp_t mem_flags);
++	int (*urb_dequeue)(struct usb_hcd *hcd, struct urb *urb, int status);
++	int (*hub_control)(struct usb_hcd *hcd, struct usb_device *dev,
++			   unsigned long pipe, void *buffer, int len,
++			   struct devrequest *setup);
++	irqreturn_t (*isr)(int irq, void *priv);
++};
+ 
+ struct usb_hcd {
+ 	void *hcd_priv;
++	const struct usb_urb_ops *urb_ops;
+ };
+ 
+ struct usb_host_endpoint {
+-	struct usb_endpoint_descriptor		desc;
++	struct usb_endpoint_descriptor desc;
+ 	struct list_head urb_list;
+ 	void *hcpriv;
+ };
+@@ -23,8 +38,6 @@ struct usb_host_endpoint {
+ #define URB_SHORT_NOT_OK	0x0001	/* report short reads as errors */
+ #define URB_ZERO_PACKET		0x0040	/* Finish bulk OUT with short packet */
+ 
+-struct urb;
+-
+ typedef void (*usb_complete_t)(struct urb *);
+ 
+ struct urb {
+@@ -76,4 +89,25 @@ static inline int usb_hcd_unmap_urb_for_dma(struct usb_hcd *hcd,
+  */
+ struct usb_device *usb_dev_get_parent(struct usb_device *udev);
+ 
++int usb_urb_submit_control(struct usb_hcd *hcd, struct urb *urb,
++			   struct usb_host_endpoint *hep,
++			   struct usb_device *dev, unsigned long pipe,
++			   void *buffer, int len, struct devrequest *setup,
++			   int interval, enum usb_device_speed speed);
++
++int usb_urb_submit_bulk(struct usb_hcd *hcd, struct urb *urb,
++			struct usb_host_endpoint *hep, struct usb_device *dev,
++			unsigned long pipe, void *buffer, int len);
++
++int usb_urb_submit_irq(struct usb_hcd *hcd, struct urb *urb,
++		       struct usb_host_endpoint *hep, struct usb_device *dev,
++		       unsigned long pipe, void *buffer, int len, int interval);
++
++void usb_urb_fill(struct urb *urb, struct usb_host_endpoint *hep,
++		  struct usb_device *dev, int endpoint_type,
++		  unsigned long pipe, void *buffer, int len,
++		  struct devrequest *setup, int interval);
++
++int usb_urb_submit(struct usb_hcd *hcd, struct urb *urb);
++
+ #endif /* __USB_COMPAT_H__ */
+diff --git a/include/usb_defs.h b/include/usb_defs.h
+index 6dd2c997f9b3..ec00161710a5 100644
+--- a/include/usb_defs.h
++++ b/include/usb_defs.h
+@@ -81,6 +81,32 @@
+ #define EndpointOutRequest \
+ 	((USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_INTERFACE) << 8)
+ 
++/* class requests from the USB 2.0 hub spec, table 11-15 */
++#define HUB_CLASS_REQ(dir, type, request) ((((dir) | (type)) << 8) | (request))
++/* GetBusState and SetHubDescriptor are optional, omitted */
++#define ClearHubFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_HUB, \
++					      USB_REQ_CLEAR_FEATURE)
++#define ClearPortFeature	HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      USB_REQ_CLEAR_FEATURE)
++#define GetHubDescriptor	HUB_CLASS_REQ(USB_DIR_IN, USB_RT_HUB, \
++					      USB_REQ_GET_DESCRIPTOR)
++#define GetHubStatus		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_HUB, \
++					      USB_REQ_GET_STATUS)
++#define GetPortStatus		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_PORT, \
++					      USB_REQ_GET_STATUS)
++#define SetHubFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_HUB, \
++					      USB_REQ_SET_FEATURE)
++#define SetPortFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      USB_REQ_SET_FEATURE)
++#define ClearTTBuffer		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_CLEAR_TT_BUFFER)
++#define ResetTT			HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_RESET_TT)
++#define GetTTState		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_PORT, \
++					      HUB_GET_TT_STATE)
++#define StopTT			HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_STOP_TT)
++
+ /* Descriptor types */
+ #define USB_DT_DEVICE        0x01
+ #define USB_DT_CONFIG        0x02
+@@ -289,10 +315,16 @@
+ #define USB_SS_PORT_STAT_C_CONFIG_ERROR	0x0080
+ 
+ /* wHubCharacteristics (masks) */
++#define HUB_CHAR_COMMON_OCPM        0x0000 /* All ports Over-Current reporting */
++#define HUB_CHAR_INDV_PORT_LPSM     0x0001 /* per-port power control */
++#define HUB_CHAR_NO_LPSM            0x0002 /* no power switching */
+ #define HUB_CHAR_LPSM               0x0003
+ #define HUB_CHAR_COMPOUND           0x0004
++#define HUB_CHAR_INDV_PORT_OCPM     0x0008 /* per-port Over-current reporting */
++#define HUB_CHAR_NO_OCPM            0x0010 /* No Over-current Protection support */
+ #define HUB_CHAR_OCPM               0x0018
+ #define HUB_CHAR_TTTT               0x0060 /* TT Think Time mask */
++#define HUB_CHAR_PORTIND            0x0080 /* per-port indicators (LEDs) */
+ 
+ /*
+  * Hub Status & Hub Change bit masks
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch
new file mode 100644
index 0000000..5940f06
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch
@@ -0,0 +1,3805 @@
+From 62a666a83766f5517e90464638455286bb33f433 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Mon, 28 Jun 2021 23:31:25 +0100
+Subject: [PATCH 04/27] usb: add isp1760 family driver
+
+ISP1760/61/63 are a family of usb controllers, blah, blah, more info
+here.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ Makefile                            |    1 +
+ drivers/usb/Kconfig                 |    2 +
+ drivers/usb/common/Makefile         |    1 +
+ drivers/usb/isp1760/Kconfig         |   12 +
+ drivers/usb/isp1760/Makefile        |    6 +
+ drivers/usb/isp1760/isp1760-core.c  |  378 ++++
+ drivers/usb/isp1760/isp1760-core.h  |   96 +
+ drivers/usb/isp1760/isp1760-hcd.c   | 2574 +++++++++++++++++++++++++++
+ drivers/usb/isp1760/isp1760-hcd.h   |   82 +
+ drivers/usb/isp1760/isp1760-if.c    |  127 ++
+ drivers/usb/isp1760/isp1760-regs.h  |  292 +++
+ drivers/usb/isp1760/isp1760-uboot.c |   76 +
+ drivers/usb/isp1760/isp1760-uboot.h |   27 +
+ 13 files changed, 3674 insertions(+)
+ create mode 100644 drivers/usb/isp1760/Kconfig
+ create mode 100644 drivers/usb/isp1760/Makefile
+ create mode 100644 drivers/usb/isp1760/isp1760-core.c
+ create mode 100644 drivers/usb/isp1760/isp1760-core.h
+ create mode 100644 drivers/usb/isp1760/isp1760-hcd.c
+ create mode 100644 drivers/usb/isp1760/isp1760-hcd.h
+ create mode 100644 drivers/usb/isp1760/isp1760-if.c
+ create mode 100644 drivers/usb/isp1760/isp1760-regs.h
+ create mode 100644 drivers/usb/isp1760/isp1760-uboot.c
+ create mode 100644 drivers/usb/isp1760/isp1760-uboot.h
+
+diff --git a/Makefile b/Makefile
+index ad83d60dc39d..2c857c1fe2cc 100644
+--- a/Makefile
++++ b/Makefile
+@@ -834,6 +834,7 @@ libs-y += drivers/usb/host/
+ libs-y += drivers/usb/mtu3/
+ libs-y += drivers/usb/musb/
+ libs-y += drivers/usb/musb-new/
++libs-y += drivers/usb/isp1760/
+ libs-y += drivers/usb/phy/
+ libs-y += drivers/usb/ulpi/
+ ifdef CONFIG_POST
+diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
+index ab1d061bd0d5..bbe07be02cab 100644
+--- a/drivers/usb/Kconfig
++++ b/drivers/usb/Kconfig
+@@ -78,6 +78,8 @@ source "drivers/usb/musb/Kconfig"
+ 
+ source "drivers/usb/musb-new/Kconfig"
+ 
++source "drivers/usb/isp1760/Kconfig"
++
+ source "drivers/usb/emul/Kconfig"
+ 
+ source "drivers/usb/phy/Kconfig"
+diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
+index dc05cb0a5077..f08b064d2493 100644
+--- a/drivers/usb/common/Makefile
++++ b/drivers/usb/common/Makefile
+@@ -4,6 +4,7 @@
+ #
+ 
+ obj-$(CONFIG_$(SPL_)DM_USB) += common.o
++obj-$(CONFIG_USB_ISP1760) += usb_urb.o
+ obj-$(CONFIG_USB_MUSB_HCD) += usb_urb.o
+ obj-$(CONFIG_USB_MUSB_UDC) += usb_urb.o
+ obj-$(CONFIG_USB_EHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+diff --git a/drivers/usb/isp1760/Kconfig b/drivers/usb/isp1760/Kconfig
+new file mode 100644
+index 000000000000..993d71e74cd2
+--- /dev/null
++++ b/drivers/usb/isp1760/Kconfig
+@@ -0,0 +1,12 @@
++# SPDX-License-Identifier: GPL-2.0
++
++config USB_ISP1760
++	tristate "NXP ISP 1760/1761/1763 support"
++	select DM_USB
++	select USB_HOST
++	help
++	  Say Y or M here if your system as an ISP1760/1761/1763 USB host
++	  controller.
++
++	  This USB controller is usually attached to a non-DMA-Master
++	  capable bus.
+diff --git a/drivers/usb/isp1760/Makefile b/drivers/usb/isp1760/Makefile
+new file mode 100644
+index 000000000000..2c809c01b118
+--- /dev/null
++++ b/drivers/usb/isp1760/Makefile
+@@ -0,0 +1,6 @@
++# SPDX-License-Identifier: GPL-2.0
++isp1760-y := isp1760-core.o isp1760-if.o isp1760-uboot.o isp1760-hcd.o
++
++#isp1760-hcd.o
++
++obj-$(CONFIG_USB_ISP1760) += isp1760.o
+diff --git a/drivers/usb/isp1760/isp1760-core.c b/drivers/usb/isp1760/isp1760-core.c
+new file mode 100644
+index 000000000000..3080595549c5
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-core.c
+@@ -0,0 +1,378 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * This is based on linux kernel driver, original developed:
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ */
++
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <linux/compat.h>
++#include <linux/delay.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <regmap.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++
++#define msleep(a) udelay(a * 1000)
++
++static int isp1760_init_core(struct isp1760_device *isp)
++{
++	struct isp1760_hcd *hcd = &isp->hcd;
++
++	/*
++	 * Reset the host controller, including the CPU interface
++	 * configuration.
++	 */
++	isp1760_field_set(hcd->fields, SW_RESET_RESET_ALL);
++	msleep(100);
++
++	/* Setup HW Mode Control: This assumes a level active-low interrupt */
++	if ((isp->devflags & ISP1760_FLAG_ANALOG_OC) && hcd->is_isp1763)
++		return -EINVAL;
++
++	if (isp->devflags & ISP1760_FLAG_BUS_WIDTH_16)
++		isp1760_field_clear(hcd->fields, HW_DATA_BUS_WIDTH);
++	if (isp->devflags & ISP1760_FLAG_BUS_WIDTH_8)
++		isp1760_field_set(hcd->fields, HW_DATA_BUS_WIDTH);
++	if (isp->devflags & ISP1760_FLAG_ANALOG_OC)
++		isp1760_field_set(hcd->fields, HW_ANA_DIGI_OC);
++	if (isp->devflags & ISP1760_FLAG_DACK_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_DACK_POL_HIGH);
++	if (isp->devflags & ISP1760_FLAG_DREQ_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_DREQ_POL_HIGH);
++	if (isp->devflags & ISP1760_FLAG_INTR_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_INTR_HIGH_ACT);
++	if (isp->devflags & ISP1760_FLAG_INTR_EDGE_TRIG)
++		isp1760_field_set(hcd->fields, HW_INTR_EDGE_TRIG);
++
++	/*
++	 * The ISP1761 has a dedicated DC IRQ line but supports sharing the HC
++	 * IRQ line for both the host and device controllers. Hardcode IRQ
++	 * sharing for now and disable the DC interrupts globally to avoid
++	 * spurious interrupts during HCD registration.
++	 */
++	if (isp->devflags & ISP1760_FLAG_ISP1761) {
++		isp1760_reg_write(hcd->regs, ISP176x_DC_MODE, 0);
++		isp1760_field_set(hcd->fields, HW_COMN_IRQ);
++	}
++
++	/*
++	 * PORT 1 Control register of the ISP1760 is the OTG control register
++	 * on ISP1761.
++	 *
++	 * TODO: Really support OTG. For now we configure port 1 in device mode
++	 */
++	if (((isp->devflags & ISP1760_FLAG_ISP1761) ||
++	     (isp->devflags & ISP1760_FLAG_ISP1763)) &&
++	    (isp->devflags & ISP1760_FLAG_PERIPHERAL_EN)) {
++		isp1760_field_set(hcd->fields, HW_DM_PULLDOWN);
++		isp1760_field_set(hcd->fields, HW_DP_PULLDOWN);
++		isp1760_field_set(hcd->fields, HW_OTG_DISABLE);
++	} else {
++		isp1760_field_set(hcd->fields, HW_SW_SEL_HC_DC);
++		isp1760_field_set(hcd->fields, HW_VBUS_DRV);
++		isp1760_field_set(hcd->fields, HW_SEL_CP_EXT);
++	}
++
++	printf( "%s bus width: %u, oc: %s\n",
++		 hcd->is_isp1763 ? "isp1763" : "isp1760",
++		 isp->devflags & ISP1760_FLAG_BUS_WIDTH_8 ? 8 :
++		 isp->devflags & ISP1760_FLAG_BUS_WIDTH_16 ? 16 : 32,
++		 hcd->is_isp1763 ? "not available" :
++		 isp->devflags & ISP1760_FLAG_ANALOG_OC ? "analog" : "digital");
++
++	return 0;
++}
++
++void isp1760_set_pullup(struct isp1760_device *isp, bool enable)
++{
++	struct isp1760_hcd *hcd = &isp->hcd;
++
++	if (enable)
++		isp1760_field_set(hcd->fields, HW_DP_PULLUP);
++	else
++		isp1760_field_set(hcd->fields, HW_DP_PULLUP_CLEAR);
++}
++
++/*
++ * ISP1760/61:
++ *
++ * 60kb divided in:
++ * - 32 blocks @ 256  bytes
++ * - 20 blocks @ 1024 bytes
++ * -  4 blocks @ 8192 bytes
++ */
++static const struct isp1760_memory_layout isp176x_memory_conf = {
++	.blocks[0]		= 32,
++	.blocks_size[0]		= 256,
++	.blocks[1]		= 20,
++	.blocks_size[1]		= 1024,
++	.blocks[2]		= 4,
++	.blocks_size[2]		= 8192,
++
++	.slot_num		= 32,
++	.payload_blocks		= 32 + 20 + 4,
++	.payload_area_size	= 0xf000,
++};
++
++/*
++ * ISP1763:
++ *
++ * 20kb divided in:
++ * - 8 blocks @ 256  bytes
++ * - 2 blocks @ 1024 bytes
++ * - 4 blocks @ 4096 bytes
++ */
++static const struct isp1760_memory_layout isp1763_memory_conf = {
++	.blocks[0]		= 8,
++	.blocks_size[0]		= 256,
++	.blocks[1]		= 2,
++	.blocks_size[1]		= 1024,
++	.blocks[2]		= 4,
++	.blocks_size[2]		= 4096,
++
++	.slot_num		= 16,
++	.payload_blocks		= 8 + 2 + 4,
++	.payload_area_size	= 0x5000,
++};
++
++static const struct regmap_config isp1760_hc_regmap_conf = {
++	.width = REGMAP_SIZE_16,
++};
++
++static const struct reg_field isp1760_hc_reg_fields[] = {
++	[HCS_PPC]		= REG_FIELD(ISP176x_HC_HCSPARAMS, 4, 4),
++	[HCS_N_PORTS]		= REG_FIELD(ISP176x_HC_HCSPARAMS, 0, 3),
++	[HCC_ISOC_CACHE]	= REG_FIELD(ISP176x_HC_HCCPARAMS, 7, 7),
++	[HCC_ISOC_THRES]	= REG_FIELD(ISP176x_HC_HCCPARAMS, 4, 6),
++	[CMD_LRESET]		= REG_FIELD(ISP176x_HC_USBCMD, 7, 7),
++	[CMD_RESET]		= REG_FIELD(ISP176x_HC_USBCMD, 1, 1),
++	[CMD_RUN]		= REG_FIELD(ISP176x_HC_USBCMD, 0, 0),
++	[STS_PCD]		= REG_FIELD(ISP176x_HC_USBSTS, 2, 2),
++	[HC_FRINDEX]		= REG_FIELD(ISP176x_HC_FRINDEX, 0, 13),
++	[FLAG_CF]		= REG_FIELD(ISP176x_HC_CONFIGFLAG, 0, 0),
++	[HC_ISO_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_ISO_PTD_DONEMAP, 0, 31),
++	[HC_ISO_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_ISO_PTD_SKIPMAP, 0, 31),
++	[HC_ISO_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_ISO_PTD_LASTPTD, 0, 31),
++	[HC_INT_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_INT_PTD_DONEMAP, 0, 31),
++	[HC_INT_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_INT_PTD_SKIPMAP, 0, 31),
++	[HC_INT_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_INT_PTD_LASTPTD, 0, 31),
++	[HC_ATL_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_ATL_PTD_DONEMAP, 0, 31),
++	[HC_ATL_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_ATL_PTD_SKIPMAP, 0, 31),
++	[HC_ATL_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_ATL_PTD_LASTPTD, 0, 31),
++	[PORT_OWNER]		= REG_FIELD(ISP176x_HC_PORTSC1, 13, 13),
++	[PORT_POWER]		= REG_FIELD(ISP176x_HC_PORTSC1, 12, 12),
++	[PORT_LSTATUS]		= REG_FIELD(ISP176x_HC_PORTSC1, 10, 11),
++	[PORT_RESET]		= REG_FIELD(ISP176x_HC_PORTSC1, 8, 8),
++	[PORT_SUSPEND]		= REG_FIELD(ISP176x_HC_PORTSC1, 7, 7),
++	[PORT_RESUME]		= REG_FIELD(ISP176x_HC_PORTSC1, 6, 6),
++	[PORT_PE]		= REG_FIELD(ISP176x_HC_PORTSC1, 2, 2),
++	[PORT_CSC]		= REG_FIELD(ISP176x_HC_PORTSC1, 1, 1),
++	[PORT_CONNECT]		= REG_FIELD(ISP176x_HC_PORTSC1, 0, 0),
++	[ALL_ATX_RESET]		= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 31, 31),
++	[HW_ANA_DIGI_OC]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 15, 15),
++	[HW_COMN_IRQ]		= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 10, 10),
++	[HW_DATA_BUS_WIDTH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 8, 8),
++	[HW_DACK_POL_HIGH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 6, 6),
++	[HW_DREQ_POL_HIGH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 5, 5),
++	[HW_INTR_HIGH_ACT]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 2, 2),
++	[HW_INTR_EDGE_TRIG]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 1, 1),
++	[HW_GLOBAL_INTR_EN]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 0, 0),
++	[HC_CHIP_REV]		= REG_FIELD(ISP176x_HC_CHIP_ID, 16, 31),
++	[HC_CHIP_ID_HIGH]	= REG_FIELD(ISP176x_HC_CHIP_ID, 8, 15),
++	[HC_CHIP_ID_LOW]	= REG_FIELD(ISP176x_HC_CHIP_ID, 0, 7),
++	[HC_SCRATCH]		= REG_FIELD(ISP176x_HC_SCRATCH, 0, 31),
++	[SW_RESET_RESET_ALL]	= REG_FIELD(ISP176x_HC_RESET, 0, 0),
++	[ISO_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 2, 2),
++	[INT_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 1, 1),
++	[ATL_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 0, 0),
++	[MEM_BANK_SEL]		= REG_FIELD(ISP176x_HC_MEMORY, 16, 17),
++	[MEM_START_ADDR]	= REG_FIELD(ISP176x_HC_MEMORY, 0, 15),
++	[HC_INTERRUPT]		= REG_FIELD(ISP176x_HC_INTERRUPT, 0, 9),
++	[HC_ATL_IRQ_ENABLE]	= REG_FIELD(ISP176x_HC_INTERRUPT_ENABLE, 8, 8),
++	[HC_INT_IRQ_ENABLE]	= REG_FIELD(ISP176x_HC_INTERRUPT_ENABLE, 7, 7),
++	[HC_ISO_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_ISO_IRQ_MASK_OR, 0, 31),
++	[HC_INT_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_INT_IRQ_MASK_OR, 0, 31),
++	[HC_ATL_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_ATL_IRQ_MASK_OR, 0, 31),
++	[HC_ISO_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_ISO_IRQ_MASK_AND, 0, 31),
++	[HC_INT_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_INT_IRQ_MASK_AND, 0, 31),
++	[HC_ATL_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_ATL_IRQ_MASK_AND, 0, 31),
++	[HW_OTG_DISABLE]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 10, 10),
++	[HW_SW_SEL_HC_DC]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 7, 7),
++	[HW_VBUS_DRV]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 4, 4),
++	[HW_SEL_CP_EXT]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 3, 3),
++	[HW_DM_PULLDOWN]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 2, 2),
++	[HW_DP_PULLDOWN]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 1, 1),
++	[HW_DP_PULLUP]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 0, 0),
++	[HW_OTG_DISABLE_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 10, 10),
++	[HW_SW_SEL_HC_DC_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 7, 7),
++	[HW_VBUS_DRV_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 4, 4),
++	[HW_SEL_CP_EXT_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 3, 3),
++	[HW_DM_PULLDOWN_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 2, 2),
++	[HW_DP_PULLDOWN_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 1, 1),
++	[HW_DP_PULLUP_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 0, 0),
++};
++
++static const struct regmap_config isp1763_hc_regmap_conf = {
++	.width = REGMAP_SIZE_16,
++};
++
++static const struct reg_field isp1763_hc_reg_fields[] = {
++	[CMD_LRESET]		= REG_FIELD(ISP1763_HC_USBCMD, 7, 7),
++	[CMD_RESET]		= REG_FIELD(ISP1763_HC_USBCMD, 1, 1),
++	[CMD_RUN]		= REG_FIELD(ISP1763_HC_USBCMD, 0, 0),
++	[STS_PCD]		= REG_FIELD(ISP1763_HC_USBSTS, 2, 2),
++	[HC_FRINDEX]		= REG_FIELD(ISP1763_HC_FRINDEX, 0, 13),
++	[FLAG_CF]		= REG_FIELD(ISP1763_HC_CONFIGFLAG, 0, 0),
++	[HC_ISO_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_ISO_PTD_DONEMAP, 0, 15),
++	[HC_ISO_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_ISO_PTD_SKIPMAP, 0, 15),
++	[HC_ISO_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_ISO_PTD_LASTPTD, 0, 15),
++	[HC_INT_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_INT_PTD_DONEMAP, 0, 15),
++	[HC_INT_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_INT_PTD_SKIPMAP, 0, 15),
++	[HC_INT_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_INT_PTD_LASTPTD, 0, 15),
++	[HC_ATL_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_ATL_PTD_DONEMAP, 0, 15),
++	[HC_ATL_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_ATL_PTD_SKIPMAP, 0, 15),
++	[HC_ATL_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_ATL_PTD_LASTPTD, 0, 15),
++	[PORT_OWNER]		= REG_FIELD(ISP1763_HC_PORTSC1, 13, 13),
++	[PORT_POWER]		= REG_FIELD(ISP1763_HC_PORTSC1, 12, 12),
++	[PORT_LSTATUS]		= REG_FIELD(ISP1763_HC_PORTSC1, 10, 11),
++	[PORT_RESET]		= REG_FIELD(ISP1763_HC_PORTSC1, 8, 8),
++	[PORT_SUSPEND]		= REG_FIELD(ISP1763_HC_PORTSC1, 7, 7),
++	[PORT_RESUME]		= REG_FIELD(ISP1763_HC_PORTSC1, 6, 6),
++	[PORT_PE]		= REG_FIELD(ISP1763_HC_PORTSC1, 2, 2),
++	[PORT_CSC]		= REG_FIELD(ISP1763_HC_PORTSC1, 1, 1),
++	[PORT_CONNECT]		= REG_FIELD(ISP1763_HC_PORTSC1, 0, 0),
++	[HW_DATA_BUS_WIDTH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 4, 4),
++	[HW_DACK_POL_HIGH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 6, 6),
++	[HW_DREQ_POL_HIGH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 5, 5),
++	[HW_INTF_LOCK]		= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 3, 3),
++	[HW_INTR_HIGH_ACT]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 2, 2),
++	[HW_INTR_EDGE_TRIG]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 1, 1),
++	[HW_GLOBAL_INTR_EN]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 0, 0),
++	[SW_RESET_RESET_ATX]	= REG_FIELD(ISP1763_HC_RESET, 3, 3),
++	[SW_RESET_RESET_ALL]	= REG_FIELD(ISP1763_HC_RESET, 0, 0),
++	[HC_CHIP_ID_HIGH]	= REG_FIELD(ISP1763_HC_CHIP_ID, 0, 15),
++	[HC_CHIP_ID_LOW]	= REG_FIELD(ISP1763_HC_CHIP_REV, 8, 15),
++	[HC_CHIP_REV]		= REG_FIELD(ISP1763_HC_CHIP_REV, 0, 7),
++	[HC_SCRATCH]		= REG_FIELD(ISP1763_HC_SCRATCH, 0, 15),
++	[ISO_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 2, 2),
++	[INT_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 1, 1),
++	[ATL_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 0, 0),
++	[MEM_START_ADDR]	= REG_FIELD(ISP1763_HC_MEMORY, 0, 15),
++	[HC_DATA]		= REG_FIELD(ISP1763_HC_DATA, 0, 15),
++	[HC_INTERRUPT]		= REG_FIELD(ISP1763_HC_INTERRUPT, 0, 10),
++	[HC_ATL_IRQ_ENABLE]	= REG_FIELD(ISP1763_HC_INTERRUPT_ENABLE, 8, 8),
++	[HC_INT_IRQ_ENABLE]	= REG_FIELD(ISP1763_HC_INTERRUPT_ENABLE, 7, 7),
++	[HC_ISO_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_ISO_IRQ_MASK_OR, 0, 15),
++	[HC_INT_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_INT_IRQ_MASK_OR, 0, 15),
++	[HC_ATL_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_ATL_IRQ_MASK_OR, 0, 15),
++	[HC_ISO_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_ISO_IRQ_MASK_AND, 0, 15),
++	[HC_INT_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_INT_IRQ_MASK_AND, 0, 15),
++	[HC_ATL_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_ATL_IRQ_MASK_AND, 0, 15),
++	[HW_HC_2_DIS]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 15, 15),
++	[HW_OTG_DISABLE]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 10, 10),
++	[HW_SW_SEL_HC_DC]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 7, 7),
++	[HW_VBUS_DRV]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 4, 4),
++	[HW_SEL_CP_EXT]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 3, 3),
++	[HW_DM_PULLDOWN]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 2, 2),
++	[HW_DP_PULLDOWN]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 1, 1),
++	[HW_DP_PULLUP]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 0, 0),
++	[HW_HC_2_DIS_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 15, 15),
++	[HW_OTG_DISABLE_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 10, 10),
++	[HW_SW_SEL_HC_DC_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 7, 7),
++	[HW_VBUS_DRV_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 4, 4),
++	[HW_SEL_CP_EXT_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 3, 3),
++	[HW_DM_PULLDOWN_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 2, 2),
++	[HW_DP_PULLDOWN_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 1, 1),
++	[HW_DP_PULLUP_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 0, 0),
++};
++
++int isp1760_register(struct isp1760_device *isp, struct resource *mem, int irq,
++		     unsigned long irqflags)
++{
++	const struct regmap_config *hc_regmap;
++	const struct reg_field *hc_reg_fields;
++	struct isp1760_hcd *hcd;
++	struct regmap_field *f;
++	unsigned int devflags;
++	struct udevice *dev;
++	int ret;
++	int i;
++
++	hcd = &isp->hcd;
++	devflags = isp->devflags;
++	dev = isp->dev;
++
++	hcd->is_isp1763 = !!(devflags & ISP1760_FLAG_ISP1763);
++
++	if (!hcd->is_isp1763 && (devflags & ISP1760_FLAG_BUS_WIDTH_8)) {
++		dev_err(dev, "isp1760/61 do not support data width 8\n");
++		return -EINVAL;
++	}
++
++	if (hcd->is_isp1763) {
++		hc_regmap = &isp1763_hc_regmap_conf;
++		hc_reg_fields = &isp1763_hc_reg_fields[0];
++	} else {
++		hc_regmap = &isp1760_hc_regmap_conf;
++		hc_reg_fields = &isp1760_hc_reg_fields[0];
++	}
++
++	hcd->base = devm_ioremap(dev, mem->start, resource_size(mem));
++	if (IS_ERR(hcd->base))
++		return PTR_ERR(hcd->base);
++
++	hcd->regs = devm_regmap_init(dev, NULL, NULL, hc_regmap);
++	if (IS_ERR(hcd->regs))
++		return PTR_ERR(hcd->regs);
++
++	for (i = 0; i < HC_FIELD_MAX; i++) {
++		f = devm_regmap_field_alloc(dev, hcd->regs, hc_reg_fields[i]);
++		if (IS_ERR(f))
++			return PTR_ERR(f);
++
++		hcd->fields[i] = f;
++	}
++
++	if (hcd->is_isp1763)
++		hcd->memory_layout = &isp1763_memory_conf;
++	else
++		hcd->memory_layout = &isp176x_memory_conf;
++
++	ret = isp1760_init_core(isp);
++	if (ret < 0)
++		return ret;
++
++	hcd->dev = dev;
++
++	ret = isp1760_hcd_register(hcd, mem, irq, irqflags, dev);
++	if (ret < 0)
++		return ret;
++
++	ret = isp1760_hcd_lowlevel_init(hcd);
++	if (ret < 0)
++		return ret;
++
++	dev_set_drvdata(dev, isp);
++
++	return 0;
++}
++
++void isp1760_unregister(struct isp1760_device *isp)
++{
++	isp1760_hcd_unregister(&isp->hcd);
++
++	return;
++}
+diff --git a/drivers/usb/isp1760/isp1760-core.h b/drivers/usb/isp1760/isp1760-core.h
+new file mode 100644
+index 000000000000..0a60e30b5fe7
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-core.h
+@@ -0,0 +1,96 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ * Contacts:
++ *	Sebastian Siewior <bigeasy@linutronix.de>
++ *	Laurent Pinchart <laurent.pinchart@ideasonboard.com>
++ *	Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#ifndef _ISP1760_CORE_H_
++#define _ISP1760_CORE_H_
++
++#include <linux/compat.h>
++#include <linux/ioport.h>
++#include <regmap.h>
++
++#include "isp1760-hcd.h"
++
++struct device;
++struct gpio_desc;
++
++/*
++ * Device flags that can vary from board to board.  All of these
++ * indicate the most "atypical" case, so that a devflags of 0 is
++ * a sane default configuration.
++ */
++#define ISP1760_FLAG_BUS_WIDTH_16	0x00000002 /* 16-bit data bus width */
++#define ISP1760_FLAG_PERIPHERAL_EN	0x00000004 /* Port 1 supports Peripheral mode*/
++#define ISP1760_FLAG_ANALOG_OC		0x00000008 /* Analog overcurrent */
++#define ISP1760_FLAG_DACK_POL_HIGH	0x00000010 /* DACK active high */
++#define ISP1760_FLAG_DREQ_POL_HIGH	0x00000020 /* DREQ active high */
++#define ISP1760_FLAG_ISP1761		0x00000040 /* Chip is ISP1761 */
++#define ISP1760_FLAG_INTR_POL_HIGH	0x00000080 /* Interrupt polarity active high */
++#define ISP1760_FLAG_INTR_EDGE_TRIG	0x00000100 /* Interrupt edge triggered */
++#define ISP1760_FLAG_ISP1763		0x00000200 /* Chip is ISP1763 */
++#define ISP1760_FLAG_BUS_WIDTH_8	0x00000400 /* 8-bit data bus width */
++
++struct isp1760_device {
++	struct udevice *dev;
++
++	unsigned int devflags;
++	struct gpio_desc *rst_gpio;
++
++	struct isp1760_hcd hcd;
++};
++
++int isp1760_register(struct isp1760_device *isp, struct resource *mem, int irq,
++		     unsigned long irqflags);
++void isp1760_unregister(struct isp1760_device *isp);
++
++void isp1760_set_pullup(struct isp1760_device *isp, bool enable);
++
++static inline u32 isp1760_field_read(struct regmap_field **fields, u32 field)
++{
++	unsigned int val;
++
++	regmap_field_read(fields[field], &val);
++
++	return val;
++}
++
++static inline void isp1760_field_write(struct regmap_field **fields, u32 field,
++				       u32 val)
++{
++	regmap_field_write(fields[field], val);
++}
++
++static inline void isp1760_field_set(struct regmap_field **fields, u32 field)
++{
++	isp1760_field_write(fields, field, 0xFFFFFFFF);
++}
++
++static inline void isp1760_field_clear(struct regmap_field **fields, u32 field)
++{
++	isp1760_field_write(fields, field, 0);
++}
++
++static inline u32 isp1760_reg_read(struct regmap *regs, u32 reg)
++{
++	unsigned int val;
++
++	regmap_read(regs, reg, &val);
++
++	return val;
++}
++
++static inline void isp1760_reg_write(struct regmap *regs, u32 reg, u32 val)
++{
++	regmap_write(regs, reg, val);
++}
++#endif
+diff --git a/drivers/usb/isp1760/isp1760-hcd.c b/drivers/usb/isp1760/isp1760-hcd.c
+new file mode 100644
+index 000000000000..b1d86dd69b94
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-hcd.c
+@@ -0,0 +1,2574 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <hexdump.h>
++#include <common.h>
++#include <asm/cache.h>
++#include <cpu_func.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <linux/bug.h>
++#include <linux/kernel.h>
++#include <linux/list.h>
++#include <linux/usb/usb_urb_compat.h>
++#include <usb.h>
++#include <linux/io.h>
++#include <linux/iopoll.h>
++#include <asm/unaligned.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++static struct kmem_cache *qtd_cachep;
++static struct kmem_cache *qh_cachep;
++static struct kmem_cache *urb_listitem_cachep;
++
++typedef void (packet_enqueue)(struct usb_hcd *hcd, struct isp1760_qh *qh,
++			      struct isp1760_qtd *qtd);
++
++static inline struct isp1760_hcd *hcd_to_priv(struct usb_hcd *hcd)
++{
++	return hcd->hcd_priv;
++}
++
++#define dw_to_le32(x)	(cpu_to_le32((__force u32)x))
++#define le32_to_dw(x)	((__force __dw)(le32_to_cpu(x)))
++
++/* urb state*/
++#define DELETE_URB		(0x0008)
++#define NO_TRANSFER_ACTIVE	(0xffffffff)
++
++/* Philips Proprietary Transfer Descriptor (PTD) */
++typedef __u32 __bitwise __dw;
++struct ptd {
++	__dw dw0;
++	__dw dw1;
++	__dw dw2;
++	__dw dw3;
++	__dw dw4;
++	__dw dw5;
++	__dw dw6;
++	__dw dw7;
++};
++
++struct ptd_le32 {
++	__le32 dw0;
++	__le32 dw1;
++	__le32 dw2;
++	__le32 dw3;
++	__le32 dw4;
++	__le32 dw5;
++	__le32 dw6;
++	__le32 dw7;
++};
++
++#define PTD_OFFSET		0x0400
++#define ISO_PTD_OFFSET		0x0400
++#define INT_PTD_OFFSET		0x0800
++#define ATL_PTD_OFFSET		0x0c00
++#define PAYLOAD_OFFSET		0x1000
++
++#define ISP_BANK_0		0x00
++#define ISP_BANK_1		0x01
++#define ISP_BANK_2		0x02
++#define ISP_BANK_3		0x03
++
++#define TO_DW(x)	((__force __dw)x)
++#define TO_U32(x)	((__force u32)x)
++
++ /* ATL */
++ /* DW0 */
++#define DW0_VALID_BIT			TO_DW(1)
++#define FROM_DW0_VALID(x)		(TO_U32(x) & 0x01)
++#define TO_DW0_LENGTH(x)		TO_DW((((u32)x) << 3))
++#define TO_DW0_MAXPACKET(x)		TO_DW((((u32)x) << 18))
++#define TO_DW0_MULTI(x)			TO_DW((((u32)x) << 29))
++#define TO_DW0_ENDPOINT(x)		TO_DW((((u32)x) << 31))
++/* DW1 */
++#define TO_DW1_DEVICE_ADDR(x)		TO_DW((((u32)x) << 3))
++#define TO_DW1_PID_TOKEN(x)		TO_DW((((u32)x) << 10))
++#define DW1_TRANS_BULK			TO_DW(((u32)2 << 12))
++#define DW1_TRANS_INT			TO_DW(((u32)3 << 12))
++#define DW1_TRANS_SPLIT			TO_DW(((u32)1 << 14))
++#define DW1_SE_USB_LOSPEED		TO_DW(((u32)2 << 16))
++#define TO_DW1_PORT_NUM(x)		TO_DW((((u32)x) << 18))
++#define TO_DW1_HUB_NUM(x)		TO_DW((((u32)x) << 25))
++/* DW2 */
++#define TO_DW2_DATA_START_ADDR(x)	TO_DW((((u32)x) << 8))
++#define TO_DW2_RL(x)			TO_DW(((x) << 25))
++#define FROM_DW2_RL(x)			((TO_U32(x) >> 25) & 0xf)
++/* DW3 */
++#define FROM_DW3_NRBYTESTRANSFERRED(x)		TO_U32((x) & 0x3fff)
++#define FROM_DW3_SCS_NRBYTESTRANSFERRED(x)	TO_U32((x) & 0x07ff)
++#define TO_DW3_NAKCOUNT(x)		TO_DW(((x) << 19))
++#define FROM_DW3_NAKCOUNT(x)		((TO_U32(x) >> 19) & 0xf)
++#define TO_DW3_CERR(x)			TO_DW(((x) << 23))
++#define FROM_DW3_CERR(x)		((TO_U32(x) >> 23) & 0x3)
++#define TO_DW3_DATA_TOGGLE(x)		TO_DW(((x) << 25))
++#define FROM_DW3_DATA_TOGGLE(x)		((TO_U32(x) >> 25) & 0x1)
++#define TO_DW3_PING(x)			TO_DW(((x) << 26))
++#define FROM_DW3_PING(x)		((TO_U32(x) >> 26) & 0x1)
++#define DW3_ERROR_BIT			TO_DW((1 << 28))
++#define DW3_BABBLE_BIT			TO_DW((1 << 29))
++#define DW3_HALT_BIT			TO_DW((1 << 30))
++#define DW3_ACTIVE_BIT			TO_DW((1 << 31))
++#define FROM_DW3_ACTIVE(x)		((TO_U32(x) >> 31) & 0x01)
++
++#define INT_UNDERRUN			(1 << 2)
++#define INT_BABBLE			(1 << 1)
++#define INT_EXACT			(1 << 0)
++
++#define SETUP_PID	(2)
++#define IN_PID		(1)
++#define OUT_PID		(0)
++
++/* Errata 1 */
++#define RL_COUNTER	(0)
++#define NAK_COUNTER	(0)
++#define ERR_COUNTER	(3)
++
++struct isp1760_qtd {
++	u8 packet_type;
++	void *data_buffer;
++	u32 payload_addr;
++
++	/* the rest is HCD-private */
++	struct list_head qtd_list;
++	struct urb *urb;
++	size_t length;
++	size_t actual_length;
++
++	/* QTD_ENQUEUED:	waiting for transfer (inactive) */
++	/* QTD_PAYLOAD_ALLOC:	chip mem has been allocated for payload */
++	/* QTD_XFER_STARTED:	valid ptd has been written to isp176x - only
++				interrupt handler may touch this qtd! */
++	/* QTD_XFER_COMPLETE:	payload has been transferred successfully */
++	/* QTD_RETIRE:		transfer error/abort qtd */
++#define QTD_ENQUEUED		0
++#define QTD_PAYLOAD_ALLOC	1
++#define QTD_XFER_STARTED	2
++#define QTD_XFER_COMPLETE	3
++#define QTD_RETIRE		4
++	u32 status;
++};
++
++/* Queue head, one for each active endpoint */
++struct isp1760_qh {
++	struct list_head qh_list;
++	struct list_head qtd_list;
++	int epnum;
++	u32 toggle;
++	u32 ping;
++	int slot;
++	int tt_buffer_dirty;	/* See USB2.0 spec section 11.17.5 */
++};
++
++struct urb_listitem {
++	struct list_head urb_list;
++	struct urb *urb;
++};
++
++static const u32 isp1763_hc_portsc1_fields[] = {
++	[PORT_OWNER]		= BIT(13),
++	[PORT_POWER]		= BIT(12),
++	[PORT_LSTATUS]		= BIT(10),
++	[PORT_RESET]		= BIT(8),
++	[PORT_SUSPEND]		= BIT(7),
++	[PORT_RESUME]		= BIT(6),
++	[PORT_PE]		= BIT(2),
++	[PORT_CSC]		= BIT(1),
++	[PORT_CONNECT]		= BIT(0),
++};
++
++static struct descriptor {
++	struct usb_device_descriptor device;
++	struct usb_config_descriptor config;
++	struct usb_interface_descriptor interface;
++	struct usb_endpoint_descriptor endpoint;
++} __packed rh_descriptor = {
++	{
++		/* usb 2.0 root hub device descriptor */
++		0x12,       /*  __u8  bLength; */
++		USB_DT_DEVICE, /* __u8 bDescriptorType; Device */
++		0x0002, /*  __le16 bcdUSB; v2.0 */
++
++		0x09,       /*  __u8  bDeviceClass; HUB_CLASSCODE */
++		0x00,       /*  __u8  bDeviceSubClass; */
++		0x00,       /*  __u8  bDeviceProtocol; [ usb 2.0 no TT ] */
++		0x40,       /*  __u8  bMaxPacketSize0; 64 Bytes */
++
++		0x6b1d,     /*  __le16 idVendor; Linux Foundation 0x1d6b */
++		0x0200,     /*  __le16 idProduct; device 0x0002 */
++		0x0001,     /*  __le16 bcdDevice */
++
++		0x03,       /*  __u8  iManufacturer; */
++		0x02,       /*  __u8  iProduct; */
++		0x01,       /*  __u8  iSerialNumber; */
++		0x01        /*  __u8  bNumConfigurations; */
++	}, {
++		/* one configuration */
++		0x09,       /*  __u8  bLength; */
++		USB_DT_CONFIG, /* __u8 bDescriptorType; Configuration */
++		0x1900,     /*  __le16 wTotalLength; */
++		0x01,       /*  __u8  bNumInterfaces; (1) */
++		0x01,       /*  __u8  bConfigurationValue; */
++		0x00,       /*  __u8  iConfiguration; */
++		0xc0,       /*  __u8  bmAttributes;
++					Bit 7: must be set,
++					6: Self-powered,
++					5: Remote wakeup,
++					4..0: resvd */
++		0x00,       /*  __u8  MaxPower; */
++	}, {
++		/* one interface */
++		0x09,       /*  __u8  if_bLength; */
++		USB_DT_INTERFACE, /* __u8 if_bDescriptorType; Interface */
++		0x00,       /*  __u8  if_bInterfaceNumber; */
++		0x00,       /*  __u8  if_bAlternateSetting; */
++		0x01,       /*  __u8  if_bNumEndpoints; */
++		0x09,       /*  __u8  if_bInterfaceClass; HUB_CLASSCODE */
++		0x00,       /*  __u8  if_bInterfaceSubClass; */
++		0x00,       /*  __u8  if_bInterfaceProtocol; [usb1.1 or single tt] */
++		0x00,       /*  __u8  if_iInterface; */
++	}, {
++		/* one endpoint (status change endpoint) */
++		0x07,       /*  __u8  ep_bLength; */
++		USB_DT_ENDPOINT, /* __u8 ep_bDescriptorType; Endpoint */
++		0x81,       /*  __u8  ep_bEndpointAddress; IN Endpoint 1 */
++		0x03,       /*  __u8  ep_bmAttributes; Interrupt */
++			/* __le16 ep_wMaxPacketSize; 1 + (MAX_ROOT_PORTS / 8)
++			* see hub.c:hub_configure() for details. */
++		(USB_MAXCHILDREN + 1 + 7) / 8, 0x00,
++		0x0c        /*  __u8  ep_bInterval; (256ms -- usb 2.0 spec) */
++	},
++};
++
++/*
++ * Access functions for isp176x registers regmap fields
++ */
++static u32 isp1760_hcd_read(struct usb_hcd *hcd, u32 field)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	return isp1760_field_read(priv->fields, field);
++}
++
++/*
++ * We need, in isp1763, to write directly the values to the portsc1
++ * register so it will make the other values to trigger.
++ */
++static void isp1760_hcd_portsc1_set_clear(struct isp1760_hcd *priv, u32 field,
++					  u32 val)
++{
++	u32 bit = isp1763_hc_portsc1_fields[field];
++	u32 port_status = readl(priv->base + ISP1763_HC_PORTSC1);
++
++	if (val)
++		writel(port_status | bit, priv->base + ISP1763_HC_PORTSC1);
++	else
++		writel(port_status & ~bit, priv->base + ISP1763_HC_PORTSC1);
++}
++
++static void isp1760_hcd_write(struct usb_hcd *hcd, u32 field, u32 val)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (unlikely(priv->is_isp1763 &&
++		     (field >= PORT_OWNER && field <= PORT_CONNECT)))
++		return isp1760_hcd_portsc1_set_clear(priv, field, val);
++
++	isp1760_field_write(priv->fields, field, val);
++}
++
++static void isp1760_hcd_set(struct usb_hcd *hcd, u32 field)
++{
++	isp1760_hcd_write(hcd, field, 0xFFFFFFFF);
++}
++
++static void isp1760_hcd_clear(struct usb_hcd *hcd, u32 field)
++{
++	isp1760_hcd_write(hcd, field, 0);
++}
++
++static int isp1760_hcd_set_and_wait(struct usb_hcd *hcd, u32 field,
++				    u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_set(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      val, 10, timeout_us);
++}
++
++static int isp1760_hcd_set_and_wait_swap(struct usb_hcd *hcd, u32 field,
++					 u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_set(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      !val, 10, timeout_us);
++}
++
++static int isp1760_hcd_clear_and_wait(struct usb_hcd *hcd, u32 field,
++				      u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_clear(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      !val, 10, timeout_us);
++}
++
++static bool isp1760_hcd_is_set(struct usb_hcd *hcd, u32 field)
++{
++	return !!isp1760_hcd_read(hcd, field);
++}
++
++static bool isp1760_hcd_ppc_is_set(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (priv->is_isp1763)
++		return true;
++
++	return isp1760_hcd_is_set(hcd, HCS_PPC);
++}
++
++static u32 isp1760_hcd_n_ports(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (priv->is_isp1763)
++		return 1;
++
++	return isp1760_hcd_read(hcd, HCS_N_PORTS);
++}
++
++/*
++ * Access functions for isp176x memory (offset >= 0x0400).
++ *
++ * bank_reads8() reads memory locations prefetched by an earlier write to
++ * HC_MEMORY_REG (see isp176x datasheet). Unless you want to do fancy multi-
++ * bank optimizations, you should use the more generic mem_read() below.
++ *
++ * For access to ptd memory, use the specialized ptd_read() and ptd_write()
++ * below.
++ *
++ * These functions copy via MMIO data to/from the device. memcpy_{to|from}io()
++ * doesn't quite work because some people have to enforce 32-bit access
++ */
++static void bank_reads8(void __iomem *src_base, u32 src_offset, u32 bank_addr,
++							__u32 *dst, u32 bytes)
++{
++	__u32 __iomem *src;
++	u32 val;
++	__u8 *src_byteptr;
++	__u8 *dst_byteptr;
++
++	src = src_base + (bank_addr | src_offset);
++
++	if (src_offset < PAYLOAD_OFFSET) {
++		while (bytes >= 4) {
++			*dst = readl_relaxed(src);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	} else {
++		while (bytes >= 4) {
++			*dst = __raw_readl(src);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	}
++
++	if (!bytes)
++		return;
++
++	/* in case we have 3, 2 or 1 by left. The dst buffer may not be fully
++	 * allocated.
++	 */
++	if (src_offset < PAYLOAD_OFFSET)
++		val = readl_relaxed(src);
++	else
++		val = __raw_readl(src);
++
++	dst_byteptr = (void *) dst;
++	src_byteptr = (void *) &val;
++	while (bytes > 0) {
++		*dst_byteptr = *src_byteptr;
++		dst_byteptr++;
++		src_byteptr++;
++		bytes--;
++	}
++}
++
++static void isp1760_mem_read(struct usb_hcd *hcd, u32 src_offset, void *dst,
++			     u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	isp1760_hcd_write(hcd, MEM_BANK_SEL, ISP_BANK_0);
++	isp1760_hcd_write(hcd, MEM_START_ADDR, src_offset);
++	ndelay(100);
++
++	bank_reads8(priv->base, src_offset, ISP_BANK_0, dst, bytes);
++}
++
++/*
++ * ISP1763 does not have the banks direct host controller memory access,
++ * needs to use the HC_DATA register. Add data read/write according to this,
++ * and also adjust 16bit access.
++ */
++static void isp1763_mem_read(struct usb_hcd *hcd, u16 srcaddr,
++			     u16 *dstptr, u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++
++	/* Write the starting device address to the hcd memory register */
++	isp1760_reg_write(priv->regs, ISP1763_HC_MEMORY, srcaddr);
++	ndelay(100); /* Delay between consecutive access */
++
++	/* As long there are at least 16-bit to read ... */
++	while (bytes >= 2) {
++		*dstptr = __raw_readw(priv->base + ISP1763_HC_DATA);
++		bytes -= 2;
++		dstptr++;
++	}
++
++	/* If there are no more bytes to read, return */
++	if (bytes <= 0)
++		return;
++
++	*((u8 *)dstptr) = (u8)(readw(priv->base + ISP1763_HC_DATA) & 0xFF);
++}
++
++static void mem_read(struct usb_hcd *hcd, u32 src_offset, __u32 *dst,
++		     u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_mem_read(hcd, src_offset, (u16 *)dst, bytes);
++
++	isp1763_mem_read(hcd, (u16)src_offset, (u16 *)dst, bytes);
++}
++
++static void isp1760_mem_write(void __iomem *dst_base, u32 dst_offset,
++			      __u32 const *src, u32 bytes)
++{
++	__u32 __iomem *dst;
++
++	dst = dst_base + dst_offset;
++
++	if (dst_offset < PAYLOAD_OFFSET) {
++		while (bytes >= 4) {
++			writel_relaxed(*src, dst);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	} else {
++		while (bytes >= 4) {
++			__raw_writel(*src, dst);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	}
++
++	if (!bytes)
++		return;
++	/* in case we have 3, 2 or 1 bytes left. The buffer is allocated and the
++	 * extra bytes should not be read by the HW.
++	 */
++
++	if (dst_offset < PAYLOAD_OFFSET)
++		writel_relaxed(*src, dst);
++	else
++		__raw_writel(*src, dst);
++}
++
++static void isp1763_mem_write(struct usb_hcd *hcd, u16 dstaddr, u16 *src,
++			      u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	/* Write the starting device address to the hcd memory register */
++	isp1760_reg_write(priv->regs, ISP1763_HC_MEMORY, dstaddr);
++	ndelay(100); /* Delay between consecutive access */
++
++	while (bytes >= 2) {
++		/* Get and write the data; then adjust the data ptr and len */
++		__raw_writew(*src, priv->base + ISP1763_HC_DATA);
++		bytes -= 2;
++		src++;
++	}
++
++	/* If there are no more bytes to process, return */
++	if (bytes <= 0)
++		return;
++
++	/*
++	 * The only way to get here is if there is a single byte left,
++	 * get it and write it to the data reg;
++	 */
++	writew(*((u8 *)src), priv->base + ISP1763_HC_DATA);
++}
++
++static void mem_write(struct usb_hcd *hcd, u32 dst_offset, __u32 *src,
++		      u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_mem_write(priv->base, dst_offset, src, bytes);
++
++	isp1763_mem_write(hcd, dst_offset, (u16 *)src, bytes);
++}
++
++/*
++ * Read and write ptds. 'ptd_offset' should be one of ISO_PTD_OFFSET,
++ * INT_PTD_OFFSET, and ATL_PTD_OFFSET. 'slot' should be less than 32.
++ */
++static void isp1760_ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			     struct ptd *ptd)
++{
++	u16 src_offset = ptd_offset + slot * sizeof(*ptd);
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	isp1760_hcd_write(hcd, MEM_BANK_SEL, ISP_BANK_0);
++	isp1760_hcd_write(hcd, MEM_START_ADDR, src_offset);
++	ndelay(90);
++
++	bank_reads8(priv->base, src_offset, ISP_BANK_0, (void *)ptd,
++		    sizeof(*ptd));
++}
++
++static void isp1763_ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			     struct ptd *ptd)
++{
++	u16 src_offset = ptd_offset + slot * sizeof(*ptd);
++	struct ptd_le32 le32_ptd;
++
++	isp1763_mem_read(hcd, src_offset, (u16 *)&le32_ptd, sizeof(le32_ptd));
++	/* Normalize the data obtained */
++	ptd->dw0 = le32_to_dw(le32_ptd.dw0);
++	ptd->dw1 = le32_to_dw(le32_ptd.dw1);
++	ptd->dw2 = le32_to_dw(le32_ptd.dw2);
++	ptd->dw3 = le32_to_dw(le32_ptd.dw3);
++	ptd->dw4 = le32_to_dw(le32_ptd.dw4);
++	ptd->dw5 = le32_to_dw(le32_ptd.dw5);
++	ptd->dw6 = le32_to_dw(le32_ptd.dw6);
++	ptd->dw7 = le32_to_dw(le32_ptd.dw7);
++}
++
++static void ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++		     struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_ptd_read(hcd, ptd_offset, slot, ptd);
++
++	isp1763_ptd_read(hcd, ptd_offset, slot, ptd);
++}
++
++static void isp1763_ptd_write(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			      struct ptd *cpu_ptd)
++{
++	u16 dst_offset = ptd_offset + slot * sizeof(*cpu_ptd);
++	struct ptd_le32 ptd;
++
++	ptd.dw0 = dw_to_le32(cpu_ptd->dw0);
++	ptd.dw1 = dw_to_le32(cpu_ptd->dw1);
++	ptd.dw2 = dw_to_le32(cpu_ptd->dw2);
++	ptd.dw3 = dw_to_le32(cpu_ptd->dw3);
++	ptd.dw4 = dw_to_le32(cpu_ptd->dw4);
++	ptd.dw5 = dw_to_le32(cpu_ptd->dw5);
++	ptd.dw6 = dw_to_le32(cpu_ptd->dw6);
++	ptd.dw7 = dw_to_le32(cpu_ptd->dw7);
++
++	isp1763_mem_write(hcd, dst_offset,  (u16 *)&ptd.dw0,
++			  8 * sizeof(ptd.dw0));
++}
++
++static void isp1760_ptd_write(void __iomem *base, u32 ptd_offset, u32 slot,
++			      struct ptd *ptd)
++{
++	u32 dst_offset = ptd_offset + slot * sizeof(*ptd);
++
++	/*
++	 * Make sure dw0 gets written last (after other dw's and after payload)
++	 *  since it contains the enable bit
++	 */
++	isp1760_mem_write(base, dst_offset + sizeof(ptd->dw0),
++			  (__force u32 *)&ptd->dw1, 7 * sizeof(ptd->dw1));
++	wmb();
++	isp1760_mem_write(base, dst_offset, (__force u32 *)&ptd->dw0,
++			  sizeof(ptd->dw0));
++}
++
++static void ptd_write(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++		      struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_ptd_write(priv->base, ptd_offset, slot, ptd);
++
++	isp1763_ptd_write(hcd, ptd_offset, slot, ptd);
++}
++
++/* memory management of the 60kb on the chip from 0x1000 to 0xffff */
++static void init_memory(struct isp1760_hcd *priv)
++{
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i, j, curr;
++	u32 payload_addr;
++
++	payload_addr = PAYLOAD_OFFSET;
++
++	for (i = 0, curr = 0; i < ARRAY_SIZE(mem->blocks); i++) {
++		for (j = 0; j < mem->blocks[i]; j++, curr++) {
++			priv->memory_pool[curr + j].start = payload_addr;
++			priv->memory_pool[curr + j].size = mem->blocks_size[i];
++			priv->memory_pool[curr + j].free = 1;
++			payload_addr += priv->memory_pool[curr + j].size;
++		}
++	}
++
++	WARN_ON(payload_addr - priv->memory_pool[0].start >
++		mem->payload_area_size);
++}
++
++static void alloc_mem(struct usb_hcd *hcd, struct isp1760_qtd *qtd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i;
++
++	WARN_ON(qtd->payload_addr);
++
++	if (!qtd->length)
++		return;
++
++	for (i = 0; i < mem->payload_blocks; i++) {
++		if (priv->memory_pool[i].size >= qtd->length &&
++				priv->memory_pool[i].free) {
++			priv->memory_pool[i].free = 0;
++			qtd->payload_addr = priv->memory_pool[i].start;
++			return;
++		}
++	}
++}
++
++static void free_mem(struct usb_hcd *hcd, struct isp1760_qtd *qtd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i;
++
++	if (!qtd->payload_addr)
++		return;
++
++	for (i = 0; i < mem->payload_blocks; i++) {
++		if (priv->memory_pool[i].start == qtd->payload_addr) {
++			WARN_ON(priv->memory_pool[i].free);
++			priv->memory_pool[i].free = 1;
++			qtd->payload_addr = 0;
++			return;
++		}
++	}
++
++	WARN_ON(1);
++	qtd->payload_addr = 0;
++}
++
++/* reset a non-running (STS_HALT == 1) controller */
++static int ehci_reset(struct usb_hcd *hcd)
++{
++	return isp1760_hcd_set_and_wait_swap(hcd, CMD_RESET, 250 * 1000);
++}
++
++static struct isp1760_qh *qh_alloc(gfp_t flags)
++{
++	struct isp1760_qh *qh;
++
++	qh = kmem_cache_alloc(qh_cachep, flags);
++	if (!qh)
++		return NULL;
++
++	memset(qh, '\0', qh_cachep->sz);
++	INIT_LIST_HEAD(&qh->qh_list);
++	INIT_LIST_HEAD(&qh->qtd_list);
++	qh->slot = -1;
++
++	return qh;
++}
++
++static void qh_free(struct isp1760_qh *qh)
++{
++	WARN_ON(!list_empty(&qh->qtd_list));
++	WARN_ON(qh->slot > -1);
++	kmem_cache_free(qh_cachep, qh);
++}
++
++/* one-time init, only for memory state */
++static int priv_init(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 isoc_cache;
++	u32 isoc_thres;
++	int i;
++
++	spin_lock_init(&priv->lock);
++
++	for (i = 0; i < QH_END; i++)
++		INIT_LIST_HEAD(&priv->qh_list[i]);
++
++	/*
++	 * hw default: 1K periodic list heads, one per frame.
++	 * periodic_size can shrink by USBCMD update if hcc_params allows.
++	 */
++	priv->periodic_size = DEFAULT_I_TDPS;
++
++	if (priv->is_isp1763) {
++		priv->i_thresh = 2;
++		return 0;
++	}
++
++	/* controllers may cache some of the periodic schedule ... */
++	isoc_cache = isp1760_hcd_read(hcd, HCC_ISOC_CACHE);
++	isoc_thres = isp1760_hcd_read(hcd, HCC_ISOC_THRES);
++
++	/* full frame cache */
++	if (isoc_cache)
++		priv->i_thresh = 8;
++	else /* N microframes cached */
++		priv->i_thresh = 2 + isoc_thres;
++
++	return 0;
++}
++
++static int isp1760_hc_setup(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 atx_reset;
++	int result;
++	u32 scratch;
++	u32 pattern;
++
++	if (priv->is_isp1763)
++		pattern = 0xcafe;
++	else
++		pattern = 0xdeadcafe;
++
++	isp1760_hcd_write(hcd, HC_SCRATCH, pattern);
++
++	/* Change bus pattern */
++	scratch = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	scratch = isp1760_hcd_read(hcd, HC_SCRATCH);
++	if (scratch != pattern) {
++		printf("Scratch test failed. 0x%08x\n", scratch);
++		return -ENODEV;
++	}
++
++	/*
++	 * The RESET_HC bit in the SW_RESET register is supposed to reset the
++	 * host controller without touching the CPU interface registers, but at
++	 * least on the ISP1761 it seems to behave as the RESET_ALL bit and
++	 * reset the whole device. We thus can't use it here, so let's reset
++	 * the host controller through the EHCI USB Command register. The device
++	 * has been reset in core code anyway, so this shouldn't matter.
++	 */
++	isp1760_hcd_clear(hcd, ISO_BUF_FILL);
++	isp1760_hcd_clear(hcd, INT_BUF_FILL);
++	isp1760_hcd_clear(hcd, ATL_BUF_FILL);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++
++	result = ehci_reset(hcd);
++	if (result)
++		return result;
++
++	/* Step 11 passed */
++
++	/* ATL reset */
++	if (priv->is_isp1763)
++		atx_reset = SW_RESET_RESET_ATX;
++	else
++		atx_reset = ALL_ATX_RESET;
++
++	isp1760_hcd_set(hcd, atx_reset);
++	mdelay(10);
++	isp1760_hcd_clear(hcd, atx_reset);
++
++	if (priv->is_isp1763) {
++		isp1760_hcd_set(hcd, HW_OTG_DISABLE);
++		isp1760_hcd_set(hcd, HW_SW_SEL_HC_DC_CLEAR);
++		isp1760_hcd_set(hcd, HW_HC_2_DIS_CLEAR);
++		isp1760_hcd_set(hcd, HW_DM_PULLDOWN);
++		isp1760_hcd_set(hcd, HW_DP_PULLDOWN);
++		mdelay(10);
++
++		isp1760_hcd_set(hcd, HW_INTF_LOCK);
++	}
++
++	isp1760_hcd_set(hcd, HC_INT_IRQ_ENABLE);
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_ENABLE);
++
++	return priv_init(hcd);
++}
++
++static u32 base_to_chip(u32 base)
++{
++	return ((base - 0x400) >> 3);
++}
++
++static int last_qtd_of_urb(struct isp1760_qtd *qtd, struct isp1760_qh *qh)
++{
++	struct urb *urb;
++
++	if (list_is_last(&qtd->qtd_list, &qh->qtd_list))
++		return 1;
++
++	urb = qtd->urb;
++	qtd = list_entry(qtd->qtd_list.next, typeof(*qtd), qtd_list);
++
++	return (qtd->urb != urb);
++}
++
++/* magic numbers that can affect system performance */
++#define	EHCI_TUNE_CERR		3	/* 0-3 qtd retries; 0 == don't stop */
++#define	EHCI_TUNE_RL_HS		4	/* nak throttle; see 4.9 */
++#define	EHCI_TUNE_RL_TT		0
++#define	EHCI_TUNE_MULT_HS	1	/* 1-3 transactions/uframe; 4.10.3 */
++#define	EHCI_TUNE_MULT_TT	1
++#define	EHCI_TUNE_FLS		2	/* (small) 256 frame schedule */
++
++static void create_ptd_atl(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			   struct ptd *ptd)
++{
++	u32 maxpacket;
++	u32 multi;
++	u32 rl = RL_COUNTER;
++	u32 nak = NAK_COUNTER;
++	u8 portnr;
++	u8 hubaddr;
++
++	memset(ptd, 0, sizeof(*ptd));
++
++	/* according to 3.6.2, max packet len can not be > 0x400 */
++	maxpacket = usb_maxpacket(qtd->urb->dev, qtd->urb->pipe);
++	multi =  1 + ((maxpacket >> 11) & 0x3);
++	maxpacket &= 0x7ff;
++
++	/* DW0 */
++	ptd->dw0 = DW0_VALID_BIT;
++	ptd->dw0 |= TO_DW0_LENGTH(qtd->length);
++	ptd->dw0 |= TO_DW0_MAXPACKET(maxpacket);
++	ptd->dw0 |= TO_DW0_ENDPOINT(usb_pipeendpoint(qtd->urb->pipe));
++
++	/* DW1 */
++	ptd->dw1 = TO_DW((usb_pipeendpoint(qtd->urb->pipe) >> 1));
++	ptd->dw1 |= TO_DW1_DEVICE_ADDR(usb_pipedevice(qtd->urb->pipe));
++	ptd->dw1 |= TO_DW1_PID_TOKEN(qtd->packet_type);
++
++	if (usb_pipebulk(qtd->urb->pipe))
++		ptd->dw1 |= DW1_TRANS_BULK;
++	else if  (usb_pipeint(qtd->urb->pipe))
++		ptd->dw1 |= DW1_TRANS_INT;
++
++	if (qtd->urb->dev->speed != USB_SPEED_HIGH) {
++		/* split transaction */
++
++		ptd->dw1 |= DW1_TRANS_SPLIT;
++		if (qtd->urb->dev->speed == USB_SPEED_LOW)
++			ptd->dw1 |= DW1_SE_USB_LOSPEED;
++
++		if (!qtd->urb->dev->dev->parent_priv_) {
++			portnr = qtd->urb->dev->portnr;
++			hubaddr = qtd->urb->dev->devnum;
++		} else {
++			usb_find_usb2_hub_address_port(qtd->urb->dev, &hubaddr,
++						       &portnr);
++		}
++
++		ptd->dw1 |= TO_DW1_PORT_NUM(portnr);
++		ptd->dw1 |= TO_DW1_HUB_NUM(hubaddr);
++
++		/* SE bit for Split INT transfers */
++		if (usb_pipeint(qtd->urb->pipe) &&
++				(qtd->urb->dev->speed == USB_SPEED_LOW))
++			ptd->dw1 |= DW1_SE_USB_LOSPEED;
++
++		rl = 0;
++		nak = 0;
++	} else {
++		ptd->dw0 |= TO_DW0_MULTI(multi);
++		if (usb_pipecontrol(qtd->urb->pipe) ||
++						usb_pipebulk(qtd->urb->pipe))
++			ptd->dw3 |= TO_DW3_PING(qh->ping);
++	}
++	/* DW2 */
++	ptd->dw2 = 0;
++	ptd->dw2 |= TO_DW2_DATA_START_ADDR(base_to_chip(qtd->payload_addr));
++	ptd->dw2 |= TO_DW2_RL(rl);
++
++	/* DW3 */
++	ptd->dw3 |= TO_DW3_NAKCOUNT(nak);
++	ptd->dw3 |= TO_DW3_DATA_TOGGLE(qh->toggle);
++
++	if (usb_pipecontrol(qtd->urb->pipe)) {
++		if (qtd->data_buffer == qtd->urb->setup_packet) {
++			ptd->dw3 &= ~TO_DW3_DATA_TOGGLE(1);
++		} else if (last_qtd_of_urb(qtd, qh)) {
++			ptd->dw3 |= TO_DW3_DATA_TOGGLE(1);
++		}
++	}
++
++	ptd->dw3 |= DW3_ACTIVE_BIT;
++	/* Cerr */
++	ptd->dw3 |= TO_DW3_CERR(ERR_COUNTER);
++}
++
++static void transform_add_int(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			      struct ptd *ptd)
++{
++	struct usb_host_endpoint *hep = qtd->urb->ep;
++	struct usb_endpoint_descriptor *epd = &hep->desc;
++	u32 usof;
++	u32 period;
++
++	/*
++	 * Most of this is guessing. ISP1761 datasheet is quite unclear, and
++	 * the algorithm from the original Philips driver code, which was
++	 * pretty much used in this driver before as well, is quite horrendous
++	 * and, i believe, incorrect. The code below follows the datasheet and
++	 * USB2.0 spec as far as I can tell, and plug/unplug seems to be much
++	 * more reliable this way (fingers crossed...).
++	 */
++
++	if (qtd->urb->dev->speed == USB_SPEED_HIGH) {
++		/* urb->interval is in units of microframes (1/8 ms) */
++		period = epd->bInterval >> 3;
++
++		if (epd->bInterval > 4)
++			usof = 0x01; /* One bit set =>
++						interval 1 ms * uFrame-match */
++		else if (epd->bInterval > 2)
++			usof = 0x22; /* Two bits set => interval 1/2 ms */
++		else if (epd->bInterval > 1)
++			usof = 0x55; /* Four bits set => interval 1/4 ms */
++		else
++			usof = 0xff; /* All bits set => interval 1/8 ms */
++	} else {
++		/* urb->interval is in units of frames (1 ms) */
++		period = epd->bInterval;
++		usof = 0x0f;		/* Execute Start Split on any of the
++					   four first uFrames */
++
++		/*
++		 * First 8 bits in dw5 is uSCS and "specifies which uSOF the
++		 * complete split needs to be sent. Valid only for IN." Also,
++		 * "All bits can be set to one for every transfer." (p 82,
++		 * ISP1761 data sheet.) 0x1c is from Philips driver. Where did
++		 * that number come from? 0xff seems to work fine...
++		 */
++		/* ptd->dw5 = 0x1c; */
++		ptd->dw5 = TO_DW(0xff); /* Execute Complete Split on any uFrame */
++	}
++
++	period = period >> 1;/* Ensure equal or shorter period than requested */
++	period &= 0xf8; /* Mask off too large values and lowest unused 3 bits */
++
++	ptd->dw2 |= TO_DW(period);
++	ptd->dw4 = TO_DW(usof);
++}
++
++static void create_ptd_int(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			   struct ptd *ptd)
++{
++	create_ptd_atl(qh, qtd, ptd);
++	transform_add_int(qh, qtd, ptd);
++}
++
++static void isp1760_urb_done(struct usb_hcd *hcd, struct urb *urb)
++{
++	if (usb_pipein(urb->pipe) && usb_pipetype(urb->pipe) != PIPE_CONTROL) {
++		void *ptr;
++		for (ptr = urb->transfer_buffer;
++		     ptr < urb->transfer_buffer + urb->transfer_buffer_length;
++		     ptr += PAGE_SIZE)
++			flush_dcache_range((unsigned  long)ptr,
++					   (unsigned long)ptr + PAGE_SIZE);
++	}
++
++	/* complete() can reenter this HCD */
++	usb_hcd_unlink_urb_from_ep(hcd, urb);
++	usb_hcd_giveback_urb(hcd, urb, urb->status);
++}
++
++static struct isp1760_qtd *qtd_alloc(gfp_t flags, struct urb *urb,
++				     u8 packet_type)
++{
++	struct isp1760_qtd *qtd;
++
++	qtd = kmem_cache_alloc(qtd_cachep, flags);
++	if (!qtd)
++		return NULL;
++
++	memset(qtd, '\0', sizeof(*qtd));
++	INIT_LIST_HEAD(&qtd->qtd_list);
++	qtd->urb = urb;
++	qtd->packet_type = packet_type;
++	qtd->status = QTD_ENQUEUED;
++	qtd->actual_length = 0;
++
++	return qtd;
++}
++
++static void qtd_free(struct isp1760_qtd *qtd)
++{
++	WARN_ON(qtd->payload_addr);
++	kmem_cache_free(qtd_cachep, qtd);
++}
++
++static void start_bus_transfer(struct usb_hcd *hcd, u32 ptd_offset, int slot,
++				struct isp1760_slotinfo *slots,
++				struct isp1760_qtd *qtd, struct isp1760_qh *qh,
++				struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int skip_map;
++
++	WARN_ON((slot < 0) || (slot > mem->slot_num - 1));
++	WARN_ON(qtd->length && !qtd->payload_addr);
++	WARN_ON(slots[slot].qtd);
++	WARN_ON(slots[slot].qh);
++	WARN_ON(qtd->status != QTD_PAYLOAD_ALLOC);
++
++	if (priv->is_isp1763)
++		ndelay(100);
++
++	/* Make sure done map has not triggered from some unlinked transfer */
++	if (ptd_offset == ATL_PTD_OFFSET) {
++		skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP,
++				  skip_map | (1 << slot));
++		priv->atl_done_map |= isp1760_hcd_read(hcd, HC_ATL_PTD_DONEMAP);
++		priv->atl_done_map &= ~(1 << slot);
++	} else {
++		skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP,
++				  skip_map | (1 << slot));
++		priv->int_done_map |= isp1760_hcd_read(hcd, HC_INT_PTD_DONEMAP);
++		priv->int_done_map &= ~(1 << slot);
++	}
++
++	skip_map &= ~(1 << slot);
++	qh->slot = slot;
++	qtd->status = QTD_XFER_STARTED;
++	slots[slot].qtd = qtd;
++	slots[slot].qh = qh;
++
++	ptd_write(hcd, ptd_offset, slot, ptd);
++
++	if (ptd_offset == ATL_PTD_OFFSET)
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP, skip_map);
++	else
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP, skip_map);
++}
++
++static int is_short_bulk(struct isp1760_qtd *qtd)
++{
++	return (usb_pipebulk(qtd->urb->pipe) &&
++					(qtd->actual_length < qtd->length));
++}
++
++static void collect_qtds(struct usb_hcd *hcd, struct isp1760_qh *qh,
++						struct list_head *urb_list)
++{
++	struct isp1760_qtd *qtd, *qtd_next;
++	struct urb_listitem *urb_listitem;
++	int last_qtd;
++
++	list_for_each_entry_safe(qtd, qtd_next, &qh->qtd_list, qtd_list) {
++		if (qtd->status < QTD_XFER_COMPLETE)
++			break;
++
++		last_qtd = last_qtd_of_urb(qtd, qh);
++
++		if ((!last_qtd) && (qtd->status == QTD_RETIRE))
++			qtd_next->status = QTD_RETIRE;
++
++		if (qtd->status == QTD_XFER_COMPLETE) {
++			if (qtd->actual_length) {
++				switch (qtd->packet_type) {
++				case IN_PID:
++					mem_read(hcd, qtd->payload_addr,
++						 qtd->data_buffer,
++						 qtd->actual_length);
++					fallthrough;
++				case OUT_PID:
++					qtd->urb->actual_length +=
++							qtd->actual_length;
++					fallthrough;
++				case SETUP_PID:
++					break;
++				}
++			}
++
++			if (is_short_bulk(qtd)) {
++				if (qtd->urb->transfer_flags & URB_SHORT_NOT_OK)
++					qtd->urb->status = -EREMOTEIO;
++				if (!last_qtd)
++					qtd_next->status = QTD_RETIRE;
++			}
++		}
++
++		if (qtd->payload_addr)
++			free_mem(hcd, qtd);
++
++		if (last_qtd) {
++			if ((qtd->status == QTD_RETIRE) &&
++					(qtd->urb->status == -EINPROGRESS))
++				qtd->urb->status = -EPIPE;
++			/* Defer calling of urb_done() since it releases lock */
++			urb_listitem = kmem_cache_alloc(urb_listitem_cachep,
++								GFP_ATOMIC);
++			if (unlikely(!urb_listitem))
++				break; /* Try again on next call */
++			urb_listitem->urb = qtd->urb;
++			list_add_tail(&urb_listitem->urb_list, urb_list);
++		}
++
++		list_del(&qtd->qtd_list);
++		qtd_free(qtd);
++	}
++}
++
++#define ENQUEUE_DEPTH	2
++static void enqueue_qtds(struct usb_hcd *hcd, struct isp1760_qh *qh)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int slot_num = mem->slot_num;
++	int ptd_offset;
++	struct isp1760_slotinfo *slots;
++	int curr_slot, free_slot;
++	int n;
++	struct ptd ptd;
++	struct isp1760_qtd *qtd;
++
++	if (unlikely(list_empty(&qh->qtd_list)))
++		return;
++
++	/* Make sure this endpoint's TT buffer is clean before queueing ptds */
++	if (qh->tt_buffer_dirty)
++		return;
++
++	if (usb_pipeint(list_entry(qh->qtd_list.next, struct isp1760_qtd,
++				   qtd_list)->urb->pipe)) {
++		ptd_offset = INT_PTD_OFFSET;
++		slots = priv->int_slots;
++	} else {
++		ptd_offset = ATL_PTD_OFFSET;
++		slots = priv->atl_slots;
++	}
++
++	free_slot = -1;
++	for (curr_slot = 0; curr_slot < slot_num; curr_slot++) {
++		if ((free_slot == -1) && (slots[curr_slot].qtd == NULL))
++			free_slot = curr_slot;
++		if (slots[curr_slot].qh == qh)
++			break;
++	}
++
++	n = 0;
++	list_for_each_entry(qtd, &qh->qtd_list, qtd_list) {
++		if (qtd->status == QTD_ENQUEUED) {
++			WARN_ON(qtd->payload_addr);
++			alloc_mem(hcd, qtd);
++			if ((qtd->length) && (!qtd->payload_addr))
++				break;
++
++			if (qtd->length && (qtd->packet_type == SETUP_PID ||
++					    qtd->packet_type == OUT_PID)) {
++				mem_write(hcd, qtd->payload_addr,
++					  qtd->data_buffer, qtd->length);
++			}
++
++			qtd->status = QTD_PAYLOAD_ALLOC;
++		}
++
++		if (qtd->status == QTD_PAYLOAD_ALLOC) {
++/*
++			if ((curr_slot > 31) && (free_slot == -1))
++				printf("%s: No slot "
++					"available for transfer\n", __func__);
++*/
++			/* Start xfer for this endpoint if not already done */
++			if ((curr_slot > slot_num - 1) && (free_slot > -1)) {
++				if (usb_pipeint(qtd->urb->pipe))
++					create_ptd_int(qh, qtd, &ptd);
++				else
++					create_ptd_atl(qh, qtd, &ptd);
++
++				start_bus_transfer(hcd, ptd_offset, free_slot,
++							slots, qtd, qh, &ptd);
++				curr_slot = free_slot;
++			}
++
++			n++;
++			if (n >= ENQUEUE_DEPTH)
++				break;
++		}
++	}
++}
++
++static void schedule_ptds(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv;
++	struct isp1760_qh *qh, *qh_next;
++	struct list_head *ep_queue;
++	LIST_HEAD(urb_list);
++	struct urb_listitem *urb_listitem, *urb_listitem_next;
++	int i;
++
++	if (!hcd) {
++		WARN_ON(1);
++		return;
++	}
++
++	priv = hcd_to_priv(hcd);
++
++	/*
++	 * check finished/retired xfers, transfer payloads, call urb_done()
++	 */
++	for (i = 0; i < QH_END; i++) {
++		ep_queue = &priv->qh_list[i];
++		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list)
++			collect_qtds(hcd, qh, &urb_list);
++	}
++
++	list_for_each_entry_safe(urb_listitem, urb_listitem_next, &urb_list,
++				 urb_list) {
++		isp1760_urb_done(hcd, urb_listitem->urb);
++		kmem_cache_free(urb_listitem_cachep, urb_listitem);
++	}
++
++	/*
++	 * Schedule packets for transfer.
++	 *
++	 * According to USB2.0 specification:
++	 *
++	 * 1st prio: interrupt xfers, up to 80 % of bandwidth
++	 * 2nd prio: control xfers
++	 * 3rd prio: bulk xfers
++	 *
++	 * ... but let's use a simpler scheme here (mostly because ISP1761 doc
++	 * is very unclear on how to prioritize traffic):
++	 *
++	 * 1) Enqueue any queued control transfers, as long as payload chip mem
++	 *    and PTD ATL slots are available.
++	 * 2) Enqueue any queued INT transfers, as long as payload chip mem
++	 *    and PTD INT slots are available.
++	 * 3) Enqueue any queued bulk transfers, as long as payload chip mem
++	 *    and PTD ATL slots are available.
++	 *
++	 * Use double buffering (ENQUEUE_DEPTH==2) as a compromise between
++	 * conservation of chip mem and performance.
++	 *
++	 * I'm sure this scheme could be improved upon!
++	 */
++	for (i = 0; i < QH_END; i++) {
++		ep_queue = &priv->qh_list[i];
++		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list)
++			enqueue_qtds(hcd, qh);
++	}
++}
++
++#define PTD_STATE_QTD_DONE	1
++#define PTD_STATE_QTD_RELOAD	2
++#define PTD_STATE_URB_RETIRE	3
++
++static int check_int_transfer(struct usb_hcd *hcd, struct ptd *ptd,
++								struct urb *urb)
++{
++	u32 dw4;
++	int i;
++
++	dw4 = TO_U32(ptd->dw4);
++	dw4 >>= 8;
++
++	/* FIXME: ISP1761 datasheet does not say what to do with these. Do we
++	   need to handle these errors? Is it done in hardware? */
++	if (ptd->dw3 & DW3_HALT_BIT) {
++
++		urb->status = -EPROTO; /* Default unknown error */
++
++		for (i = 0; i < 8; i++) {
++			switch (dw4 & 0x7) {
++			case INT_UNDERRUN:
++				printf("underrun during uFrame %d\n", i);
++				urb->status = -ECOMM; /* Could not write data */
++				break;
++			case INT_EXACT:
++				printf("transaction error uFrame %d\n", i);
++				urb->status = -EPROTO; /* timeout, bad CRC, PID
++							  error etc. */
++				break;
++			case INT_BABBLE:
++				printf("babble error during uFrame %d\n", i);
++				urb->status = -EOVERFLOW;
++				break;
++			}
++			dw4 >>= 3;
++		}
++
++		return PTD_STATE_URB_RETIRE;
++	}
++
++	return PTD_STATE_QTD_DONE;
++}
++
++static int check_atl_transfer(struct usb_hcd *hcd, struct ptd *ptd,
++			      struct urb *urb)
++{
++	WARN_ON(!ptd);
++	if (ptd->dw3 & DW3_HALT_BIT) {
++		if (ptd->dw3 & DW3_BABBLE_BIT)
++			urb->status = -EOVERFLOW;
++		else if (FROM_DW3_CERR(ptd->dw3))
++			urb->status = -EPIPE;  /* Stall */
++		else
++			urb->status = -EPROTO; /* Unknown */
++
++		/* usefull debug
++		printf("%s: ptd error:\n"
++			"        dw0: %08x dw1: %08x dw2: %08x dw3: %08x\n"
++			"        dw4: %08x dw5: %08x dw6: %08x dw7: %08x\n",
++			__func__,
++			ptd->dw0, ptd->dw1, ptd->dw2, ptd->dw3,
++			ptd->dw4, ptd->dw5, ptd->dw6, ptd->dw7);
++		*/
++
++		return PTD_STATE_URB_RETIRE;
++	}
++
++	/* Transfer Error, *but* active and no HALT -> reload */
++	if ((ptd->dw3 & DW3_ERROR_BIT) && (ptd->dw3 & DW3_ACTIVE_BIT))
++		return PTD_STATE_QTD_RELOAD;
++
++	/*
++	* NAKs are handled in HW by the chip. Usually if the
++	* device is not able to send data fast enough.
++	* This happens mostly on slower hardware.
++	*/
++	if (!FROM_DW3_NAKCOUNT(ptd->dw3) && (ptd->dw3 & DW3_ACTIVE_BIT))
++		return PTD_STATE_QTD_RELOAD;
++
++	return PTD_STATE_QTD_DONE;
++}
++
++static void handle_done_ptds(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	struct isp1760_slotinfo *slots;
++	struct isp1760_qtd *qtd;
++	struct isp1760_qh *qh;
++	struct ptd ptd;
++	u32 ptd_offset;
++	int modified;
++	int skip_map;
++	int state;
++	int slot;
++
++	skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++	priv->int_done_map &= ~skip_map;
++	skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++	priv->atl_done_map &= ~skip_map;
++
++	modified = priv->int_done_map || priv->atl_done_map;
++
++	while (priv->int_done_map || priv->atl_done_map) {
++		if (priv->int_done_map) {
++			/* INT ptd */
++			slot = __ffs(priv->int_done_map);
++			priv->int_done_map &= ~(1 << slot);
++			slots = priv->int_slots;
++			/* This should not trigger, and could be removed if
++			   noone have any problems with it triggering: */
++			if (!slots[slot].qh) {
++				WARN_ON(1);
++				continue;
++			}
++			ptd_offset = INT_PTD_OFFSET;
++			ptd_read(hcd, INT_PTD_OFFSET, slot, &ptd);
++			state = check_int_transfer(hcd, &ptd,
++						   slots[slot].qtd->urb);
++		} else {
++			/* ATL ptd */
++			slot = __ffs(priv->atl_done_map);
++			priv->atl_done_map &= ~(1 << slot);
++			slots = priv->atl_slots;
++			/* This should not trigger, and could be removed if
++			   noone have any problems with it triggering: */
++			if (!slots[slot].qh) {
++				WARN_ON(1);
++				continue;
++			}
++			ptd_offset = ATL_PTD_OFFSET;
++			ptd_read(hcd, ATL_PTD_OFFSET, slot, &ptd);
++			state = check_atl_transfer(hcd, &ptd,
++						   slots[slot].qtd->urb);
++		}
++
++		qtd = slots[slot].qtd;
++		slots[slot].qtd = NULL;
++		qh = slots[slot].qh;
++		slots[slot].qh = NULL;
++		qh->slot = -1;
++
++		WARN_ON(qtd->status != QTD_XFER_STARTED);
++
++		switch (state) {
++		case PTD_STATE_QTD_DONE:
++			if ((usb_pipeint(qtd->urb->pipe)) &&
++			    (qtd->urb->dev->speed != USB_SPEED_HIGH))
++				qtd->actual_length =
++				       FROM_DW3_SCS_NRBYTESTRANSFERRED(ptd.dw3);
++			else
++				qtd->actual_length =
++					FROM_DW3_NRBYTESTRANSFERRED(ptd.dw3);
++
++			qtd->status = QTD_XFER_COMPLETE;
++
++			if (list_is_last(&qtd->qtd_list, &qh->qtd_list) ||
++			    is_short_bulk(qtd))
++				qtd = NULL;
++			else
++				qtd = list_entry(qtd->qtd_list.next,
++							typeof(*qtd), qtd_list);
++
++			qh->toggle = FROM_DW3_DATA_TOGGLE(ptd.dw3);
++			qh->ping = FROM_DW3_PING(ptd.dw3);
++
++			break;
++
++		case PTD_STATE_QTD_RELOAD: /* QTD_RETRY, for atls only */
++			qtd->status = QTD_PAYLOAD_ALLOC;
++			ptd.dw0 |= DW0_VALID_BIT;
++			/* RL counter = ERR counter */
++			ptd.dw3 &= ~TO_DW3_NAKCOUNT(0xf);
++			ptd.dw3 |= TO_DW3_NAKCOUNT(FROM_DW2_RL(ptd.dw2));
++			ptd.dw3 &= ~TO_DW3_CERR(3);
++			ptd.dw3 |= TO_DW3_CERR(ERR_COUNTER);
++
++			qh->toggle = FROM_DW3_DATA_TOGGLE(ptd.dw3);
++			qh->ping = FROM_DW3_PING(ptd.dw3);
++			break;
++
++		case PTD_STATE_URB_RETIRE:
++			qtd->status = QTD_RETIRE;
++			qtd = NULL;
++			qh->toggle = 0;
++			qh->ping = 0;
++			break;
++
++		default:
++			WARN_ON(1);
++			continue;
++		}
++
++		if (qtd && (qtd->status == QTD_PAYLOAD_ALLOC)) {
++			if (slots == priv->int_slots) {
++				if (state == PTD_STATE_QTD_RELOAD)
++					dev_err(priv->dev,
++						"%s: PTD_STATE_QTD_RELOAD on "
++						"interrupt packet\n", __func__);
++				if (state != PTD_STATE_QTD_RELOAD)
++					create_ptd_int(qh, qtd, &ptd);
++			} else {
++				if (state != PTD_STATE_QTD_RELOAD)
++					create_ptd_atl(qh, qtd, &ptd);
++			}
++
++			start_bus_transfer(hcd, ptd_offset, slot, slots, qtd,
++				qh, &ptd);
++		}
++	}
++
++	if (modified)
++		schedule_ptds(hcd);
++}
++
++static irqreturn_t isp1760_irq(int irq, void *__hci)
++{
++	struct usb_hcd *hcd = __hci;
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	irqreturn_t irqret = IRQ_NONE;
++	u32 int_reg;
++	u32 imask;
++
++	imask = isp1760_hcd_read(hcd, HC_INTERRUPT);
++	if (unlikely(!imask))
++		return irqret;
++
++	int_reg = priv->is_isp1763 ? ISP1763_HC_INTERRUPT :
++		ISP176x_HC_INTERRUPT;
++	isp1760_reg_write(priv->regs, int_reg, imask);
++
++	priv->int_done_map |= isp1760_hcd_read(hcd, HC_INT_PTD_DONEMAP);
++	priv->atl_done_map |= isp1760_hcd_read(hcd, HC_ATL_PTD_DONEMAP);
++
++	handle_done_ptds(hcd);
++
++	irqret = IRQ_HANDLED;
++
++	return irqret;
++}
++
++static int isp1763_run(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int retval;
++	u32 chipid_h;
++	u32 chipid_l;
++	u32 chip_rev;
++	u32 ptd_atl_int;
++	u32 ptd_iso;
++
++	chipid_h = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	chipid_l = isp1760_hcd_read(hcd, HC_CHIP_ID_LOW);
++	chip_rev = isp1760_hcd_read(hcd, HC_CHIP_REV);
++	printf("USB ISP %02x%02x HW rev. %d started\n", chipid_h,
++	       chipid_l, chip_rev);
++
++	isp1760_hcd_clear(hcd, ISO_BUF_FILL);
++	isp1760_hcd_clear(hcd, INT_BUF_FILL);
++	isp1760_hcd_clear(hcd, ATL_BUF_FILL);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++	ndelay(100);
++	isp1760_hcd_clear(hcd, HC_ATL_PTD_DONEMAP);
++	isp1760_hcd_clear(hcd, HC_INT_PTD_DONEMAP);
++	isp1760_hcd_clear(hcd, HC_ISO_PTD_DONEMAP);
++
++	isp1760_hcd_set(hcd, HW_OTG_DISABLE);
++	isp1760_reg_write(priv->regs, ISP1763_HC_OTG_CTRL_CLEAR, BIT(7));
++	isp1760_reg_write(priv->regs, ISP1763_HC_OTG_CTRL_CLEAR, BIT(15));
++	mdelay(10);
++
++	isp1760_hcd_set(hcd, HC_INT_IRQ_ENABLE);
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_ENABLE);
++
++	isp1760_hcd_set(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, HC_ATL_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_INT_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_ISO_IRQ_MASK_AND);
++
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_INT_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_ISO_IRQ_MASK_OR);
++
++	ptd_atl_int = 0x8000;
++	ptd_iso = 0x0001;
++
++	isp1760_hcd_write(hcd, HC_ATL_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_INT_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_ISO_PTD_LASTPTD, ptd_iso);
++
++	isp1760_hcd_set(hcd, ATL_BUF_FILL);
++	isp1760_hcd_set(hcd, INT_BUF_FILL);
++
++	isp1760_hcd_clear(hcd, CMD_LRESET);
++	isp1760_hcd_clear(hcd, CMD_RESET);
++
++	retval = isp1760_hcd_set_and_wait(hcd, CMD_RUN, 250 * 1000);
++	if (retval)
++		return retval;
++
++	down_write(&ehci_cf_port_reset_rwsem);
++	retval = isp1760_hcd_set_and_wait(hcd, FLAG_CF, 250 * 1000);
++	up_write(&ehci_cf_port_reset_rwsem);
++	retval = 0;
++	if (retval)
++		return retval;
++
++	return 0;
++}
++
++static int isp1760_run(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int retval;
++	u32 chipid_h;
++	u32 chipid_l;
++	u32 chip_rev;
++	u32 ptd_atl_int;
++	u32 ptd_iso;
++
++	/*
++	 * ISP1763 have some differences in the setup and order to enable
++	 * the ports, disable otg, setup buffers, and ATL, INT, ISO status.
++	 * So, just handle it a separate sequence.
++	 */
++	if (priv->is_isp1763)
++		return isp1763_run(hcd);
++
++	/* Set PTD interrupt AND & OR maps */
++	isp1760_hcd_clear(hcd, HC_ATL_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_INT_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_ISO_IRQ_MASK_AND);
++
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_INT_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_ISO_IRQ_MASK_OR);
++
++	/* step 23 passed */
++
++	isp1760_hcd_set(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, CMD_LRESET);
++	isp1760_hcd_clear(hcd, CMD_RESET);
++
++	retval = isp1760_hcd_set_and_wait(hcd, CMD_RUN, 250 * 1000);
++	if (retval)
++		return retval;
++
++	/*
++	 * XXX
++	 * Spec says to write FLAG_CF as last config action, priv code grabs
++	 * the semaphore while doing so.
++	 */
++	down_write(&ehci_cf_port_reset_rwsem);
++
++	retval = isp1760_hcd_set_and_wait(hcd, FLAG_CF, 250 * 1000);
++	up_write(&ehci_cf_port_reset_rwsem);
++	if (retval)
++		return retval;
++
++	chipid_h = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	chipid_l = isp1760_hcd_read(hcd, HC_CHIP_ID_LOW);
++	chip_rev = isp1760_hcd_read(hcd, HC_CHIP_REV);
++	dev_info(priv->dev, "USB ISP %02x%02x HW rev. %d started\n",
++		 chipid_h, chipid_l, chip_rev);
++
++	/* PTD Register Init Part 2, Step 28 */
++
++	/* Setup registers controlling PTD checking */
++	ptd_atl_int = 0x80000000;
++	ptd_iso = 0x00000001;
++
++	isp1760_hcd_write(hcd, HC_ATL_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_INT_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_ISO_PTD_LASTPTD, ptd_iso);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++
++	isp1760_hcd_set(hcd, ATL_BUF_FILL);
++	isp1760_hcd_set(hcd, INT_BUF_FILL);
++
++	/* GRR this is run-once init(), being done every time the HC starts.
++	 * So long as they're part of class devices, we can't do it init()
++	 * since the class device isn't created that early.
++	 */
++	return 0;
++}
++
++static int qtd_fill(struct isp1760_qtd *qtd, void *databuffer, size_t len)
++{
++	qtd->data_buffer = databuffer;
++
++	qtd->length = len;
++
++	return qtd->length;
++}
++
++static void qtd_list_free(struct list_head *qtd_list)
++{
++	struct isp1760_qtd *qtd, *qtd_next;
++
++	list_for_each_entry_safe(qtd, qtd_next, qtd_list, qtd_list) {
++		list_del(&qtd->qtd_list);
++		qtd_free(qtd);
++	}
++}
++
++/*
++ * Packetize urb->transfer_buffer into list of packets of size wMaxPacketSize.
++ * Also calculate the PID type (SETUP/IN/OUT) for each packet.
++ */
++#define max_packet(wMaxPacketSize) ((wMaxPacketSize) & 0x07ff)
++static void packetize_urb(struct usb_hcd *hcd,
++		struct urb *urb, struct list_head *head, gfp_t flags)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	struct isp1760_qtd *qtd;
++	void *buf;
++	int len, maxpacketsize;
++	u8 packet_type;
++
++	/*
++	 * URBs map to sequences of QTDs:  one logical transaction
++	 */
++
++	if (!urb->transfer_buffer && urb->transfer_buffer_length) {
++		/* XXX This looks like usb storage / SCSI bug */
++		dev_err(priv->dev, "buf is null, dma is %08lx len is %d\n",
++			(long unsigned)urb->transfer_dma,
++			urb->transfer_buffer_length);
++		WARN_ON(1);
++	}
++
++	if (usb_pipein(urb->pipe))
++		packet_type = IN_PID;
++	else
++		packet_type = OUT_PID;
++
++	if (usb_pipecontrol(urb->pipe)) {
++		qtd = qtd_alloc(flags, urb, SETUP_PID);
++		if (!qtd)
++			goto cleanup;
++		qtd_fill(qtd, urb->setup_packet, sizeof(struct usb_ctrlrequest));
++		list_add_tail(&qtd->qtd_list, head);
++
++		/* for zero length DATA stages, STATUS is always IN */
++		if (urb->transfer_buffer_length == 0)
++			packet_type = IN_PID;
++	}
++
++	maxpacketsize = max_packet(usb_maxpacket(urb->dev, urb->pipe));
++
++	/*
++	 * buffer gets wrapped in one or more qtds;
++	 * last one may be "short" (including zero len)
++	 * and may serve as a control status ack
++	 */
++	buf = urb->transfer_buffer;
++	len = urb->transfer_buffer_length;
++
++	for (;;) {
++		int this_qtd_len;
++
++		qtd = qtd_alloc(flags, urb, packet_type);
++		if (!qtd)
++			goto cleanup;
++
++		if (len > mem->blocks_size[ISP176x_BLOCK_NUM - 1])
++			len = mem->blocks_size[ISP176x_BLOCK_NUM - 1];
++
++		this_qtd_len = qtd_fill(qtd, buf, len);
++		list_add_tail(&qtd->qtd_list, head);
++
++		len -= this_qtd_len;
++		buf += this_qtd_len;
++
++		if (len <= 0)
++			break;
++	}
++
++	/*
++	 * control requests may need a terminating data "status" ack;
++	 * bulk ones may need a terminating short packet (zero length).
++	 */
++	if (urb->transfer_buffer_length != 0) {
++		int one_more = 0;
++
++		if (usb_pipecontrol(urb->pipe)) {
++			one_more = 1;
++			if (packet_type == IN_PID)
++				packet_type = OUT_PID;
++			else
++				packet_type = IN_PID;
++		} else if (usb_pipebulk(urb->pipe)
++			   && (urb->transfer_flags & URB_ZERO_PACKET)
++			   && !(urb->transfer_buffer_length %
++				maxpacketsize)) {
++			one_more = 1;
++		}
++		if (one_more) {
++			qtd = qtd_alloc(flags, urb, packet_type);
++			if (!qtd)
++				goto cleanup;
++
++			/* never any data in such packets */
++			qtd_fill(qtd, NULL, 0);
++			list_add_tail(&qtd->qtd_list, head);
++		}
++	}
++
++	return;
++
++cleanup:
++	qtd_list_free(head);
++}
++
++static int isp1760_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
++			       gfp_t mem_flags)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	struct isp1760_qh *qh = NULL;
++	struct list_head *ep_queue;
++	LIST_HEAD(new_qtds);
++	int qh_in_queue;
++	int retval;
++	int epnum;
++
++	switch (usb_pipetype(urb->pipe)) {
++	case PIPE_CONTROL:
++		ep_queue = &priv->qh_list[QH_CONTROL];
++		break;
++	case PIPE_BULK:
++		ep_queue = &priv->qh_list[QH_BULK];
++		break;
++	case PIPE_INTERRUPT:
++		ep_queue = &priv->qh_list[QH_INTERRUPT];
++		break;
++	case PIPE_ISOCHRONOUS:
++		printf("isochronous USB packets not yet supported\n");
++		return -EPIPE;
++	default:
++		printf("unknown pipe type\n");
++		return -EPIPE;
++	}
++
++	if (usb_pipein(urb->pipe))
++		urb->actual_length = 0;
++
++	packetize_urb(hcd, urb, &new_qtds, mem_flags);
++	if (list_empty(&new_qtds))
++		return -ENOMEM;
++
++	retval = usb_hcd_link_urb_to_ep(hcd, urb);
++	if (retval) {
++		qtd_list_free(&new_qtds);
++		goto out;
++	}
++
++	epnum = usb_pipeendpoint(urb->pipe);
++
++	qh_in_queue = 0;
++	list_for_each_entry(qh, ep_queue, qh_list) {
++		if (qh->epnum == epnum) {
++			qh_in_queue = 1;
++			break;
++		}
++	}
++
++	if (!qh_in_queue) {
++		qh = qh_alloc(GFP_ATOMIC);
++		if (!qh) {
++			retval = -ENOMEM;
++			usb_hcd_unlink_urb_from_ep(hcd, urb);
++			qtd_list_free(&new_qtds);
++			goto out;
++		}
++
++		qh->epnum = epnum;
++		list_add_tail(&qh->qh_list, ep_queue);
++		urb->ep->hcpriv = qh;
++	}
++
++	list_splice_tail(&new_qtds, &qh->qtd_list);
++	schedule_ptds(hcd);
++
++out:
++	return retval;
++}
++
++static void kill_transfer(struct usb_hcd *hcd, struct urb *urb,
++		struct isp1760_qh *qh)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int skip_map;
++
++	WARN_ON(qh->slot == -1);
++
++	/* We need to forcefully reclaim the slot since some transfers never
++	   return, e.g. interrupt transfers and NAKed bulk transfers. */
++	if (usb_pipecontrol(urb->pipe) || usb_pipebulk(urb->pipe)) {
++		skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++		skip_map |= (1 << qh->slot);
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP, skip_map);
++		ndelay(100);
++		priv->atl_slots[qh->slot].qh = NULL;
++		priv->atl_slots[qh->slot].qtd = NULL;
++	} else {
++		skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++		skip_map |= (1 << qh->slot);
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP, skip_map);
++		priv->int_slots[qh->slot].qh = NULL;
++		priv->int_slots[qh->slot].qtd = NULL;
++	}
++
++	qh->slot = -1;
++}
++
++/*
++ * Retire the qtds beginning at 'qtd' and belonging all to the same urb, killing
++ * any active transfer belonging to the urb in the process.
++ */
++static void dequeue_urb_from_qtd(struct usb_hcd *hcd, struct isp1760_qh *qh,
++				 struct isp1760_qtd *qtd)
++{
++	struct urb *urb;
++	int urb_was_running;
++
++	urb = qtd->urb;
++	urb_was_running = 0;
++	list_for_each_entry_from(qtd, &qh->qtd_list, qtd_list) {
++		if (qtd->urb != urb)
++			break;
++
++		if (qtd->status >= QTD_XFER_STARTED)
++			urb_was_running = 1;
++		if (last_qtd_of_urb(qtd, qh) &&
++					(qtd->status >= QTD_XFER_COMPLETE))
++			urb_was_running = 0;
++
++		if (qtd->status == QTD_XFER_STARTED)
++			kill_transfer(hcd, urb, qh);
++		qtd->status = QTD_RETIRE;
++	}
++}
++
++int isp1760_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
++{
++	struct isp1760_qtd *qtd;
++	struct isp1760_qh *qh;
++	int retval = 0;
++
++	retval = usb_hcd_check_unlink_urb(hcd, urb, status);
++	if (retval)
++		goto out;
++
++	qh = urb->ep->hcpriv;
++	if (!qh) {
++		retval = -EINVAL;
++		goto out;
++	}
++
++	list_for_each_entry(qtd, &qh->qtd_list, qtd_list)
++		if (qtd->urb == urb) {
++			dequeue_urb_from_qtd(hcd, qh, qtd);
++			list_move(&qtd->qtd_list, &qh->qtd_list);
++			break;
++		}
++
++	urb->status = status;
++	schedule_ptds(hcd);
++
++out:
++	return retval;
++}
++
++static void isp1760_hub_descriptor(struct isp1760_hcd *priv,
++				   struct usb_hub_descriptor *desc)
++{
++	int ports;
++	u16 temp;
++
++	ports = isp1760_hcd_n_ports(priv->hcd);
++
++	desc->bDescriptorType = USB_DT_HUB;
++	/* priv 1.0, 2.3.9 says 20ms max */
++	desc->bPwrOn2PwrGood = 10;
++	desc->bHubContrCurrent = 0;
++
++	desc->bNbrPorts = ports;
++	temp = 1 + (ports / 8);
++	desc->bLength = 7 + 2 * temp;
++
++	/* ports removable, and usb 1.0 legacy PortPwrCtrlMask */
++	memset(&desc->u.hs.DeviceRemovable[0], 0, temp);
++	memset(&desc->u.hs.DeviceRemovable[temp], 0xff, temp);
++
++	/* per-port overcurrent reporting */
++	temp = HUB_CHAR_INDV_PORT_OCPM;
++	if (isp1760_hcd_ppc_is_set(priv->hcd))
++		/* per-port power control */
++		temp |= HUB_CHAR_INDV_PORT_LPSM;
++	else
++		/* no power switching */
++		temp |= HUB_CHAR_NO_LPSM;
++	desc->wHubCharacteristics = cpu_to_le16(temp);
++}
++
++#define	PORT_WAKE_BITS	(PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E)
++
++static void check_reset_complete(struct usb_hcd *hcd, int index)
++{
++	if (!(isp1760_hcd_is_set(hcd, PORT_CONNECT)))
++		return;
++
++	/* if reset finished and it's still not enabled -- handoff */
++	if (!isp1760_hcd_is_set(hcd, PORT_PE)) {
++		printf("port %d full speed --> companion\n", index + 1);
++
++		isp1760_hcd_set(hcd, PORT_OWNER);
++
++		isp1760_hcd_clear(hcd, PORT_CSC);
++	} else {
++		printf("port %d high speed\n", index + 1);
++	}
++
++	return;
++}
++
++static int isp1760_hub_control(struct usb_hcd *hcd, struct usb_device *dev,
++			       unsigned long pipe, void *buffer, int length,
++			       struct devrequest *setup)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u16 typeReq, wValue, wIndex;
++	unsigned long flags;
++	char *buf = buffer;
++	void *src = NULL;
++	int src_len = 0;
++	int retval = 0;
++	u32 status;
++	int ports;
++
++	if (!setup)
++		return -EINVAL;
++
++	ports = isp1760_hcd_n_ports(hcd);
++
++	typeReq = setup->request | (setup->requesttype << 8);
++	wValue = le16_to_cpu(setup->value);
++	wIndex = le16_to_cpu(setup->index);
++
++	/*
++	 * FIXME:  support SetPortFeatures USB_PORT_FEAT_INDICATOR.
++	 * HCS_INDICATOR may say we can change LEDs to off/amber/green.
++	 * (track current state ourselves) ... blink for diagnostics,
++	 * power, "this is the one", etc.  EHCI spec supports this.
++	 */
++
++	switch (typeReq) {
++	case DeviceOutRequest | USB_REQ_SET_ADDRESS:
++		break;
++	case DeviceOutRequest | USB_REQ_SET_CONFIGURATION:
++		/* Nothing to do */
++		break;
++	case DeviceRequest | USB_REQ_GET_DESCRIPTOR:
++		switch (wValue & 0xff00) {
++		case USB_DT_DEVICE << 8:
++			src = &rh_descriptor.device;
++			src_len = 0x12;
++			break;
++		case USB_DT_CONFIG << 8:
++			src = &rh_descriptor.config;
++			src_len = 0x09;
++			break;
++		case USB_DT_STRING << 8:
++			switch (wValue & 0xff) {
++			case 0:	/* Language */
++				src = "\4\3\19\4";
++				src_len = 4;
++				break;
++			case 1:	/* Vendor String  */
++				src = "\16\3U\0-\0B\0o\0o\0t\0";
++				src_len = 14;
++				break;
++			case 2:	/* Product Name */
++				src = "\52\3I\0S\0P\0-\0 "
++					 "\0H\0o\0s\0t\0 "
++					 "\0C\0o\0n\0t\0r\0o\0l\0l\0e\0r\0";
++				src_len = 42;
++				break;
++			default:
++				goto error;
++			}
++			break;
++		}
++		break;
++	case ClearHubFeature:
++		switch (wValue) {
++		case C_HUB_LOCAL_POWER:
++		case C_HUB_OVER_CURRENT:
++			/* no hub-wide feature/status flags */
++			break;
++		default:
++			goto error;
++		}
++		break;
++	case ClearPortFeature:
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++
++		/*
++		 * Even if OWNER is set, so the port is owned by the
++		 * companion controller, hub_wq needs to be able to clear
++		 * the port-change status bits (especially
++		 * USB_PORT_STAT_C_CONNECTION).
++		 */
++
++		switch (wValue) {
++		case USB_PORT_FEAT_ENABLE:
++			isp1760_hcd_clear(hcd, PORT_PE);
++			break;
++		case USB_PORT_FEAT_C_ENABLE:
++			/* XXX error? */
++			break;
++		case USB_PORT_FEAT_SUSPEND:
++			if (isp1760_hcd_is_set(hcd, PORT_RESET))
++				goto error;
++
++			if (isp1760_hcd_is_set(hcd, PORT_SUSPEND)) {
++				if (!isp1760_hcd_is_set(hcd, PORT_PE))
++					goto error;
++				/* resume signaling for 20 msec */
++				isp1760_hcd_clear(hcd, PORT_CSC);
++				isp1760_hcd_set(hcd, PORT_RESUME);
++
++				priv->reset_done = get_timer(0) + 40;
++			}
++			break;
++		case USB_PORT_FEAT_C_SUSPEND:
++			/* we auto-clear this feature */
++			break;
++		case USB_PORT_FEAT_POWER:
++			if (isp1760_hcd_ppc_is_set(hcd))
++				isp1760_hcd_clear(hcd, PORT_POWER);
++			break;
++		case USB_PORT_FEAT_C_CONNECTION:
++			isp1760_hcd_set(hcd, PORT_CSC);
++			break;
++		case USB_PORT_FEAT_C_OVER_CURRENT:
++			/* XXX error ?*/
++			break;
++		case USB_PORT_FEAT_C_RESET:
++			/* GetPortStatus clears reset */
++			break;
++		default:
++			goto error;
++		}
++		isp1760_hcd_read(hcd, CMD_RUN);
++		break;
++	case GetHubDescriptor:
++		isp1760_hub_descriptor(priv, (struct usb_hub_descriptor *) buf);
++		break;
++	case GetHubStatus:
++		/* no hub-wide feature/status flags */
++		memset(buf, 0, 4);
++		break;
++	case GetPortStatus:
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++		status = 0;
++
++		/* wPortChange bits */
++		if (isp1760_hcd_is_set(hcd, PORT_CSC))
++			status |= USB_PORT_STAT_C_CONNECTION << 16;
++
++		/* whoever resumes must GetPortStatus to complete it!! */
++		if (isp1760_hcd_is_set(hcd, PORT_RESUME)) {
++			status |= USB_PORT_STAT_C_SUSPEND << 16;
++
++			if (!priv->reset_done) {
++				priv->reset_done = get_timer(0) + 20;
++			} else if (get_timer(0) > priv->reset_done) {
++				/* stop resume signaling */
++				isp1760_hcd_clear(hcd, PORT_CSC);
++
++				retval = isp1760_hcd_clear_and_wait(hcd,
++								    PORT_RESUME, 2000);
++				if (retval != 0) {
++					printf("port %d resume error %d\n",
++					       wIndex + 1, retval);
++					goto error;
++				}
++			}
++		}
++
++		/* whoever resets must GetPortStatus to complete it!! */
++		if (isp1760_hcd_is_set(hcd, PORT_RESET) &&
++		    get_timer(0) > priv->reset_done) {
++			status |= USB_PORT_STAT_C_RESET << 16;
++			priv->reset_done = 0;
++
++			/* force reset to complete */
++			/* REVISIT:  some hardware needs 550+ usec to clear
++			 * this bit; seems too long to spin routinely...
++			 */
++			retval = isp1760_hcd_clear_and_wait(hcd, PORT_RESET,
++							    750);
++			if (retval != 0) {
++				printf("port %d reset error %d\n", wIndex + 1,
++				       retval);
++				goto error;
++			}
++
++			/* see what we found out */
++			check_reset_complete(hcd, wIndex);
++		}
++		/*
++		 * Even if OWNER is set, there's no harm letting hub_wq
++		 * see the wPortStatus values (they should all be 0 except
++		 * for PORT_POWER anyway).
++		 */
++
++		if (isp1760_hcd_is_set(hcd, PORT_OWNER))
++			printf("PORT_OWNER is set\n");
++
++		if (isp1760_hcd_is_set(hcd, PORT_CONNECT)) {
++			status |= USB_PORT_STAT_CONNECTION;
++
++			/* status may be from integrated TT */
++			status |= USB_PORT_STAT_HIGH_SPEED;
++		}
++		if (isp1760_hcd_is_set(hcd, PORT_PE))
++			status |= USB_PORT_STAT_ENABLE;
++		if (isp1760_hcd_is_set(hcd, PORT_SUSPEND) &&
++		    isp1760_hcd_is_set(hcd, PORT_RESUME))
++			status |= USB_PORT_STAT_SUSPEND;
++		if (isp1760_hcd_is_set(hcd, PORT_RESET))
++			status |= USB_PORT_STAT_RESET;
++		if (isp1760_hcd_is_set(hcd, PORT_POWER))
++			status |= USB_PORT_STAT_POWER;
++
++		put_unaligned(cpu_to_le32(status), (__le32 *) buf);
++		break;
++	case SetHubFeature:
++		switch (wValue) {
++		case C_HUB_LOCAL_POWER:
++		case C_HUB_OVER_CURRENT:
++			/* no hub-wide feature/status flags */
++			break;
++		default:
++			goto error;
++		}
++		break;
++	case SetPortFeature:
++		wIndex &= 0xff;
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++
++		if (isp1760_hcd_is_set(hcd, PORT_OWNER))
++			break;
++
++		switch (wValue) {
++		case USB_PORT_FEAT_ENABLE:
++			isp1760_hcd_set(hcd, PORT_PE);
++			break;
++
++		case USB_PORT_FEAT_SUSPEND:
++			if (!isp1760_hcd_is_set(hcd, PORT_PE) ||
++			    isp1760_hcd_is_set(hcd, PORT_RESET))
++				goto error;
++
++			isp1760_hcd_set(hcd, PORT_SUSPEND);
++			break;
++		case USB_PORT_FEAT_POWER:
++			if (isp1760_hcd_ppc_is_set(hcd))
++				isp1760_hcd_set(hcd, PORT_POWER);
++			break;
++		case USB_PORT_FEAT_RESET:
++			if (isp1760_hcd_is_set(hcd, PORT_RESUME))
++				goto error;
++			/* line status bits may report this as low speed,
++			 * which can be fine if this root hub has a
++			 * transaction translator built in.
++			 */
++			if ((isp1760_hcd_is_set(hcd, PORT_CONNECT) &&
++			     !isp1760_hcd_is_set(hcd, PORT_PE)) &&
++			    (isp1760_hcd_read(hcd, PORT_LSTATUS) == 1)) {
++				isp1760_hcd_set(hcd, PORT_OWNER);
++			} else {
++				isp1760_hcd_set(hcd, PORT_RESET);
++				isp1760_hcd_clear(hcd, PORT_PE);
++
++				priv->reset_done = get_timer(0) + 50;
++			}
++			break;
++		default:
++			goto error;
++		}
++		break;
++
++	default:
++		printf("root: unknown request: 0x%0x\n", typeReq);
++		goto error;
++	}
++	spin_unlock_irqrestore(&priv->lock, flags);
++
++	if (src_len) {
++		length = min(src_len, length);
++
++		if (src != NULL && length > 0)
++			memcpy(buffer, src, length);
++		else
++			printf("zero copy USB descriptor\n");
++	}
++
++	dev->act_len = length;
++	dev->status = 0;
++
++	return 0;
++
++error:
++	/* "stall" on error */
++	dev->act_len = 0;
++	dev->status = USB_ST_STALLED;
++	return -EPIPE;
++}
++
++#ifndef __UBOOT__
++static int isp1760_hub_status_data(struct usb_hcd *hcd, char *buf)
++{
++	u32 status = 0;
++	int retval = 1;
++
++	/* init status to no-changes */
++	buf[0] = 0;
++
++	if (isp1760_hcd_is_set(hcd, PORT_OWNER) &&
++	    isp1760_hcd_is_set(hcd, PORT_CSC)) {
++		isp1760_hcd_clear(hcd, PORT_CSC);
++		goto done;
++	}
++
++done:
++	return status ? retval : 0;
++}
++
++static void isp1760_endpoint_disable(struct usb_hcd *hcd,
++		struct usb_host_endpoint *ep)
++{
++	struct isp1760_qh *qh, *qh_iter;
++	unsigned long spinflags;
++	int i;
++
++	qh = ep->hcpriv;
++	if (!qh)
++		return;
++
++	WARN_ON(!list_empty(&qh->qtd_list));
++
++	for (i = 0; i < QH_END; i++)
++		list_for_each_entry(qh_iter, &priv->qh_list[i], qh_list)
++			if (qh_iter == qh) {
++				list_del(&qh_iter->qh_list);
++				i = QH_END;
++				break;
++			}
++	qh_free(qh);
++	ep->hcpriv = NULL;
++
++	schedule_ptds(hcd);
++}
++
++static int isp1760_get_frame(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 fr;
++
++	fr = isp1760_hcd_read(hcd, HC_FRINDEX);
++	return (fr >> 3) % priv->periodic_size;
++}
++
++static void isp1760_stop(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	msleep(20);
++
++	spin_lock_irq(&priv->lock);
++	ehci_reset(hcd);
++	/* Disable IRQ */
++	isp1760_hcd_clear(hcd, HW_GLOBAL_INTR_EN);
++	spin_unlock_irq(&priv->lock);
++
++	isp1760_hcd_clear(hcd, FLAG_CF);
++}
++
++static void isp1760_shutdown(struct usb_hcd *hcd)
++{
++	isp1760_stop(hcd);
++
++	isp1760_hcd_clear(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, CMD_RUN);
++}
++
++static void isp1760_clear_tt_buffer_complete(struct usb_hcd *hcd,
++						struct usb_host_endpoint *ep)
++{
++	struct isp1760_qh *qh = ep->hcpriv;
++	unsigned long spinflags;
++
++	if (!qh)
++		return;
++
++	qh->tt_buffer_dirty = 0;
++	schedule_ptds(hcd);
++}
++
++
++static const struct hc_driver isp1760_hc_driver = {
++	.description		= "isp1760-hcd",
++	.product_desc		= "NXP ISP1760 USB Host Controller",
++	.hcd_priv_size		= sizeof(struct isp1760_hcd *),
++	.irq			= isp1760_irq,
++	.flags			= HCD_MEMORY | HCD_USB2,
++	.reset			= isp1760_hc_setup,
++	.start			= isp1760_run,
++	.stop			= isp1760_stop,
++	.shutdown		= isp1760_shutdown,
++	.urb_enqueue		= isp1760_urb_enqueue,
++	.urb_dequeue		= isp1760_urb_dequeue,
++	.endpoint_disable	= isp1760_endpoint_disable,
++	.get_frame_number	= isp1760_get_frame,
++	.hub_status_data	= isp1760_hub_status_data,
++	.hub_control		= isp1760_hub_control,
++	.clear_tt_buffer_complete	= isp1760_clear_tt_buffer_complete,
++};
++#endif // __UBOOT__
++
++int __init isp1760_init_kmem_once(void)
++{
++	urb_listitem_cachep = kmem_cache_create("isp1760_urb_listitem",
++			sizeof(struct urb_listitem), 0, SLAB_TEMPORARY |
++			SLAB_MEM_SPREAD, NULL);
++
++	if (!urb_listitem_cachep)
++		return -ENOMEM;
++
++	qtd_cachep = kmem_cache_create("isp1760_qtd",
++			sizeof(struct isp1760_qtd), 0, SLAB_TEMPORARY |
++			SLAB_MEM_SPREAD, NULL);
++
++	if (!qtd_cachep)
++		return -ENOMEM;
++
++	qh_cachep = kmem_cache_create("isp1760_qh", sizeof(struct isp1760_qh),
++			0, SLAB_TEMPORARY | SLAB_MEM_SPREAD, NULL);
++
++	if (!qh_cachep) {
++		kmem_cache_destroy(qtd_cachep);
++		return -ENOMEM;
++	}
++
++	return 0;
++}
++
++void isp1760_deinit_kmem_cache(void)
++{
++	kmem_cache_destroy(qtd_cachep);
++	kmem_cache_destroy(qh_cachep);
++	kmem_cache_destroy(urb_listitem_cachep);
++}
++
++int isp1760_hcd_lowlevel_init(struct isp1760_hcd *priv)
++{
++	int ret;
++
++	ret = isp1760_hc_setup(priv->hcd);
++	if (ret < 0)
++		return ret;
++
++	ret = isp1760_run(priv->hcd);
++	if (ret < 0)
++		return ret;
++
++	return 0;
++}
++
++static const struct usb_urb_ops isp1760_urb_ops = {
++	.urb_enqueue = isp1760_urb_enqueue,
++	.urb_dequeue = isp1760_urb_dequeue,
++	.hub_control = isp1760_hub_control,
++	.isr = isp1760_irq,
++};
++
++int isp1760_hcd_register(struct isp1760_hcd *priv, struct resource *mem,
++			 int irq, unsigned long irqflags,
++			 struct udevice *dev)
++{
++	const struct isp1760_memory_layout *mem_layout = priv->memory_layout;
++	struct isp1760_host_data *host = dev_get_priv(dev);
++	struct usb_hcd *hcd = &host->hcd;
++	int ret;
++
++	priv->hcd = hcd;
++
++	hcd->hcd_priv = priv;
++
++	priv->hcd = hcd;
++
++	hcd->urb_ops = &isp1760_urb_ops;
++
++	priv->atl_slots = kcalloc(mem_layout->slot_num,
++				  sizeof(struct isp1760_slotinfo), GFP_KERNEL);
++	if (!priv->atl_slots)
++		return -ENOMEM;
++
++	priv->int_slots = kcalloc(mem_layout->slot_num,
++				  sizeof(struct isp1760_slotinfo), GFP_KERNEL);
++	if (!priv->int_slots) {
++		ret = -ENOMEM;
++		goto free_atl_slots;
++	}
++
++	host->host_speed = USB_SPEED_HIGH;
++
++	init_memory(priv);
++
++	return 0;
++
++free_atl_slots:
++	kfree(priv->atl_slots);
++
++	return ret;
++}
++
++void isp1760_hcd_unregister(struct isp1760_hcd *priv)
++{
++	struct isp1760_qh *qh, *qh_next;
++	int i;
++
++	for (i = 0; i < QH_END; i++)
++		list_for_each_entry_safe(qh, qh_next, &priv->qh_list[i],
++					 qh_list) {
++			qtd_list_free(&qh->qtd_list);
++			list_del(&qh->qh_list);
++			qh_free(qh);
++		}
++
++	kfree(priv->atl_slots);
++	kfree(priv->int_slots);
++}
+diff --git a/drivers/usb/isp1760/isp1760-hcd.h b/drivers/usb/isp1760/isp1760-hcd.h
+new file mode 100644
+index 000000000000..00f5ca8c1f75
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-hcd.h
+@@ -0,0 +1,82 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++#ifndef _ISP1760_HCD_H_
++#define _ISP1760_HCD_H_
++
++#include <regmap.h>
++
++#include "isp1760-regs.h"
++
++struct isp1760_qh;
++struct isp1760_qtd;
++struct resource;
++struct usb_hcd;
++
++struct isp1760_slotinfo {
++	struct isp1760_qh *qh;
++	struct isp1760_qtd *qtd;
++	unsigned long timestamp;
++};
++
++/* chip memory management */
++#define ISP176x_BLOCK_MAX (32 + 20 + 4)
++#define ISP176x_BLOCK_NUM 3
++
++struct isp1760_memory_layout {
++	unsigned int blocks[ISP176x_BLOCK_NUM];
++	unsigned int blocks_size[ISP176x_BLOCK_NUM];
++
++	unsigned int slot_num;
++	unsigned int payload_blocks;
++	unsigned int payload_area_size;
++};
++
++struct isp1760_memory_chunk {
++	unsigned int start;
++	unsigned int size;
++	unsigned int free;
++};
++
++enum isp1760_queue_head_types {
++	QH_CONTROL,
++	QH_BULK,
++	QH_INTERRUPT,
++	QH_END
++};
++
++struct isp1760_hcd {
++	struct usb_hcd		*hcd;
++	struct udevice		*dev;
++
++	void __iomem		*base;
++
++	struct regmap		*regs;
++	struct regmap_field	*fields[HC_FIELD_MAX];
++
++	bool			is_isp1763;
++	const struct isp1760_memory_layout	*memory_layout;
++
++	spinlock_t		lock;
++	struct isp1760_slotinfo	*atl_slots;
++	int			atl_done_map;
++	struct isp1760_slotinfo	*int_slots;
++	int			int_done_map;
++	struct isp1760_memory_chunk memory_pool[ISP176x_BLOCK_MAX];
++	struct list_head	qh_list[QH_END];
++
++	/* periodic schedule support */
++#define	DEFAULT_I_TDPS		1024
++	unsigned		periodic_size;
++	unsigned		i_thresh;
++	unsigned long		reset_done;
++	unsigned long		next_statechange;
++};
++
++int isp1760_hcd_register(struct isp1760_hcd *priv, struct resource *mem,
++			 int irq, unsigned long irqflags, struct udevice *dev);
++void isp1760_hcd_unregister(struct isp1760_hcd *priv);
++int isp1760_hcd_lowlevel_init(struct isp1760_hcd *priv);
++
++int isp1760_init_kmem_once(void);
++void isp1760_deinit_kmem_cache(void);
++
++#endif /* _ISP1760_HCD_H_ */
+diff --git a/drivers/usb/isp1760/isp1760-if.c b/drivers/usb/isp1760/isp1760-if.c
+new file mode 100644
+index 000000000000..c610da6b23fb
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-if.c
+@@ -0,0 +1,127 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * based on original code from:
++ * (c) 2007 Sebastian Siewior <bigeasy@linutronix.de>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <dm/lists.h>
++#include <linux/bug.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/usb/otg.h>
++#include <log.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++
++static int isp1760_of_to_plat(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++	unsigned int devflags = 0;
++	u32 bus_width = 0;
++	ofnode dp;
++
++
++	if (!dev_has_ofnode(dev)) {
++		/* select isp1763 as the default device */
++		devflags = ISP1760_FLAG_ISP1763 | ISP1760_FLAG_BUS_WIDTH_16;
++		pr_err("isp1760: no platform data\n");
++		goto isp_setup;
++	}
++
++	dp = dev_ofnode(dev);
++
++	if (ofnode_device_is_compatible(dp, "nxp,usb-isp1761"))
++		devflags |= ISP1760_FLAG_ISP1761;
++
++	if (ofnode_device_is_compatible(dp, "nxp,usb-isp1763"))
++		devflags |= ISP1760_FLAG_ISP1763;
++
++	/*
++	* Some systems wire up only 8 of 16 data lines or
++	* 16 of the 32 data lines
++	*/
++	bus_width = ofnode_read_u32_default(dp, "bus-width", 16);
++	if (bus_width == 16)
++		devflags |= ISP1760_FLAG_BUS_WIDTH_16;
++	else if (bus_width == 8)
++		devflags |= ISP1760_FLAG_BUS_WIDTH_8;
++
++	if (usb_get_dr_mode(dev_ofnode(dev)) == USB_DR_MODE_PERIPHERAL)
++		devflags |= ISP1760_FLAG_PERIPHERAL_EN;
++
++	if (ofnode_read_bool(dp, "analog-oc"))
++		devflags |= ISP1760_FLAG_ANALOG_OC;
++
++	if (ofnode_read_bool(dp, "dack-polarity"))
++		devflags |= ISP1760_FLAG_DACK_POL_HIGH;
++
++	if (ofnode_read_bool(dp, "dreq-polarity"))
++		devflags |= ISP1760_FLAG_DREQ_POL_HIGH;
++
++isp_setup:
++	isp->devflags = devflags;
++	isp->dev = dev;
++
++	return 0;
++}
++
++static int isp1760_plat_probe(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++	struct resource mem_res;
++	struct resource irq_res;
++	int ret;
++
++	dev_read_resource(dev, 0, &mem_res);
++	dev_read_resource(dev, 1, &irq_res);
++
++	isp1760_init_kmem_once();
++
++	ret = isp1760_register(isp, &mem_res, irq_res.start, irq_res.flags);
++	if (ret < 0) {
++		isp1760_deinit_kmem_cache();
++		return ret;
++	}
++
++	return 0;
++}
++
++static int isp1760_plat_remove(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++
++	isp1760_deinit_kmem_cache();
++	isp1760_unregister(isp);
++
++	return 0;
++}
++
++static const struct udevice_id isp1760_ids[] = {
++	{ .compatible = "nxp,usb-isp1760", },
++	{ .compatible = "nxp,usb-isp1761", },
++	{ .compatible = "nxp,usb-isp1763", },
++	{ },
++};
++
++U_BOOT_DRIVER(isp1760) = {
++	.name		= "isp1760",
++	.id		= UCLASS_USB,
++	.of_match	= isp1760_ids,
++	.of_to_plat	= isp1760_of_to_plat,
++	.ops		= &isp1760_usb_ops,
++	.probe		= isp1760_plat_probe,
++	.remove		= isp1760_plat_remove,
++	.plat_auto	= sizeof(struct isp1760_device),
++	.priv_auto	= sizeof(struct isp1760_host_data),
++};
+diff --git a/drivers/usb/isp1760/isp1760-regs.h b/drivers/usb/isp1760/isp1760-regs.h
+new file mode 100644
+index 000000000000..94ea60c20b2a
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-regs.h
+@@ -0,0 +1,292 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ * Contacts:
++ *     Sebastian Siewior <bigeasy@linutronix.de>
++ *     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
++ *     Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#ifndef _ISP176x_REGS_H_
++#define _ISP176x_REGS_H_
++
++/* -----------------------------------------------------------------------------
++ * Host Controller
++ */
++
++/* ISP1760/31 */
++/* EHCI capability registers */
++#define ISP176x_HC_VERSION		0x002
++#define ISP176x_HC_HCSPARAMS		0x004
++#define ISP176x_HC_HCCPARAMS		0x008
++
++/* EHCI operational registers */
++#define ISP176x_HC_USBCMD		0x020
++#define ISP176x_HC_USBSTS		0x024
++#define ISP176x_HC_FRINDEX		0x02c
++
++#define ISP176x_HC_CONFIGFLAG		0x060
++#define ISP176x_HC_PORTSC1		0x064
++
++#define ISP176x_HC_ISO_PTD_DONEMAP	0x130
++#define ISP176x_HC_ISO_PTD_SKIPMAP	0x134
++#define ISP176x_HC_ISO_PTD_LASTPTD	0x138
++#define ISP176x_HC_INT_PTD_DONEMAP	0x140
++#define ISP176x_HC_INT_PTD_SKIPMAP	0x144
++#define ISP176x_HC_INT_PTD_LASTPTD	0x148
++#define ISP176x_HC_ATL_PTD_DONEMAP	0x150
++#define ISP176x_HC_ATL_PTD_SKIPMAP	0x154
++#define ISP176x_HC_ATL_PTD_LASTPTD	0x158
++
++/* Configuration Register */
++#define ISP176x_HC_HW_MODE_CTRL		0x300
++#define ISP176x_HC_CHIP_ID		0x304
++#define ISP176x_HC_SCRATCH		0x308
++#define ISP176x_HC_RESET		0x30c
++#define ISP176x_HC_BUFFER_STATUS	0x334
++#define ISP176x_HC_MEMORY		0x33c
++
++/* Interrupt Register */
++#define ISP176x_HC_INTERRUPT		0x310
++#define ISP176x_HC_INTERRUPT_ENABLE	0x314
++#define ISP176x_HC_ISO_IRQ_MASK_OR	0x318
++#define ISP176x_HC_INT_IRQ_MASK_OR	0x31c
++#define ISP176x_HC_ATL_IRQ_MASK_OR	0x320
++#define ISP176x_HC_ISO_IRQ_MASK_AND	0x324
++#define ISP176x_HC_INT_IRQ_MASK_AND	0x328
++#define ISP176x_HC_ATL_IRQ_MASK_AND	0x32c
++
++#define ISP176x_HC_OTG_CTRL_SET		0x374
++#define ISP176x_HC_OTG_CTRL_CLEAR	0x376
++
++enum isp176x_host_controller_fields {
++	/* HC_PORTSC1 */
++	PORT_OWNER, PORT_POWER, PORT_LSTATUS, PORT_RESET, PORT_SUSPEND,
++	PORT_RESUME, PORT_PE, PORT_CSC, PORT_CONNECT,
++	/* HC_HCSPARAMS */
++	HCS_PPC, HCS_N_PORTS,
++	/* HC_HCCPARAMS */
++	HCC_ISOC_CACHE, HCC_ISOC_THRES,
++	/* HC_USBCMD */
++	CMD_LRESET, CMD_RESET, CMD_RUN,
++	/* HC_USBSTS */
++	STS_PCD,
++	/* HC_FRINDEX */
++	HC_FRINDEX,
++	/* HC_CONFIGFLAG */
++	FLAG_CF,
++	/* ISO/INT/ATL PTD */
++	HC_ISO_PTD_DONEMAP, HC_ISO_PTD_SKIPMAP, HC_ISO_PTD_LASTPTD,
++	HC_INT_PTD_DONEMAP, HC_INT_PTD_SKIPMAP, HC_INT_PTD_LASTPTD,
++	HC_ATL_PTD_DONEMAP, HC_ATL_PTD_SKIPMAP, HC_ATL_PTD_LASTPTD,
++	/* HC_HW_MODE_CTRL */
++	ALL_ATX_RESET, HW_ANA_DIGI_OC, HW_DEV_DMA, HW_COMN_IRQ, HW_COMN_DMA,
++	HW_DATA_BUS_WIDTH, HW_DACK_POL_HIGH, HW_DREQ_POL_HIGH, HW_INTR_HIGH_ACT,
++	HW_INTF_LOCK, HW_INTR_EDGE_TRIG, HW_GLOBAL_INTR_EN,
++	/* HC_CHIP_ID */
++	HC_CHIP_ID_HIGH, HC_CHIP_ID_LOW, HC_CHIP_REV,
++	/* HC_SCRATCH */
++	HC_SCRATCH,
++	/* HC_RESET */
++	SW_RESET_RESET_ATX, SW_RESET_RESET_HC, SW_RESET_RESET_ALL,
++	/* HC_BUFFER_STATUS */
++	ISO_BUF_FILL, INT_BUF_FILL, ATL_BUF_FILL,
++	/* HC_MEMORY */
++	MEM_BANK_SEL, MEM_START_ADDR,
++	/* HC_DATA */
++	HC_DATA,
++	/* HC_INTERRUPT */
++	HC_INTERRUPT,
++	/* HC_INTERRUPT_ENABLE */
++	HC_INT_IRQ_ENABLE, HC_ATL_IRQ_ENABLE,
++	/* INTERRUPT MASKS */
++	HC_ISO_IRQ_MASK_OR, HC_INT_IRQ_MASK_OR, HC_ATL_IRQ_MASK_OR,
++	HC_ISO_IRQ_MASK_AND, HC_INT_IRQ_MASK_AND, HC_ATL_IRQ_MASK_AND,
++	/* HW_OTG_CTRL_SET */
++	HW_OTG_DISABLE, HW_SW_SEL_HC_DC, HW_VBUS_DRV, HW_SEL_CP_EXT,
++	HW_DM_PULLDOWN, HW_DP_PULLDOWN, HW_DP_PULLUP, HW_HC_2_DIS,
++	/* HW_OTG_CTRL_CLR */
++	HW_OTG_DISABLE_CLEAR, HW_SW_SEL_HC_DC_CLEAR, HW_VBUS_DRV_CLEAR,
++	HW_SEL_CP_EXT_CLEAR, HW_DM_PULLDOWN_CLEAR, HW_DP_PULLDOWN_CLEAR,
++	HW_DP_PULLUP_CLEAR, HW_HC_2_DIS_CLEAR,
++	/* Last element */
++	HC_FIELD_MAX,
++};
++
++/* ISP1763 */
++/* EHCI operational registers */
++#define ISP1763_HC_USBCMD		0x8c
++#define ISP1763_HC_USBSTS		0x90
++#define ISP1763_HC_FRINDEX		0x98
++
++#define ISP1763_HC_CONFIGFLAG		0x9c
++#define ISP1763_HC_PORTSC1		0xa0
++
++#define ISP1763_HC_ISO_PTD_DONEMAP	0xa4
++#define ISP1763_HC_ISO_PTD_SKIPMAP	0xa6
++#define ISP1763_HC_ISO_PTD_LASTPTD	0xa8
++#define ISP1763_HC_INT_PTD_DONEMAP	0xaa
++#define ISP1763_HC_INT_PTD_SKIPMAP	0xac
++#define ISP1763_HC_INT_PTD_LASTPTD	0xae
++#define ISP1763_HC_ATL_PTD_DONEMAP	0xb0
++#define ISP1763_HC_ATL_PTD_SKIPMAP	0xb2
++#define ISP1763_HC_ATL_PTD_LASTPTD	0xb4
++
++/* Configuration Register */
++#define ISP1763_HC_HW_MODE_CTRL		0xb6
++#define ISP1763_HC_CHIP_REV		0x70
++#define ISP1763_HC_CHIP_ID		0x72
++#define ISP1763_HC_SCRATCH		0x78
++#define ISP1763_HC_RESET		0xb8
++#define ISP1763_HC_BUFFER_STATUS	0xba
++#define ISP1763_HC_MEMORY		0xc4
++#define ISP1763_HC_DATA			0xc6
++
++/* Interrupt Register */
++#define ISP1763_HC_INTERRUPT		0xd4
++#define ISP1763_HC_INTERRUPT_ENABLE	0xd6
++#define ISP1763_HC_ISO_IRQ_MASK_OR	0xd8
++#define ISP1763_HC_INT_IRQ_MASK_OR	0xda
++#define ISP1763_HC_ATL_IRQ_MASK_OR	0xdc
++#define ISP1763_HC_ISO_IRQ_MASK_AND	0xde
++#define ISP1763_HC_INT_IRQ_MASK_AND	0xe0
++#define ISP1763_HC_ATL_IRQ_MASK_AND	0xe2
++
++#define ISP1763_HC_OTG_CTRL_SET		0xe4
++#define ISP1763_HC_OTG_CTRL_CLEAR	0xe6
++
++/* -----------------------------------------------------------------------------
++ * Peripheral Controller
++ */
++
++#define DC_IEPTX(n)			(1 << (11 + 2 * (n)))
++#define DC_IEPRX(n)			(1 << (10 + 2 * (n)))
++#define DC_IEPRXTX(n)			(3 << (10 + 2 * (n)))
++
++#define ISP176x_DC_CDBGMOD_ACK		BIT(6)
++#define ISP176x_DC_DDBGMODIN_ACK	BIT(4)
++#define ISP176x_DC_DDBGMODOUT_ACK	BIT(2)
++
++#define ISP176x_DC_IEP0SETUP		BIT(8)
++#define ISP176x_DC_IEVBUS		BIT(7)
++#define ISP176x_DC_IEHS_STA		BIT(5)
++#define ISP176x_DC_IERESM		BIT(4)
++#define ISP176x_DC_IESUSP		BIT(3)
++#define ISP176x_DC_IEBRST		BIT(0)
++
++#define ISP176x_DC_ENDPTYP_ISOC		0x01
++#define ISP176x_DC_ENDPTYP_BULK		0x02
++#define ISP176x_DC_ENDPTYP_INTERRUPT	0x03
++
++/* Initialization Registers */
++#define ISP176x_DC_ADDRESS		0x0200
++#define ISP176x_DC_MODE			0x020c
++#define ISP176x_DC_INTCONF		0x0210
++#define ISP176x_DC_DEBUG		0x0212
++#define ISP176x_DC_INTENABLE		0x0214
++
++/* Data Flow Registers */
++#define ISP176x_DC_EPMAXPKTSZ		0x0204
++#define ISP176x_DC_EPTYPE		0x0208
++
++#define ISP176x_DC_BUFLEN		0x021c
++#define ISP176x_DC_BUFSTAT		0x021e
++#define ISP176x_DC_DATAPORT		0x0220
++
++#define ISP176x_DC_CTRLFUNC		0x0228
++#define ISP176x_DC_EPINDEX		0x022c
++
++/* DMA Registers */
++#define ISP176x_DC_DMACMD		0x0230
++#define ISP176x_DC_DMATXCOUNT		0x0234
++#define ISP176x_DC_DMACONF		0x0238
++#define ISP176x_DC_DMAHW		0x023c
++#define ISP176x_DC_DMAINTREASON		0x0250
++#define ISP176x_DC_DMAINTEN		0x0254
++#define ISP176x_DC_DMAEP		0x0258
++#define ISP176x_DC_DMABURSTCOUNT	0x0264
++
++/* General Registers */
++#define ISP176x_DC_INTERRUPT		0x0218
++#define ISP176x_DC_CHIPID		0x0270
++#define ISP176x_DC_FRAMENUM		0x0274
++#define ISP176x_DC_SCRATCH		0x0278
++#define ISP176x_DC_UNLOCKDEV		0x027c
++#define ISP176x_DC_INTPULSEWIDTH	0x0280
++#define ISP176x_DC_TESTMODE		0x0284
++
++enum isp176x_device_controller_fields {
++	/* DC_ADDRESS */
++	DC_DEVEN, DC_DEVADDR,
++	/* DC_MODE */
++	DC_VBUSSTAT, DC_SFRESET, DC_GLINTENA,
++	/* DC_INTCONF */
++	DC_CDBGMOD_ACK, DC_DDBGMODIN_ACK, DC_DDBGMODOUT_ACK, DC_INTPOL,
++	/* DC_INTENABLE */
++	DC_IEPRXTX_7, DC_IEPRXTX_6, DC_IEPRXTX_5, DC_IEPRXTX_4, DC_IEPRXTX_3,
++	DC_IEPRXTX_2, DC_IEPRXTX_1, DC_IEPRXTX_0,
++	DC_IEP0SETUP, DC_IEVBUS, DC_IEHS_STA, DC_IERESM, DC_IESUSP, DC_IEBRST,
++	/* DC_EPINDEX */
++	DC_EP0SETUP, DC_ENDPIDX, DC_EPDIR,
++	/* DC_CTRLFUNC */
++	DC_CLBUF, DC_VENDP, DC_DSEN, DC_STATUS, DC_STALL,
++	/* DC_BUFLEN */
++	DC_BUFLEN,
++	/* DC_EPMAXPKTSZ */
++	DC_FFOSZ,
++	/* DC_EPTYPE */
++	DC_EPENABLE, DC_ENDPTYP,
++	/* DC_FRAMENUM */
++	DC_FRAMENUM, DC_UFRAMENUM,
++	/* DC_CHIP_ID */
++	DC_CHIP_ID_HIGH, DC_CHIP_ID_LOW,
++	/* DC_SCRATCH */
++	DC_SCRATCH,
++	/* Last element */
++	DC_FIELD_MAX,
++};
++
++/* ISP1763 */
++/* Initialization Registers */
++#define ISP1763_DC_ADDRESS		0x00
++#define ISP1763_DC_MODE			0x0c
++#define ISP1763_DC_INTCONF		0x10
++#define ISP1763_DC_INTENABLE		0x14
++
++/* Data Flow Registers */
++#define ISP1763_DC_EPMAXPKTSZ		0x04
++#define ISP1763_DC_EPTYPE		0x08
++
++#define ISP1763_DC_BUFLEN		0x1c
++#define ISP1763_DC_BUFSTAT		0x1e
++#define ISP1763_DC_DATAPORT		0x20
++
++#define ISP1763_DC_CTRLFUNC		0x28
++#define ISP1763_DC_EPINDEX		0x2c
++
++/* DMA Registers */
++#define ISP1763_DC_DMACMD		0x30
++#define ISP1763_DC_DMATXCOUNT		0x34
++#define ISP1763_DC_DMACONF		0x38
++#define ISP1763_DC_DMAHW		0x3c
++#define ISP1763_DC_DMAINTREASON		0x50
++#define ISP1763_DC_DMAINTEN		0x54
++#define ISP1763_DC_DMAEP		0x58
++#define ISP1763_DC_DMABURSTCOUNT	0x64
++
++/* General Registers */
++#define ISP1763_DC_INTERRUPT		0x18
++#define ISP1763_DC_CHIPID_LOW		0x70
++#define ISP1763_DC_CHIPID_HIGH		0x72
++#define ISP1763_DC_FRAMENUM		0x74
++#define ISP1763_DC_SCRATCH		0x78
++#define ISP1763_DC_UNLOCKDEV		0x7c
++#define ISP1763_DC_INTPULSEWIDTH	0x80
++#define ISP1763_DC_TESTMODE		0x84
++
++#endif
+diff --git a/drivers/usb/isp1760/isp1760-uboot.c b/drivers/usb/isp1760/isp1760-uboot.c
+new file mode 100644
+index 000000000000..7635210fe2b4
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-uboot.c
+@@ -0,0 +1,76 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <dm/lists.h>
++#include <linux/bug.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/usb/otg.h>
++#include <linux/usb/usb_urb_compat.h>
++#include <log.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++static int isp1760_msg_submit_control(struct udevice *dev,
++				      struct usb_device *udev,
++				      unsigned long pipe, void *buffer,
++				      int length, struct devrequest *setup)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_control(&host->hcd, &host->urb, &host->hep, udev,
++				      pipe, buffer, length, setup, 0,
++				      host->host_speed);
++}
++
++static int isp1760_msg_submit_bulk(struct udevice *dev, struct usb_device *udev,
++				   unsigned long pipe, void *buffer, int length)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_bulk(&host->hcd, &host->urb, &host->hep, udev,
++				   pipe, buffer, length);
++}
++
++static int isp1760_msg_submit_irq(struct udevice *dev, struct usb_device *udev,
++				  unsigned long pipe, void *buffer, int length,
++				  int interval, bool nonblock)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_irq(&host->hcd, &host->urb, &host->hep, udev,
++				  pipe, buffer, length, interval);
++}
++
++static int isp1760_get_max_xfer_size(struct udevice *dev, size_t *size)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++	struct isp1760_hcd *priv = host->hcd.hcd_priv;
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++
++	*size = mem->blocks_size[ISP176x_BLOCK_NUM - 1];
++
++	return 0;
++}
++
++
++struct dm_usb_ops isp1760_usb_ops = {
++	.control		= isp1760_msg_submit_control,
++	.bulk			= isp1760_msg_submit_bulk,
++	.interrupt		= isp1760_msg_submit_irq,
++	.get_max_xfer_size	= isp1760_get_max_xfer_size,
++};
+diff --git a/drivers/usb/isp1760/isp1760-uboot.h b/drivers/usb/isp1760/isp1760-uboot.h
+new file mode 100644
+index 000000000000..2486de6f9e27
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-uboot.h
+@@ -0,0 +1,27 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#ifndef __ISP1760_UBOOT_H__
++#define __ISP1760_UBOOT_H__
++
++#include <linux/usb/usb_urb_compat.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++
++struct isp1760_host_data {
++	struct isp1760_hcd *priv;
++	struct usb_hcd hcd;
++	enum usb_device_speed host_speed;
++	struct usb_host_endpoint hep;
++	struct urb urb;
++};
++
++extern struct dm_usb_ops isp1760_usb_ops;
++
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch
new file mode 100644
index 0000000..2ce03b6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch
@@ -0,0 +1,48 @@
+From b7fb62e512e00a5fdbb4321bb0b4109261518481 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Thu, 3 Mar 2022 16:52:02 +0000
+Subject: [PATCH 05/27] corstone1000: enable isp1763 usb controller
+
+MPS3 board have a ISP1763 usb controller, add the
+correspondent mmio area and enable it to be used for mass
+storage access for example.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ configs/corstone1000_defconfig | 1 +
+ include/configs/corstone1000.h | 6 ++++++
+ 2 files changed, 7 insertions(+)
+
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index 02f931b0d469..e573fe6fe6a2 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -42,6 +42,7 @@ CONFIG_REGMAP=y
+ CONFIG_DM_SERIAL=y
+ CONFIG_USB=y
+ CONFIG_DM_USB=y
++CONFIG_USB_ISP1760=y
+ CONFIG_USB_STORAGE=y
+ CONFIG_EFI_MM_COMM_TEE=y
+ # CONFIG_OPTEE is not set
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index cf166f107efd..8ba0effb0ab2 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -55,7 +55,13 @@
+ #define CONFIG_SYS_CBSIZE	512	/* Console I/O Buffer Size */
+ #define CONFIG_SYS_MAXARGS	64	/* max command args */
+ 
++#define BOOT_TARGET_DEVICES(func) \
++	func(USB, usb, 0)
++
++#include <config_distro_bootcmd.h>
++
+ #define CONFIG_EXTRA_ENV_SETTINGS							\
++				BOOTENV							\
+ 				"usb_pgood_delay=250\0"					\
+ 				"boot_bank_flag=0x08002000\0"				\
+ 				"kernel_addr_bank_0=0x083EE000\0"			\
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch
new file mode 100644
index 0000000..8cc848f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch
@@ -0,0 +1,2610 @@
+From ede21dc1ca75132698cc81e88357c5789ccf67c7 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:34:52 +0000
+Subject: [PATCH 06/27] arm_ffa: introducing Arm FF-A low-level driver
+
+This driver implements Arm Firmware Framework for Armv8-A on u-boot
+
+The Firmware Framework for Arm A-profile processors (FF-A)
+describes interfaces (ABIs) that standardize communication
+between the Secure World and Normal World leveraging TrustZone
+technology.
+
+This driver is based on FF-A specification v1.0 and uses SMC32
+calling convention.
+
+FF-A specification:
+
+https://developer.arm.com/documentation/den0077/a/?lang=en
+
+The driver provides helper FF-A interfaces for user layers.
+These helper functions allow clients to pass data and select the
+FF-A function to use for the communication with secure world.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS                      |    8 +
+ arch/arm/cpu/armv8/smccc-call.S  |   27 +
+ arch/arm/lib/asm-offsets.c       |    6 +
+ common/board_r.c                 |    6 +
+ drivers/Kconfig                  |    2 +
+ drivers/Makefile                 |    1 +
+ drivers/arm-ffa/Kconfig          |   26 +
+ drivers/arm-ffa/Makefile         |    3 +
+ drivers/arm-ffa/arm-ffa-uclass.c |   67 ++
+ drivers/arm-ffa/arm_ffa_prv.h    |  199 ++++
+ drivers/arm-ffa/core.c           | 1484 ++++++++++++++++++++++++++++++
+ include/arm_ffa.h                |  191 ++++
+ include/arm_ffa_helper.h         |   45 +
+ include/dm/uclass-id.h           |    1 +
+ include/linux/arm-smccc.h        |   28 +-
+ lib/Kconfig                      |    1 +
+ lib/Makefile                     |    1 +
+ lib/arm-ffa/Kconfig              |    6 +
+ lib/arm-ffa/Makefile             |    8 +
+ lib/arm-ffa/arm_ffa_helper.c     |  188 ++++
+ lib/efi_loader/efi_boottime.c    |   17 +
+ 21 files changed, 2314 insertions(+), 1 deletion(-)
+ create mode 100644 drivers/arm-ffa/Kconfig
+ create mode 100644 drivers/arm-ffa/Makefile
+ create mode 100644 drivers/arm-ffa/arm-ffa-uclass.c
+ create mode 100644 drivers/arm-ffa/arm_ffa_prv.h
+ create mode 100644 drivers/arm-ffa/core.c
+ create mode 100644 include/arm_ffa.h
+ create mode 100644 include/arm_ffa_helper.h
+ create mode 100644 lib/arm-ffa/Kconfig
+ create mode 100644 lib/arm-ffa/Makefile
+ create mode 100644 lib/arm-ffa/arm_ffa_helper.c
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 96582fc67777..14307e6da644 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -232,6 +232,14 @@ F:	board/CZ.NIC/
+ F:	configs/turris_*_defconfig
+ F:	include/configs/turris_*.h
+ 
++ARM FF-A
++M:	Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++S:	Maintained
++F:	drivers/arm-ffa/
++F:	include/arm_ffa.h
++F:	include/arm_ffa_helper.h
++F:	lib/arm-ffa/
++
+ ARM FREESCALE IMX
+ M:	Stefano Babic <sbabic@denx.de>
+ M:	Fabio Estevam <festevam@gmail.com>
+diff --git a/arch/arm/cpu/armv8/smccc-call.S b/arch/arm/cpu/armv8/smccc-call.S
+index dc92b28777c3..ffc39c9fefa2 100644
+--- a/arch/arm/cpu/armv8/smccc-call.S
++++ b/arch/arm/cpu/armv8/smccc-call.S
+@@ -1,6 +1,8 @@
+ /* SPDX-License-Identifier: GPL-2.0 */
+ /*
+  * Copyright (c) 2015, Linaro Limited
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+  */
+ #include <linux/linkage.h>
+ #include <linux/arm-smccc.h>
+@@ -45,3 +47,28 @@ ENDPROC(__arm_smccc_smc)
+ ENTRY(__arm_smccc_hvc)
+ 	SMCCC	hvc
+ ENDPROC(__arm_smccc_hvc)
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++	.macro FFASMCCC instr
++	.cfi_startproc
++	\instr	#0
++	ldr	x9, [sp]
++	stp	x0, x1, [x9, #ARM_SMCCC_RES_X0_OFFS]
++	stp	x2, x3, [x9, #ARM_SMCCC_RES_X2_OFFS]
++	stp	x4, x5, [x9, #ARM_SMCCC_RES_X4_OFFS]
++	stp	x6, x7, [x9, #ARM_SMCCC_RES_X6_OFFS]
++	ret
++	.cfi_endproc
++	.endm
++
++/*
++ * void arm_ffa_smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
++ *		  unsigned long a3, unsigned long a4, unsigned long a5,
++ *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res)
++ */
++ENTRY(__arm_ffa_smccc_smc)
++	FFASMCCC	smc
++ENDPROC(__arm_ffa_smccc_smc)
++
++#endif
+diff --git a/arch/arm/lib/asm-offsets.c b/arch/arm/lib/asm-offsets.c
+index 22fd541f9a28..45eca83a473c 100644
+--- a/arch/arm/lib/asm-offsets.c
++++ b/arch/arm/lib/asm-offsets.c
+@@ -9,6 +9,8 @@
+  * generate asm statements containing #defines,
+  * compile this file to assembler, and then extract the
+  * #defines from the assembly-language output.
++ *
++ * (C) Copyright 2021 ARM Limited
+  */
+ 
+ #include <common.h>
+@@ -115,6 +117,10 @@ int main(void)
+ #ifdef CONFIG_ARM_SMCCC
+ 	DEFINE(ARM_SMCCC_RES_X0_OFFS, offsetof(struct arm_smccc_res, a0));
+ 	DEFINE(ARM_SMCCC_RES_X2_OFFS, offsetof(struct arm_smccc_res, a2));
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++	DEFINE(ARM_SMCCC_RES_X4_OFFS, offsetof(struct arm_smccc_res, a4));
++	DEFINE(ARM_SMCCC_RES_X6_OFFS, offsetof(struct arm_smccc_res, a6));
++#endif
+ 	DEFINE(ARM_SMCCC_QUIRK_ID_OFFS, offsetof(struct arm_smccc_quirk, id));
+ 	DEFINE(ARM_SMCCC_QUIRK_STATE_OFFS, offsetof(struct arm_smccc_quirk, state));
+ #endif
+diff --git a/common/board_r.c b/common/board_r.c
+index c24d9b4e220b..af20f38b104c 100644
+--- a/common/board_r.c
++++ b/common/board_r.c
+@@ -61,6 +61,9 @@
+ #include <wdt.h>
+ #include <asm-generic/gpio.h>
+ #include <efi_loader.h>
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++#include <arm_ffa_helper.h>
++#endif
+ 
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+@@ -770,6 +773,9 @@ static init_fnc_t init_sequence_r[] = {
+ 	INIT_FUNC_WATCHDOG_RESET
+ 	initr_net,
+ #endif
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++	ffa_helper_init_device,
++#endif
+ #ifdef CONFIG_POST
+ 	initr_post,
+ #endif
+diff --git a/drivers/Kconfig b/drivers/Kconfig
+index b26ca8cf70c9..e83c23789d1b 100644
+--- a/drivers/Kconfig
++++ b/drivers/Kconfig
+@@ -6,6 +6,8 @@ source "drivers/core/Kconfig"
+ 
+ source "drivers/adc/Kconfig"
+ 
++source "drivers/arm-ffa/Kconfig"
++
+ source "drivers/ata/Kconfig"
+ 
+ source "drivers/axi/Kconfig"
+diff --git a/drivers/Makefile b/drivers/Makefile
+index 4e7cf284405a..6671d2a604ab 100644
+--- a/drivers/Makefile
++++ b/drivers/Makefile
+@@ -107,6 +107,7 @@ obj-y += iommu/
+ obj-y += smem/
+ obj-y += thermal/
+ obj-$(CONFIG_TEE) += tee/
++obj-$(CONFIG_ARM_FFA_TRANSPORT) += arm-ffa/
+ obj-y += axi/
+ obj-y += ufs/
+ obj-$(CONFIG_W1) += w1/
+diff --git a/drivers/arm-ffa/Kconfig b/drivers/arm-ffa/Kconfig
+new file mode 100644
+index 000000000000..d71444c1fa90
+--- /dev/null
++++ b/drivers/arm-ffa/Kconfig
+@@ -0,0 +1,26 @@
++# SPDX-License-Identifier: GPL-2.0
++
++config ARM_FFA_TRANSPORT
++	bool "Enable Arm Firmware Framework for Armv8-A driver"
++	depends on DM && ARM64
++	select ARM_SMCCC
++	select LIB_UUID
++	select ARM_FFA_TRANSPORT_HELPERS
++	select CMD_ARMFFA
++	help
++	  The Firmware Framework for Arm A-profile processors (FF-A)
++	  describes interfaces (ABIs) that standardize communication
++	  between the Secure World and Normal World leveraging TrustZone
++	  technology.
++
++	  This driver is based on FF-A specification v1.0 and uses SMC32
++	  calling convention.
++
++	  FF-A specification:
++
++	  https://developer.arm.com/documentation/den0077/a/?lang=en
++
++	  In u-boot FF-A design, the Secure World is considered as one
++	  entity to communicate with. FF-A communication is handled by
++	  one device and one instance. This device takes care of
++	  all the interactions between Normal world and Secure World.
+diff --git a/drivers/arm-ffa/Makefile b/drivers/arm-ffa/Makefile
+new file mode 100644
+index 000000000000..9fb5bea52299
+--- /dev/null
++++ b/drivers/arm-ffa/Makefile
+@@ -0,0 +1,3 @@
++# SPDX-License-Identifier: GPL-2.0+
++
++obj-y += arm-ffa-uclass.o core.o
+diff --git a/drivers/arm-ffa/arm-ffa-uclass.c b/drivers/arm-ffa/arm-ffa-uclass.c
+new file mode 100644
+index 000000000000..43f6066281fe
+--- /dev/null
++++ b/drivers/arm-ffa/arm-ffa-uclass.c
+@@ -0,0 +1,67 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <arm_ffa.h>
++#include <errno.h>
++#include <log.h>
++#include <asm/global_data.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++UCLASS_DRIVER(ffa) = {
++	.name		= "ffa",
++	.id		= UCLASS_FFA,
++};
++
++/**
++ * ffa_get_invoke_func - performs a call to the FF-A driver dispatcher
++ * @func_id:	The FF-A function to be used
++ * @func_data:  Pointer to the FF-A function arguments
++ *				container structure. This also includes
++ *				pointers to the returned data needed by
++ *				clients.
++ *
++ * This runtime function passes the FF-A function ID and its arguments to
++ * the FF-A driver dispatcher.
++ * This function is called by the FF-A helper functions.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime ffa_get_invoke_func(u32 func_id, struct ffa_interface_data *func_data)
++{
++	if (!ffa_device_get_ops()->invoke_func)
++		return -EINVAL;
++
++	return ffa_device_get_ops()->invoke_func(func_id, func_data);
++}
++
++/**
++ * ffa_init_device - probes the arm_ffa device
++ *
++ * This boot time function makes sure the arm_ffa device is probed
++ * and ready for use.
++ * This function is called automatically at initcalls
++ * level (after u-boot relocation).
++ *
++ * Arm FF-A transport is implemented through a single u-boot
++ * device (arm_ffa). So, there is only one device belonging to UCLASS_FFA.
++ * All FF-A clients should use the arm_ffa device to use the FF-A
++ * transport.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_init_device(void)
++{
++	ffa_dbg("[%s]", __func__);
++
++	return ffa_get_device();
++}
+diff --git a/drivers/arm-ffa/arm_ffa_prv.h b/drivers/arm-ffa/arm_ffa_prv.h
+new file mode 100644
+index 000000000000..38ea4ba83efc
+--- /dev/null
++++ b/drivers/arm-ffa/arm_ffa_prv.h
+@@ -0,0 +1,199 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_PRV_H
++#define __ARM_FFA_PRV_H
++
++#include <arm_ffa.h>
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++
++/*
++ * This header is private. It is exclusively used by the FF-A driver
++ */
++
++/* FF-A driver version definitions */
++
++#define MAJOR_VERSION_MASK		GENMASK(30, 16)
++#define MINOR_VERSION_MASK		GENMASK(15, 0)
++#define GET_FFA_MAJOR_VERSION(x)		\
++				((u16)(FIELD_GET(MAJOR_VERSION_MASK, (x))))
++#define GET_FFA_MINOR_VERSION(x)		\
++				((u16)(FIELD_GET(MINOR_VERSION_MASK, (x))))
++#define PACK_VERSION_INFO(major, minor)			\
++	(FIELD_PREP(MAJOR_VERSION_MASK, (major)) |	\
++	 FIELD_PREP(MINOR_VERSION_MASK, (minor)))
++
++#define FFA_MAJOR_VERSION		(1)
++#define FFA_MINOR_VERSION		(0)
++#define FFA_VERSION_1_0		\
++			PACK_VERSION_INFO(FFA_MAJOR_VERSION, FFA_MINOR_VERSION)
++
++/* Endpoint ID mask (u-boot endpoint ID) */
++
++#define GET_SELF_ENDPOINT_ID_MASK		GENMASK(15, 0)
++#define GET_SELF_ENDPOINT_ID(x)		\
++			((u16)(FIELD_GET(GET_SELF_ENDPOINT_ID_MASK, (x))))
++
++#define PREP_SELF_ENDPOINT_ID_MASK		GENMASK(31, 16)
++#define PREP_SELF_ENDPOINT_ID(x)		\
++			((u16)(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x))))
++
++/* Partition endpoint ID mask  (partition with which u-boot communicates with) */
++
++#define PREP_PART_ENDPOINT_ID_MASK		GENMASK(15, 0)
++#define PREP_PART_ENDPOINT_ID(x)		\
++			((u16)(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x))))
++
++/* The FF-A SMC function prototype definition */
++
++typedef void (*invoke_ffa_fn_t)(unsigned long a0, unsigned long a1,
++			unsigned long a2, unsigned long a3, unsigned long a4,
++			unsigned long a5, unsigned long a6, unsigned long a7,
++			struct arm_smccc_res *res);
++
++/**
++ * enum ffa_conduit - Arm FF-A conduits supported by the Arm FF-A driver
++ * Currently only SMC32 is supported.
++ */
++enum ffa_conduit {
++	FFA_CONDUIT_SMC = 0,
++};
++
++/**
++ * FFA_DECLARE_ARGS - FF-A functions local variables
++ * @a0-a7:	local variables used to set registers x0-x7
++ * @res:	the structure hosting the FF-A function return data
++ *
++ * A helper macro for declaring local variables for the FF-A functions  arguments.
++ * The x0-x7 registers are used to exchange data with the secure world.
++ * But, only the bottom 32-bit of thes registers contains the data.
++ */
++#define FFA_DECLARE_ARGS \
++	unsigned long a0 = 0; \
++	unsigned long a1 = 0; \
++	unsigned long a2 = 0; \
++	unsigned long a3 = 0; \
++	unsigned long a4 = 0; \
++	unsigned long a5 = 0; \
++	unsigned long a6 = 0; \
++	unsigned long a7 = 0; \
++	struct arm_smccc_res res = {0}
++
++/* FF-A error codes */
++#define FFA_ERR_STAT_NOT_SUPPORTED				(-1)
++#define FFA_ERR_STAT_INVALID_PARAMETERS				(-2)
++#define FFA_ERR_STAT_NO_MEMORY				(-3)
++#define FFA_ERR_STAT_BUSY				(-4)
++#define FFA_ERR_STAT_INTERRUPTED				(-5)
++#define FFA_ERR_STAT_DENIED				(-6)
++#define FFA_ERR_STAT_RETRY				(-7)
++#define FFA_ERR_STAT_ABORTED				(-8)
++
++/**
++ * struct ffa_features_desc - FF-A functions features
++ * @func_id:	FF-A function
++ * @field1:	features read from register w2
++ * @field2:	features read from register w3
++ *
++ * Data structure describing the features of the  FF-A functions queried by
++ * FFA_FEATURES
++ */
++struct ffa_features_desc {
++	u32 func_id;
++	u32 field1;
++	u32 field2;
++};
++
++/**
++ * enum ffa_rxtx_buf_sizes - minimum sizes supported
++ * for the RX/TX buffers
++ */
++enum ffa_rxtx_buf_sizes {
++	RXTX_4K,
++	RXTX_64K,
++	RXTX_16K
++};
++
++/*
++ * Number of the FF-A interfaces features descriptors
++ * currently only FFA_RXTX_MAP descriptor is supported
++ */
++#define FFA_FEATURE_DESC_CNT (1)
++
++/**
++ * struct ffa_pdata - platform data for the arm_ffa device
++ * @conduit: The FF-A conduit used
++ *
++ * Platform data structure read from the device tree
++ */
++struct ffa_pdata {
++	enum ffa_conduit conduit;
++};
++
++/**
++ * struct ffa_rxtxpair - structure hosting the RX/TX buffers physical addresses
++ * @rxbuf:	physical address of the RX buffer
++ * @txbuf:	physical address of the TX buffer
++ *
++ * Data structure hosting the physical addresses of the mapped RX/TX buffers
++ * These physical address are used by the FF-A functions that use the RX/TX buffers
++ */
++struct ffa_rxtxpair {
++	u64 rxbuf; /* physical address */
++	u64 txbuf; /* physical address */
++};
++
++/**
++ * struct ffa_partition_desc - the secure partition descriptor
++ * @info:	partition information
++ * @UUID:	UUID
++ *
++ * Each partition has its descriptor containing the partitions information and the UUID
++ */
++struct ffa_partition_desc {
++	struct ffa_partition_info info;
++	union ffa_partition_uuid UUID;
++};
++
++/**
++ * struct ffa_partitions - descriptors for all secure partitions
++ * @count:	The number of partitions descriptors
++ * @descs	The partitions descriptors table
++ *
++ * This data structure contains the partitions descriptors table
++ */
++struct ffa_partitions {
++	u32 count;
++	struct ffa_partition_desc *descs; /* virtual address */
++};
++
++/**
++ * struct ffa_prvdata - the driver private data structure
++ *
++ * @dev:	The arm_ffa device under u-boot driver model
++ * @fwk_version:	FF-A framework version
++ * @id:	u-boot endpoint ID
++ * @partitions:	The partitions descriptors structure
++ * @pair:	The RX/TX buffers pair
++ * @conduit:	The selected conduit
++ * @invoke_ffa_fn:	The function executing the FF-A function
++ * @features:	Table of the FF-A functions having features
++ *
++ * The driver data structure hosting all resident data.
++ */
++struct ffa_prvdata {
++	struct udevice *dev;
++	u32 fwk_version;
++	u16 id;
++	struct ffa_partitions partitions;
++	struct ffa_rxtxpair pair;
++	enum ffa_conduit conduit;
++	invoke_ffa_fn_t invoke_ffa_fn;
++	struct ffa_features_desc features[FFA_FEATURE_DESC_CNT];
++};
++
++#endif
+diff --git a/drivers/arm-ffa/core.c b/drivers/arm-ffa/core.c
+new file mode 100644
+index 000000000000..98e2d2fa1767
+--- /dev/null
++++ b/drivers/arm-ffa/core.c
+@@ -0,0 +1,1484 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include "arm_ffa_prv.h"
++#include <asm/global_data.h>
++#include <asm/io.h>
++#include <common.h>
++#include <dm.h>
++#include <fdtdec.h>
++#include <linux/errno.h>
++#include <linux/sizes.h>
++#include <log.h>
++#include <malloc.h>
++#include <mapmem.h>
++#include <string.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++/**
++ * The device private data structure containing all the resident
++ * data read from secure world
++ */
++struct ffa_prvdata __ffa_runtime_data ffa_priv_data = {0};
++
++/*
++ * Driver functions
++ */
++
++/**
++ * ffa_get_device - probes the arm_ffa device
++ *
++ * This boot time function makes sure the arm_ffa device is probed
++ * and ready for use. This is done using uclass_get_device.
++ * The arm_ffa driver belongs to UCLASS_FFA.
++ * This function should be called before using the driver.
++ *
++ * Arm FF-A transport is implemented through a single u-boot
++ * device (arm_ffa). So, there is only one device belonging to UCLASS_FFA.
++ * All FF-A clients should use the arm_ffa device to use the FF-A
++ * transport.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_get_device(void)
++{
++	int ret;
++	int devnum = 0;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (ffa_priv_data.dev)
++		return FFA_ERR_STAT_SUCCESS;
++
++	/*
++	 * searching and probing the device
++	 */
++	ret = uclass_get_device(UCLASS_FFA, devnum, &ffa_priv_data.dev);
++	if (ret) {
++		ffa_err("can not find the device");
++		ffa_priv_data.dev = NULL;
++		return -ENODEV;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_get_version - FFA_VERSION handler function
++ *
++ * This is the boot time function that implements FFA_VERSION FF-A function
++ * to get from the secure world the FF-A framework version
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_version(void)
++{
++	u16 major, minor;
++
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_VERSION;
++	a1 = FFA_VERSION_1_0;
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	if (res.a0 == FFA_ERR_STAT_NOT_SUPPORTED) {
++		ffa_err("A Firmware Framework implementation does not exist");
++		return -EOPNOTSUPP;
++	}
++
++	major = GET_FFA_MAJOR_VERSION(res.a0);
++	minor = GET_FFA_MINOR_VERSION(res.a0);
++
++	ffa_info("FF-A driver %d.%d\nFF-A framework %d.%d",
++		 FFA_MAJOR_VERSION, FFA_MINOR_VERSION, major, minor);
++
++	if ((major == FFA_MAJOR_VERSION && minor >= FFA_MINOR_VERSION)) {
++		ffa_info("Versions are compatible ");
++
++		ffa_priv_data.fwk_version = res.a0;
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++
++	ffa_info("Versions are incompatible ");
++	return -EPROTONOSUPPORT;
++}
++
++/**
++ * ffa_get_endpoint_id - FFA_ID_GET handler function
++ *
++ * This is the boot time function that implements FFA_ID_GET FF-A function
++ * to get from the secure world u-boot endpoint ID
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_endpoint_id(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_ID_GET;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED) {
++			ffa_err("This function is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		}
++
++		ffa_err("Undefined error code (%d)", ((int)res.a2));
++		return -EINVAL;
++	}
++	case FFA_SUCCESS:
++	{
++		ffa_priv_data.id = GET_SELF_ENDPOINT_ID(res.a2);
++		ffa_info("endpoint ID is %u", ffa_priv_data.id);
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++	default:
++	{
++		ffa_err("Undefined response function (0x%lx)", res.a0);
++		return -EINVAL;
++	}
++	}
++}
++
++/**
++ * ffa_get_features_desc - returns the features descriptor of the specified
++ *						FF-A function
++ * @func_id:	the FF-A function which the features are to be retrieved
++ *
++ * This is a boot time function that searches the features descriptor of the
++ * specified FF-A function
++ *
++ * Return:
++ *
++ * When found, the address of the features descriptor is returned. Otherwise, NULL.
++ */
++static struct ffa_features_desc *ffa_get_features_desc(u32 func_id)
++{
++	u32 desc_idx;
++
++	/*
++	 * search for the descriptor of the selected FF-A interface
++	 */
++	for (desc_idx = 0; desc_idx < FFA_FEATURE_DESC_CNT ; desc_idx++)
++		if (ffa_priv_data.features[desc_idx].func_id == func_id)
++			return &ffa_priv_data.features[desc_idx];
++
++	return NULL;
++}
++
++/**
++ * ffa_get_rxtx_map_features - FFA_FEATURES handler function with FFA_RXTX_MAP
++ *							argument
++ *
++ * This is the boot time function that implements FFA_FEATURES FF-A function
++ * to retrieve the FFA_RXTX_MAP features
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_rxtx_map_features(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_FEATURES;
++	a1 = FFA_RXTX_MAP;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED) {
++			ffa_err("FFA_RXTX_MAP is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		}
++
++		ffa_err("Undefined error code (%d)", ((int)res.a2));
++		return -EINVAL;
++	}
++	case FFA_SUCCESS:
++	{
++		u32 desc_idx;
++
++		/*
++		 * search for an empty descriptor
++		 */
++		for (desc_idx = 0; desc_idx < FFA_FEATURE_DESC_CNT ; desc_idx++)
++			if (!ffa_priv_data.features[desc_idx].func_id) {
++				/*
++				 * populate the descriptor with
++				 * the interface features data
++				 */
++				ffa_priv_data.features[desc_idx].func_id =
++					FFA_RXTX_MAP;
++				ffa_priv_data.features[desc_idx].field1 =
++					res.a2;
++
++				ffa_info("FFA_RXTX_MAP features data 0x%lx",
++					 res.a2);
++
++				return FFA_ERR_STAT_SUCCESS;
++			}
++
++		ffa_err("Cannot save FFA_RXTX_MAP features data. Descriptors table full");
++		return -ENOBUFS;
++	}
++	default:
++	{
++		ffa_err("Undefined response function (0x%lx)",
++			res.a0);
++		return -EINVAL;
++	}
++	}
++}
++
++/**
++ * ffa_get_rxtx_buffers_pages_cnt - reads from the features data descriptors
++ *						the minimum number of pages in each of the RX/TX
++ *						buffers
++ * @buf_4k_pages: Pointer to the minimum number of pages
++ *
++ * This is the boot time function that  returns the minimum number of pages
++ *  in each of the RX/TX buffers
++ *
++ * Return:
++ *
++ * buf_4k_pages points to the returned number of pages
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_rxtx_buffers_pages_cnt(size_t *buf_4k_pages)
++{
++	struct ffa_features_desc *desc = NULL;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!buf_4k_pages)
++		return -EINVAL;
++
++	desc = ffa_get_features_desc(FFA_RXTX_MAP);
++	if (!desc)
++		return -EINVAL;
++
++	ffa_dbg("FFA_RXTX_MAP descriptor found");
++
++	switch (desc->field1) {
++	case RXTX_4K:
++		*buf_4k_pages = 1;
++		break;
++	case RXTX_16K:
++		*buf_4k_pages = 4;
++		break;
++	case RXTX_64K:
++		*buf_4k_pages = 16;
++		break;
++	default:
++		ffa_err("RX/TX buffer size not supported");
++		return -EINVAL;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_free_rxtx_buffers - frees the RX/TX buffers
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function used to free the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_free_rxtx_buffers(size_t buf_4k_pages)
++{
++	efi_status_t free_rxbuf_ret, free_txbuf_ret;
++
++	ffa_info("Freeing RX/TX buffers");
++
++	free_rxbuf_ret = efi_free_pages(ffa_priv_data.pair.rxbuf, buf_4k_pages);
++	free_txbuf_ret = efi_free_pages(ffa_priv_data.pair.txbuf, buf_4k_pages);
++
++	if (free_rxbuf_ret != EFI_SUCCESS || free_txbuf_ret != EFI_SUCCESS) {
++		ffa_err("Failed to free RX/TX buffers (rx: %lu , tx: %lu)",
++			free_rxbuf_ret,
++			free_txbuf_ret);
++		return -EINVAL;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_alloc_rxtx_buffers - allocates the RX/TX buffers
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function used by ffa_map_rxtx_buffers to allocate
++ * the RX/TX buffers before mapping them
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_alloc_rxtx_buffers(size_t buf_4k_pages)
++{
++	ffa_dbg("[%s]", __func__);
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++	efi_status_t efi_ret;
++	void *virt_txbuf;
++	void *virt_rxbuf;
++
++	ffa_info("Using %lu 4KB page(s) for RX/TX buffers size",
++		 buf_4k_pages);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++				     EFI_BOOT_SERVICES_DATA,
++				     buf_4k_pages,
++				     &ffa_priv_data.pair.rxbuf);
++
++	if (efi_ret != EFI_SUCCESS) {
++		ffa_priv_data.pair.rxbuf = 0;
++		ffa_err("Failure to allocate RX buffer (EFI error: 0x%lx)",
++			efi_ret);
++
++		return -ENOBUFS;
++	}
++
++	ffa_info("RX buffer at phys 0x%llx",
++		 ffa_priv_data.pair.rxbuf);
++
++	/*
++	 * convert the RX buffer physical address to virtual address
++	 */
++	virt_rxbuf = (void *)map_sysmem((phys_addr_t)ffa_priv_data.pair.rxbuf, 0);
++
++	/*
++	 * make sure the buffer is clean before use
++	 */
++	memset(virt_rxbuf, 0, buf_4k_pages * SZ_4K);
++
++	unmap_sysmem(virt_rxbuf);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++				     EFI_RUNTIME_SERVICES_DATA,
++				     buf_4k_pages,
++				     &ffa_priv_data.pair.txbuf);
++
++	if (efi_ret != EFI_SUCCESS) {
++		ffa_dbg("FFA_RXTX_MAP: freeing RX buffer");
++		efi_free_pages(ffa_priv_data.pair.rxbuf, buf_4k_pages);
++		ffa_priv_data.pair.rxbuf = 0;
++		ffa_priv_data.pair.txbuf = 0;
++		ffa_err("Failure to allocate the TX buffer (EFI error: 0x%lx)"
++			, efi_ret);
++
++		return -ENOBUFS;
++	}
++
++	ffa_info("TX buffer at phys 0x%llx",
++		 ffa_priv_data.pair.txbuf);
++
++	/*
++	 * convert the TX buffer physical address to virtual address
++	 */
++	virt_txbuf = (void *)map_sysmem((phys_addr_t)ffa_priv_data.pair.txbuf, 0);
++
++	/*
++	 * make sure the buffer is clean before use
++	 */
++	memset(virt_txbuf, 0, buf_4k_pages * SZ_4K);
++
++	unmap_sysmem(virt_txbuf);
++
++	return FFA_ERR_STAT_SUCCESS;
++
++#else
++	return -ENOBUFS;
++#endif
++}
++
++/**
++ * ffa_map_rxtx_buffers - FFA_RXTX_MAP handler function
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function that implements FFA_RXTX_MAP FF-A function
++ * to map the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_map_rxtx_buffers(size_t buf_4k_pages)
++{
++	int ret;
++
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	ret = ffa_alloc_rxtx_buffers(buf_4k_pages);
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	a0 = FFA_RXTX_MAP;
++	a1 = ffa_priv_data.pair.txbuf;
++	a2 = ffa_priv_data.pair.rxbuf;
++	a3 = buf_4k_pages;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("One or more fields in input parameters is incorrectly encoded");
++			ret = -EPERM;
++			break;
++		case FFA_ERR_STAT_NO_MEMORY:
++			ffa_err("Not enough memory");
++			ret = -ENOMEM;
++			break;
++		case FFA_ERR_STAT_DENIED:
++			ffa_err("Buffer pair already registered");
++			ret = -EACCES;
++			break;
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			ffa_err("This function is not implemented at this FF-A instance");
++			ret = -EOPNOTSUPP;
++			break;
++		default:
++			ffa_err("Undefined error (%d)",
++				((int)res.a2));
++			ret = -EINVAL;
++		}
++		break;
++	}
++	case FFA_SUCCESS:
++		ffa_info("RX/TX buffers mapped");
++		return FFA_ERR_STAT_SUCCESS;
++	default:
++		ffa_err("Undefined response function (0x%lx)",
++			res.a0);
++		ret = -EINVAL;
++	}
++
++	ffa_free_rxtx_buffers(buf_4k_pages);
++
++	return ret;
++}
++
++/**
++ * ffa_unmap_rxtx_buffers - FFA_RXTX_UNMAP handler function
++ *
++ * This is the boot time function that implements FFA_RXTX_UNMAP FF-A function
++ * to unmap the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_unmap_rxtx_buffers(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_RXTX_UNMAP;
++	a1 = PREP_SELF_ENDPOINT_ID(ffa_priv_data.id);
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED)
++			panic("[FFA] FFA_RXTX_UNMAP is not implemented at this FF-A instance\n");
++		else if (((int)res.a2) == FFA_ERR_STAT_INVALID_PARAMETERS)
++			panic("[FFA] There is no buffer pair registered on behalf of the caller\n");
++		else
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++	}
++	case FFA_SUCCESS:
++	{
++		size_t buf_4k_pages = 0;
++		int ret;
++
++		ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS)
++			panic("[FFA] RX/TX buffers unmapped but failure in getting pages count\n");
++
++		ret = ffa_free_rxtx_buffers(buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS)
++			panic("[FFA] RX/TX buffers unmapped but failure in freeing the memory\n");
++
++		ffa_info("RX/TX buffers unmapped and memory freed");
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++	default:
++		panic("[FFA] Undefined response function (0x%lx)", res.a0);
++	}
++}
++
++/**
++ * ffa_release_rx_buffer - FFA_RX_RELEASE handler function
++ *
++ * This is the boot time function that invokes FFA_RX_RELEASE FF-A function
++ * to release the ownership of the RX buffer
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_release_rx_buffer(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_RX_RELEASE;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED)
++			panic("[FFA] FFA_RX_RELEASE is not implemented at this FF-A instance\n");
++		else if (((int)res.a2) == FFA_ERR_STAT_DENIED)
++			panic("[FFA] Caller did not have ownership of the RX buffer\n");
++		else
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++	}
++	case FFA_SUCCESS:
++		ffa_info("RX buffer released");
++		return FFA_ERR_STAT_SUCCESS;
++
++	default:
++		panic("[FFA] Undefined response function (0x%lx)\n", res.a0);
++	}
++}
++
++/**
++ * ffa_uuid_are_identical - checks whether two given UUIDs are identical
++ * @uuid1: first UUID
++ * @uuid2: second UUID
++ *
++ * This is a boot time function used by ffa_read_partitions_info to search
++ * for a UUID in the partitions descriptors table
++ *
++ * Return:
++ *
++ * 1 when UUIDs match. Otherwise, 0
++ */
++int ffa_uuid_are_identical(const union ffa_partition_uuid *uuid1,
++			   const union ffa_partition_uuid *uuid2)
++{
++	if (!uuid1 || !uuid2)
++		return 0;
++
++	return (!memcmp(uuid1, uuid2, sizeof(union ffa_partition_uuid)));
++}
++
++/**
++ * ffa_read_partitions_info - reads the data queried by FFA_PARTITION_INFO_GET
++ *							and saves it in the private structure
++ * @count: The number of partitions queried
++ * @part_uuid: Pointer to the partition(s) UUID
++ *
++ * This is the boot time function that reads the partitions information
++ * returned by the FFA_PARTITION_INFO_GET and saves it in the private
++ * data structure.
++ *
++ * Return:
++ *
++ * The private data structure is updated with the partition(s) information
++ * FFA_ERR_STAT_SUCCESS is returned on success. Otherwise, failure
++ */
++static int ffa_read_partitions_info(u32 count, union ffa_partition_uuid *part_uuid)
++{
++	ffa_dbg("[%s]", __func__);
++
++	if (!count) {
++		ffa_err("No partition detected");
++		return -ENODATA;
++	}
++
++	ffa_info("Reading partitions data from the RX buffer");
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++	if (!part_uuid) {
++		/*
++		 * querying information of all partitions
++		 */
++		u64 data_pages;
++		u64 data_bytes;
++		efi_status_t efi_ret;
++		size_t buf_4k_pages = 0;
++		u32 desc_idx;
++		struct ffa_partition_info *parts_info;
++		int ret;
++
++		data_bytes = count * sizeof(struct ffa_partition_desc);
++		data_pages = efi_size_in_pages(data_bytes);
++
++		/*
++		 * get the RX buffer size in pages
++		 */
++		ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS) {
++			ffa_err("Can not get the RX buffer size (error %d)", ret);
++			return ret;
++		}
++
++		if (data_pages > buf_4k_pages) {
++			ffa_err("Partitions data size exceeds the RX buffer size:");
++			ffa_err("    Sizes in pages: data %llu , RX buffer %lu ",
++				data_pages,
++				buf_4k_pages);
++
++			return -ENOMEM;
++		}
++
++		efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++					     EFI_RUNTIME_SERVICES_DATA,
++					     data_pages,
++					     (u64 *)&ffa_priv_data.partitions.descs);
++
++		if (efi_ret != EFI_SUCCESS) {
++			ffa_priv_data.partitions.descs = NULL;
++
++			ffa_err("Cannot  allocate partitions data buffer (EFI error 0x%lx)",
++				efi_ret);
++
++			return -ENOBUFS;
++		}
++
++		/*
++		 * convert the descs buffer physical address
++		 * to virtual address
++		 * This virtual address should not be unmapped
++		 * descs is expected to be a virtual address
++		 */
++		ffa_priv_data.partitions.descs =
++			(struct ffa_partition_desc *)
++			map_sysmem((phys_addr_t)
++				   ffa_priv_data.partitions.descs, 0);
++
++		/*
++		 * make sure the buffer is clean before use
++		 */
++		memset(ffa_priv_data.partitions.descs, 0,
++		       data_pages * SZ_4K);
++
++		ffa_info("Copying %lld page(s) of partitions data from RX buffer",
++			 data_pages);
++
++		/*
++		 * convert the RX buffer physical address to
++		 * virtual address
++		 */
++		parts_info = (struct ffa_partition_info *)
++			map_sysmem((phys_addr_t)
++				   ffa_priv_data.pair.rxbuf, 0);
++
++		for (desc_idx = 0 ; desc_idx < count ; desc_idx++) {
++			ffa_priv_data.partitions.descs[desc_idx].info =
++				parts_info[desc_idx];
++
++			ffa_info("Partition ID %x : info cached",
++				 ffa_priv_data.partitions.descs[desc_idx].info.id);
++		}
++		unmap_sysmem(parts_info);
++
++		ffa_priv_data.partitions.count = count;
++
++		ffa_info("%d partition(s) found and cached", count);
++
++	} else {
++		u32 rx_desc_idx, cached_desc_idx;
++		struct ffa_partition_info *parts_info;
++		u8 desc_found;
++
++		/*
++		 * convert the RX buffer physical address to virtual address
++		 */
++		parts_info = (struct ffa_partition_info *)
++			map_sysmem((phys_addr_t)ffa_priv_data.pair.rxbuf, 0);
++
++		/*
++		 * search for the SP IDs read from the RX buffer
++		 * in the already cached SPs.
++		 * Update the UUID when ID found.
++		 */
++		for (rx_desc_idx = 0; rx_desc_idx < count ; rx_desc_idx++) {
++			desc_found = 0;
++
++			/*
++			 * search the current ID in the cached partitions
++			 */
++			for (cached_desc_idx = 0;
++			     cached_desc_idx < ffa_priv_data.partitions.count;
++			     cached_desc_idx++) {
++				/*
++				 * save the UUID
++				 */
++				if (ffa_priv_data.partitions.descs[cached_desc_idx].info.id ==
++				    parts_info[rx_desc_idx].id) {
++					ffa_priv_data.partitions.descs[cached_desc_idx].UUID =
++						*part_uuid;
++
++					desc_found = 1;
++					break;
++				}
++			}
++
++			if (!desc_found) {
++				unmap_sysmem(parts_info);
++				return -ENODATA;
++			}
++		}
++		unmap_sysmem(parts_info);
++	}
++#else
++#warning "arm_ffa: reading FFA_PARTITION_INFO_GET data not implemented"
++#endif
++
++	return  FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_query_partitions_info - invokes FFA_PARTITION_INFO_GET
++ *							and saves partitions data
++ * @part_uuid: Pointer to the partition(s) UUID
++ * @pcount: Pointer to the number of partitions variable filled when querying
++ *
++ * This is the boot time function that executes the FFA_PARTITION_INFO_GET
++ * to query the partitions data. Then, it calls ffa_read_partitions_info
++ * to save the data in the private data structure.
++ *
++ * After reading the data the RX buffer is released using ffa_release_rx_buffer
++ *
++ * Return:
++ *
++ * When part_uuid is NULL, all partitions data are retrieved from secure world
++ * When part_uuid is non NULL, data for partitions matching the given UUID are
++ * retrieved and the number of partitions is returned
++ * FFA_ERR_STAT_SUCCESS is returned on success. Otherwise, failure
++ */
++static int ffa_query_partitions_info(union ffa_partition_uuid *part_uuid,
++				     u32 *pcount)
++{
++	unsigned long a0 = 0;
++	union ffa_partition_uuid query_uuid = {0};
++	unsigned long a5 = 0;
++	unsigned long a6 = 0;
++	unsigned long a7 = 0;
++	struct arm_smccc_res res = {0};
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_PARTITION_INFO_GET;
++
++	/*
++	 * If a UUID is specified. Information for one or more
++	 * partitions in the system is queried. Otherwise, information
++	 * for all installed partitions is queried
++	 */
++
++	if (part_uuid) {
++		if (!pcount)
++			return -EINVAL;
++
++		query_uuid = *part_uuid;
++	}
++
++	ffa_priv_data.invoke_ffa_fn(a0, query_uuid.words.a1, query_uuid.words.a2,
++				    query_uuid.words.a3, query_uuid.words.a4,
++				    a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("Unrecognized UUID");
++			return -EPERM;
++		case FFA_ERR_STAT_NO_MEMORY:
++			ffa_err("Results cannot fit in RX buffer of the caller");
++			return -ENOMEM;
++		case FFA_ERR_STAT_DENIED:
++			ffa_err("Callee is not in a state to handle this request");
++			return -EACCES;
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			ffa_err("This function is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		case FFA_ERR_STAT_BUSY:
++			ffa_err("RX buffer of the caller is not free");
++			return -EBUSY;
++		default:
++			ffa_err("Undefined error (%d)", ((int)res.a2));
++			return -EINVAL;
++		}
++	}
++	case FFA_SUCCESS:
++	{
++		int ret;
++
++		/*
++		 * res.a2 contains the count of partition information descriptors
++		 * populated in the RX buffer
++		 */
++		if (res.a2) {
++			ret = ffa_read_partitions_info(res.a2, part_uuid);
++			if (ret)
++				ffa_err("Failed to read partition(s) data , error (%d)", ret);
++		}
++
++		/*
++		 * return the SP count
++		 */
++		if (part_uuid) {
++			if (!ret)
++				*pcount = res.a2;
++			else
++				*pcount = 0;
++		}
++		/*
++		 * After calling FFA_PARTITION_INFO_GET the buffer ownership
++		 * is assigned to the consumer (u-boot). So, we need to give
++		 * the ownership back to the secure world
++		 */
++		ret = ffa_release_rx_buffer();
++
++		if (!part_uuid && !res.a2) {
++			ffa_err("[FFA] no partition installed in the system");
++			return -ENODEV;
++		}
++
++		return ret;
++	}
++	default:
++		ffa_err("Undefined response function (0x%lx)", res.a0);
++		return  -EINVAL;
++	}
++}
++
++/**
++ * ffa_get_partitions_info - FFA_PARTITION_INFO_GET handler function
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ * The passed arguments:
++ * Mode 1: When getting from the driver the number of
++ *	secure partitions:
++ *	@data0_size: UUID size
++ *	@data0: pointer to the UUID (little endian)
++ *	@data1_size: size of the number of partitions
++ *				variable
++ *	@data1: pointer to the number of partitions
++ *			 variable. The variable will be set
++ *			 by the driver
++ * Mode 2: When requesting the driver to return the
++ *	partitions information:
++ *	@data0_size: UUID size
++ *	@data0: pointer to the UUID (little endian)
++ *	@data1_size: size of the SPs information buffer
++ *	@data1: pointer to SPs information buffer
++ *		(allocated by the client).
++ *		The buffer will be filled by the driver
++ *
++ * This is the boot time function that queries the secure partition data from
++ * the private data structure. If not found, it invokes FFA_PARTITION_INFO_GET
++ * FF-A function to query the partition information from secure world.
++ *
++ * A client of the FF-A driver should know the UUID of the service it wants to
++ * access. It should use the UUID to request the FF-A driver to provide the
++ * partition(s) information of the service. The FF-A driver uses
++ * PARTITION_INFO_GET to obtain this information. This is implemented through
++ * ffa_get_partitions_info function.
++ * A new FFA_PARTITION_INFO_GET call is issued (first one performed through
++ * ffa_cache_partitions_info) allowing to retrieve the partition(s) information.
++ * They are not saved (already done). We only update the UUID in the cached area.
++ * This assumes that partitions data does not change in the secure world.
++ * Otherwise u-boot will have an outdated partition data. The benefit of caching
++ * the information in the FF-A driver is to accommodate discovery after
++ * ExitBootServices().
++ *
++ * When invoked through a client request, ffa_get_partitions_info should be
++ * called twice. First call is to get from the driver the number of secure
++ * partitions (SPs) associated to a particular UUID.
++ * Then, the caller (client) allocates the buffer to host the SPs data and
++ * issues a 2nd call. Then, the driver fills the SPs data in the pre-allocated
++ * buffer.
++ *
++ * To achieve the mechanism described above, ffa_get_partitions_info uses the
++ * following functions:
++ *		ffa_read_partitions_info
++ *		ffa_query_partitions_info
++ *
++ * Return:
++ *
++ * @data1: When pointing to the number of partitions variable, the number is
++ * set by the driver.
++ * When pointing to the partitions information buffer, the buffer will be
++ * filled by the driver.
++ *
++ * On success FFA_ERR_STAT_SUCCESS is returned. Otherwise, failure
++ */
++static int ffa_get_partitions_info(struct ffa_interface_data *func_data)
++{
++	/*
++	 * fill_data:
++	 * 0: return the SP count
++	 * 1: fill SP data and return it to the caller
++	 * -1: undefined mode
++	 */
++	int fill_data = -1;
++	u32 desc_idx, client_desc_idx;
++	union ffa_partition_uuid *part_uuid;
++	u32 client_desc_max_cnt;
++	u32 parts_found = 0;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!func_data) {
++		ffa_err("No function data provided");
++		return -EINVAL;
++	}
++
++	if (!ffa_priv_data.partitions.count || !ffa_priv_data.partitions.descs)
++		panic("[FFA] No partition installed\n");
++
++	if (func_data->data0_size == sizeof(union ffa_partition_uuid) &&
++	    func_data->data0 &&
++	    func_data->data1_size == sizeof(u32) &&
++	    func_data->data1) {
++		/*
++		 * data0 (in): pointer to UUID
++		 * data1 (in): pointer to SP count
++		 * Out: SP count returned in the count variable pointed by data1
++		 */
++
++		fill_data = 0;
++
++		ffa_info("Preparing for checking partitions count");
++
++	} else if ((func_data->data0_size == sizeof(union ffa_partition_uuid)) &&
++		   func_data->data0 &&
++		   (func_data->data1_size >= sizeof(struct ffa_partition_info)) &&
++		   !(func_data->data1_size % sizeof(struct ffa_partition_info)) &&
++		   func_data->data1) {
++		/*
++		 * data0 (in): pointer to UUID
++		 * data1 (in): pointer to SPs descriptors buffer
++		 *             (created by the client)
++		 * Out: SPs descriptors returned in the buffer
++		 *      pointed by data1
++		 */
++
++		fill_data = 1;
++
++		client_desc_idx = 0;
++
++		/*
++		 * number of empty descriptors preallocated by the caller
++		 */
++		client_desc_max_cnt =
++			func_data->data1_size / sizeof(struct ffa_partition_info);
++
++		ffa_info("Preparing for filling partitions info");
++
++	} else {
++		ffa_err("Invalid function arguments provided");
++		return -EINVAL;
++	}
++
++	part_uuid = (union ffa_partition_uuid *)func_data->data0;
++
++	ffa_info("Searching partitions using the provided UUID");
++
++#ifdef DEBUG
++	{
++		u32 dbg_uuid_cnt;
++
++		ffa_dbg("UUID: [LSB]");
++
++		for (dbg_uuid_cnt = 0 ; dbg_uuid_cnt < UUID_SIZE ; dbg_uuid_cnt++)
++			ffa_dbg(" %02x", part_uuid->bytes[dbg_uuid_cnt]);
++	}
++#endif
++
++	/*
++	 * search in the cached partitions
++	 */
++	for (desc_idx = 0;
++	     desc_idx < ffa_priv_data.partitions.count;
++	     desc_idx++) {
++		if (ffa_uuid_are_identical(&ffa_priv_data.partitions.descs[desc_idx].UUID,
++					   part_uuid)) {
++			ffa_info("Partition ID %x matches the provided UUID",
++				 ffa_priv_data.partitions.descs[desc_idx].info.id);
++
++			parts_found++;
++
++			if (fill_data) {
++				/*
++				 * trying to fill the partition info in data1
++				 */
++
++				if (client_desc_idx < client_desc_max_cnt) {
++					((struct ffa_partition_info *)
++					 func_data->data1)[client_desc_idx++] =
++						ffa_priv_data.partitions.descs[desc_idx].info;
++					continue;
++				}
++
++				ffa_err("Failed to fill the current descriptor client buffer full");
++				return -ENOBUFS;
++			}
++		}
++	}
++
++	if (!parts_found) {
++		int ret;
++
++		ffa_info("No partition found. Querying framework ...");
++
++		ret = ffa_query_partitions_info(part_uuid, &parts_found);
++
++		if (ret == FFA_ERR_STAT_SUCCESS) {
++			if (!fill_data) {
++				*((u32 *)func_data->data1) = parts_found;
++
++				ffa_info("Number of partition(s) found matching the UUID: %d",
++					 parts_found);
++			} else {
++				/*
++				 * we want to read SPs info
++				 */
++
++				/*
++				 * If SPs data filled, retry searching SP info again
++				 */
++				if (parts_found)
++					ret = ffa_get_partitions_info(func_data);
++				else
++					ret = -ENODATA;
++			}
++		}
++
++		return ret;
++	}
++
++	/* partition(s) found */
++	if (!fill_data)
++		*((u32 *)func_data->data1) = parts_found;
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_cache_partitions_info - Queries and saves all secure partitions data
++ *
++ * This is a boot time function that invokes FFA_PARTITION_INFO_GET FF-A
++ * function to query from secure world all partitions information.
++ *
++ * The FFA_PARTITION_INFO_GET call is issued with nil UUID as an argument.
++ * All installed partitions information are returned. We cache them in the
++ * resident private data structure and we keep the UUID field empty
++ * (in FF-A 1.0 UUID is not provided by the partition descriptor)
++ *
++ * This function is called at the device probing level.
++ * ffa_cache_partitions_info uses ffa_query_partitions_info to get the data
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_cache_partitions_info(void)
++{
++	ffa_dbg("[%s]", __func__);
++	return ffa_query_partitions_info(NULL, NULL);
++}
++
++/**
++ * ffa_msg_send_direct_req - FFA_MSG_SEND_DIRECT_{REQ,RESP} handler function
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ *					The passed arguments:
++ *						@data0_size: partition ID size
++ *						@data0: pointer to the partition ID
++ *						@data1_size: exchanged data size
++ *						@data1: pointer to the data buffer preallocated by
++ *								the client (in/out)
++ *
++ * This is the runtime function that implements FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * FF-A functions.
++ *
++ * FFA_MSG_SEND_DIRECT_REQ is used to send the data to the secure partition.
++ * The response from the secure partition is handled by reading the
++ * FFA_MSG_SEND_DIRECT_RESP arguments.
++ *
++ * The maximum size of the data that can be exchanged is 20 bytes which is
++ * sizeof(struct ffa_send_direct_data) as defined by the FF-A specification 1.0
++ * in the section relevant to FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int __ffa_runtime ffa_msg_send_direct_req(struct ffa_interface_data
++						 *func_data)
++{
++	u16 dst_part_id;
++	unsigned long a0 = 0;
++	unsigned long a1 = 0;
++	unsigned long a2 = 0;
++	struct ffa_send_direct_data *msg;
++	struct arm_smccc_res res = {0};
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	if (!func_data)
++		return -EINVAL;
++
++	if (!ffa_priv_data.partitions.count || !ffa_priv_data.partitions.descs)
++		panic("[FFA] no partition installed\n");
++
++	if (func_data->data0_size != sizeof(u16) ||
++	    !func_data->data0 ||
++	    func_data->data1_size != FFA_MSG_SEND_DIRECT_MAX_SIZE ||
++	    !func_data->data1) {
++		ffa_err("Undefined interface parameters");
++		return -EINVAL;
++	}
++
++	dst_part_id = *((u16 *)func_data->data0);
++	msg = func_data->data1;
++
++	ffa_dbg("Sending data to partition ID 0x%x", dst_part_id);
++
++	a0 = FFA_MSG_SEND_DIRECT_REQ;
++
++	a1 = PREP_SELF_ENDPOINT_ID(ffa_priv_data.id) |
++		PREP_PART_ENDPOINT_ID(dst_part_id);
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2,
++				    msg->a3,
++				    msg->a4,
++				    msg->a5,
++				    msg->a6,
++				    msg->a7,
++				    &res);
++
++	while (res.a0 == FFA_INTERRUPT)
++		ffa_priv_data.invoke_ffa_fn(FFA_RUN, res.a1,
++					    0, 0, 0, 0, 0, 0,
++					    &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("Invalid endpoint ID or non-zero reserved register");
++			return -EPERM;
++		case FFA_ERR_STAT_ABORTED:
++			panic("[FFA] Message target ran into unexpected error and has aborted\n");
++		case FFA_ERR_STAT_DENIED:
++			panic("[FFA] Callee is not in a state to handle this request\n");
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			panic("[FFA] This function is not implemented at this FF-A instance\n");
++		case FFA_ERR_STAT_BUSY:
++			panic("[FFA] Message target is busy\n");
++		default:
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++		}
++	}
++	case FFA_SUCCESS:
++
++		ffa_dbg("Message sent with no response");
++		return FFA_ERR_STAT_SUCCESS;
++
++	case FFA_MSG_SEND_DIRECT_RESP:
++
++		ffa_dbg("Message sent with response");
++
++		/*
++		 * extract the 32-bit wide return data
++		 */
++		msg->a3 = (u32)res.a3;
++		msg->a4 = (u32)res.a4;
++		msg->a5 = (u32)res.a5;
++		msg->a6 = (u32)res.a6;
++		msg->a7 = (u32)res.a7;
++
++		return FFA_ERR_STAT_SUCCESS;
++
++	default:
++		panic("[FFA] Undefined response function (0x%lx)\n", res.a0);
++	}
++}
++
++/**
++ * invoke_ffa_drv_api - The driver dispatcher function
++ * @func_id:	The FF-A function to be used
++ * @func_data:  Pointer to the FF-A function arguments container
++ *					structure. This also includes pointers to the
++ *					returned data needed by clients.
++ * The dispatcher is a runtime function that selects the FF-A function handler
++ * based on the input FF-A function ID.
++ * The input arguments are passed to the handler function.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime invoke_ffa_drv_api(u32 func_id,
++				     struct ffa_interface_data *func_data)
++{
++	if (!ffa_priv_data.dev)
++		panic("[FFA] no device found\n");
++
++	switch (func_id) {
++	case FFA_PARTITION_INFO_GET:
++		return ffa_get_partitions_info(func_data);
++	case FFA_RXTX_UNMAP:
++		return ffa_unmap_rxtx_buffers();
++	case FFA_MSG_SEND_DIRECT_REQ:
++		return ffa_msg_send_direct_req(func_data);
++	default:
++
++		ffa_err("Undefined FF-A interface (%d)", func_id);
++
++		return -EINVAL;
++	}
++}
++
++/**
++ * ffa_init_private_data - Initialization of the private data
++ * @dev:	the arm_ffa device
++ *
++ * This boot time function reads data from the platform data structure
++ * and populates the private data structure
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_init_private_data(struct udevice *dev)
++{
++	struct ffa_pdata *pdata = dev_get_plat(dev);
++
++	ffa_priv_data.conduit = pdata->conduit;
++
++	if (ffa_priv_data.conduit == FFA_CONDUIT_SMC) {
++		ffa_priv_data.invoke_ffa_fn = arm_ffa_smccc_smc;
++	} else {
++		ffa_err("Undefined FF-A conduit (%d)", ffa_priv_data.conduit);
++		return -EINVAL;
++	}
++
++	ffa_info("Conduit is %s",
++		 ((ffa_priv_data.conduit == FFA_CONDUIT_SMC) ?
++		   "SMC" : "NOT SUPPORTED"));
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_probe - The driver probe function
++ * @dev:	the arm_ffa device
++ *
++ * Probing is done at boot time and triggered by the uclass device discovery.
++ * At probe level the following actions are done:
++ *	- initialization of the driver private data structure
++ *	- querying from secure world the FF-A framework version
++ *	- querying from secure world the u-boot endpoint ID
++ *	- querying from secure world the supported features of the specified FF-A calls
++ *	- mapping the RX/TX buffers
++ *	- querying from secure world all the partitions information
++ *
++ * All data queried from secure world is saved in the resident private data structure.
++ *
++ * The probe will fail if either FF-A framework is not detected or the
++ * FF-A requests are not behaving correctly. This ensures that the
++ * driver is not installed and its operations are not exported to the clients.
++ * However, once the driver is successfully probed and an FF-A anomaly is
++ * detected when clients invoke the driver operations, the driver cause
++ * u-boot to panic because the client would not know what to do in such conditions.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_probe(struct udevice *dev)
++{
++	int ret;
++	size_t buf_4k_pages = 0;
++
++	ffa_dbg("[%s]: initializing the FF-A driver", __func__);
++
++	ret = ffa_init_private_data(dev);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_version();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_endpoint_id();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_rxtx_map_features();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_map_rxtx_buffers(buf_4k_pages);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_cache_partitions_info();
++
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_free_rxtx_buffers(buf_4k_pages);
++		return ret;
++	}
++
++	ffa_dbg("[%s]: initialization done", __func__);
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_of_to_plat - Reads the device tree node
++ * @dev:	the arm_ffa device
++ *
++ * This boot time function reads data from the device tree node and populates
++ * the platform data structure
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_of_to_plat(struct udevice *dev)
++{
++	struct ffa_pdata *pdata = dev_get_plat(dev);
++	const char *conduit;
++
++	ffa_dbg("[%s]", __func__);
++
++	conduit = fdt_getprop(gd->fdt_blob, dev_of_offset(dev), "method", NULL);
++
++	if (strcmp("smc", conduit)) {
++		ffa_err("Unsupported conduit");
++		return -EINVAL;
++	}
++
++	pdata->conduit = FFA_CONDUIT_SMC;
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_drv_ops - The driver operations runtime structure
++ * @invoke_func:	The driver dispatcher
++ */
++struct ffa_ops __ffa_runtime_data ffa_drv_ops = {
++	.invoke_func = invoke_ffa_drv_api
++};
++
++/**
++ * ffa_device_get_ops - driver operations getter
++ *
++ * Return:
++ * This runtime function returns a pointer to the driver operations structure
++ */
++const struct ffa_ops * __ffa_runtime ffa_device_get_ops(void)
++{
++	return &ffa_drv_ops;
++}
++
++/**
++ * Defining the device tree compatible string
++ */
++
++static const struct udevice_id ffa_match_id[] = {
++	{"arm,ffa", 0},
++	{},
++};
++
++/**
++ * Declaring the arm_ffa driver under UCLASS_FFA
++ */
++
++U_BOOT_DRIVER(arm_ffa) = {
++	.name		= "arm_ffa",
++	.of_match	= ffa_match_id,
++	.id		= UCLASS_FFA,
++	.of_to_plat	= ffa_of_to_plat,
++	.probe		= ffa_probe,
++	.plat_auto	= sizeof(struct ffa_pdata),
++};
+diff --git a/include/arm_ffa.h b/include/arm_ffa.h
+new file mode 100644
+index 000000000000..313f46f74764
+--- /dev/null
++++ b/include/arm_ffa.h
+@@ -0,0 +1,191 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_H
++#define __ARM_FFA_H
++
++#include <linux/arm-smccc.h>
++#include <linux/printk.h>
++
++/*
++ * This header is public. It can be used by clients to access
++ * data structures and definitions they need
++ */
++
++/*
++ * Macros for displaying logs
++ */
++
++#define ffa_dbg(fmt, ...)  pr_debug("[FFA] " fmt "\n", ##__VA_ARGS__)
++#define ffa_info(fmt, ...)  pr_info("[FFA] " fmt "\n", ##__VA_ARGS__)
++#define ffa_err(fmt, ...)  pr_err("[FFA] " fmt "\n", ##__VA_ARGS__)
++
++/*
++ * The driver operations success error code
++ */
++#define FFA_ERR_STAT_SUCCESS				(0)
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++#include <efi_loader.h>
++
++/*
++ * __ffa_runtime_data and __ffa_runtime - controls whether data/code are
++ * available after calling the EFI ExitBootServices service.
++ * Data/code tagged with these keywords are resident (available at boot time and
++ * at runtime)
++ */
++
++#define __ffa_runtime_data __efi_runtime_data
++#define __ffa_runtime __efi_runtime
++
++#else
++
++#define __ffa_runtime_data
++#define __ffa_runtime
++
++#endif
++
++/*
++ * Definitions of the Arm FF-A interfaces supported by the Arm FF-A driver
++ */
++
++#define FFA_SMC(calling_convention, func_num)				\
++	ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL, (calling_convention),	\
++			   ARM_SMCCC_OWNER_STANDARD, (func_num))
++
++#define FFA_SMC_32(func_num)	FFA_SMC(ARM_SMCCC_SMC_32, (func_num))
++
++#define FFA_VERSION				FFA_SMC_32(0x63)
++#define FFA_ID_GET				FFA_SMC_32(0x69)
++#define FFA_FEATURES				FFA_SMC_32(0x64)
++#define FFA_PARTITION_INFO_GET				FFA_SMC_32(0x68)
++#define FFA_RXTX_MAP				FFA_SMC_32(0x66)
++#define FFA_RXTX_UNMAP				FFA_SMC_32(0x67)
++#define FFA_RX_RELEASE				FFA_SMC_32(0x65)
++#define FFA_MSG_SEND_DIRECT_REQ				FFA_SMC_32(0x6F)
++#define FFA_MSG_SEND_DIRECT_RESP				FFA_SMC_32(0x70)
++#define FFA_RUN				FFA_SMC_32(0x6D)
++#define FFA_ERROR				FFA_SMC_32(0x60)
++#define FFA_SUCCESS				FFA_SMC_32(0x61)
++#define FFA_INTERRUPT				FFA_SMC_32(0x62)
++
++/*
++ * struct ffa_partition_info - Partition information descriptor
++ * @id:	Partition ID
++ * @exec_ctxt:	Execution context count
++ * @properties:	Partition properties
++ *
++ * Data structure containing information about partitions instantiated in the system
++ * This structure is filled with the data queried by FFA_PARTITION_INFO_GET
++ */
++struct  __packed ffa_partition_info {
++	u16 id;
++	u16 exec_ctxt;
++/* partition supports receipt of direct requests */
++#define FFA_PARTITION_DIRECT_RECV	BIT(0)
++/* partition can send direct requests. */
++#define FFA_PARTITION_DIRECT_SEND	BIT(1)
++/* partition can send and receive indirect messages. */
++#define FFA_PARTITION_INDIRECT_MSG	BIT(2)
++	u32 properties;
++};
++
++/*
++ * struct ffa_send_direct_data - Data structure hosting the data
++ *                                       used by FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * @a3-a7:	Data read/written from/to w3-w7 registers
++ *
++ * Data structure containing the data to be sent by FFA_MSG_SEND_DIRECT_REQ
++ * or read from FFA_MSG_SEND_DIRECT_RESP
++ */
++struct __packed ffa_send_direct_data {
++	u32 a3; /* w3 */
++	u32 a4; /* w4 */
++	u32 a5; /* w5 */
++	u32 a6; /* w6 */
++	u32 a7; /* w7 */
++};
++
++#define FFA_MSG_SEND_DIRECT_MAX_SIZE (sizeof(struct ffa_send_direct_data))
++
++/* UUID data size */
++#define UUID_SIZE (16)
++
++/*
++ * union ffa_partition_uuid - Data union hosting the UUID
++ *							transmitted by FFA_PARTITION_INFO_GET
++ * @words:	data structure giving 32-bit words access to the UUID data
++ * @bytes:	data structure giving byte access to the UUID data
++ *
++ * The structure holds little-endian UUID data.
++ */
++union ffa_partition_uuid {
++	struct __packed words {
++	u32 a1; /* w1 */
++	u32 a2; /* w2 */
++	u32 a3; /* w3 */
++	u32 a4; /* w4 */
++	} words;
++	u8 bytes[UUID_SIZE];
++};
++
++/**
++ * struct ffa_interface_data - generic FF-A interface data structure used to exchange
++ *							data between user layers and the driver
++ * @data0_size:	size of the first argument
++ * @data0:	pointer to the first argument
++ * @data1_size>:	size of the second argument
++ * @data1:	pointer to the second argument
++ *
++ * Using this structure user layers can pass various types of data with different sizes.
++ * The driver internal functions can detect the nature of this data, verfy compliance
++ * then execute the request when appropriate.
++ */
++struct ffa_interface_data {
++	u32 data0_size; /* size of the first argument */
++	void *data0; /* pointer to the first argument */
++	u32 data1_size; /* size of the second argument */
++	void *data1; /* pointer to the second argument */
++};
++
++/**
++ * struct ffa_ops - The driver operations structure
++ * @invoke_func:	function pointer to the invoke function
++ *
++ * The data structure providing all the operations supported by the driver.
++ * This structure is resident.
++ */
++struct ffa_ops {
++	/* the driver dispatcher */
++	int (*invoke_func)(u32 func_id, struct ffa_interface_data *func_data);
++};
++
++/**
++ * The device driver and the Uclass driver public functions
++ */
++
++/**
++ * ffa_get_invoke_func - performs a call to the FF-A driver dispatcher
++ */
++int __ffa_runtime ffa_get_invoke_func(u32 func_id,
++				      struct ffa_interface_data *func_data);
++
++/**
++ * ffa_device_get_ops - driver operations getter
++ */
++const struct ffa_ops * __ffa_runtime ffa_device_get_ops(void);
++
++/**
++ * ffa_get_device - probes the arm_ffa device
++ */
++int ffa_get_device(void);
++
++/**
++ * ffa_init_device - probes the arm_ffa device
++ */
++int ffa_init_device(void);
++#endif
+diff --git a/include/arm_ffa_helper.h b/include/arm_ffa_helper.h
+new file mode 100644
+index 000000000000..0e143e54511e
+--- /dev/null
++++ b/include/arm_ffa_helper.h
+@@ -0,0 +1,45 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_HELPER_H
++#define __ARM_FFA_HELPER_H
++
++#include <arm_ffa.h>
++
++/*
++ * This header is public. Including this header provides all data structures
++ * and definitions needed by clients to use the FF-A transport driver
++ *
++ * It also provides helper functions allowing to pass data and invoke  FF-A functions
++ */
++
++/**
++ * ffa_helper_get_partitions_info - Wrapper function for FFA_PARTITION_INFO_GET
++ */
++int ffa_helper_get_partitions_info(struct ffa_interface_data *func_data);
++
++/**
++ * ffa_helper_unmap_rxtx_buffers - Wrapper function for FFA_RXTX_UNMAP
++ */
++int ffa_helper_unmap_rxtx_buffers(void);
++
++/**
++ * ffa_helper_msg_send_direct_req - Wrapper function for
++ *								FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ */
++int __ffa_runtime ffa_helper_msg_send_direct_req(struct ffa_interface_data
++						 *func_data);
++
++/**
++ * ffa_helper_init_device - Wrapper function for probing the arm_ffa device
++ */
++int ffa_helper_init_device(void);
++
++/**
++ * ffa_uuid_str_to_bin - Converts a big endian UUID string to a little endian buffer
++ */
++int ffa_uuid_str_to_bin(const char *uuid_str, unsigned char *uuid_bin);
++#endif
+diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
+index 0e26e1d13824..a1181b8f48e7 100644
+--- a/include/dm/uclass-id.h
++++ b/include/dm/uclass-id.h
+@@ -52,6 +52,7 @@ enum uclass_id {
+ 	UCLASS_EFI_MEDIA,	/* Devices provided by UEFI firmware */
+ 	UCLASS_ETH,		/* Ethernet device */
+ 	UCLASS_ETH_PHY,		/* Ethernet PHY device */
++	UCLASS_FFA,		/* Arm Firmware Framework for Armv8-A */
+ 	UCLASS_FIRMWARE,	/* Firmware */
+ 	UCLASS_FS_FIRMWARE_LOADER,		/* Generic loader */
+ 	UCLASS_GPIO,		/* Bank of general-purpose I/O pins */
+diff --git a/include/linux/arm-smccc.h b/include/linux/arm-smccc.h
+index 7f2be2339475..54980a130fdb 100644
+--- a/include/linux/arm-smccc.h
++++ b/include/linux/arm-smccc.h
+@@ -1,6 +1,8 @@
+ /* SPDX-License-Identifier: GPL-2.0 */
+ /*
+  * Copyright (c) 2015, Linaro Limited
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+  */
+ #ifndef __LINUX_ARM_SMCCC_H
+ #define __LINUX_ARM_SMCCC_H
+@@ -57,13 +59,17 @@
+ #include <linux/types.h>
+ /**
+  * struct arm_smccc_res - Result from SMC/HVC call
+- * @a0-a3 result values from registers 0 to 3
++ * @a0-a7 result values from registers 0 to 7
+  */
+ struct arm_smccc_res {
+ 	unsigned long a0;
+ 	unsigned long a1;
+ 	unsigned long a2;
+ 	unsigned long a3;
++	unsigned long a4;
++	unsigned long a5;
++	unsigned long a6;
++	unsigned long a7;
+ };
+ 
+ /**
+@@ -113,6 +119,26 @@ asmlinkage void __arm_smccc_hvc(unsigned long a0, unsigned long a1,
+ 			unsigned long a5, unsigned long a6, unsigned long a7,
+ 			struct arm_smccc_res *res, struct arm_smccc_quirk *quirk);
+ 
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++/**
++ * __arm_ffa_smccc_smc() - make SMC calls used for FF-A transport
++ * @a0-a7: arguments passed in 64-bit registers x0 to x7
++ * @res: result values from 64-bit registers x0 to x7
++ *
++ * This function is used to make SMC calls following SMC32 Calling Convention.
++ * The content of the supplied parameters is copied to registers x0 to x7 prior
++ * to the SMC instruction. The SMC call return data is 32-bit data read from
++ * registers x0 tp x7.
++ */
++asmlinkage void __arm_ffa_smccc_smc(unsigned long a0, unsigned long a1,
++				    unsigned long a2, unsigned long a3, unsigned long a4,
++				    unsigned long a5, unsigned long a6, unsigned long a7,
++				    struct arm_smccc_res *res);
++
++#define arm_ffa_smccc_smc __arm_ffa_smccc_smc
++
++#endif
++
+ #define arm_smccc_smc(...) __arm_smccc_smc(__VA_ARGS__, NULL)
+ 
+ #define arm_smccc_smc_quirk(...) __arm_smccc_smc(__VA_ARGS__)
+diff --git a/lib/Kconfig b/lib/Kconfig
+index 3c6fa99b1a6a..473821b882e2 100644
+--- a/lib/Kconfig
++++ b/lib/Kconfig
+@@ -810,6 +810,7 @@ config SMBIOS_PARSER
+ source lib/efi/Kconfig
+ source lib/efi_loader/Kconfig
+ source lib/optee/Kconfig
++source lib/arm-ffa/Kconfig
+ 
+ config TEST_FDTDEC
+ 	bool "enable fdtdec test"
+diff --git a/lib/Makefile b/lib/Makefile
+index 11b03d1cbec8..8e6fad613067 100644
+--- a/lib/Makefile
++++ b/lib/Makefile
+@@ -9,6 +9,7 @@ obj-$(CONFIG_EFI) += efi/
+ obj-$(CONFIG_EFI_LOADER) += efi_driver/
+ obj-$(CONFIG_EFI_LOADER) += efi_loader/
+ obj-$(CONFIG_CMD_BOOTEFI_SELFTEST) += efi_selftest/
++obj-$(CONFIG_ARM_FFA_TRANSPORT_HELPERS) += arm-ffa/
+ obj-$(CONFIG_LZMA) += lzma/
+ obj-$(CONFIG_BZIP2) += bzip2/
+ obj-$(CONFIG_TIZEN) += tizen/
+diff --git a/lib/arm-ffa/Kconfig b/lib/arm-ffa/Kconfig
+new file mode 100644
+index 000000000000..79acbc5a8fe3
+--- /dev/null
++++ b/lib/arm-ffa/Kconfig
+@@ -0,0 +1,6 @@
++config ARM_FFA_TRANSPORT_HELPERS
++	bool "Enable interface helpers for Arm Firmware Framework for Armv8-A"
++	depends on ARM_FFA_TRANSPORT
++	help
++	  User layers call FF-A interfaces using helper functions which
++	  pass the data and the FF-A function ID to the low level driver
+diff --git a/lib/arm-ffa/Makefile b/lib/arm-ffa/Makefile
+new file mode 100644
+index 000000000000..c30c0f398126
+--- /dev/null
++++ b/lib/arm-ffa/Makefile
+@@ -0,0 +1,8 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2021 Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++#
++
++# This file only gets included when CONFIG_ARM_FFA_TRANSPORT_HELPERS is set
++
++obj-y += arm_ffa_helper.o
+diff --git a/lib/arm-ffa/arm_ffa_helper.c b/lib/arm-ffa/arm_ffa_helper.c
+new file mode 100644
+index 000000000000..623899d38044
+--- /dev/null
++++ b/lib/arm-ffa/arm_ffa_helper.c
+@@ -0,0 +1,188 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <arm_ffa_helper.h>
++#include <uuid.h>
++
++/**
++ * ffa_helper_get_partitions_info - Wrapper function for FFA_PARTITION_INFO_GET
++ *
++ * @func_data: Pointer to the FF-A function arguments container
++ *				structure.
++ *				The passed arguments:
++ *					Mode 1: When getting from the driver the number of
++ *						secure partitions:
++ *						@data0_size: UUID size
++ *						@data0: pointer to the UUID (little endian)
++ *						@data1_size: size of the number of partitions
++ *									variable
++ *						@data1: pointer to the number of partitions
++ *								 variable. The variable will be set
++ *								 by the driver
++ *					Mode 2: When requesting the driver to return the
++ *						partitions information:
++ *						@data0_size: UUID size
++ *						@data0: pointer to the UUID (little endian)
++ *						@data1_size: size of the SPs information buffer
++ *						@data1: pointer to SPs information buffer
++ *								(allocated by the client).
++ *							The buffer will be filled by the driver
++ *
++ * This is the boot time function used by clients who wants to get from secure
++ * world the partition(s) information.
++ *
++ * A client of the FF-A driver should know the UUID of the service it wants to
++ * access. It should use the UUID to request the FF-A driver to provide the
++ * partition(s) information of the service. The client should use
++ * ffa_helper_get_partitions_info to pass the UUID information to the driver
++ * which uses PARTITION_INFO_GET to obtain the partition(s) information.
++ *
++ * ffa_helper_get_partitions_info should be called twice. First call is to get
++ * from the driver the number of secure partitions (SPs) associated to a
++ * particular UUID. Then, the caller (client) allocates the buffer to host the
++ * SPs data and issues a 2nd call. Then, the driver fills the SPs data in the
++ * pre-allocated buffer.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_get_partitions_info(struct ffa_interface_data *func_data)
++{
++	return ffa_get_invoke_func(FFA_PARTITION_INFO_GET, func_data);
++}
++
++/**
++ * ffa_helper_unmap_rxtx_buffers - Wrapper function for FFA_RXTX_UNMAP
++ *
++ * This is the boot time function that allows clients to unmap the RX/TX
++ * buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_unmap_rxtx_buffers(void)
++{
++	return ffa_get_invoke_func(FFA_RXTX_UNMAP, NULL);
++}
++
++/**
++ * ffa_helper_msg_send_direct_req - Wrapper function for
++ *								FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ * The passed arguments:
++ *	@data0_size: partition ID size
++ *	@data0: pointer to the partition ID
++ *	@data1_size: exchanged data size
++ *	@data1: pointer to the data buffer preallocated by the client (in/out)
++ *
++ * This is the runtime function that allows clients to send data to the secure
++ * world partitions. The arm_ffa driver uses FFA_MSG_SEND_DIRECT_REQ to send the
++ * data to the secure partition. The response from the secure partition is
++ * handled internally by the driver using FFA_MSG_SEND_DIRECT_RESP and returned
++ * to ffa_helper_msg_send_direct_req through @func_data
++ *
++ * The maximum size of the data that can be exchanged is 20 bytes which is
++ * sizeof(struct ffa_send_direct_data) as defined by the FF-A specification 1.0
++ * in the section relevant to FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ *
++ * The client should pre-allocate a buffer pointed by @data1 which the size
++ * is sizeof(struct ffa_send_direct_data)
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime ffa_helper_msg_send_direct_req(struct ffa_interface_data
++											 *func_data)
++{
++	return ffa_get_invoke_func(FFA_MSG_SEND_DIRECT_REQ, func_data);
++}
++
++/**
++ * ffa_helper_init_device - Wrapper function for probing the arm_ffa device
++ *
++ * This boot time function should be called to probe the arm_ffa device so
++ * it becomes ready for use.
++ * To achieve that, this function is called automatically at initcalls
++ * level (after u-boot relocation).
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_init_device(void)
++{
++	return ffa_init_device();
++}
++
++/**
++ * ffa_uuid_str_to_bin - Converts a big endian UUID string to a little endian buffer
++ * @uuid_str:	UUID string in big endian format (36 bytes wide + '/0')
++ * @uuid_bin:	preallocated 16 bytes UUID buffer in little endian format
++ *
++ * UUID binary format used by the FF-A framework (16 bytes):
++ *
++ * [LSB] 4B-2B-2B-2B-6B (little endian data fields)
++ *
++ * UUID string is 36 length of characters (36 bytes):
++ *
++ * xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx
++ * be                be      be       be      be
++ *
++ * where x is a hexadecimal character. Fields are separated by '-'s.
++ * When converting to a binary UUID, these endianness rules apply:
++ *     be: means the field in the string is considered a big endian hex number
++ *         and should be converted to little endian binary format
++ *
++ * Return:
++ *
++ *    uuid_bin filled with little endian UUID data
++ *    On success 0 is returned. Otherwise, failure code.
++ */
++int ffa_uuid_str_to_bin(const char *uuid_str, unsigned char *uuid_bin)
++{
++	u16 tmp16 = 0;
++	u32 tmp32 = 0;
++	u64 tmp64 = 0;
++
++	if (!uuid_str_valid(uuid_str) || !uuid_bin)
++		return -EINVAL;
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp32 = simple_strtoul(uuid_str, NULL, 16);
++	memcpy(uuid_bin, &tmp32, 4);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 9, NULL, 16);
++	memcpy(uuid_bin + 4, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 14, NULL, 16);
++	memcpy(uuid_bin + 6, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 19, NULL, 16);
++	memcpy(uuid_bin + 8, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp64 = simple_strtoull(uuid_str + 24, NULL, 16);
++	memcpy(uuid_bin + 10, (char *)&tmp64, 6);
++
++	return 0;
++}
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index 5bcb8253edba..cffa2c69d621 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -23,6 +23,10 @@
+ #include <asm/setjmp.h>
+ #include <linux/libfdt_env.h>
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++#include <arm_ffa_helper.h>
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ /* Task priority level */
+@@ -2114,6 +2118,10 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	struct efi_event *evt, *next_event;
+ 	efi_status_t ret = EFI_SUCCESS;
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++	int ffa_ret;
++#endif
++
+ 	EFI_ENTRY("%p, %zx", image_handle, map_key);
+ 
+ 	/* Check that the caller has read the current memory map */
+@@ -2174,6 +2182,15 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 		dm_remove_devices_flags(DM_REMOVE_ACTIVE_ALL);
+ 	}
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++	/* unmap FF-A RX/TX buffers */
++	ffa_ret = ffa_helper_unmap_rxtx_buffers();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: can not unmap FF-A RX/TX buffers\n");
++	else
++		debug("[efi_boottime][INFO]: FF-A RX/TX buffers unmapped\n");
++#endif
++
+ 	/* Patch out unsupported runtime function */
+ 	efi_runtime_detach();
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch
new file mode 100644
index 0000000..83f0547
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch
@@ -0,0 +1,342 @@
+From 541b2b51dc77832ab5845cab4762d29976838d36 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:36:27 +0000
+Subject: [PATCH 07/27] arm_ffa: introducing armffa command
+
+A new armffa command is provided as an example of how to use
+the FF-A helper functions to communicate with secure world.
+
+The armffa command allows to query secure partitions data from
+the secure world and exchanging messages with the partitions.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS  |   1 +
+ cmd/Kconfig  |  10 ++
+ cmd/Makefile |   2 +
+ cmd/armffa.c | 266 +++++++++++++++++++++++++++++++++++++++++++++++++++
+ 4 files changed, 279 insertions(+)
+ create mode 100644 cmd/armffa.c
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 14307e6da644..f3fd559da54a 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -235,6 +235,7 @@ F:	include/configs/turris_*.h
+ ARM FF-A
+ M:	Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+ S:	Maintained
++F:	cmd/armffa.c
+ F:	drivers/arm-ffa/
+ F:	include/arm_ffa.h
+ F:	include/arm_ffa_helper.h
+diff --git a/cmd/Kconfig b/cmd/Kconfig
+index ff50102a89c7..ff124bf4bad0 100644
+--- a/cmd/Kconfig
++++ b/cmd/Kconfig
+@@ -813,6 +813,16 @@ endmenu
+ 
+ menu "Device access commands"
+ 
++config CMD_ARMFFA
++	bool "Arm FF-A test command"
++	depends on ARM_FFA_TRANSPORT
++	help
++	  Provides a test command for the Arm FF-A driver
++	  supported options:
++		- Listing the partition(s) info
++		- Sending a data pattern to the specified partition
++		- Displaying the arm_ffa device info
++
+ config CMD_ARMFLASH
+ 	#depends on FLASH_CFI_DRIVER
+ 	bool "armflash"
+diff --git a/cmd/Makefile b/cmd/Makefile
+index 166c652d9825..770b846c44e0 100644
+--- a/cmd/Makefile
++++ b/cmd/Makefile
+@@ -12,6 +12,8 @@ obj-y += panic.o
+ obj-y += version.o
+ 
+ # command
++
++obj-$(CONFIG_CMD_ARMFFA) += armffa.o
+ obj-$(CONFIG_CMD_ACPI) += acpi.o
+ obj-$(CONFIG_CMD_ADDRMAP) += addrmap.o
+ obj-$(CONFIG_CMD_AES) += aes.o
+diff --git a/cmd/armffa.c b/cmd/armffa.c
+new file mode 100644
+index 000000000000..71a6ebb656d1
+--- /dev/null
++++ b/cmd/armffa.c
+@@ -0,0 +1,266 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <arm_ffa_helper.h>
++#include <asm/io.h>
++#include <common.h>
++#include <command.h>
++#include <dm.h>
++#include <mapmem.h>
++#include <stdlib.h>
++
++/**
++ * do_ffa_get_singular_partition_info - implementation of the getpart subcommand
++ * @cmdtp:		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function queries the secure partition information which the UUID is provided
++ * as an argument. The function uses the arm_ffa driver helper function
++ * to retrieve the data.
++ * The input UUID string is expected to be in big endian format.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++static int do_ffa_get_singular_partition_info(struct cmd_tbl *cmdtp, int flag, int argc,
++					      char *const argv[])
++{
++	struct ffa_interface_data func_data = {0};
++	u32 count = 0;
++	int ret;
++	union ffa_partition_uuid service_uuid = {0};
++	struct ffa_partition_info *parts_info;
++	u32 info_idx;
++
++	if (argc != 1)
++		return -EINVAL;
++
++	if (ffa_uuid_str_to_bin(argv[0], (unsigned char *)&service_uuid)) {
++		ffa_err("Invalid UUID");
++		return -EINVAL;
++	}
++
++	/*
++	 * get from the driver the count of the SPs matching the UUID
++	 */
++	func_data.data0_size = sizeof(service_uuid);
++	func_data.data0 = &service_uuid;
++	func_data.data1_size = sizeof(count);
++	func_data.data1 = &count;
++
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_err("Failure in querying partitions count (error code: %d)", ret);
++		return ret;
++	}
++
++	if (!count) {
++		ffa_info("No secure partition found");
++		return ret;
++	}
++
++	/*
++	 * pre-allocate a buffer to be filled by the driver
++	 * with	 ffa_partition_info structs
++	 */
++
++	parts_info = calloc(count, sizeof(struct ffa_partition_info));
++	if (!parts_info)
++		return -EINVAL;
++
++	ffa_info("Pre-allocating %d partition(s) info structures", count);
++
++	func_data.data1_size = count * sizeof(struct ffa_partition_info);
++	func_data.data1 = parts_info;
++
++	/*
++	 * ask the driver to fill the buffer with the SPs info
++	 */
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_err("Failure in querying partition(s) info (error code: %d)", ret);
++		free(parts_info);
++		return ret;
++	}
++
++	/*
++	 * SPs found , show the partition information
++	 */
++	for (info_idx = 0; info_idx < count ; info_idx++) {
++		ffa_info("Partition: id = 0x%x , exec_ctxt 0x%x , properties 0x%x",
++			 parts_info[info_idx].id,
++			 parts_info[info_idx].exec_ctxt,
++			 parts_info[info_idx].properties);
++	}
++
++	free(parts_info);
++
++	return 0;
++}
++
++/**
++ * do_ffa_msg_send_direct_req - implementation of the ping subcommand
++ * @cmdtp:		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function sends data to the secure partition which the ID is provided
++ * as an argument. The function uses the arm_ffa driver helper function
++ * to send data.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++int  do_ffa_msg_send_direct_req(struct cmd_tbl *cmdtp, int flag, int argc,
++				char *const argv[])
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u32 pattern = 0xaabbccd0;
++	u16 part_id;
++	int ret;
++
++	if (argc != 1)
++		return -EINVAL;
++
++	errno = 0;
++	part_id = strtoul(argv[0], NULL, 16);
++
++	if (errno) {
++		ffa_err("Invalid partition ID");
++		return -EINVAL;
++	}
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * filling the message data
++	 */
++	msg.a3 = ++pattern;
++	msg.a4 = ++pattern;
++	msg.a5 = ++pattern;
++	msg.a6 = ++pattern;
++	msg.a7 = ++pattern;
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	ret = ffa_helper_msg_send_direct_req(&func_data);
++	if (ret == FFA_ERR_STAT_SUCCESS) {
++		u8 cnt;
++
++		ffa_info("SP response:\n[LSB]");
++		for (cnt = 0;
++		     cnt < sizeof(struct ffa_send_direct_data) / sizeof(u32);
++		     cnt++)
++			ffa_info("0x%x", ((u32 *)&msg)[cnt]);
++	} else {
++		ffa_err("Sending direct request error (%d)", ret);
++	}
++
++	return ret;
++}
++
++/**
++ *do_ffa_dev_list - implementation of the devlist subcommand
++ * @cmdtp: [in]		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function queries the devices belonging to the UCLASS_FFA
++ * class. Currently, one device is expected to show up: the arm_ffa device
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++int do_ffa_dev_list(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
++{
++	struct udevice *dev = NULL;
++	int i, ret;
++
++	ffa_info("arm_ffa uclass entries:");
++
++	for (i = 0, ret = uclass_first_device(UCLASS_FFA, &dev);
++	     dev;
++	     ret = uclass_next_device(&dev), i++) {
++		if (ret)
++			break;
++
++		ffa_info("entry %d - instance %08x, ops %08x, plat %08x",
++			 i,
++			 (u32)map_to_sysmem(dev),
++			 (u32)map_to_sysmem(dev->driver->ops),
++			 (u32)map_to_sysmem(dev_get_plat(dev)));
++	}
++
++	return cmd_process_error(cmdtp, ret);
++}
++
++static struct cmd_tbl armffa_commands[] = {
++	U_BOOT_CMD_MKENT(getpart, 1, 1, do_ffa_get_singular_partition_info, "", ""),
++	U_BOOT_CMD_MKENT(ping, 1, 1, do_ffa_msg_send_direct_req, "", ""),
++	U_BOOT_CMD_MKENT(devlist, 0, 1, do_ffa_dev_list, "", ""),
++};
++
++/**
++ * do_armffa - the armffa command main function
++ * @cmdtp:	Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function identifies which armffa subcommand to run.
++ * Then, it makes sure the arm_ffa device is probed and
++ * ready for use.
++ * Then, it runs the subcommand.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++static int do_armffa(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
++{
++	struct cmd_tbl *armffa_cmd;
++	int ret;
++
++	if (argc < 2)
++		return CMD_RET_USAGE;
++
++	armffa_cmd = find_cmd_tbl(argv[1], armffa_commands, ARRAY_SIZE(armffa_commands));
++
++	argc -= 2;
++	argv += 2;
++
++	if (!armffa_cmd || argc > armffa_cmd->maxargs)
++		return CMD_RET_USAGE;
++
++	ret = ffa_helper_init_device();
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return cmd_process_error(cmdtp, ret);
++
++	ret = armffa_cmd->cmd(armffa_cmd, flag, argc, argv);
++
++	return cmd_process_error(armffa_cmd, ret);
++}
++
++U_BOOT_CMD(armffa, 4, 1, do_armffa,
++	   "Arm FF-A operations test command",
++	   "getpart <partition UUID>\n"
++	   "	 - lists the partition(s) info\n"
++	   "ping <partition ID>\n"
++	   "	 - sends a data pattern to the specified partition\n"
++	   "devlist\n"
++	   "	 - displays the arm_ffa device info\n");
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch
new file mode 100644
index 0000000..9b1383e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch
@@ -0,0 +1,383 @@
+From 2f09d4a2e87febd7365b9e18d669208ff2c35edc Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Wed, 13 Oct 2021 17:51:44 +0100
+Subject: [PATCH 08/27] arm_ffa: introducing MM communication with FF-A
+
+This commit allows to perform MM communication using FF-A transport.
+
+The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
+Both partitions run in OP-TEE.
+
+When using the u-boot FF-A driver, StandAlonneMM and smm-gateway are
+supported.
+
+On EFI services such as GetVariable()/SetVariable(), the data
+is copied from the communication buffer to the MM shared buffer.
+
+Then, notifies the MM SP about data availability in the MM shared buffer.
+Communication with the MM SP is performed using FF-A transport.
+
+On such event, MM SP can read the data and updates the MM shared buffer
+with response data.
+
+The response data is copied back to the communication buffer.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/Kconfig            |  14 +-
+ lib/efi_loader/efi_variable_tee.c | 265 +++++++++++++++++++++++++++++-
+ 2 files changed, 273 insertions(+), 6 deletions(-)
+
+diff --git a/lib/efi_loader/Kconfig b/lib/efi_loader/Kconfig
+index e5e35fe51f65..6827b821545e 100644
+--- a/lib/efi_loader/Kconfig
++++ b/lib/efi_loader/Kconfig
+@@ -56,13 +56,23 @@ config EFI_VARIABLE_FILE_STORE
+ 	  stored as file /ubootefi.var on the EFI system partition.
+ 
+ config EFI_MM_COMM_TEE
+-	bool "UEFI variables storage service via OP-TEE"
+-	depends on OPTEE
++	bool "UEFI variables storage service via the trusted world"
++	depends on OPTEE || ARM_FFA_TRANSPORT
+ 	help
++	  The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
++	  When using the u-boot OP-TEE driver, StandAlonneMM is supported.
++	  When using the u-boot FF-A  driver, StandAlonneMM and smm-gateway are supported.
++
+ 	  If OP-TEE is present and running StandAloneMM, dispatch all UEFI
+ 	  variable related operations to that. The application will verify,
+ 	  authenticate and store the variables on an RPMB.
+ 
++	  When ARM_FFA_TRANSPORT is used, dispatch all UEFI variable related
++	  operations to the MM SP running under Optee in the trusted world.
++	  A door bell mechanism is used to notify the SP when there is data in the shared
++	  MM buffer. The data is copied by u-boot to thea shared buffer before issuing
++	  the door bell event.
++
+ endchoice
+ 
+ config EFI_VARIABLES_PRESEED
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index dfef18435dfa..9cb8cfb9c779 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -15,6 +15,28 @@
+ #include <malloc.h>
+ #include <mm_communication.h>
+ 
++#if (IS_ENABLED(CONFIG_OPTEE))
++#define OPTEE_PAGE_SIZE BIT(12)
++#endif
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++#include <arm_ffa_helper.h>
++#include <mapmem.h>
++
++/* MM return codes */
++#define MM_SUCCESS (0)
++
++#define ARM_SVC_ID_SP_EVENT_COMPLETE_AARCH64 (0xC4000061)
++#define ARM_SVC_ID_SP_EVENT_COMPLETE ARM_SVC_ID_SP_EVENT_COMPLETE_AARCH64
++
++/* MM_SP_UUID_DATA defined by the platform */
++union ffa_partition_uuid mm_sp_svc_uuid = {.bytes = {MM_SP_UUID_DATA}};
++
++static u16 __efi_runtime_data mm_sp_id;
++
++#endif
++
+ extern struct efi_var_file __efi_runtime_data *efi_var_buf;
+ static efi_uintn_t max_buffer_size;	/* comm + var + func + data */
+ static efi_uintn_t max_payload_size;	/* func + data */
+@@ -24,6 +46,7 @@ struct mm_connection {
+ 	u32 session;
+ };
+ 
++#if (IS_ENABLED(CONFIG_OPTEE))
+ /**
+  * get_connection() - Retrieve OP-TEE session for a specific UUID.
+  *
+@@ -143,16 +166,229 @@ static efi_status_t optee_mm_communicate(void *comm_buf, ulong dsize)
+ 
+ 	return ret;
+ }
++#endif
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++/**
++ * ffa_notify_mm_sp() - Announce there is data in the shared buffer
++ *
++ * Notifies the MM partition in the trusted world that
++ * data is available in the shared buffer.
++ * This is a blocking call during which trusted world has exclusive access
++ * to the MM shared buffer.
++ *
++ * Return:
++ *
++ * 0 on success
++ */
++static int __efi_runtime ffa_notify_mm_sp(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	int ret;
++	u32 sp_event_complete;
++	int sp_event_ret;
++
++	func_data.data0_size = sizeof(mm_sp_id);
++	func_data.data0 = &mm_sp_id;
++
++	msg.a3 = FFA_SHARED_MM_BUFFER_ADDR;
++	msg.a4 = FFA_SHARED_MM_BUFFER_SIZE;
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	ret = ffa_helper_msg_send_direct_req(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure to notify the MM SP , FF-A  error (%d)\n", ret);
++		return ret;
++	}
++
++	sp_event_complete = msg.a3;
++	sp_event_ret = (int)msg.a4;
++
++	if (sp_event_complete == ARM_SVC_ID_SP_EVENT_COMPLETE && sp_event_ret == MM_SUCCESS)
++		return 0;
++
++	log_err("EFI: Failure to notify the MM SP (0x%x , %d)\n",
++		sp_event_complete,
++		sp_event_ret);
++
++	return -EACCES;
++}
++
++/**
++ * ffa_discover_mm_sp_id() - Query the MM partition ID
++ *
++ * Use the FF-A driver to get the MM partition ID.
++ * If multiple partitions are found, use the first one
++ *
++ * Return:
++ *
++ * 0 on success
++ */
++static int __efi_runtime ffa_discover_mm_sp_id(void)
++{
++	struct ffa_interface_data func_data = {0};
++	u32 count = 0;
++	int ret;
++	struct ffa_partition_info *parts_info;
++
++	/*
++	 * get from the driver the count of the SPs matching the UUID
++	 */
++	func_data.data0_size = sizeof(mm_sp_svc_uuid);
++	func_data.data0 = &mm_sp_svc_uuid;
++	func_data.data1_size = sizeof(count);
++	func_data.data1 = &count;
++
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure in querying partitions count (error code: %d)\n", ret);
++		return ret;
++	}
++
++	if (!count) {
++		log_info("EFI: No MM partition found\n");
++		return ret;
++	}
++
++	/*
++	 * pre-allocate a buffer to be filled by the driver
++	 * with	 ffa_partition_info structs
++	 */
++
++	parts_info = calloc(count, sizeof(struct ffa_partition_info));
++	if (!parts_info)
++		return -EINVAL;
++
++	log_info("EFI: Pre-allocating %d partition(s) info structures\n", count);
++
++	func_data.data1_size = count *
++		sizeof(struct ffa_partition_info);
++	func_data.data1 = parts_info;
++
++	/*
++	 * ask the driver to fill the
++	 * buffer with the SPs info
++	 */
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure in querying partition(s) info (error code: %d)\n", ret);
++		free(parts_info);
++		return ret;
++	}
++
++	/*
++	 * MM SPs found , use the first one
++	 */
++
++	mm_sp_id = parts_info[0].id;
++
++	log_info("EFI: MM partition ID 0x%x\n", mm_sp_id);
++
++	free(parts_info);
++
++	return 0;
++}
+ 
+ /**
+- * mm_communicate() - Adjust the cmonnucation buffer to StandAlonneMM and send
++ * ffa_mm_communicate() - Exchange EFI services data with  the MM partition using FF-A
++ * @comm_buf:		locally allocated communication buffer used for for rx/tx
++ * @dsize:				communication buffer size
++ *
++ * Issues a door bell event to notify the MM partition (SP) running in OP-TEE
++ * that there is data to read from the shared buffer.
++ * Communication with the MM SP is performed using FF-A transport.
++ * On the event, MM SP can read the data from the buffer and
++ * update the MM shared buffer with response data.
++ * The response data is copied back to the communication buffer.
++ *
++ * Return:
++ *
++ * EFI status code
++ */
++static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_buf_size)
++{
++	ulong tx_data_size;
++	int ffa_ret;
++	struct efi_mm_communicate_header *mm_hdr;
++	void *virt_shared_buf;
++
++	if (!comm_buf)
++		return EFI_INVALID_PARAMETER;
++
++	/* Discover MM partition ID */
++	if (!mm_sp_id && ffa_discover_mm_sp_id()  != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure to discover MM partition ID\n");
++		return EFI_UNSUPPORTED;
++	}
++
++	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
++	tx_data_size = mm_hdr->message_len + sizeof(efi_guid_t) + sizeof(size_t);
++
++	if (comm_buf_size != tx_data_size || tx_data_size > FFA_SHARED_MM_BUFFER_SIZE)
++		return EFI_INVALID_PARAMETER;
++
++	/* Copy the data to the shared buffer */
++
++	virt_shared_buf = (void *)map_sysmem((phys_addr_t)FFA_SHARED_MM_BUFFER_ADDR, 0);
++	efi_memcpy_runtime(virt_shared_buf, comm_buf, tx_data_size);
++
++	/* Announce there is data in the shared buffer */
++
++	ffa_ret = ffa_notify_mm_sp();
++	if (ffa_ret)
++		unmap_sysmem(virt_shared_buf);
++
++	switch (ffa_ret) {
++	case 0:
++	{
++		ulong rx_data_size;
++		/* Copy the MM SP response from the shared buffer to the communication buffer */
++		rx_data_size = ((struct efi_mm_communicate_header *)virt_shared_buf)->message_len +
++			sizeof(efi_guid_t) +
++			sizeof(size_t);
++
++		if (rx_data_size > comm_buf_size) {
++			unmap_sysmem(virt_shared_buf);
++			return EFI_OUT_OF_RESOURCES;
++		}
++
++		efi_memcpy_runtime(comm_buf, virt_shared_buf, rx_data_size);
++		unmap_sysmem(virt_shared_buf);
++
++		return EFI_SUCCESS;
++	}
++	case -EINVAL:
++		return EFI_DEVICE_ERROR;
++	case -EPERM:
++		return EFI_INVALID_PARAMETER;
++	case -EACCES:
++		return EFI_ACCESS_DENIED;
++	case -EBUSY:
++		return EFI_OUT_OF_RESOURCES;
++	default:
++		return EFI_ACCESS_DENIED;
++	}
++}
++#endif
++
++/**
++ * mm_communicate() - Adjust the communication buffer to the MM SP and send
+  * it to OP-TEE
+  *
+- * @comm_buf:		locally allocted communcation buffer
++ * @comm_buf:		locally allocted communication buffer
+  * @dsize:		buffer size
++ *
++ * The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
++ * The comm_buf format is the same for both partitions.
++ * When using the u-boot OP-TEE driver, StandAlonneMM is supported.
++ * When using the u-boot FF-A  driver, StandAlonneMM and smm-gateway are supported.
++ *
+  * Return:		status code
+  */
+-static efi_status_t mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
++static efi_status_t __efi_runtime mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
+ {
+ 	efi_status_t ret;
+ 	struct efi_mm_communicate_header *mm_hdr;
+@@ -162,7 +398,11 @@ static efi_status_t mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
+ 	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
+ 	var_hdr = (struct smm_variable_communicate_header *)mm_hdr->data;
+ 
++	#if (IS_ENABLED(CONFIG_OPTEE))
+ 	ret = optee_mm_communicate(comm_buf, dsize);
++	#elif (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++	ret = ffa_mm_communicate(comm_buf, dsize);
++	#endif
+ 	if (ret != EFI_SUCCESS) {
+ 		log_err("%s failed!\n", __func__);
+ 		return ret;
+@@ -258,6 +498,23 @@ efi_status_t EFIAPI get_max_payload(efi_uintn_t *size)
+ 		goto out;
+ 	}
+ 	*size = var_payload->size;
++
++	#if (IS_ENABLED(CONFIG_OPTEE))
++	/*
++	 * Although the max payload is configurable on StMM, we only share a
++	 * single page from OP-TEE for the non-secure buffer used to communicate
++	 * with StMM. Since OP-TEE will reject to map anything bigger than that,
++	 * make sure we are in bounds.
++	 */
++	if (*size > OPTEE_PAGE_SIZE)
++		*size = OPTEE_PAGE_SIZE - MM_COMMUNICATE_HEADER_SIZE  -
++			MM_VARIABLE_COMMUNICATE_SIZE;
++	#elif (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++		if (*size > FFA_SHARED_MM_BUFFER_SIZE)
++			*size = FFA_SHARED_MM_BUFFER_SIZE - MM_COMMUNICATE_HEADER_SIZE  -
++				MM_VARIABLE_COMMUNICATE_SIZE;
++	#endif
++
+ 	/*
+ 	 * There seems to be a bug in EDK2 miscalculating the boundaries and
+ 	 * size checks, so deduct 2 more bytes to fulfill this requirement. Fix
+@@ -697,7 +954,7 @@ void efi_variables_boot_exit_notify(void)
+ 		ret = EFI_NOT_FOUND;
+ 
+ 	if (ret != EFI_SUCCESS)
+-		log_err("Unable to notify StMM for ExitBootServices\n");
++		log_err("Unable to notify the MM partition for ExitBootServices\n");
+ 	free(comm_buf);
+ 
+ 	/*
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch
new file mode 100644
index 0000000..4fb317b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch
@@ -0,0 +1,132 @@
+From c9a2c457648b732292482fae59a7fd61cefffd33 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:38:48 +0000
+Subject: [PATCH 09/27] arm_ffa: introducing test module for UCLASS_FFA
+
+This is the test module for the UCLASS_FFA class.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS      |  1 +
+ test/dm/Makefile |  1 +
+ test/dm/ffa.c    | 56 ++++++++++++++++++++++++++++++++++++++++++++++++
+ test/dm/ffa.h    | 19 ++++++++++++++++
+ 4 files changed, 77 insertions(+)
+ create mode 100644 test/dm/ffa.c
+ create mode 100644 test/dm/ffa.h
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index f3fd559da54a..6510f844fe09 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -240,6 +240,7 @@ F:	drivers/arm-ffa/
+ F:	include/arm_ffa.h
+ F:	include/arm_ffa_helper.h
+ F:	lib/arm-ffa/
++F:	test/dm/ffa.c
+ 
+ ARM FREESCALE IMX
+ M:	Stefano Babic <sbabic@denx.de>
+diff --git a/test/dm/Makefile b/test/dm/Makefile
+index d46552fbf320..ddac250cdff0 100644
+--- a/test/dm/Makefile
++++ b/test/dm/Makefile
+@@ -79,6 +79,7 @@ obj-$(CONFIG_POWER_DOMAIN) += power-domain.o
+ obj-$(CONFIG_ACPI_PMC) += pmc.o
+ obj-$(CONFIG_DM_PMIC) += pmic.o
+ obj-$(CONFIG_DM_PWM) += pwm.o
++obj-$(CONFIG_ARM_FFA_TRANSPORT) += ffa.o
+ obj-$(CONFIG_QFW) += qfw.o
+ obj-$(CONFIG_RAM) += ram.o
+ obj-y += regmap.o
+diff --git a/test/dm/ffa.c b/test/dm/ffa.c
+new file mode 100644
+index 000000000000..b937cea57b80
+--- /dev/null
++++ b/test/dm/ffa.c
+@@ -0,0 +1,56 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * Test for UCLASS_FFA  class
++ *
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/test.h>
++#include <test/test.h>
++#include <test/ut.h>
++#include <arm_ffa_helper.h>
++#include "ffa.h"
++
++/* Basic test of 'armffa' command */
++static int dm_test_armffa_cmd(struct unit_test_state *uts)
++{
++	ut_assertok(ffa_helper_init_device());
++
++	ut_assertok(console_record_reset_enable());
++
++	/* armffa getpart <UUID> */
++	ut_assertok(run_command("armffa getpart " SE_PROXY_PARTITION_UUID, 0));
++	ut_assert_console_end();
++
++	/* armffa ping <ID> */
++	ut_assertok(run_command("armffa ping " SE_PROXY_PARTITION_ID, 0));
++	ut_assert_console_end();
++
++	/* armffa devlist */
++	ut_assertok(run_command("armffa devlist", 0));
++	ut_assert_console_end();
++
++	return 0;
++}
++
++DM_TEST(dm_test_armffa_cmd, UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT | UT_TESTF_CONSOLE_REC);
++
++static int  test_ffa_msg_send_direct_req(void)
++{
++	char *const argv[1] = {SE_PROXY_PARTITION_ID}; /* Corstone1000 SE Proxy ID */
++
++	return do_ffa_msg_send_direct_req(NULL, 0, 1, argv);
++}
++
++/* Basic test of the FFA uclass */
++static int dm_test_ffa_uclass(struct unit_test_state *uts)
++{
++	ut_assertok(ffa_init_device());
++	ut_assertok(test_ffa_msg_send_direct_req());
++	return 0;
++}
++
++DM_TEST(dm_test_ffa_uclass, UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
+diff --git a/test/dm/ffa.h b/test/dm/ffa.h
+new file mode 100644
+index 000000000000..a0802bd6928a
+--- /dev/null
++++ b/test/dm/ffa.h
+@@ -0,0 +1,19 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __TEST_DM_FFA_H
++#define __TEST_DM_FFA_H
++
++#define SE_PROXY_PARTITION_ID "0x8002"
++#define SE_PROXY_PARTITION_UUID "46bb39d1-b4d9-45b5-88ff-040027dab249"
++
++/**
++ * do_ffa_msg_send_direct_req - implementation of the ping subcommand
++ */
++int  do_ffa_msg_send_direct_req(struct cmd_tbl *cmdtp, int flag, int argc,
++				char *const argv[]);
++
++#endif /*__TEST_DM_FFA_H */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch
new file mode 100644
index 0000000..bc96fc4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch
@@ -0,0 +1,57 @@
+From ce6598d255113458fd5c9d19bb7469b721e37f6f Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 2 Nov 2021 16:44:39 +0000
+Subject: [PATCH 10/27] arm_ffa: corstone1000: enable FF-A and MM support
+
+This commit allows corstone1000 platform to perform
+MM communication between u-boot and the secure world
+using FF-A transport.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ configs/corstone1000_defconfig |  1 +
+ include/configs/corstone1000.h | 15 +++++++++++++++
+ 2 files changed, 16 insertions(+)
+
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index e573fe6fe6a2..b042d4e49419 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -44,6 +44,7 @@ CONFIG_USB=y
+ CONFIG_DM_USB=y
+ CONFIG_USB_ISP1760=y
+ CONFIG_USB_STORAGE=y
++CONFIG_ARM_FFA_TRANSPORT=y
+ CONFIG_EFI_MM_COMM_TEE=y
+ # CONFIG_OPTEE is not set
+ # CONFIG_GENERATE_SMBIOS_TABLE is not set
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index 8ba0effb0ab2..afc9ccfc192b 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -14,6 +14,21 @@
+ 
+ #include <linux/sizes.h>
+ 
++/* MM SP UUID binary data (little-endian format) */
++#define MM_SP_UUID_DATA	\
++	0xed, 0x32, 0xd5, 0x33,	\
++	0x99, 0xe6, 0x42, 0x09,	\
++	0x9c, 0xc0, 0x2d, 0x72,	\
++	0xcd, 0xd9, 0x98, 0xa7
++
++#define FFA_SHARED_MM_BUFFER_SIZE	SZ_4K /* 4 KB */
++
++/*
++ * shared buffer physical address used for communication between
++ * u-boot and the MM SP
++ */
++#define FFA_SHARED_MM_BUFFER_ADDR	(0x023F8000)
++
+ #define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
+ #define CONFIG_SKIP_LOWLEVEL_INIT
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch
new file mode 100644
index 0000000..cb24ec3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch
@@ -0,0 +1,359 @@
+From e70d0128090158872847b82b82cdbcf0e2f13885 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Thu, 11 Nov 2021 16:27:59 +0000
+Subject: [PATCH 11/27] efi: corstone1000: introduce EFI capsule update
+
+This commit provides capsule update feature for Corstone1000.
+
+This feature is available before and after ExitBootServices().
+
+A platform specific capsule buffer is allocated. This buffer
+is physically contiguous and allocated at the start of the DDR
+memory after u-boot relocation to the end of DDR.
+
+The capsule buffer is shared between u-boot and the secure world.
+On UpdateCapsule() , capsule data is copied to the buffer
+and a buffer ready event is generated using FF-A transport.
+
+On ExitBootServices() a kernel started event is sent to the
+SE Proxy FW update service. This event is generated on each boot.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  18 +++++
+ include/efi_loader.h           |   4 +-
+ lib/efi_loader/efi_boottime.c  |  47 ++++++++++++
+ lib/efi_loader/efi_capsule.c   | 135 ++++++++++++++++++++++++++++++++-
+ lib/efi_loader/efi_setup.c     |  15 ++++
+ 5 files changed, 215 insertions(+), 4 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index afc9ccfc192b..a400cdef69d0 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -14,6 +14,24 @@
+ 
+ #include <linux/sizes.h>
+ 
++/* The SE Proxy partition ID*/
++#define CORSTONE1000_SEPROXY_PART_ID		(0x8002)
++
++/* Update service ID provided by  the SE Proxy secure partition*/
++#define CORSTONE1000_SEPROXY_UPDATE_SVC_ID	(0x4)
++
++/* Notification events used with SE Proxy update service */
++#define CORSTONE1000_BUFFER_READY_EVT		(0x1)
++#define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
++
++/* Size in 4KB pages of the EFI capsule buffer */
++#define CORSTONE1000_CAPSULE_BUFFER_SIZE	(8192) /* 32 MB */
++
++/* Capsule GUID */
++#define EFI_CORSTONE1000_CAPSULE_ID_GUID \
++	EFI_GUID(0x3a770ddc, 0x409b, 0x48b2, 0x81, 0x41, \
++		 0x93, 0xb7, 0xc6, 0x0b, 0x20, 0x9e)
++
+ /* MM SP UUID binary data (little-endian format) */
+ #define MM_SP_UUID_DATA	\
+ 	0xed, 0x32, 0xd5, 0x33,	\
+diff --git a/include/efi_loader.h b/include/efi_loader.h
+index 126db279dd3e..01b432e6184b 100644
+--- a/include/efi_loader.h
++++ b/include/efi_loader.h
+@@ -965,11 +965,11 @@ extern const struct efi_firmware_management_protocol efi_fmp_fit;
+ extern const struct efi_firmware_management_protocol efi_fmp_raw;
+ 
+ /* Capsule update */
+-efi_status_t EFIAPI efi_update_capsule(
++efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 scatter_gather_list);
+-efi_status_t EFIAPI efi_query_capsule_caps(
++efi_status_t __efi_runtime EFIAPI efi_query_capsule_caps(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 *maximum_capsule_size,
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index cffa2c69d621..5c77a40c3ebe 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -2096,6 +2096,44 @@ static void efi_exit_caches(void)
+ #endif
+ }
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_kernel_started_event - notifies SE Proxy FW update service
++ *
++ * This function notifies the SE Proxy update service that the kernel has already started
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++static int efi_corstone1000_kernel_started_event(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * setting the kernel started  event arguments
++	 */
++	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
++	msg.a5 = CORSTONE1000_KERNEL_STARTED_EVT;
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++
++#endif
++
+ /**
+  * efi_exit_boot_services() - stop all boot services
+  * @image_handle: handle of the loaded image
+@@ -2209,6 +2247,15 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	/* Recalculate CRC32 */
+ 	efi_update_table_header_crc32(&systab.hdr);
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	/* Notifying SE Proxy FW update service */
++	ffa_ret = efi_corstone1000_kernel_started_event();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
++	else
++		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
++#endif
++
+ 	/* Give the payload some time to boot */
+ 	efi_set_watchdog(0);
+ 	WATCHDOG_RESET();
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index f00440163d41..c100c1b95298 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -24,6 +24,14 @@
+ #include <crypto/pkcs7_parser.h>
+ #include <linux/err.h>
+ 
++#ifdef CONFIG_TARGET_CORSTONE1000
++#include <arm_ffa_helper.h>
++#include <cpu_func.h>
++
++void *__efi_runtime_data corstone1000_capsule_buf; /* capsule shared buffer virtual address */
++efi_guid_t corstone1000_capsule_guid = EFI_CORSTONE1000_CAPSULE_ID_GUID;
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ const efi_guid_t efi_guid_capsule_report = EFI_CAPSULE_REPORT_GUID;
+@@ -509,6 +517,89 @@ static efi_status_t efi_capsule_update_firmware(
+ }
+ #endif /* CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT */
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++
++/**
++ * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
++ * @capsule_image_size:	The capsule data (header + payload)
++ *
++ * This function allocates the physically contiguous buffer shared between u-boot
++ * and the secure world. On UpdateCapsule() capsule data is copied to the buffer
++ * and a door bell event is generated.
++ * The buffer is allocated at the start of the DDR memory after u-boot has been relocated
++ * to the end of DDR.
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++efi_status_t efi_corstone1000_alloc_capsule_shared_buf(void)
++{
++	efi_status_t efi_ret;
++	u64 ram_base = CONFIG_SYS_SDRAM_BASE;
++
++	log_debug("[%s]\n", __func__);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ADDRESS,
++				     EFI_RUNTIME_SERVICES_DATA,
++				     CORSTONE1000_CAPSULE_BUFFER_SIZE,
++				     &ram_base);
++
++	if (efi_ret != EFI_SUCCESS) {
++		corstone1000_capsule_buf = NULL;
++		log_err("EFI: Corstone1000: Allocating capsule shared buffer error (%d)\n"
++			, (int)efi_ret);
++		return efi_ret;
++	}
++
++	log_info("EFI: Corstone1000: Capsule shared buffer at 0x%x , size %d pages\n"
++		, (unsigned int)ram_base,
++		CORSTONE1000_CAPSULE_BUFFER_SIZE);
++
++	corstone1000_capsule_buf = (void *)map_sysmem((phys_addr_t)ram_base, 0);
++
++	return EFI_SUCCESS;
++}
++
++/**
++ * efi_corstone1000_buffer_ready_event - issue door bell event
++ * @capsule_image_size:	The capsule data (header + payload)
++ *
++ * This function notifies the SE Proxy update service that capsule data is available
++ * in the capsule shared buffer.
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++static int __efi_runtime efi_corstone1000_buffer_ready_event(u32 capsule_image_size)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * setting the buffer ready event arguments
++	 */
++	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
++	msg.a4 = capsule_image_size;
++	msg.a5 = CORSTONE1000_BUFFER_READY_EVT;
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++#endif
++
+ /**
+  * efi_update_capsule() - process information from operating system
+  * @capsule_header_array:	Array of virtual address pointers
+@@ -522,7 +613,7 @@ static efi_status_t efi_capsule_update_firmware(
+  *
+  * Return:			status code
+  */
+-efi_status_t EFIAPI efi_update_capsule(
++efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 scatter_gather_list)
+@@ -539,6 +630,13 @@ efi_status_t EFIAPI efi_update_capsule(
+ 		goto out;
+ 	}
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++		if (capsule_count != 1 || !corstone1000_capsule_buf) {
++			ret = EFI_INVALID_PARAMETER;
++			goto out;
++		}
++#endif
++
+ 	ret = EFI_SUCCESS;
+ 	for (i = 0, capsule = *capsule_header_array; i < capsule_count;
+ 	     i++, capsule = *(++capsule_header_array)) {
+@@ -551,6 +649,39 @@ efi_status_t EFIAPI efi_update_capsule(
+ 
+ 		log_debug("Capsule[%d] (guid:%pUs)\n",
+ 			  i, &capsule->capsule_guid);
++
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++		if (guidcmp(&corstone1000_capsule_guid, &capsule->capsule_guid)) {
++			ret = EFI_INVALID_PARAMETER;
++			log_err("Corstone1000: Invalid capsule GUID\n");
++			goto out;
++		}
++
++		if (efi_size_in_pages(capsule->capsule_image_size) >
++		    CORSTONE1000_CAPSULE_BUFFER_SIZE) {
++			log_err("Corstone1000: Capsule data size exceeds the shared buffer size\n");
++			ret = EFI_BUFFER_TOO_SMALL;
++			goto out;
++		}
++
++		/* copy the data to the contiguous buffer */
++		efi_memcpy_runtime(corstone1000_capsule_buf, capsule, capsule->capsule_image_size);
++
++		/* invalidate the data cache */
++		invalidate_dcache_all();
++
++		/* issue buffer ready event */
++		ret = efi_corstone1000_buffer_ready_event(capsule->capsule_image_size);
++		if (ret) {
++			log_err("EFI: Corstone1000: Buffer ready event error (%d)\n", (int)ret);
++			ret = EFI_DEVICE_ERROR;
++		} else {
++			ret = EFI_SUCCESS;
++		}
++
++		goto out;
++#endif
++
+ 		if (!guidcmp(&capsule->capsule_guid,
+ 			     &efi_guid_firmware_management_capsule_id)) {
+ 			ret  = efi_capsule_update_firmware(capsule);
+@@ -589,7 +720,7 @@ out:
+  *
+  * Return:			status code
+  */
+-efi_status_t EFIAPI efi_query_capsule_caps(
++efi_status_t __efi_runtime EFIAPI efi_query_capsule_caps(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 *maximum_capsule_size,
+diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
+index eee54e48784f..989380d4f8cd 100644
+--- a/lib/efi_loader/efi_setup.c
++++ b/lib/efi_loader/efi_setup.c
+@@ -16,6 +16,13 @@
+ 
+ efi_status_t efi_obj_list_initialized = OBJ_LIST_NOT_INITIALIZED;
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
++ */
++extern efi_status_t efi_corstone1000_alloc_capsule_shared_buf(void);
++#endif
++
+ /*
+  * Allow unaligned memory access.
+  *
+@@ -128,6 +135,14 @@ static efi_status_t efi_init_capsule(void)
+ {
+ 	efi_status_t ret = EFI_SUCCESS;
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	ret = efi_corstone1000_alloc_capsule_shared_buf();
++	if (ret != EFI_SUCCESS) {
++		printf("EFI: Corstone-1000: cannot allocate caspsule shared buffer\n");
++		return ret;
++	}
++#endif
++
+ 	if (IS_ENABLED(CONFIG_EFI_HAVE_CAPSULE_UPDATE)) {
+ 		ret = efi_set_variable_int(u"CapsuleMax",
+ 					   &efi_guid_capsule_report,
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch
new file mode 100644
index 0000000..60dc850
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch
@@ -0,0 +1,37 @@
+From b2d752b4bbd5b2dc4cb22d2d652a261287505926 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 17 Nov 2021 15:28:06 +0000
+Subject: [PATCH 12/27] corstone1000: Update FFA shared buffer address
+
+FFA shared buffer address changed to 0x02000000.
+
+The existing address 0x023F8000 is currently being used by
+Optee so the virtual address returned to the SMM gateway is 0x0000.
+So the buffer is moved to 0x02000000.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+%% original patch: 0025-Update-FFA-shared-buffer-address.patch
+
+%% original patch: 0025-Update-FFA-shared-buffer-address.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index a400cdef69d0..db0f91335cef 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -45,7 +45,7 @@
+  * shared buffer physical address used for communication between
+  * u-boot and the MM SP
+  */
+-#define FFA_SHARED_MM_BUFFER_ADDR	(0x023F8000)
++#define FFA_SHARED_MM_BUFFER_ADDR	(0x02000000)
+ 
+ #define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
+ #define CONFIG_SKIP_LOWLEVEL_INIT
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch
new file mode 100644
index 0000000..2495538
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch
@@ -0,0 +1,52 @@
+From 67a755f74716068cfd44a8897c31151fe9ee4328 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Thu, 18 Nov 2021 16:42:59 +0000
+Subject: [PATCH 13/27] corstone1000: Make sure shared buffer contents are not
+ cached
+
+After updating the shared buffer, it is required to flush the cache
+to ensure that the secure world sees expected the shared buffer
+contents.
+
+The MM communication shared buffer is configured in device region of optee
+which has cache disabled. So we need to invalidate the cache every time we
+update the buffer on uboot otherwise the secure world does not see the
+accurate values.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+%% original patch: 0027-Make-sure-shared-buffer-contents-are-not-cached.patch
+
+%% original patch: 0027-Make-sure-shared-buffer-contents-are-not-cached.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 6 ++++++
+ 1 file changed, 6 insertions(+)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 9cb8cfb9c779..b6be2b54a030 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -22,6 +22,7 @@
+ #if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
+ 
+ #include <arm_ffa_helper.h>
++#include <cpu_func.h>
+ #include <mapmem.h>
+ 
+ /* MM return codes */
+@@ -335,6 +336,11 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 	virt_shared_buf = (void *)map_sysmem((phys_addr_t)FFA_SHARED_MM_BUFFER_ADDR, 0);
+ 	efi_memcpy_runtime(virt_shared_buf, comm_buf, tx_data_size);
+ 
++	/* The secure world has cache disabled for device region which we use for shared buffer
++	So, the secure world reads the data from DDR. Let's flush the cache so the DDR is
++	updated with the latest data */
++	invalidate_dcache_all();
++
+ 	/* Announce there is data in the shared buffer */
+ 
+ 	ffa_ret = ffa_notify_mm_sp();
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch
new file mode 100644
index 0000000..fa201eb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch
@@ -0,0 +1,30 @@
+From e2463e3ef52260b38131085c0901de8708d52693 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Fri, 4 Mar 2022 15:56:09 +0000
+Subject: [PATCH 14/27] arm: corstone1000: fix unrecognized filesystem type
+
+Some usb sticks are not recognized by usb, just add a
+delay before checking status.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ common/usb_storage.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/common/usb_storage.c b/common/usb_storage.c
+index c9e2d7343ce2..ae72338323ba 100644
+--- a/common/usb_storage.c
++++ b/common/usb_storage.c
+@@ -769,6 +769,9 @@ static int usb_stor_BBB_transport(struct scsi_cmd *srb, struct us_data *us)
+ st:
+ 	retry = 0;
+ again:
++	if (srb->cmd[0] == SCSI_TST_U_RDY)
++		mdelay(100);
++
+ 	debug("STATUS phase\n");
+ 	result = usb_bulk_msg(us->pusb_dev, pipein, csw, UMASS_BBB_CSW_SIZE,
+ 				&actlen, USB_CNTL_TIMEOUT*5);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch
new file mode 100644
index 0000000..0d1912c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch
@@ -0,0 +1,73 @@
+From 81bf9ed7e8e858cef13cfc3d1435c44445e523df Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 10 Dec 2021 20:03:35 +0000
+Subject: [PATCH 15/27] efi_capsule: corstone1000: pass interface id and buffer
+ event id using register w4
+
+Initially the interface/event IDs are passed to the SP using register
+w3 and w5.
+
+Now the SE proxy SP requires this arguments to be in register w4.
+
+This change is to pass interface ID(31:16) and event ID(15:0)
+to SE proxy SP to trigger an event to secure enclave about
+firmware update.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  6 ++++++
+ lib/efi_loader/efi_capsule.c   | 11 +++++++----
+ 2 files changed, 13 insertions(+), 4 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index db0f91335cef..a7445e61348b 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -24,6 +24,12 @@
+ #define CORSTONE1000_BUFFER_READY_EVT		(0x1)
+ #define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
+ 
++#define PREP_SEPROXY_SVC_ID_MASK	GENMASK(31, 16)
++#define PREP_SEPROXY_SVC_ID(x)	 (FIELD_PREP(PREP_SEPROXY_SVC_ID_MASK, (x)))
++
++#define PREP_SEPROXY_EVT_MASK		GENMASK(15, 0)
++#define PREP_SEPROXY_EVT(x)	(FIELD_PREP(PREP_SEPROXY_EVT_MASK, (x)))
++
+ /* Size in 4KB pages of the EFI capsule buffer */
+ #define CORSTONE1000_CAPSULE_BUFFER_SIZE	(8192) /* 32 MB */
+ 
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index c100c1b95298..17d769803b2a 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -27,6 +27,8 @@
+ #ifdef CONFIG_TARGET_CORSTONE1000
+ #include <arm_ffa_helper.h>
+ #include <cpu_func.h>
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
+ 
+ void *__efi_runtime_data corstone1000_capsule_buf; /* capsule shared buffer virtual address */
+ efi_guid_t corstone1000_capsule_guid = EFI_CORSTONE1000_CAPSULE_ID_GUID;
+@@ -587,11 +589,12 @@ static int __efi_runtime efi_corstone1000_buffer_ready_event(u32 capsule_image_s
+ 	func_data.data0 = &part_id;
+ 
+ 	/*
+-	 * setting the buffer ready event arguments
++	 * setting the buffer ready event arguments in register w4:
++	 * - capsule update interface ID (31:16)
++	 * - the buffer ready event ID (15:0)
+ 	 */
+-	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
+-	msg.a4 = capsule_image_size;
+-	msg.a5 = CORSTONE1000_BUFFER_READY_EVT;
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++		PREP_SEPROXY_EVT(CORSTONE1000_BUFFER_READY_EVT);
+ 
+ 	func_data.data1_size = sizeof(msg);
+ 	func_data.data1 = &msg;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch
new file mode 100644
index 0000000..f460fad
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch
@@ -0,0 +1,57 @@
+From 10d0ffc26ddcecd83921c2b3b37cb4eff54a154f Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 10 Dec 2021 20:10:41 +0000
+Subject: [PATCH 16/27] efi_boottime: corstone1000: pass interface id and
+ kernel event id using register w4
+
+Initially the interface/event IDs are passed to the SP using register
+w3 and w5.
+
+Now the SE proxy SP requires this arguments to be in register w4.
+
+This change is to pass interface ID(31:16) and kernel started
+event ID(15:0) to SE proxy SP to trigger an event to
+secure enclave just before ExitbootService().
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_boottime.c | 13 ++++++++++---
+ 1 file changed, 10 insertions(+), 3 deletions(-)
+
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index 5c77a40c3ebe..b58a8c98fd05 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -27,6 +27,11 @@
+ #include <arm_ffa_helper.h>
+ #endif
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ /* Task priority level */
+@@ -2121,10 +2126,12 @@ static int efi_corstone1000_kernel_started_event(void)
+ 	func_data.data0 = &part_id;
+ 
+ 	/*
+-	 * setting the kernel started  event arguments
++	 * setting the kernel started  event arguments:
++	 * setting capsule update interface ID(31:16)
++	 * the kernel started event ID(15:0)
+ 	 */
+-	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
+-	msg.a5 = CORSTONE1000_KERNEL_STARTED_EVT;
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++	PREP_SEPROXY_EVT(CORSTONE1000_KERNEL_STARTED_EVT);
+ 
+ 	func_data.data1_size = sizeof(msg);
+ 	func_data.data1 = &msg;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch
new file mode 100644
index 0000000..fa6ab32
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch
@@ -0,0 +1,52 @@
+From c463798489e41725f8ba33debeedc7c4011cda38 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Sat, 11 Dec 2021 13:23:55 +0000
+Subject: [PATCH 17/27] efi_loader: corstone1000: remove guid check from
+ corstone1000 config option
+
+Use generic fmp guid and no separte check is required for
+CORSTONE1000 target.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_capsule.c | 16 +---------------
+ 1 file changed, 1 insertion(+), 15 deletions(-)
+
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index 17d769803b2a..939040d2755e 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -654,12 +654,6 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 			  i, &capsule->capsule_guid);
+ 
+ #if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
+-		if (guidcmp(&corstone1000_capsule_guid, &capsule->capsule_guid)) {
+-			ret = EFI_INVALID_PARAMETER;
+-			log_err("Corstone1000: Invalid capsule GUID\n");
+-			goto out;
+-		}
+-
+ 		if (efi_size_in_pages(capsule->capsule_image_size) >
+ 		    CORSTONE1000_CAPSULE_BUFFER_SIZE) {
+ 			log_err("Corstone1000: Capsule data size exceeds the shared buffer size\n");
+@@ -685,15 +679,7 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		goto out;
+ #endif
+ 
+-		if (!guidcmp(&capsule->capsule_guid,
+-			     &efi_guid_firmware_management_capsule_id)) {
+-			ret  = efi_capsule_update_firmware(capsule);
+-		} else {
+-			log_err("Unsupported capsule type: %pUs\n",
+-				&capsule->capsule_guid);
+-			ret = EFI_UNSUPPORTED;
+-		}
+-
++		ret  = efi_capsule_update_firmware(capsule);
+ 		if (ret != EFI_SUCCESS)
+ 			goto out;
+ 	}
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch
new file mode 100644
index 0000000..4ee10a0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch
@@ -0,0 +1,40 @@
+From 1cfca60850727448bdbfe720d98d9e0d4523f6aa Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Sat, 11 Dec 2021 21:05:10 +0000
+Subject: [PATCH 18/27] arm_ffa: removing the cast when using binary OR on
+ FIELD_PREP macros
+
+When the GENMASK used is above 16-bits wide a u16 cast will cause
+loss of data.
+
+This commit fixes that.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ drivers/arm-ffa/arm_ffa_prv.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/drivers/arm-ffa/arm_ffa_prv.h b/drivers/arm-ffa/arm_ffa_prv.h
+index 38ea4ba83efc..d0db3ef508a1 100644
+--- a/drivers/arm-ffa/arm_ffa_prv.h
++++ b/drivers/arm-ffa/arm_ffa_prv.h
+@@ -40,13 +40,13 @@
+ 
+ #define PREP_SELF_ENDPOINT_ID_MASK		GENMASK(31, 16)
+ #define PREP_SELF_ENDPOINT_ID(x)		\
+-			((u16)(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x))))
++			(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x)))
+ 
+ /* Partition endpoint ID mask  (partition with which u-boot communicates with) */
+ 
+ #define PREP_PART_ENDPOINT_ID_MASK		GENMASK(15, 0)
+ #define PREP_PART_ENDPOINT_ID(x)		\
+-			((u16)(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x))))
++			(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x)))
+ 
+ /* The FF-A SMC function prototype definition */
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch
new file mode 100644
index 0000000..21a89a4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch
@@ -0,0 +1,31 @@
+From 7db27eeaba0fd5ddb1e49977bb7e342a1980aa3d Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Sun, 12 Dec 2021 17:51:17 +0000
+Subject: [PATCH 19/27] Return proper error code when rx buffer is larger
+
+ffa_mm_communicate should return EFI_BUFFER_TOO_SMALL when
+the buffer received from the secure world is larger than the
+comm buffer as this value is forwarded by mm_communicate.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index b6be2b54a030..38655a9dbb7c 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -358,7 +358,7 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 
+ 		if (rx_data_size > comm_buf_size) {
+ 			unmap_sysmem(virt_shared_buf);
+-			return EFI_OUT_OF_RESOURCES;
++			return EFI_BUFFER_TOO_SMALL;
+ 		}
+ 
+ 		efi_memcpy_runtime(comm_buf, virt_shared_buf, rx_data_size);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch
new file mode 100644
index 0000000..54328a7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch
@@ -0,0 +1,40 @@
+From 9ad9ead58e8e9e4f9e7a283c916421b443b424ce Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Mon, 13 Dec 2021 15:25:23 +0000
+Subject: [PATCH 20/27] Use correct buffer size
+
+The comm buffer created has additional 4 bytes length which
+needs to be trimmed. This change will reduce the size of the
+comm buffer to what is expected.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/mm_communication.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/include/mm_communication.h b/include/mm_communication.h
+index e65fbde60d0a..bb9919095649 100644
+--- a/include/mm_communication.h
++++ b/include/mm_communication.h
+@@ -123,7 +123,7 @@ struct __packed efi_mm_communicate_header {
+  *
+  * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_HEADER.
+  */
+-struct smm_variable_communicate_header {
++struct __packed smm_variable_communicate_header {
+ 	efi_uintn_t  function;
+ 	efi_status_t ret_status;
+ 	u8           data[];
+@@ -145,7 +145,7 @@ struct smm_variable_communicate_header {
+  * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+  *
+  */
+-struct smm_variable_access {
++struct __packed smm_variable_access {
+ 	efi_guid_t  guid;
+ 	efi_uintn_t data_size;
+ 	efi_uintn_t name_size;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch
new file mode 100644
index 0000000..c7ac38f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch
@@ -0,0 +1,30 @@
+From b81214dea7056c3877aa9eb775557dc4702660ec Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Sun, 12 Dec 2021 17:58:08 +0000
+Subject: [PATCH 21/27] Update comm_buf when EFI_BUFFER_TOO_SMALL
+
+When the received buffer is larger than the comm buffer,
+the contents of the shared buffer which can fit in the
+comm buffer should be read before returning.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 38655a9dbb7c..67743d1f8fce 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -357,6 +357,7 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 			sizeof(size_t);
+ 
+ 		if (rx_data_size > comm_buf_size) {
++			efi_memcpy_runtime(comm_buf, virt_shared_buf, comm_buf_size);
+ 			unmap_sysmem(virt_shared_buf);
+ 			return EFI_BUFFER_TOO_SMALL;
+ 		}
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch
new file mode 100644
index 0000000..aaea20e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch
@@ -0,0 +1,36 @@
+From 5fec641015f8f1ca80f55f05b5e1f67653321303 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 17 Dec 2021 19:49:02 +0000
+Subject: [PATCH 22/27] efi_loader: populate ESRT table if EFI_ESRT config
+ option is set
+
+This change is to call efi_esrt_populate function if CONFIG_EFI_ESRT
+is set. This will populte esrt table with firmware image info
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_capsule.c | 7 +++++++
+ 1 file changed, 7 insertions(+)
+
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index 939040d2755e..790d2ba8fe19 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -676,6 +676,13 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 			ret = EFI_SUCCESS;
+ 		}
+ 
++		if (IS_ENABLED(CONFIG_EFI_ESRT)) {
++			/* Rebuild the ESRT to reflect any updated FW images. */
++			ret = efi_esrt_populate();
++	               if (ret != EFI_SUCCESS)
++				log_warning("EFI Capsule: failed to update ESRT\n");
++	       }
++
+ 		goto out;
+ #endif
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch
new file mode 100644
index 0000000..c86b658
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch
@@ -0,0 +1,121 @@
+From 34fadec4f659248a6020676f5894895977ccf79d Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 17 Dec 2021 19:50:25 +0000
+Subject: [PATCH 23/27] efi_firmware: add get_image_info for corstone1000
+
+This change is to populate get_image_info which eventually
+will be populated in ESRT table
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+%% original patch: 0047-efi_firmware-add-get_image_info-for-corstone1000.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_firmware.c | 64 ++++++++++++++++++++++++++++++++++-
+ 1 file changed, 63 insertions(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index a5ff32f121f4..9eb89849b28d 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -241,6 +241,7 @@ const efi_guid_t efi_firmware_image_type_uboot_fit =
+  *
+  * Return		status code
+  */
++
+ static
+ efi_status_t EFIAPI efi_firmware_fit_get_image_info(
+ 	struct efi_firmware_management_protocol *this,
+@@ -332,6 +333,56 @@ const struct efi_firmware_management_protocol efi_fmp_fit = {
+ const efi_guid_t efi_firmware_image_type_uboot_raw =
+ 	EFI_FIRMWARE_IMAGE_TYPE_UBOOT_RAW_GUID;
+ 
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++static efi_status_t efi_corstone1000_img_info_get (
++	efi_uintn_t *image_info_size,
++	struct efi_firmware_image_descriptor *image_info,
++	u32 *descriptor_version,
++	u8 *descriptor_count,
++	efi_uintn_t *descriptor_size,
++	u32 *package_version,
++	u16 **package_version_name,
++	const efi_guid_t *image_type)
++{
++	int i = 0;
++
++	*image_info_size = sizeof(*image_info);
++	*descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
++	*descriptor_count = 1;//dfu_num;
++	*descriptor_size = sizeof(*image_info);
++	if (package_version)
++		*package_version = 0xffffffff; /* not supported */
++	if(package_version_name)
++		*package_version_name = NULL; /* not supported */
++
++	if(image_info == NULL) {
++		log_warning("image_info is null\n");
++		return EFI_BUFFER_TOO_SMALL;
++	}
++
++	image_info[i].image_index = i;
++	image_info[i].image_type_id = *image_type;
++	image_info[i].image_id = 0;
++	image_info[i].image_id_name = "wic";
++	image_info[i].version = 1;
++	image_info[i].version_name = NULL;
++	image_info[i].size = 0x1000;
++	image_info[i].attributes_supported = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE |
++					     IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
++	image_info[i].attributes_setting = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
++	/* Check if the capsule authentication is enabled */
++	if (IS_ENABLED(CONFIG_EFI_CAPSULE_AUTHENTICATE))
++		image_info[0].attributes_setting |=
++			IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
++	image_info[i].lowest_supported_image_version = 0;
++	image_info[i].last_attempt_version = 0;
++	image_info[i].last_attempt_status = LAST_ATTEMPT_STATUS_SUCCESS;
++	image_info[i].hardware_instance = 1;
++	image_info[i].dependencies = NULL;
++
++	return EFI_SUCCESS;
++}
++#endif
+ /**
+  * efi_firmware_raw_get_image_info - return information about the current
+ 				     firmware image
+@@ -376,12 +427,20 @@ efi_status_t EFIAPI efi_firmware_raw_get_image_info(
+ 	     !descriptor_size || !package_version || !package_version_name))
+ 		return EFI_EXIT(EFI_INVALID_PARAMETER);
+ 
+-	ret = efi_get_dfu_info(image_info_size, image_info,
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++	ret = efi_corstone1000_img_info_get(image_info_size, image_info,
+ 			       descriptor_version, descriptor_count,
+ 			       descriptor_size,
+ 			       package_version, package_version_name,
+ 			       &efi_firmware_image_type_uboot_raw);
++#else
+ 
++	ret = efi_get_dfu_info(image_info_size, image_info,
++			       descriptor_version, descriptor_count,
++			       descriptor_size,
++			       package_version, package_version_name,
++			       &efi_firmware_image_type_uboot_raw);
++#endif
+ 	return EFI_EXIT(ret);
+ }
+ 
+@@ -462,6 +521,9 @@ efi_status_t EFIAPI efi_firmware_raw_set_image(
+ 
+ 	}
+ 
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++	return EFI_EXIT(EFI_SUCCESS);
++#endif
+ 	if (dfu_write_by_alt(image_index - 1, (void *)image, image_size,
+ 			     NULL, NULL))
+ 		return EFI_EXIT(EFI_DEVICE_ERROR);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch
new file mode 100644
index 0000000..c6a1aed
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch
@@ -0,0 +1,34 @@
+From c0c6e4c1166c4868afc36649b9ed98081a6966e1 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Fri, 24 Dec 2021 14:22:52 +0000
+Subject: [PATCH 24/27] Comment mm_communicate failure log
+
+When a getVariable() call is made with data size set to 0,
+mm_communicate should return EFI_BUFFER_TOO_SMALL. This is
+an expected behavior. There should not be any failure logs
+in this case. So the error log is commented here.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 5 ++++-
+ 1 file changed, 4 insertions(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 67743d1f8fce..a34989efac83 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -411,7 +411,10 @@ static efi_status_t __efi_runtime mm_communicate(u8 *comm_buf, efi_uintn_t dsize
+ 	ret = ffa_mm_communicate(comm_buf, dsize);
+ 	#endif
+ 	if (ret != EFI_SUCCESS) {
+-		log_err("%s failed!\n", __func__);
++		/* mm_communicate failure is logged even when getVariable() is called
++		 * with data size set to 0. This is not expected so logging is commented.
++		*/
++		//log_err("%s failed!\n", __func__);
+ 		return ret;
+ 	}
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch
new file mode 100644
index 0000000..d5a0ec0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch
@@ -0,0 +1,191 @@
+From af2defbfaffa4264052e30f269b91794068e4773 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Wed, 5 Jan 2022 17:56:09 +0000
+Subject: [PATCH 25/27] efi_loader: send bootcomplete message to secure enclave
+
+On corstone1000 platform, Secure Enclave will be expecting
+an event from uboot when it performs capsule update. Previously,
+an event is sent at exitbootservice level. This will create a problem
+when user wants to interrupt at UEFI shell, hence, it is required
+to send an uboot efi initialized event at efi sub-system initialization
+stage.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  2 +-
+ lib/efi_loader/efi_boottime.c  | 49 ----------------------------------
+ lib/efi_loader/efi_firmware.c  |  2 +-
+ lib/efi_loader/efi_setup.c     | 48 +++++++++++++++++++++++++++++++++
+ 4 files changed, 50 insertions(+), 51 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index a7445e61348b..06b605e43bdf 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -22,7 +22,7 @@
+ 
+ /* Notification events used with SE Proxy update service */
+ #define CORSTONE1000_BUFFER_READY_EVT		(0x1)
+-#define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
++#define CORSTONE1000_UBOOT_EFI_STARTED_EVT	(0x2)
+ 
+ #define PREP_SEPROXY_SVC_ID_MASK	GENMASK(31, 16)
+ #define PREP_SEPROXY_SVC_ID(x)	 (FIELD_PREP(PREP_SEPROXY_SVC_ID_MASK, (x)))
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index b58a8c98fd05..d0703060491b 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -2101,46 +2101,6 @@ static void efi_exit_caches(void)
+ #endif
+ }
+ 
+-#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
+-/**
+- * efi_corstone1000_kernel_started_event - notifies SE Proxy FW update service
+- *
+- * This function notifies the SE Proxy update service that the kernel has already started
+- *
+- * Return:
+- *
+- * 0: on success, otherwise failure
+- */
+-static int efi_corstone1000_kernel_started_event(void)
+-{
+-	struct ffa_interface_data func_data = {0};
+-	struct ffa_send_direct_data msg = {0};
+-	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
+-
+-	log_debug("[%s]\n", __func__);
+-
+-	/*
+-	 * telling the driver which partition to use
+-	 */
+-	func_data.data0_size = sizeof(part_id);
+-	func_data.data0 = &part_id;
+-
+-	/*
+-	 * setting the kernel started  event arguments:
+-	 * setting capsule update interface ID(31:16)
+-	 * the kernel started event ID(15:0)
+-	 */
+-	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
+-	PREP_SEPROXY_EVT(CORSTONE1000_KERNEL_STARTED_EVT);
+-
+-	func_data.data1_size = sizeof(msg);
+-	func_data.data1 = &msg;
+-
+-	return ffa_helper_msg_send_direct_req(&func_data);
+-}
+-
+-#endif
+-
+ /**
+  * efi_exit_boot_services() - stop all boot services
+  * @image_handle: handle of the loaded image
+@@ -2254,15 +2214,6 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	/* Recalculate CRC32 */
+ 	efi_update_table_header_crc32(&systab.hdr);
+ 
+-#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
+-	/* Notifying SE Proxy FW update service */
+-	ffa_ret = efi_corstone1000_kernel_started_event();
+-	if (ffa_ret)
+-		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
+-	else
+-		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
+-#endif
+-
+ 	/* Give the payload some time to boot */
+ 	efi_set_watchdog(0);
+ 	WATCHDOG_RESET();
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index 9eb89849b28d..477ad072070e 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -356,7 +356,7 @@ static efi_status_t efi_corstone1000_img_info_get (
+ 		*package_version_name = NULL; /* not supported */
+ 
+ 	if(image_info == NULL) {
+-		log_warning("image_info is null\n");
++		log_info("image_info is null\n");
+ 		return EFI_BUFFER_TOO_SMALL;
+ 	}
+ 
+diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
+index 989380d4f8cd..515a0bdf74ef 100644
+--- a/lib/efi_loader/efi_setup.c
++++ b/lib/efi_loader/efi_setup.c
+@@ -17,6 +17,9 @@
+ efi_status_t efi_obj_list_initialized = OBJ_LIST_NOT_INITIALIZED;
+ 
+ #if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++#include <arm_ffa_helper.h>
+ /**
+  * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
+  */
+@@ -126,6 +129,44 @@ static efi_status_t efi_init_secure_boot(void)
+ }
+ #endif /* CONFIG_EFI_SECURE_BOOT */
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_uboot-efi_started_event - notifies SE Proxy FW update service
++ *
++ * This function notifies the SE Proxy update service that uboot efi has already started
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ * */
++static int efi_corstone1000_uboot_efi_started_event(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++	/*
++	 * setting the uboot efi subsystem started  event arguments:
++	 * setting capsule update interface ID(31:16)
++	 * the uboot efi subsystem started event ID(15:0)
++	 */
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++			PREP_SEPROXY_EVT(CORSTONE1000_UBOOT_EFI_STARTED_EVT);
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++#endif
++
+ /**
+  * efi_init_capsule - initialize capsule update state
+  *
+@@ -134,8 +175,15 @@ static efi_status_t efi_init_secure_boot(void)
+ static efi_status_t efi_init_capsule(void)
+ {
+ 	efi_status_t ret = EFI_SUCCESS;
++	int ffa_ret;
+ 
+ #if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	ffa_ret = efi_corstone1000_uboot_efi_started_event();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
++	else
++		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
++
+ 	ret = efi_corstone1000_alloc_capsule_shared_buf();
+ 	if (ret != EFI_SUCCESS) {
+ 		printf("EFI: Corstone-1000: cannot allocate caspsule shared buffer\n");
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch
new file mode 100644
index 0000000..532e872
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch
@@ -0,0 +1,62 @@
+From 2da8554ab732c59c7ca624ac4b16412fa9c2e39c Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 14 Jan 2022 15:24:18 +0000
+Subject: [PATCH 26/27] efi_loader: fix null pointer exception with
+ get_image_info
+
+get_img_info API implemented for corstone1000 target does not
+check the input attributes and as a result uboot crash's with
+null pointer access. This change is to fix the null pointer
+exception.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_firmware.c | 19 +++++++++++--------
+ 1 file changed, 11 insertions(+), 8 deletions(-)
+
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index 477ad072070e..f99c57fde576 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -347,26 +347,29 @@ static efi_status_t efi_corstone1000_img_info_get (
+ 	int i = 0;
+ 
+ 	*image_info_size = sizeof(*image_info);
+-	*descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
+-	*descriptor_count = 1;//dfu_num;
+-	*descriptor_size = sizeof(*image_info);
++	if(descriptor_version)
++	    *descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
++	if(descriptor_count)
++	    *descriptor_count = 1;
++	if(descriptor_size)
++	    *descriptor_size = sizeof(*image_info);
+ 	if (package_version)
+ 		*package_version = 0xffffffff; /* not supported */
+ 	if(package_version_name)
+ 		*package_version_name = NULL; /* not supported */
+ 
+ 	if(image_info == NULL) {
+-		log_info("image_info is null\n");
++		log_debug("image_info is null\n");
+ 		return EFI_BUFFER_TOO_SMALL;
+ 	}
+ 
+-	image_info[i].image_index = i;
++	image_info[i].image_index = 1;
+ 	image_info[i].image_type_id = *image_type;
+ 	image_info[i].image_id = 0;
+-	image_info[i].image_id_name = "wic";
+-	image_info[i].version = 1;
++	image_info[i].image_id_name = L"wic image";
++	image_info[i].version = 0;
+ 	image_info[i].version_name = NULL;
+-	image_info[i].size = 0x1000;
++	image_info[i].size = 0;
+ 	image_info[i].attributes_supported = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE |
+ 					     IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
+ 	image_info[i].attributes_setting = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch
new file mode 100644
index 0000000..bf95ed7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch
@@ -0,0 +1,148 @@
+From cbf16548dc6dcc8eea97aa18c6ae17fb848e5c6c Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Tue, 5 Apr 2022 10:24:38 +0100
+Subject: [PATCH 27/27] arm:corstone1000: add mmc for fvp
+
+Enable support mmc/sdcard for the corstone1000 FVP.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ arch/arm/dts/corstone1000-fvp.dts        | 28 +++++++++++++++
+ board/armltd/corstone1000/corstone1000.c | 46 ++++++++++++++++--------
+ configs/corstone1000_defconfig           |  8 ++++-
+ include/configs/corstone1000.h           |  4 ++-
+ 4 files changed, 69 insertions(+), 17 deletions(-)
+
+diff --git a/arch/arm/dts/corstone1000-fvp.dts b/arch/arm/dts/corstone1000-fvp.dts
+index 1fcc137a493c..26b0f1b3cea6 100644
+--- a/arch/arm/dts/corstone1000-fvp.dts
++++ b/arch/arm/dts/corstone1000-fvp.dts
+@@ -20,4 +20,32 @@
+ 		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
+ 		reg-io-width = <2>;
+ 	};
++
++	vmmc_v3_3d: fixed_v3_3d {
++		compatible = "regulator-fixed";
++		regulator-name = "vmmc_supply";
++		regulator-min-microvolt = <3300000>;
++		regulator-max-microvolt = <3300000>;
++		regulator-always-on;
++	};
++
++	sdmmc0: mmc@40300000 {
++		compatible = "arm,pl18x", "arm,primecell";
++		reg = <0x40300000 0x1000>;
++		interrupts = <GIC_SPI 117 IRQ_TYPE_LEVEL_HIGH>;
++		max-frequency = <12000000>;
++		vmmc-supply = <&vmmc_v3_3d>;
++		clocks = <&smbclk>, <&refclk100mhz>;
++		clock-names = "smclk", "apb_pclk";
++	};
++
++	sdmmc1: mmc@50000000 {
++		compatible = "arm,pl18x", "arm,primecell";
++		reg = <0x50000000 0x10000>;
++		interrupts = <GIC_SPI 115 IRQ_TYPE_LEVEL_HIGH>;
++		max-frequency = <12000000>;
++		vmmc-supply = <&vmmc_v3_3d>;
++		clocks = <&smbclk>, <&refclk100mhz>;
++		clock-names = "smclk", "apb_pclk";
++	};
+ };
+diff --git a/board/armltd/corstone1000/corstone1000.c b/board/armltd/corstone1000/corstone1000.c
+index eff1739f0b02..936a6c9f8b89 100644
+--- a/board/armltd/corstone1000/corstone1000.c
++++ b/board/armltd/corstone1000/corstone1000.c
+@@ -46,22 +46,38 @@ static struct mm_region corstone1000_mem_map[] = {
+ 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+ 			 PTE_BLOCK_NON_SHARE |
+ 			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+-        }, {
+-                /* USB */
+-                .virt = 0x40200000UL,
+-                .phys = 0x40200000UL,
+-                .size = 0x00100000UL,
+-                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+-                         PTE_BLOCK_NON_SHARE |
+-                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
+ 	}, {
+-                 /* ethernet */
+-                .virt = 0x40100000UL,
+-                .phys = 0x40100000UL,
+-                .size = 0x00100000UL,
+-                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+-                         PTE_BLOCK_NON_SHARE |
+-                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++		/* USB */
++		.virt = 0x40200000UL,
++			.phys = 0x40200000UL,
++			.size = 0x00100000UL,
++			.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				PTE_BLOCK_NON_SHARE |
++				PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* MMC0 */
++		.virt = 0x40300000UL,
++		.phys = 0x40300000UL,
++		.size = 0x00100000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				 PTE_BLOCK_NON_SHARE |
++				 PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* ethernet */
++		.virt = 0x40100000UL,
++			.phys = 0x40100000UL,
++			.size = 0x00100000UL,
++			.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				PTE_BLOCK_NON_SHARE |
++				PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* MMC1 */
++		.virt = 0x50000000UL,
++		.phys = 0x50000000UL,
++		.size = 0x00100000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				 PTE_BLOCK_NON_SHARE |
++				 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+ 	}, {
+ 		/* OCVM */
+ 		.virt = 0x80000000UL,
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index b042d4e49419..147c14c94865 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -38,7 +38,13 @@ CONFIG_CMD_EFIDEBUG=y
+ CONFIG_CMD_FAT=y
+ CONFIG_OF_CONTROL=y
+ CONFIG_REGMAP=y
+-# CONFIG_MMC is not set
++CONFIG_CLK=y
++CONFIG_CMD_MMC=y
++CONFIG_DM_MMC=y
++CONFIG_ARM_PL180_MMCI=y
++CONFIG_MMC_SDHCI_ADMA_HELPERS=y
++CONFIG_MMC_WRITE=y
++CONFIG_DM_GPIO=y
+ CONFIG_DM_SERIAL=y
+ CONFIG_USB=y
+ CONFIG_DM_USB=y
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index 06b605e43bdf..d9855bf91ebf 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -95,7 +95,9 @@
+ #define CONFIG_SYS_MAXARGS	64	/* max command args */
+ 
+ #define BOOT_TARGET_DEVICES(func) \
+-	func(USB, usb, 0)
++	func(USB, usb, 0) \
++	func(MMC, mmc, 0) \
++	func(MMC, mmc, 1)
+ 
+ #include <config_distro_bootcmd.h>
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch
new file mode 100644
index 0000000..8a98f9d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch
@@ -0,0 +1,138 @@
+From fff63cfd7d9654dc9ed0c106f29d3a7ad01b0502 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Wed, 18 Dec 2019 21:52:34 +0000
+Subject: [PATCH 1/2] armv7: adding generic timer access through MMIO
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+This driver enables the ARMv7 generic timer.
+
+The access to the timer registers is through memory mapping (MMIO).
+
+This driver can be used by u-boot to access to the timer through MMIO
+when arch_timer is not available in the core (access using system
+instructions not possible), for example, in case of Cortex-A5.
+
+This driver configures and enables the generic timer at
+the u-boot initcall level (timer_init) before u-boot relocation.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+%% original patch: 0001-armv7-adding-generic-timer-access-through-MMIO.patch
+---
+ arch/arm/cpu/armv7/Makefile     |  1 +
+ arch/arm/cpu/armv7/mmio_timer.c | 75 +++++++++++++++++++++++++++++++++
+ scripts/config_whitelist.txt    |  1 +
+ 3 files changed, 77 insertions(+)
+ create mode 100644 arch/arm/cpu/armv7/mmio_timer.c
+
+diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
+index bfbd85ae64..1a0a24e531 100644
+--- a/arch/arm/cpu/armv7/Makefile
++++ b/arch/arm/cpu/armv7/Makefile
+@@ -28,6 +28,7 @@ obj-$(CONFIG_ARMV7_PSCI)	+= psci.o psci-common.o
+ obj-$(CONFIG_IPROC) += iproc-common/
+ obj-$(CONFIG_KONA) += kona-common/
+ obj-$(CONFIG_SYS_ARCH_TIMER) += arch_timer.o
++obj-$(CONFIG_SYS_MMIO_TIMER) += mmio_timer.o
+ 
+ ifneq (,$(filter s5pc1xx exynos,$(SOC)))
+ obj-y += s5p-common/
+diff --git a/arch/arm/cpu/armv7/mmio_timer.c b/arch/arm/cpu/armv7/mmio_timer.c
+new file mode 100644
+index 0000000000..edd806e06e
+--- /dev/null
++++ b/arch/arm/cpu/armv7/mmio_timer.c
+@@ -0,0 +1,75 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * Copyright (c) 2019, Arm Limited. All rights reserved.
++ *
++ */
++
++#include <common.h>
++#include <asm/io.h>
++#include <div64.h>
++#include <bootstage.h>
++#include <asm/global_data.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++#define CNTCTLBASE    0x1a020000UL
++#define CNTREADBASE   0x1a030000UL
++#define CNTEN         (1 << 0)
++#define CNTFCREQ      (1 << 8)
++
++static inline uint32_t mmio_read32(uintptr_t addr)
++{
++	return *(volatile uint32_t*)addr;
++}
++
++static inline void mmio_write32(uintptr_t addr, uint32_t data)
++{
++	*(volatile uint32_t*)addr = data;
++}
++
++int timer_init(void)
++{
++	/* calculate the frequency in ms */
++	gd->arch.timer_rate_hz = COUNTER_FREQUENCY / CONFIG_SYS_HZ;
++
++	/* configure CNTFID0 register: set the base frequency */
++	mmio_write32(CNTCTLBASE + 0x20, COUNTER_FREQUENCY);
++
++	/*
++	 * configure CNTCR register:
++	 *    enable the generic counter and;
++	 *    select the first frequency entry
++	 */
++	mmio_write32(CNTCTLBASE, CNTFCREQ | CNTEN);
++
++	return 0;
++}
++
++unsigned long long get_ticks(void)
++{
++	return (((u64)(mmio_read32(CNTREADBASE + 0x4)) << 32) |
++		mmio_read32(CNTREADBASE));
++}
++
++ulong get_timer(ulong base)
++{
++	return lldiv(get_ticks(), gd->arch.timer_rate_hz) - base;
++}
++
++void __udelay(unsigned long usec)
++{
++	unsigned long endtime;
++
++	endtime = lldiv((unsigned long long)usec * gd->arch.timer_rate_hz,
++			1000UL);
++
++	endtime += get_ticks();
++
++	while (get_ticks() < endtime)
++		;
++}
++
++ulong get_tbclk(void)
++{
++	return gd->arch.timer_rate_hz;
++}
+diff --git a/scripts/config_whitelist.txt b/scripts/config_whitelist.txt
+index a6bc234f51..8d5cd67ace 100644
+--- a/scripts/config_whitelist.txt
++++ b/scripts/config_whitelist.txt
+@@ -1524,6 +1524,7 @@ CONFIG_SYS_MMC_U_BOOT_DST
+ CONFIG_SYS_MMC_U_BOOT_OFFS
+ CONFIG_SYS_MMC_U_BOOT_SIZE
+ CONFIG_SYS_MMC_U_BOOT_START
++CONFIG_SYS_MMIO_TIMER
+ CONFIG_SYS_MONITOR_BASE
+ CONFIG_SYS_MONITOR_LEN
+ CONFIG_SYS_MONITOR_SEC
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch
new file mode 100644
index 0000000..29b2943
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch
@@ -0,0 +1,307 @@
+From 73c319a1096259652853fa2538a733a8ebea96a8 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Wed, 8 Jan 2020 09:48:11 +0000
+Subject: [PATCH 2/2] board: arm: add corstone500 board
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+Add support for the Arm corstone500 platform, with a cortex-a5
+chip, add the default configuration, initialization and
+makefile for this system.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+
+%% original patch: 0002-board-arm-add-corstone500-board.patch
+---
+ arch/arm/Kconfig                       |  10 +++
+ board/armltd/corstone500/Kconfig       |  12 +++
+ board/armltd/corstone500/Makefile      |   8 ++
+ board/armltd/corstone500/corstone500.c |  48 +++++++++++
+ configs/corstone500_defconfig          |  40 +++++++++
+ include/configs/corstone500.h          | 109 +++++++++++++++++++++++++
+ 6 files changed, 227 insertions(+)
+ create mode 100644 board/armltd/corstone500/Kconfig
+ create mode 100644 board/armltd/corstone500/Makefile
+ create mode 100644 board/armltd/corstone500/corstone500.c
+ create mode 100644 configs/corstone500_defconfig
+ create mode 100644 include/configs/corstone500.h
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb..66f99fdf4f 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -641,6 +641,15 @@ config ARCH_BCMSTB
+ 	  This enables support for Broadcom ARM-based set-top box
+ 	  chipsets, including the 7445 family of chips.
+ 
++config TARGET_CORSTONE500
++	bool "Support Corstone500"
++	select CPU_V7A
++	select SEMIHOSTING
++	select PL01X_SERIAL
++	help
++	  This enables support for Corstone500 ARM which is a
++	  Cortex-A5 system
++
+ config TARGET_VEXPRESS_CA9X4
+ 	bool "Support vexpress_ca9x4"
+ 	select CPU_V7A
+@@ -2202,6 +2211,7 @@ source "board/bosch/shc/Kconfig"
+ source "board/bosch/guardian/Kconfig"
+ source "board/Marvell/octeontx/Kconfig"
+ source "board/Marvell/octeontx2/Kconfig"
++source "board/armltd/corstone500/Kconfig"
+ source "board/armltd/vexpress/Kconfig"
+ source "board/armltd/vexpress64/Kconfig"
+ source "board/cortina/presidio-asic/Kconfig"
+diff --git a/board/armltd/corstone500/Kconfig b/board/armltd/corstone500/Kconfig
+new file mode 100644
+index 0000000000..8e689bd1fd
+--- /dev/null
++++ b/board/armltd/corstone500/Kconfig
+@@ -0,0 +1,12 @@
++if TARGET_CORSTONE500
++
++config SYS_BOARD
++	default "corstone500"
++
++config SYS_VENDOR
++	default "armltd"
++
++config SYS_CONFIG_NAME
++	default "corstone500"
++
++endif
+diff --git a/board/armltd/corstone500/Makefile b/board/armltd/corstone500/Makefile
+new file mode 100644
+index 0000000000..6598fdd3ae
+--- /dev/null
++++ b/board/armltd/corstone500/Makefile
+@@ -0,0 +1,8 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2022 ARM Limited
++# (C) Copyright 2022 Linaro
++# Rui Miguel Silva <rui.silva@linaro.org>
++#
++
++obj-y := corstone500.o
+diff --git a/board/armltd/corstone500/corstone500.c b/board/armltd/corstone500/corstone500.c
+new file mode 100644
+index 0000000000..e878f5c6a5
+--- /dev/null
++++ b/board/armltd/corstone500/corstone500.c
+@@ -0,0 +1,48 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/platform_data/serial_pl01x.h>
++#include <malloc.h>
++#include <asm/global_data.h>
++
++static const struct pl01x_serial_plat serial_platdata = {
++	.base = V2M_UART0,
++	.type = TYPE_PL011,
++	.clock = CONFIG_PL011_CLOCK,
++};
++
++U_BOOT_DRVINFO(corstone500_serials) = {
++	.name = "serial_pl01x",
++	.plat = &serial_platdata,
++};
++
++int board_init(void)
++{
++	return 0;
++}
++
++int dram_init(void)
++{
++	gd->ram_size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++int dram_init_banksize(void)
++{
++	gd->bd->bi_dram[0].start = PHYS_SDRAM_1;
++	gd->bd->bi_dram[0].size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++void reset_cpu(ulong addr)
++{
++}
++
+diff --git a/configs/corstone500_defconfig b/configs/corstone500_defconfig
+new file mode 100644
+index 0000000000..d3161a4b40
+--- /dev/null
++++ b/configs/corstone500_defconfig
+@@ -0,0 +1,40 @@
++CONFIG_ARM=y
++CONFIG_SKIP_LOWLEVEL_INIT=y
++CONFIG_TARGET_CORSTONE500=y
++CONFIG_SYS_TEXT_BASE=0x88000000
++CONFIG_SYS_MALLOC_LEN=0x840000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_NR_DRAM_BANKS=1
++CONFIG_SYS_MEMTEST_START=0x80000000
++CONFIG_SYS_MEMTEST_END=0xff000000
++CONFIG_ENV_SIZE=0x40000
++CONFIG_IDENT_STRING=" corstone500 aarch32"
++CONFIG_SYS_LOAD_ADDR=0x90000000
++CONFIG_SUPPORT_RAW_INITRD=y
++CONFIG_BOOTDELAY=1
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1a200000 root=/dev/ram0 rw loglevel=9"
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="corstone500# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_EDITENV is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_MEMTEST=y
++CONFIG_CMD_ARMFLASH=y
++# CONFIG_CMD_LOADS is not set
++# CONFIG_CMD_ITEST is not set
++# CONFIG_CMD_SETEXPR is not set
++CONFIG_CMD_DHCP=y
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_PING=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_FAT=y
++CONFIG_DM=y
++CONFIG_MTD_NOR_FLASH=y
++CONFIG_DM_SERIAL=y
++CONFIG_OF_LIBFDT=y
+diff --git a/include/configs/corstone500.h b/include/configs/corstone500.h
+new file mode 100644
+index 0000000000..93c397d2f5
+--- /dev/null
++++ b/include/configs/corstone500.h
+@@ -0,0 +1,109 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * Configuration for Cortex-A5 Corstone500. Parts were derived from other ARM
++ * configurations.
++ */
++
++#ifndef __CORSTONE500_H
++#define __CORSTONE500_H
++
++#define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
++
++/* Generic Timer Definitions */
++#define CONFIG_SYS_HZ_CLOCK	7500000
++#define CONFIG_SYS_HZ		1000
++#define COUNTER_FREQUENCY	CONFIG_SYS_HZ_CLOCK
++
++#ifdef CONFIG_CORSTONE500_MEMORY_MAP_EXTENDED
++#define V2M_SRAM0		0x00010000
++#define V2M_SRAM1		0x02200000
++#define V2M_QSPI		0x0a800000
++#else
++#define V2M_SRAM0		0x00000000
++#define V2M_SRAM1		0x02000000
++#define V2M_QSPI		0x08000000
++#endif
++
++#define V2M_DEBUG		0x10000000
++#define V2M_BASE_PERIPH		0x1a000000
++#define V2M_A5_PERIPH		0x1c000000
++#define V2M_L2CC_PERIPH		0x1c010000
++
++#define V2M_MASTER_EXPANSION0	0x40000000
++#define V2M_MASTER_EXPANSION1	0x60000000
++
++#define V2M_BASE		0x80000000
++
++#define V2M_PERIPH_OFFSET(x)  (x << 16)
++
++#define V2M_SYSID		(V2M_BASE_PERIPH)
++#define V2M_SYCTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(1))
++#define V2M_COUNTER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(2))
++#define V2M_COUNTER_READ	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(3))
++#define V2M_TIMER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(4))
++#define V2M_TIMER0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(5))
++
++#define V2M_WATCHDOG_CTL	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(16))
++#define V2M_WATCHDOG_REFRESH	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(17))
++
++#define V2M_UART0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(32))
++#define V2M_UART1		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(33))
++
++#define V2M_RTC			(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(34))
++#define V2M_TRNG		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(35))
++
++/* PL011 Serial Configuration */
++#define CONFIG_CONS_INDEX	0
++#define CONFIG_PL011_CLOCK	7500000
++
++/* Physical Memory Map */
++#define PHYS_SDRAM_1		(V2M_BASE)
++
++/* Top 16MB reserved for secure world use */
++#define DRAM_SEC_SIZE		0x01000000
++#define PHYS_SDRAM_1_SIZE	(0x80000000 - DRAM_SEC_SIZE)
++
++/* Miscellaneous configurable options */
++#define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
++
++#define CONFIG_SYS_MMIO_TIMER
++
++#define CONFIG_EXTRA_ENV_SETTINGS     \
++				"kernel_name=Image\0"           \
++				"kernel_addr=0x80f00000\0"      \
++				"initrd_name=ramdisk.img\0"     \
++				"initrd_addr=0x84000000\0"      \
++				"fdt_name=devtree.dtb\0"        \
++				"fdt_addr=0x83000000\0"         \
++				"fdt_high=0xffffffff\0"         \
++				"initrd_high=0xffffffff\0"
++
++#define CONFIG_BOOTCOMMAND	"echo copy to RAM...; " \
++				"cp.b 0x80100000 $kernel_addr 0xb00000; " \
++				"cp.b 0x80d00000 $initrd_addr 0x800000; " \
++				"bootz $kernel_addr $initrd_addr:0x800000 $fdt_addr"
++
++/* Monitor Command Prompt */
++#define CONFIG_SYS_CBSIZE		512     /* Console I/O Buffer Size */
++#define CONFIG_SYS_MAXARGS		64      /* max command args */
++
++#define CONFIG_SYS_FLASH_BASE		0x80000000
++/* 256 x 256KiB sectors */
++#define CONFIG_SYS_MAX_FLASH_SECT	256
++/* Store environment at top of flash */
++#define CONFIG_ENV_ADDR			0x0a7c0000
++#define CONFIG_ENV_SECT_SIZE		0x0040000
++
++#define CONFIG_SYS_FLASH_CFI		1
++#define CONFIG_FLASH_CFI_DRIVER		1
++#define CONFIG_SYS_FLASH_CFI_WIDTH	FLASH_CFI_32BIT
++#define CONFIG_SYS_MAX_FLASH_BANKS	1
++
++#define CONFIG_SYS_FLASH_EMPTY_INFO   /* flinfo indicates empty blocks */
++#define FLASH_MAX_SECTOR_SI		0x00040000
++#define CONFIG_ENV_IS_IN_FLASH		1
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch
new file mode 100644
index 0000000..5138335
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch
@@ -0,0 +1,184 @@
+From 424d186ab0a0c4dd62dfb13ac87e8d1fd26c101e Mon Sep 17 00:00:00 2001
+From: Anders Dellien <anders.dellien@arm.com>
+Date: Thu, 23 Jul 2020 17:32:55 +0100
+Subject: [PATCH 1/2] Add vexpress_aemv8a_aarch32 variant
+
+The ARM AEMv8 FVP model can be run in Aarch64 or Aarch32 mode. Aarch32
+support is enable per-CPU when launching the model, eg:
+
+-C cluster0.cpu0.CONFIG64=0
+
+This patch adds a new defconfig and some variant specific selections in
+vexpress_armv8a.h.
+
+This patch is co-authored with Soby Mathew <Soby.Mathew@arm.com>.
+
+Upstream-Status: Denied
+
+For upstream discussion, please visit
+https://www.mail-archive.com/u-boot@lists.denx.de/msg233429.html
+
+Signed-off-by: Ryan Harkin <ryan.harkin@linaro.org>
+Signed-off-by: Asha R <asha.r@arm.com>
+Signed-off-by: Anders Dellien <anders.dellien@arm.com>
+---
+ arch/arm/Kconfig                          |  5 +++
+ board/armltd/vexpress64/Kconfig           |  2 +-
+ configs/vexpress_aemv8a_aarch32_defconfig | 40 ++++++++++++++++++
+ include/configs/vexpress_aemv8.h          | 50 +++++++++++++++--------
+ 4 files changed, 80 insertions(+), 17 deletions(-)
+ create mode 100644 configs/vexpress_aemv8a_aarch32_defconfig
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb84..99cc414d6760 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -1250,6 +1250,11 @@ config TARGET_VEXPRESS64_BASE_FVP
+ 	select PL01X_SERIAL
+ 	select SEMIHOSTING
+ 
++config TARGET_VEXPRESS64_BASE_FVP_AARCH32
++        bool "Support Versatile Express ARMv8a 32-bit FVP BASE model"
++        select CPU_V7A
++        select SEMIHOSTING
++
+ config TARGET_VEXPRESS64_JUNO
+ 	bool "Support Versatile Express Juno Development Platform"
+ 	select ARM64
+diff --git a/board/armltd/vexpress64/Kconfig b/board/armltd/vexpress64/Kconfig
+index 4aab3f092ecb..0a5e3fcc004a 100644
+--- a/board/armltd/vexpress64/Kconfig
++++ b/board/armltd/vexpress64/Kconfig
+@@ -1,4 +1,4 @@
+-if TARGET_VEXPRESS64_BASE_FVP || TARGET_VEXPRESS64_JUNO
++if TARGET_VEXPRESS64_BASE_FVP || TARGET_VEXPRESS64_JUNO || TARGET_VEXPRESS64_BASE_FVP_AARCH32
+ 
+ config SYS_BOARD
+ 	default "vexpress64"
+diff --git a/configs/vexpress_aemv8a_aarch32_defconfig b/configs/vexpress_aemv8a_aarch32_defconfig
+new file mode 100644
+index 000000000000..9c5c3367ec4d
+--- /dev/null
++++ b/configs/vexpress_aemv8a_aarch32_defconfig
+@@ -0,0 +1,40 @@
++CONFIG_ARM=y
++CONFIG_SYS_ARCH_TIMER=y
++CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32=y
++CONFIG_SYS_TEXT_BASE=0x88000000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_NR_DRAM_BANKS=2
++CONFIG_IDENT_STRING=" vexpress_aemv8a fvp aarch32"
++CONFIG_REMAKE_ELF=y
++CONFIG_SYS_LOAD_ADDR=0x90000000
++CONFIG_BOOTDELAY=1
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1c090000 debug user_debug=31 systemd.log_target=null root=/dev/vda1 rw androidboot.hardware=fvpbase rootwait loglevel=9"
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="fvp32# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_EDITENV is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_MEMTEST=y
++CONFIG_CMD_ARMFLASH=y
++# CONFIG_CMD_LOADS is not set
++# CONFIG_CMD_ITEST is not set
++# CONFIG_CMD_SETEXPR is not set
++CONFIG_CMD_DHCP=y
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_PING=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_FAT=y
++CONFIG_DM=y
++CONFIG_MTD_NOR_FLASH=y
++CONFIG_FLASH_CFI_DRIVER=y
++CONFIG_SYS_FLASH_CFI=y
++CONFIG_DM_SERIAL=y
++CONFIG_PL01X_SERIAL=y
++CONFIG_OF_LIBFDT=y
++CONFIG_REMAKE_ELF=y
+diff --git a/include/configs/vexpress_aemv8.h b/include/configs/vexpress_aemv8.h
+index f0c5ceb3849a..854fbb41bfc1 100644
+--- a/include/configs/vexpress_aemv8.h
++++ b/include/configs/vexpress_aemv8.h
+@@ -86,7 +86,7 @@
+ #endif
+ #endif /* !CONFIG_GICV3 */
+ 
+-#if defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) && !defined(CONFIG_DM_ETH)
++#if (defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) || defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32)) && !defined(CONFIG_DM_ETH)
+ /* The Vexpress64 BASE_FVP simulator uses SMSC91C111 */
+ #define CONFIG_SMC91111			1
+ #define CONFIG_SMC91111_BASE		(V2M_PA_BASE + 0x01A000000)
+@@ -114,7 +114,7 @@
+ #ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ #define PHYS_SDRAM_2			(0x880000000)
+ #define PHYS_SDRAM_2_SIZE		0x180000000
+-#elif CONFIG_NR_DRAM_BANKS == 2
++#elif CONFIG_TARGET_VEXPRESS64_BASE_FVP && CONFIG_NR_DRAM_BANKS == 2
+ #define PHYS_SDRAM_2			(0x880000000)
+ #define PHYS_SDRAM_2_SIZE		0x80000000
+ #endif
+@@ -171,23 +171,41 @@
+ 				"fdt_addr_r=0x80000000\0" \
+ 				BOOTENV
+ 
+-#elif CONFIG_TARGET_VEXPRESS64_BASE_FVP
++#elif defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) || \
++        defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32)
+ 
+-#define VEXPRESS_KERNEL_ADDR	0x80080000
+-#define VEXPRESS_FDT_ADDR	0x8fc00000
+-#define VEXPRESS_BOOT_ADDR	0x8fd00000
+-#define VEXPRESS_RAMDISK_ADDR	0x8fe00000
++#define VEXPRESS_KERNEL_ADDR   0x80080000
++#define VEXPRESS_FDT_ADDR      0x8fc00000
++#define VEXPRESS_BOOT_ADDR     0x8fd00000
++#define VEXPRESS_RAMDISK_ADDR  0x8fe00000
+ 
+-#define CONFIG_EXTRA_ENV_SETTINGS	\
++#define CONFIG_EXTRA_ENV_SETTINGS      \
+ 				"kernel_name=Image\0"		\
+-				"kernel_addr_r=" __stringify(VEXPRESS_KERNEL_ADDR) "\0"	\
+-				"ramdisk_name=ramdisk.img\0"	\
+-				"ramdisk_addr_r=" __stringify(VEXPRESS_RAMDISK_ADDR) "\0" \
+-				"fdtfile=devtree.dtb\0"	\
+-				"fdt_addr_r=" __stringify(VEXPRESS_FDT_ADDR) "\0"	\
+-				"boot_name=boot.img\0" \
+-				"boot_addr_r=" __stringify(VEXPRESS_BOOT_ADDR) "\0"
+-
++                                "kernel_addr_r=" __stringify(VEXPRESS_KERNEL_ADDR) "\0" \
++                                "ramdisk_name=ramdisk.img\0"    \
++                                "ramdisk_addr_r=" __stringify(VEXPRESS_RAMDISK_ADDR) "\0" \
++                                "fdtfile=devtree.dtb\0" \
++                                "fdt_addr_r=" __stringify(VEXPRESS_FDT_ADDR) "\0"       \
++                                "boot_name=boot.img\0" \
++                                "boot_addr_r=" __stringify(VEXPRESS_BOOT_ADDR) "\0"
++
++#ifndef CONFIG_BOOTCOMMAND
++#define CONFIG_BOOTCOMMAND	"if smhload ${boot_name} ${boot_addr_r}; then " \
++				"  set bootargs; " \
++				"  abootimg addr ${boot_addr_r}; " \
++				"  abootimg get dtb --index=0 fdt_addr_r; " \
++				"  bootm ${boot_addr_r} ${boot_addr_r} " \
++				"  ${fdt_addr_r}; " \
++				"else; " \
++				"  smhload ${kernel_name} ${kernel_addr_r}; " \
++				"  smhload ${fdtfile} ${fdt_addr_r}; " \
++				"  smhload ${ramdisk_name} ${initrd_addr_r} "\
++				"  initrd_end; " \
++				"  fdt addr ${fdt_addr_r}; fdt resize; " \
++				"  fdt chosen ${ramdisk_addr_r} ${initrd_end}; " \
++				"  bootz $kernel_addr_r - $fdt_addr_r; " \
++				"fi"
++#endif
+ #endif
+ 
+ /* Monitor Command Prompt */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch
new file mode 100644
index 0000000..d916d42
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch
@@ -0,0 +1,111 @@
+From e896d48c57d272327410416887f34ac0db550390 Mon Sep 17 00:00:00 2001
+From: Jon Mason <jdmason@kudzu.us>
+Date: Mon, 13 Jun 2022 10:59:53 -0400
+Subject: [PATCH 2/2] Revert "vexpress64: Enable OF_CONTROL and OF_BOARD for
+ VExpress64"
+
+This patch only works for aarch64 (as the 'x' registers are not
+available for ARMv7). Since this platform is ARMv7 in the previous
+patch, this either needs to be changed or removed.  I opted to remove
+it, as it doesn't seem to be necessary to boot the virtual hardware.
+Given that the previous patch was rejected upstream, it is not
+appropriate to fix this upstream.
+
+Upstream-Status: Inappropriate
+Signed-off-by: Jon Mason <jon.mason@arm.com>
+
+This reverts commit 2661397464e47d45cd25bbc5e6b9de7594b3268d.
+---
+ board/armltd/vexpress64/Makefile        |  2 +-
+ board/armltd/vexpress64/lowlevel_init.S | 12 ------------
+ board/armltd/vexpress64/vexpress64.c    | 26 -------------------------
+ 3 files changed, 1 insertion(+), 39 deletions(-)
+ delete mode 100644 board/armltd/vexpress64/lowlevel_init.S
+
+diff --git a/board/armltd/vexpress64/Makefile b/board/armltd/vexpress64/Makefile
+index 1878fbed4ec9..868dc4f629f2 100644
+--- a/board/armltd/vexpress64/Makefile
++++ b/board/armltd/vexpress64/Makefile
+@@ -3,5 +3,5 @@
+ # (C) Copyright 2000-2004
+ # Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ 
+-obj-y	:= vexpress64.o lowlevel_init.o
++obj-y	:= vexpress64.o
+ obj-$(CONFIG_TARGET_VEXPRESS64_JUNO)	+= pcie.o
+diff --git a/board/armltd/vexpress64/lowlevel_init.S b/board/armltd/vexpress64/lowlevel_init.S
+deleted file mode 100644
+index 3dcfb85d0e9a..000000000000
+--- a/board/armltd/vexpress64/lowlevel_init.S
++++ /dev/null
+@@ -1,12 +0,0 @@
+-/* SPDX-License-Identifier: GPL-2.0 */
+-/*
+- * (C) Copyright 2021 Arm Limited
+- */
+-
+-.global save_boot_params
+-save_boot_params:
+-
+-	adr	x8, prior_stage_fdt_address
+-	str	x0, [x8]
+-
+-	b	save_boot_params_ret
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 5e22e89824ee..cedab86d984b 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -92,15 +92,7 @@ int dram_init_banksize(void)
+ 	return 0;
+ }
+ 
+-/* Assigned in lowlevel_init.S
+- * Push the variable into the .data section so that it
+- * does not get cleared later.
+- */
+-unsigned long __section(".data") prior_stage_fdt_address;
+-
+ #ifdef CONFIG_OF_BOARD
+-
+-#ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ #define JUNO_FLASH_SEC_SIZE	(256 * 1024)
+ static phys_addr_t find_dtb_in_nor_flash(const char *partname)
+ {
+@@ -145,11 +137,9 @@ static phys_addr_t find_dtb_in_nor_flash(const char *partname)
+ 
+ 	return ~0;
+ }
+-#endif
+ 
+ void *board_fdt_blob_setup(int *err)
+ {
+-#ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ 	phys_addr_t fdt_rom_addr = find_dtb_in_nor_flash(CONFIG_JUNO_DTB_PART);
+ 
+ 	*err = 0;
+@@ -159,22 +149,6 @@ void *board_fdt_blob_setup(int *err)
+ 	}
+ 
+ 	return (void *)fdt_rom_addr;
+-#endif
+-
+-#ifdef VEXPRESS_FDT_ADDR
+-	if (fdt_magic(VEXPRESS_FDT_ADDR) == FDT_MAGIC) {
+-		*err = 0;
+-		return (void *)VEXPRESS_FDT_ADDR;
+-	}
+-#endif
+-
+-	if (fdt_magic(prior_stage_fdt_address) == FDT_MAGIC) {
+-		*err = 0;
+-		return (void *)prior_stage_fdt_address;
+-	}
+-
+-	*err = -ENXIO;
+-	return NULL;
+ }
+ #endif
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg
new file mode 100644
index 0000000..716600f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg
@@ -0,0 +1,3 @@
+CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1c090000 root=/dev/vda1 rw rootwait"
+# Our FVP support CRC instructions
+CONFIG_ARM64_CRC32=y
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch
new file mode 100644
index 0000000..dd6b77d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch
@@ -0,0 +1,259 @@
+From e90aa7853ae32cb03c86249a6c572ec88cdebaa2 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 26 May 2021 17:41:10 +0100
+Subject: [PATCH 1/9] armv8: Add ARMv8 MPU configuration logic
+
+Detect whether an MMU is present at the current exception level. If
+not, initialize the MPU instead of the MMU during init, and clear the
+MPU regions before transition to Linux.
+
+The MSA in use at EL1&0 may be configurable but can only by determined
+by inspecting VTCR_EL2 at EL2, so assume that there is an MMU for
+backwards compatibility.
+
+Provide a default (blank) MPU memory map, which can be overridden by
+board configurations.
+
+Issue-Id: SCM-2443
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I0ee3879f9d7f03fe940664b3551c68eeaa458d17
+---
+ arch/arm/cpu/armv8/cache_v8.c    | 101 ++++++++++++++++++++++++++++++-
+ arch/arm/include/asm/armv8/mpu.h |  59 ++++++++++++++++++
+ arch/arm/include/asm/system.h    |  19 ++++++
+ 3 files changed, 176 insertions(+), 3 deletions(-)
+ create mode 100644 arch/arm/include/asm/armv8/mpu.h
+
+diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
+index e4736e5643..798aed8058 100644
+--- a/arch/arm/cpu/armv8/cache_v8.c
++++ b/arch/arm/cpu/armv8/cache_v8.c
+@@ -15,6 +15,7 @@
+ #include <asm/global_data.h>
+ #include <asm/system.h>
+ #include <asm/armv8/mmu.h>
++#include <asm/armv8/mpu.h>
+ 
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+@@ -385,6 +386,91 @@ __weak u64 get_page_table_size(void)
+ 	return size;
+ }
+ 
++static void mpu_clear_regions(void)
++{
++	int i;
++
++	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++		setup_el2_mpu_region(i, 0, 0);
++	}
++}
++
++static struct mpu_region default_mpu_mem_map[] = {{0,}};
++__weak struct mpu_region *mpu_mem_map = default_mpu_mem_map;
++
++static void mpu_setup(void)
++{
++	int i;
++
++	if (current_el() != 2) {
++		panic("MPU configuration is only supported at EL2");
++	}
++
++	set_sctlr(get_sctlr() & ~(CR_M | CR_WXN));
++
++	asm volatile("msr MAIR_EL2, %0" : : "r" MEMORY_ATTRIBUTES);
++
++	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++		setup_el2_mpu_region(i,
++			PRBAR_ADDRESS(mpu_mem_map[i].start)
++				| PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++			PRLAR_ADDRESS(mpu_mem_map[i].end)
++				| mpu_mem_map[i].attrs | PRLAR_EN_BIT
++			);
++	}
++
++	set_sctlr(get_sctlr() | CR_M);
++}
++
++static bool el_has_mmu(void)
++{
++	if (current_el() < 2) {
++		// We have no way of knowing, so assuming we have an MMU
++		return true;
++	}
++
++	uint64_t id_aa64mmfr0;
++	asm volatile("mrs %0, id_aa64mmfr0_el1"
++			: "=r" (id_aa64mmfr0) : : "cc");
++	uint64_t msa = id_aa64mmfr0 & ID_AA64MMFR0_EL1_MSA_MASK;
++	uint64_t msa_frac = id_aa64mmfr0 & ID_AA64MMFR0_EL1_MSA_FRAC_MASK;
++
++	switch (msa) {
++		case ID_AA64MMFR0_EL1_MSA_VMSA:
++			/*
++			 * VMSA supported in all translation regimes.
++			 * No support for PMSA.
++			 */
++			return true;
++		case ID_AA64MMFR0_EL1_MSA_USE_FRAC:
++			/* See MSA_frac for the supported MSAs. */
++			switch (msa_frac) {
++				case ID_AA64MMFR0_EL1_MSA_FRAC_NO_PMSA:
++					/*
++					 * PMSA not supported in any translation
++					 * regime.
++					 */
++					return true;
++				case ID_AA64MMFR0_EL1_MSA_FRAC_VMSA:
++					/*
++					* PMSA supported in all translation
++					* regimes. No support for VMSA.
++					*/
++				case ID_AA64MMFR0_EL1_MSA_FRAC_PMSA:
++					/*
++					 * PMSA supported in all translation
++					 * regimes.
++					 */
++					return false;
++				default:
++					panic("Unsupported id_aa64mmfr0_el1 " \
++						"MSA_frac value");
++			}
++		default:
++			panic("Unsupported id_aa64mmfr0_el1 MSA value");
++	}
++}
++
+ void setup_pgtables(void)
+ {
+ 	int i;
+@@ -499,8 +585,13 @@ void dcache_enable(void)
+ 	/* The data cache is not active unless the mmu is enabled */
+ 	if (!(get_sctlr() & CR_M)) {
+ 		invalidate_dcache_all();
+-		__asm_invalidate_tlb_all();
+-		mmu_setup();
++
++		if (el_has_mmu()) {
++			__asm_invalidate_tlb_all();
++			mmu_setup();
++		} else {
++			mpu_setup();
++		}
+ 	}
+ 
+ 	set_sctlr(get_sctlr() | CR_C);
+@@ -519,7 +610,11 @@ void dcache_disable(void)
+ 	set_sctlr(sctlr & ~(CR_C|CR_M));
+ 
+ 	flush_dcache_all();
+-	__asm_invalidate_tlb_all();
++
++	if (el_has_mmu())
++		__asm_invalidate_tlb_all();
++	else
++		mpu_clear_regions();
+ }
+ 
+ int dcache_status(void)
+diff --git a/arch/arm/include/asm/armv8/mpu.h b/arch/arm/include/asm/armv8/mpu.h
+new file mode 100644
+index 0000000000..8de627cafd
+--- /dev/null
++++ b/arch/arm/include/asm/armv8/mpu.h
+@@ -0,0 +1,59 @@
++/*
++ * SPDX-License-Identifier: GPL-2.0+
++ *
++ * (C) Copyright 2021 Arm Limited
++ */
++
++#ifndef _ASM_ARMV8_MPU_H_
++#define _ASM_ARMV8_MPU_H_
++
++#include <asm/armv8/mmu.h>
++#include <linux/stringify.h>
++
++#define PRSELR_EL2		S3_4_c6_c2_1
++#define PRBAR_EL2		S3_4_c6_c8_0
++#define PRLAR_EL2		S3_4_c6_c8_1
++#define MPUIR_EL2		S3_4_c0_c0_4
++
++#define PRBAR_ADDRESS(addr)	((addr) & ~(0x3fULL))
++
++/* Access permissions */
++#define PRBAR_AP(val)		(((val) & 0x3) << 2)
++#define PRBAR_AP_RW_HYP		PRBAR_AP(0x0)
++#define PRBAR_AP_RW_ANY		PRBAR_AP(0x1)
++#define PRBAR_AP_RO_HYP		PRBAR_AP(0x2)
++#define PRBAR_AP_RO_ANY		PRBAR_AP(0x3)
++
++/* Shareability */
++#define PRBAR_SH(val)		(((val) & 0x3) << 4)
++#define PRBAR_NON_SH		PRBAR_SH(0x0)
++#define PRBAR_OUTER_SH		PRBAR_SH(0x2)
++#define PRBAR_INNER_SH		PRBAR_SH(0x3)
++
++/* Memory attribute (MAIR idx) */
++#define PRLAR_ATTRIDX(val)	(((val) & 0x7) << 1)
++#define PRLAR_EN_BIT		(0x1)
++#define PRLAR_ADDRESS(addr)	((addr) & ~(0x3fULL))
++
++#ifndef __ASSEMBLY__
++
++static inline void setup_el2_mpu_region(uint8_t region, uint64_t base, uint64_t limit)
++{
++	asm volatile("msr " __stringify(PRSELR_EL2) ", %0" : : "r" (region));
++	asm volatile("msr " __stringify(PRBAR_EL2) ", %0" : : "r" (base));
++	asm volatile("msr " __stringify(PRLAR_EL2) ", %0" : : "r" (limit));
++
++	asm volatile("isb");
++}
++
++#endif
++
++struct mpu_region {
++	u64 start;
++	u64 end;
++	u64 attrs;
++};
++
++extern struct mpu_region *mpu_mem_map;
++
++#endif /* _ASM_ARMV8_MPU_H_ */
+diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
+index 87d1c77e8b..4510db98a2 100644
+--- a/arch/arm/include/asm/system.h
++++ b/arch/arm/include/asm/system.h
+@@ -95,6 +95,25 @@
+ 				               auth algorithm                 */
+ #define ID_AA64ISAR1_EL1_APA	(0xF << 4)  /* QARMA address auth algorithm   */
+ 
++/*
++ * ID_AA64MMFR0_EL1 bits definitions
++ */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_MASK		(0xFUL << 52) /* Memory system
++								 architecture
++								 frac         */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_VMSA		(0x2UL << 52) /* EL1&0 supports
++								 VMSA         */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_PMSA		(0x1UL << 52) /* EL1&0 only
++							         supports PMSA*/
++#define ID_AA64MMFR0_EL1_MSA_FRAC_NO_PMSA	(0x0UL << 52) /* No PMSA
++								 support      */
++#define ID_AA64MMFR0_EL1_MSA_MASK		(0xFUL << 48) /* Memory system
++								 architecture */
++#define ID_AA64MMFR0_EL1_MSA_USE_FRAC		(0xFUL << 48) /* Use MSA_FRAC */
++#define ID_AA64MMFR0_EL1_MSA_VMSA		(0x0UL << 48) /* Memory system
++								 architecture
++								 is VMSA      */
++
+ /*
+  * ID_AA64PFR0_EL1 bits definitions
+  */
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch
new file mode 100644
index 0000000..f405457
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch
@@ -0,0 +1,61 @@
+From 80c64d0c035a5cd0cb23c7b01be39f25e15c7193 Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Tue, 26 Jul 2022 18:13:23 +0800
+Subject: [PATCH 2/9] vexpress64: add MPU memory map for the BASER_FVP
+
+The previous patch added support for initializing an Armv8 MPU. There is only an
+MPU at S-EL2 on the BASER_FVP, so add a platform-specific MPU memory map.
+
+See https://developer.arm.com/documentation/100964/1117/Base-Platform/Base---memory/BaseR-Platform-memory-map
+
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+---
+ board/armltd/vexpress64/vexpress64.c | 22 ++++++++++++++++++++++
+ 1 file changed, 22 insertions(+)
+
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 709ebf3fb0..1791cd986f 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -19,6 +19,7 @@
+ #include <dm/platform_data/serial_pl01x.h>
+ #include "pcie.h"
+ #include <asm/armv8/mmu.h>
++#include <asm/armv8/mpu.h>
+ #ifdef CONFIG_VIRTIO_NET
+ #include <virtio_types.h>
+ #include <virtio.h>
+@@ -37,6 +38,27 @@ U_BOOT_DRVINFO(vexpress_serials) = {
+ 	.plat = &serial_plat,
+ };
+ 
++static struct mpu_region vexpress64_aemv8r_mem_map[] = {
++	{
++		.start = 0x0UL,
++		.end = 0x7fffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++	}, {
++		.start = 0x80000000UL,
++		.end = 0xffffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
++	}, {
++		.start = 0x100000000UL,
++		.end = 0xffffffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++	}, {
++		/* List terminator */
++		0,
++	}
++};
++
++struct mpu_region *mpu_mem_map = vexpress64_aemv8r_mem_map;
++
+ static struct mm_region vexpress64_mem_map[] = {
+ 	{
+ 		.virt = V2M_PA_BASE,
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch
new file mode 100644
index 0000000..e6ccc17
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch
@@ -0,0 +1,107 @@
+From 61a2df4467c13bbc83bff8a3429e9ebb4b957b56 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Fri, 10 Dec 2021 11:41:19 +0000
+Subject: [PATCH 3/9] armv8: Allow disabling exception vectors on non-SPL
+ builds
+
+On the BASER_FVP, U-Boot shares EL2 with another bootloader, so we do
+not wish to overide the exception vector, but we are also not using an
+SPL build.
+
+Therefore, add ARMV8_EXCEPTION_VECTORS, which disables exception vectors
+in a similar way to ARMV8_SPL_EXCEPTION_VECTORS.
+
+Rename ARMV8_SPL_EXCEPTION_VECTORS -> SPL_ARMV8_EXCEPTION_VECTORS so
+that both config flags be be targeted using CONFIG_IS_ENABLED.
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I0cf0fc6d7ef4d45791411cf1f67c65e198cc8b2b
+---
+ arch/arm/cpu/armv8/Kconfig        | 10 ++++++++--
+ arch/arm/cpu/armv8/Makefile       |  6 ++----
+ arch/arm/cpu/armv8/start.S        |  4 ++--
+ configs/vexpress_aemv8r_defconfig |  1 +
+ 4 files changed, 13 insertions(+), 8 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 09f3f50fa2..031faa909c 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -1,8 +1,8 @@
+ if ARM64
+ 
+-config ARMV8_SPL_EXCEPTION_VECTORS
++config ARMV8_EXCEPTION_VECTORS
+ 	bool "Install crash dump exception vectors"
+-	depends on SPL
++	default y
+ 	help
+ 	  The default exception vector table is only used for the crash
+ 	  dump, but still takes quite a lot of space in the image size.
+@@ -10,6 +10,12 @@ config ARMV8_SPL_EXCEPTION_VECTORS
+ 	  Say N here if you are running out of code space in the image
+ 	  and want to save some space at the cost of less debugging info.
+ 
++config SPL_ARMV8_EXCEPTION_VECTORS
++	bool "Install crash dump exception vectors in the SPL"
++	depends on SPL
++	help
++	  Same as ARMV8_EXCEPTION_VECTORS, but for SPL builds
++
+ config ARMV8_MULTIENTRY
+         bool "Enable multiple CPUs to enter into U-Boot"
+ 
+diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
+index 85fe0475c8..8b3f695835 100644
+--- a/arch/arm/cpu/armv8/Makefile
++++ b/arch/arm/cpu/armv8/Makefile
+@@ -13,10 +13,8 @@ ifndef CONFIG_$(SPL_)SYS_DCACHE_OFF
+ obj-y	+= cache_v8.o
+ obj-y	+= cache.o
+ endif
+-ifdef CONFIG_SPL_BUILD
+-obj-$(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) += exceptions.o
+-else
+-obj-y	+= exceptions.o
++obj-$(CONFIG_$(SPL_)ARMV8_EXCEPTION_VECTORS) += exceptions.o
++ifndef CONFIG_SPL_BUILD
+ obj-y	+= exception_level.o
+ endif
+ obj-y	+= tlb.o
+diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
+index 28f0df13f0..f831e77af3 100644
+--- a/arch/arm/cpu/armv8/start.S
++++ b/arch/arm/cpu/armv8/start.S
+@@ -104,7 +104,7 @@ pie_skip_reloc:
+ pie_fixup_done:
+ #endif
+ 
+-#if defined(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) || !defined(CONFIG_SPL_BUILD)
++#if CONFIG_IS_ENABLED(ARMV8_EXCEPTION_VECTORS)
+ .macro	set_vbar, regname, reg
+ 	msr	\regname, \reg
+ .endm
+@@ -354,7 +354,7 @@ ENDPROC(smp_kick_all_cpus)
+ /*-----------------------------------------------------------------------*/
+ 
+ ENTRY(c_runtime_cpu_setup)
+-#if defined(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) || !defined(CONFIG_SPL_BUILD)
++#if CONFIG_IS_ENABLED(ARMV8_EXCEPTION_VECTORS)
+ 	/* Relocate vBAR */
+ 	adr	x0, vectors
+ 	switch_el x1, 3f, 2f, 1f
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index a1c5d88717..1d5b7411f0 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -13,3 +13,4 @@ CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x9c090000 rootfstype=ext4 root=
+ CONFIG_SYS_PROMPT="VExpress64# "
+ # CONFIG_MMC is not set
+ CONFIG_VIRTIO_MMIO=y
++CONFIG_ARMV8_EXCEPTION_VECTORS=n
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch
new file mode 100644
index 0000000..84f6b4b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch
@@ -0,0 +1,160 @@
+From dbc1a218e9837e39cd50dd3c19f603f29a08ddba Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 14 Jul 2021 12:44:27 +0100
+Subject: [PATCH 4/9] armv8: ARMV8_SWITCH_TO_EL1 improvements
+
+Convert CONFIG_ARMV8_SWITCH_TO_EL1 to a Kconfig variable.
+
+Add support for switching to EL1 to bootefi.
+
+Add the environment variable armv8_switch_to_el1 to allow configuring
+whether to switch to EL1 at runtime. This overrides the compile-time
+option.
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: If98478148d6d8d1f732acac5439276700614815f
+---
+ arch/arm/cpu/armv8/Kconfig           |  8 +++++++
+ arch/arm/cpu/armv8/exception_level.c | 21 ++++++++++++++--
+ arch/arm/lib/bootm.c                 | 36 ++++++++++++++++------------
+ configs/vexpress_aemv8r_defconfig    |  1 +
+ 4 files changed, 49 insertions(+), 17 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 031faa909c..110adf63b3 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -191,4 +191,12 @@ config ARMV8_EA_EL3_FIRST
+ 	  Exception handling at all exception levels for External Abort and
+ 	  SError interrupt exception are taken in EL3.
+ 
++config ARMV8_SWITCH_TO_EL1
++	bool "Switch to EL1 before booting the operating system"
++	default n
++	help
++	  Switch to EL1 before booting the operating system, if for example the
++	  operating system does not support booting at EL2, or you wish to prevent
++	  any hypervisors from running. Supported for bootm, booti and bootefi.
++
+ endif
+diff --git a/arch/arm/cpu/armv8/exception_level.c b/arch/arm/cpu/armv8/exception_level.c
+index b11936548f..4aad1550f4 100644
+--- a/arch/arm/cpu/armv8/exception_level.c
++++ b/arch/arm/cpu/armv8/exception_level.c
+@@ -40,19 +40,36 @@ static void entry_non_secure(struct jmp_buf_data *non_secure_jmp)
+  * trusted firmware being one embodiment). The operating system shall be
+  * started at exception level EL2. So here we check the exception level
+  * and switch it if necessary.
++ *
++ * If armv8_switch_to_el1 (config or env var) is enabled, also switch to EL1
++ * before booting the operating system.
+  */
+ void switch_to_non_secure_mode(void)
+ {
+ 	struct jmp_buf_data non_secure_jmp;
+ 
+ 	/* On AArch64 we need to make sure we call our payload in < EL3 */
+-	if (current_el() == 3) {
++
++	int switch_to_el1 = env_get_yesno("armv8_switch_to_el1");
++#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
++	if (switch_to_el1 == -1) {
++			switch_to_el1 = 1;
++	}
++#endif
++
++	if (current_el() > 2) {
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL2 */
+-
+ 		/* Move into EL2 and keep running there */
+ 		armv8_switch_to_el2((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
++	} else if (switch_to_el1 == 1 && current_el() > 1) {
++		if (setjmp(&non_secure_jmp))
++			return;
++		dcache_disable();	/* flush cache before switch to EL1 */
++		/* Move into EL1 and keep running there */
++		armv8_switch_to_el1((uintptr_t)&non_secure_jmp, 0, 0, 0,
++				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+ 	}
+ }
+diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
+index a59a5e6c0e..e2cf2e6ec4 100644
+--- a/arch/arm/lib/bootm.c
++++ b/arch/arm/lib/bootm.c
+@@ -272,7 +272,6 @@ __weak void update_os_arch_secondary_cores(uint8_t os_arch)
+ {
+ }
+ 
+-#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+ static void switch_to_el1(void)
+ {
+ 	if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+@@ -287,7 +286,6 @@ static void switch_to_el1(void)
+ 				    ES_TO_AARCH64);
+ }
+ #endif
+-#endif
+ 
+ /* Subcommand: GO */
+ static void boot_jump_linux(bootm_headers_t *images, int flag)
+@@ -314,21 +312,29 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
+ 
+ 		update_os_arch_secondary_cores(images->os.arch);
+ 
++		int armv8_switch_to_el1 = env_get_yesno("armv8_switch_to_el1");
+ #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+-		armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
+-				    (u64)switch_to_el1, ES_TO_AARCH64);
+-#else
+-		if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+-		    (images->os.arch == IH_ARCH_ARM))
+-			armv8_switch_to_el2(0, (u64)gd->bd->bi_arch_number,
+-					    (u64)images->ft_addr, 0,
+-					    (u64)images->ep,
+-					    ES_TO_AARCH32);
+-		else
+-			armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
+-					    images->ep,
+-					    ES_TO_AARCH64);
++		if (armv8_switch_to_el1 == -1) {
++			armv8_switch_to_el1 = 1;
++		}
+ #endif
++		if (armv8_switch_to_el1 == 1) {
++			armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
++					    (u64)switch_to_el1, ES_TO_AARCH64);
++		} else {
++			if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
++					(images->os.arch == IH_ARCH_ARM))
++				armv8_switch_to_el2(0,
++						    (u64)gd->bd->bi_arch_number,
++						    (u64)images->ft_addr, 0,
++						    (u64)images->ep,
++						    ES_TO_AARCH32);
++			else
++				armv8_switch_to_el2((u64)images->ft_addr,
++						    0, 0, 0,
++						    images->ep,
++						    ES_TO_AARCH64);
++		}
+ 	}
+ #else
+ 	unsigned long machid = gd->bd->bi_arch_number;
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 1d5b7411f0..35e5e8a5e1 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -14,3 +14,4 @@ CONFIG_SYS_PROMPT="VExpress64# "
+ # CONFIG_MMC is not set
+ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
++CONFIG_ARMV8_SWITCH_TO_EL1=y
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch
new file mode 100644
index 0000000..b546026
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch
@@ -0,0 +1,83 @@
+From f75060be04aaed4bb5a07cb40361d694b3775f4a Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Fri, 10 Dec 2021 16:37:26 +0000
+Subject: [PATCH 5/9] armv8: Make disabling HVC configurable when switching to
+ EL1
+
+On the BASER_FVP there is no EL3, so HVC is used to provide PSCI
+services. Therefore we cannot disable hypercalls.
+
+Create CONFIG_ARMV8_DISABLE_HVC (dependent on CONFIG_ARMV8_TO_EL1) to
+control whether to disable HVC exceptions in HCR_EL2->HCD
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I463d82f1db8a3cafcab40a9c0c208753569cc300
+---
+ arch/arm/cpu/armv8/Kconfig        |  9 +++++++++
+ arch/arm/include/asm/macro.h      | 10 ++++++++--
+ configs/vexpress_aemv8r_defconfig |  1 +
+ 3 files changed, 18 insertions(+), 2 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 110adf63b3..a821e219d8 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -199,4 +199,13 @@ config ARMV8_SWITCH_TO_EL1
+ 	  operating system does not support booting at EL2, or you wish to prevent
+ 	  any hypervisors from running. Supported for bootm, booti and bootefi.
+ 
++config ARMV8_DISABLE_HVC
++	bool "Disable HVC calls before switching to EL1"
++	depends on ARMV8_SWITCH_TO_EL1
++	default y
++	help
++	  If switching to EL1 before loading the operating system, disable taking
++	  hypercalls back to EL2. May be disabled if, for example, PSCI services are
++	  running at EL2.
++
+ endif
+diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
+index 1a1edc9870..7167739210 100644
+--- a/arch/arm/include/asm/macro.h
++++ b/arch/arm/include/asm/macro.h
+@@ -296,9 +296,12 @@ lr	.req	x30
+ 	ldr	\tmp2, =(ID_AA64ISAR1_EL1_GPI | ID_AA64ISAR1_EL1_GPA | \
+ 		      ID_AA64ISAR1_EL1_API | ID_AA64ISAR1_EL1_APA)
+ 	tst	\tmp, \tmp2
+-	mov	\tmp2, #(HCR_EL2_RW_AARCH64 | HCR_EL2_HCD_DIS)
++	mov	\tmp2, #(HCR_EL2_RW_AARCH64)
+ 	orr	\tmp, \tmp2, #(HCR_EL2_APK | HCR_EL2_API)
+ 	csel	\tmp, \tmp2, \tmp, eq
++#ifdef CONFIG_ARMV8_DISABLE_HVC
++	orr	\tmp, \tmp, #(HCR_EL2_HCD_DIS)
++#endif
+ 	msr	hcr_el2, \tmp
+ 
+ 	/* Return to the EL1_SP1 mode from EL2 */
+@@ -311,7 +314,10 @@ lr	.req	x30
+ 
+ 1:
+ 	/* Initialize HCR_EL2 */
+-	ldr	\tmp, =(HCR_EL2_RW_AARCH32 | HCR_EL2_HCD_DIS)
++	ldr	\tmp, =(HCR_EL2_RW_AARCH32)
++#ifdef CONFIG_ARMV8_DISABLE_HVC
++	orr	\tmp, \tmp, #(HCR_EL2_HCD_DIS)
++#endif
+ 	msr	hcr_el2, \tmp
+ 
+ 	/* Return to AArch32 Supervisor mode from EL2 */
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 35e5e8a5e1..605a724a61 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -15,3 +15,4 @@ CONFIG_SYS_PROMPT="VExpress64# "
+ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
+ CONFIG_ARMV8_SWITCH_TO_EL1=y
++CONFIG_ARMV8_DISABLE_HVC=n
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch
new file mode 100644
index 0000000..ca99d0e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch
@@ -0,0 +1,32 @@
+From db7d4cc91ae19d0cbd05c0ff1b9bf6a55ac0d04a Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Thu, 28 Jul 2022 17:47:18 +0800
+Subject: [PATCH 6/9] vexpress64: Do not set COUNTER_FREQUENCY
+
+VExpress boards normally run as a second-stage bootloader so should not
+need to modify CNTFRQ_EL0. On the BASER_FVP, U-Boot can modify it if
+running at EL2, but shouldn't because it might be different from the
+value being used by the first-stage bootloader (which might be
+providing PSCI services).
+
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+---
+ configs/vexpress_aemv8r_defconfig | 1 -
+ 1 file changed, 1 deletion(-)
+
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 605a724a61..153fa14efc 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -1,5 +1,4 @@
+ CONFIG_ARM=y
+-CONFIG_COUNTER_FREQUENCY=24000000
+ CONFIG_ARCH_VEXPRESS64=y
+ CONFIG_NR_DRAM_BANKS=2
+ CONFIG_DEFAULT_DEVICE_TREE="arm_fvp"
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch
new file mode 100644
index 0000000..467fb40
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch
@@ -0,0 +1,27 @@
+From f4986ed954ffad36abfa27db5520e702cba2531e Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Tue, 22 Feb 2022 15:32:51 +0000
+Subject: [PATCH 7/9] vexpress64: Enable LIBFDT_OVERLAY in the vexpress_aemv8r
+ defconfig
+
+Issue-Id: SCM-3874
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: Ide0532cf2de89f1bca9c8d4bd2ed0c1a1c57599f
+---
+ configs/vexpress_aemv8r_defconfig | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 153fa14efc..94651768b3 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -15,3 +15,4 @@ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
+ CONFIG_ARMV8_SWITCH_TO_EL1=y
+ CONFIG_ARMV8_DISABLE_HVC=n
++CONFIG_OF_LIBFDT_OVERLAY=y
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch
new file mode 100644
index 0000000..4b6daf8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch
@@ -0,0 +1,105 @@
+From af05764e7b64e0704291ba2e71138f2345737afa Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 18 May 2022 15:24:19 +0100
+Subject: [PATCH 8/9] armv8: Allow PRBAR MPU attributes to be configured
+
+In a previous patch, support was added to initialize an S-EL2 MPU on
+armv8r64 machines. This implementation allowed the PRLAR attribute
+index to be configured, but not the shareability and access permission
+attributes in PRBAR. These attributes were hard-coded as "outer
+shareable" and "read/write at EL1 and EL0".
+
+Add separate prlar_attrs and prbar_attrs to the MPU region struct so
+that these attributes can be configured on a per-region basis.
+
+For the BASER_FVP, ensure the MPU memory attributes match those in the
+existing vexpress64 board MMU configuration ("non shareable" for device
+memory and "inner shareable" for normal memory).
+
+Issue-Id: SCM-4641
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Change-Id: I6b72aead91ad12412262aa32c61a53e12eab3984
+---
+ arch/arm/cpu/armv8/cache_v8.c        | 12 ++++++++----
+ arch/arm/include/asm/armv8/mpu.h     |  3 ++-
+ board/armltd/vexpress64/vexpress64.c |  9 ++++++---
+ 3 files changed, 16 insertions(+), 8 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
+index 798aed8058..e336339281 100644
+--- a/arch/arm/cpu/armv8/cache_v8.c
++++ b/arch/arm/cpu/armv8/cache_v8.c
+@@ -390,7 +390,9 @@ static void mpu_clear_regions(void)
+ {
+ 	int i;
+ 
+-	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++	for (i = 0; mpu_mem_map[i].end ||
++	     mpu_mem_map[i].prbar_attrs ||
++	     mpu_mem_map[i].prlar_attrs; i++) {
+ 		setup_el2_mpu_region(i, 0, 0);
+ 	}
+ }
+@@ -410,12 +412,14 @@ static void mpu_setup(void)
+ 
+ 	asm volatile("msr MAIR_EL2, %0" : : "r" MEMORY_ATTRIBUTES);
+ 
+-	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++	for (i = 0; mpu_mem_map[i].end ||
++	     mpu_mem_map[i].prbar_attrs ||
++	     mpu_mem_map[i].prlar_attrs; i++) {
+ 		setup_el2_mpu_region(i,
+ 			PRBAR_ADDRESS(mpu_mem_map[i].start)
+-				| PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++				| mpu_mem_map[i].prbar_attrs,
+ 			PRLAR_ADDRESS(mpu_mem_map[i].end)
+-				| mpu_mem_map[i].attrs | PRLAR_EN_BIT
++				| mpu_mem_map[i].prlar_attrs | PRLAR_EN_BIT
+ 			);
+ 	}
+ 
+diff --git a/arch/arm/include/asm/armv8/mpu.h b/arch/arm/include/asm/armv8/mpu.h
+index 8de627cafd..dd4c689ea6 100644
+--- a/arch/arm/include/asm/armv8/mpu.h
++++ b/arch/arm/include/asm/armv8/mpu.h
+@@ -51,7 +51,8 @@ static inline void setup_el2_mpu_region(uint8_t region, uint64_t base, uint64_t
+ struct mpu_region {
+ 	u64 start;
+ 	u64 end;
+-	u64 attrs;
++	u64 prbar_attrs;
++	u64 prlar_attrs;
+ };
+ 
+ extern struct mpu_region *mpu_mem_map;
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 1791cd986f..7858521fec 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -42,15 +42,18 @@ static struct mpu_region vexpress64_aemv8r_mem_map[] = {
+ 	{
+ 		.start = 0x0UL,
+ 		.end = 0x7fffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++		.prbar_attrs = PRBAR_INNER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_NORMAL)
+ 	}, {
+ 		.start = 0x80000000UL,
+ 		.end = 0xffffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
++		.prbar_attrs = PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
+ 	}, {
+ 		.start = 0x100000000UL,
+ 		.end = 0xffffffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++		.prbar_attrs = PRBAR_INNER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_NORMAL)
+ 	}, {
+ 		/* List terminator */
+ 		0,
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch
new file mode 100644
index 0000000..0e0a248
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch
@@ -0,0 +1,63 @@
+From 0f15f6b02825b042ddc1d753f62cf87f30b1fe12 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Thu, 19 May 2022 09:02:32 +0100
+Subject: [PATCH 9/9] armv8: Enable icache when switching exception levels in
+ bootefi
+
+bootefi calls the function switch_to_non_secure_mode before calling the
+UEFI payload to handle the case where U-Boot is running at EL3.
+
+For AArch64, the UEFI specification states that:
+   The core will be configured as follows:
+     * MMU enabled
+     * Instruction and data caches enabled
+
+These requirements should be followed when switching exception levels
+for EFI applications.
+
+This function already disables and re-enables the data cache prior to
+switching exception levels, but omits the instruction cache, meaning
+the function returns with the instruction cache disabled at the new
+exception level. Fix this by calling icache_disable prior to switching
+exception levels and icache_enable afterwards.
+
+Issue-Id: SCM-4641
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Change-Id: I678cd5ba39b56e124ab7854608289cd14651ce65
+---
+ arch/arm/cpu/armv8/exception_level.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/arch/arm/cpu/armv8/exception_level.c b/arch/arm/cpu/armv8/exception_level.c
+index 4aad1550f4..0a3e5428e7 100644
+--- a/arch/arm/cpu/armv8/exception_level.c
++++ b/arch/arm/cpu/armv8/exception_level.c
+@@ -27,6 +27,7 @@
+ static void entry_non_secure(struct jmp_buf_data *non_secure_jmp)
+ {
+ 	dcache_enable();
++	icache_enable();
+ 	debug("Reached non-secure mode\n");
+ 
+ 	/* Restore stack and registers saved in switch_to_non_secure_mode() */
+@@ -61,6 +62,7 @@ void switch_to_non_secure_mode(void)
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL2 */
++		icache_disable();
+ 		/* Move into EL2 and keep running there */
+ 		armv8_switch_to_el2((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+@@ -68,6 +70,7 @@ void switch_to_non_secure_mode(void)
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL1 */
++		icache_disable();
+ 		/* Move into EL1 and keep running there */
+ 		armv8_switch_to_el1((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch
new file mode 100644
index 0000000..a683839
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch
@@ -0,0 +1,50 @@
+From 13dde05efae99c5261ed213108087d0f7ac9581e Mon Sep 17 00:00:00 2001
+From: Damodar Santhapuri <damodar.santhapuri@arm.com>
+Date: Thu, 5 Nov 2020 22:40:48 +0530
+Subject: [PATCH] arm: juno: add custom bootcmd to autoboot from uEnv.txt file
+
+enable autoboot support with custom bootcmd loads uEnv.txt
+from NOR to DRAM and import.
+
+Signed-off-by: Damodar Santhapuri <damodar.santhapuri@arm.com>
+Upstream-Status: Pending
+---
+ include/configs/vexpress_aemv8.h | 12 ++++++------
+ 1 file changed, 6 insertions(+), 6 deletions(-)
+
+diff --git a/include/configs/vexpress_aemv8.h b/include/configs/vexpress_aemv8.h
+index f0c5ceb3849a..cd7f6c1b9ba0 100644
+--- a/include/configs/vexpress_aemv8.h
++++ b/include/configs/vexpress_aemv8.h
+@@ -137,13 +137,13 @@
+ 		"  afs load ${fdt_alt_name} ${fdt_addr_r}; "\
+ 		"fi ; "\
+ 		"fdt addr ${fdt_addr_r}; fdt resize; " \
+-		"if afs load  ${ramdisk_name} ${ramdisk_addr_r} ; "\
++		"if afs load  ${initrd_name} ${initrd_addr_r} ; "\
+ 		"then "\
+-		"  setenv ramdisk_param ${ramdisk_addr_r}; "\
++		"  setenv initrd_param ${initrd_addr_r}; "\
+ 		"else "\
+-		"  setenv ramdisk_param -; "\
++		"  setenv initrd_param -; "\
+ 		"fi ; " \
+-		"booti ${kernel_addr_r} ${ramdisk_param} ${fdt_addr_r}\0"
++		"booti ${kernel_addr_r} ${initrd_param} ${fdt_addr_r}"
+ #define BOOTENV_DEV_NAME_AFS(devtypeu, devtypel, instance) "afs "
+ 
+ #define BOOT_TARGET_DEVICES(func)	\
+@@ -164,8 +164,8 @@
+ 				"kernel_name=norkern\0"	\
+ 				"kernel_alt_name=Image\0"	\
+ 				"kernel_addr_r=0x80080000\0" \
+-				"ramdisk_name=ramdisk.img\0"	\
+-				"ramdisk_addr_r=0x88000000\0"	\
++				"initrd_name=ramdisk.img\0"	\
++				"initrd_addr_r=0x88000000\0"	\
+ 				"fdtfile=board.dtb\0" \
+ 				"fdt_alt_name=juno\0" \
+ 				"fdt_addr_r=0x80000000\0" \
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch
new file mode 100644
index 0000000..85b14b0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch
@@ -0,0 +1,33 @@
+From f7c24393604e45012447b16aaa95eb5e7224ba07 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 12 Apr 2022 12:43:49 +0100
+Subject: [PATCH] arm: total_compute: update secure dram size
+
+Update secure DRAM size as it is increased by 64MB for additional
+secure partitions.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Id8ce99c7a5330d3c28d473009c4db04141e6fa4d
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/configs/total_compute.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/include/configs/total_compute.h b/include/configs/total_compute.h
+index 0324b1e1b217..62bdb4f6a3ae 100644
+--- a/include/configs/total_compute.h
++++ b/include/configs/total_compute.h
+@@ -23,8 +23,8 @@
+ 
+ /* Physical Memory Map */
+ #define PHYS_SDRAM_1		0x80000000
+-/* Top 48MB reserved for secure world use */
+-#define DRAM_SEC_SIZE		0x03000000
++/* Top 112MB reserved for secure world use */
++#define DRAM_SEC_SIZE		0x07000000
+ #define PHYS_SDRAM_1_SIZE	0x80000000 - DRAM_SEC_SIZE
+ #define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg
new file mode 100644
index 0000000..8c31602
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg
@@ -0,0 +1,2 @@
+CONFIG_USE_BOOTARGS=y
+CONFIG_BOOTARGS="console=ttyAMA0 debug user_debug=31 earlycon=pl011,0x7ff80000 loglevel=9 androidboot.hardware=total_compute androidboot.boot_devices=1c050000.mmci ip=dhcp androidboot.selinux=permissive allow_mismatched_32bit_el0"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend
new file mode 100644
index 0000000..e254d41
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend
@@ -0,0 +1,90 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/${PN}:"
+
+#
+# Corstone-500 MACHINE
+#
+SRC_URI:append:corstone500 = " \
+                   file://0001-armv7-adding-generic-timer-access-through-MMIO.patch \
+                   file://0002-board-arm-add-corstone500-board.patch"
+
+#
+# Corstone1000 64-bit machines
+#
+DEPENDS:append:corstone1000 = " gnutls-native"
+CORSTONE1000_DEVICE_TREE:corstone1000-mps3 = "corstone1000-mps3"
+CORSTONE1000_DEVICE_TREE:corstone1000-fvp = "corstone1000-fvp"
+EXTRA_OEMAKE:append:corstone1000 = ' DEVICE_TREE=${CORSTONE1000_DEVICE_TREE}'
+
+SYSROOT_DIRS:append:corstone1000 = " /boot"
+
+SRC_URI:append:corstone1000 = " \
+        file://0001-cmd-load-add-load-command-for-memory-mapped.patch \
+        file://0002-arm-add-support-to-corstone1000-platform.patch \
+        file://0003-usb-common-move-urb-code-to-common.patch \
+        file://0004-usb-add-isp1760-family-driver.patch \
+        file://0005-corstone1000-enable-isp1763-usb-controller.patch \
+        file://0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch \
+        file://0007-arm_ffa-introducing-armffa-command.patch \
+        file://0008-arm_ffa-introducing-MM-communication-with-FF-A.patch \
+        file://0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch \
+        file://0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch \
+        file://0011-efi-corstone1000-introduce-EFI-capsule-update.patch \
+        file://0012-corstone1000-Update-FFA-shared-buffer-address.patch \
+        file://0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch \
+        file://0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch \
+        file://0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch \
+        file://0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch \
+        file://0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch \
+        file://0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch \
+        file://0019-Return-proper-error-code-when-rx-buffer-is-larger.patch \
+        file://0020-Use-correct-buffer-size.patch \
+        file://0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch \
+        file://0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch \
+        file://0023-efi_firmware-add-get_image_info-for-corstone1000.patch \
+        file://0024-Comment-mm_communicate-failure-log.patch \
+        file://0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch \
+        file://0026-efi_loader-fix-null-pointer-exception-with-get_image.patch \
+        file://0027-arm-corstone1000-add-mmc-for-fvp.patch \
+      "
+
+#
+# FVP BASE
+#
+SRC_URI:append:fvp-base = " file://bootargs.cfg"
+
+#
+# FVP BASE ARM32
+#
+SRC_URI:append:fvp-base-arm32 = " file://0001-Add-vexpress_aemv8a_aarch32-variant.patch \
+				  file://0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch \
+				"
+
+#
+# FVP BASER
+#
+SRC_URI:append:fvp-baser-aemv8r64 = " \
+    file://0001-armv8-Add-ARMv8-MPU-configuration-logic.patch \
+    file://0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch \
+    file://0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch \
+    file://0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch \
+    file://0005-armv8-Make-disabling-HVC-configurable-when-switching.patch \
+    file://0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch \
+    file://0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch \
+    file://0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch \
+    file://0009-armv8-Enable-icache-when-switching-exception-levels-.patch \
+    "
+
+
+#
+# Juno Machines
+#
+SRC_URI:append:juno = " file://0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch"
+
+
+#
+# TC0 and TC1 MACHINES
+#
+SRC_URI:append:tc = " \
+        file://bootargs.cfg \
+        file://0001-arm-total_compute-update-secure-dram-size.patch \
+        "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb
new file mode 100644
index 0000000..bc2d6d4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb
@@ -0,0 +1,26 @@
+HOMEPAGE = "http://www.denx.de/wiki/U-Boot/WebHome"
+DESCRIPTION = "U-Boot, a boot loader for Embedded boards based on PowerPC, \
+ARM, MIPS and several other processors, which can be installed in a boot \
+ROM and used to initialize and test the hardware or to download and run \
+application code."
+SECTION = "bootloaders"
+DEPENDS += "flex-native bison-native"
+
+LICENSE = "GPL-2.0-or-later"
+LIC_FILES_CHKSUM = "file://Licenses/README;md5=5a7450c57ffe5ae63fd732446b988025"
+PE = "1"
+
+# We use the revision in order to avoid having to fetch it from the
+# repo during parse
+SRCREV = "e4b6ebd3de982ae7185dbf689a030e73fd06e0d2"
+
+SRC_URI = "git://git.denx.de/u-boot.git;branch=master \
+          "
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+do_configure[cleandirs] = "${B}"
+
+require recipes-bsp/u-boot/u-boot.inc
+
+DEPENDS += "bc-native dtc-native"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc
new file mode 100644
index 0000000..7069c4d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc
@@ -0,0 +1,6 @@
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "ArmVExpress-FVP-AArch64"
+EDK2_PLATFORM_DSC  = "Platform/ARM/VExpressPkg/ArmVExpress-FVP-AArch64.dsc"
+EDK2_BIN_NAME      = "FVP_AARCH64_EFI.fd"
+
+COMPATIBLE_MACHINE = "fvp-base"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc
new file mode 100644
index 0000000..aac0d1c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc
@@ -0,0 +1,9 @@
+EDK2_PLATFORM      = "ArmJuno"
+EDK2_PLATFORM_DSC  = "Platform/ARM/JunoPkg/ArmJuno.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "juno"
+
+# As of 2022-06-14 with 2022.05, clang builds fail:
+# "The required fv image size 0x104048 exceeds the set fv image size 0xf9000"
+TOOLCHAIN = "gcc"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc
new file mode 100644
index 0000000..90c3f2a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc
@@ -0,0 +1,23 @@
+# N1SDP specific EDK2 configurations
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "n1sdp"
+EDK2_PLATFORM_DSC  = "Platform/ARM/N1Sdp/N1SdpPlatform.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "n1sdp"
+
+# UEFI EDK2 on N1SDP is unable to detect FS2 during boot resulting in launching of
+# EDK2 shell instead of launching grub. The startup.nsh will force launching of grub
+EFIDIR             = "/EFI/BOOT"
+EFI_BOOT_IMAGE     = "bootaa64.efi"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/edk2-platforms:"
+
+SRC_URI:append = "\
+    file://add-nt-fw-config.patch;patchdir=edk2-platforms \
+"
+
+do_deploy:append() {
+    EFIPATH=$(echo "${EFIDIR}" | sed 's/\//\\/g')
+    printf 'FS2:%s\%s\n' "$EFIPATH" "${EFI_BOOT_IMAGE}" > ${DEPLOYDIR}/startup.nsh
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc
new file mode 100644
index 0000000..e26225f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc
@@ -0,0 +1,7 @@
+# SGI575 specific EDK2 configurations
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "Sgi575"
+EDK2_PLATFORM_DSC  = "Platform/ARM/SgiPkg/Sgi575/Sgi575.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "sgi575"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend
new file mode 100644
index 0000000..e5018bb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend
@@ -0,0 +1,10 @@
+# Include machine specific configurations for UEFI EDK2
+
+MACHINE_EDK2_REQUIRE ?= ""
+
+MACHINE_EDK2_REQUIRE:fvp-base = "edk2-firmware-fvp-base.inc"
+MACHINE_EDK2_REQUIRE:juno = "edk2-firmware-juno.inc"
+MACHINE_EDK2_REQUIRE:sgi575 = "edk2-firmware-sgi575.inc"
+MACHINE_EDK2_REQUIRE:n1sdp = "edk2-firmware-n1sdp.inc"
+
+require ${MACHINE_EDK2_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch
new file mode 100644
index 0000000..f6f1895
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch
@@ -0,0 +1,474 @@
+From cc58709b32d74273736886ccfc08e4723a436ea4 Mon Sep 17 00:00:00 2001
+From: sahil <sahil@arm.com>
+Date: Thu, 17 Mar 2022 16:28:05 +0530
+Subject: [PATCH] Platform/ARM/N1sdp: Add support to parse NT_FW_CONFIG
+
+NT_FW_CONFIG DTB contains platform information passed by
+Tf-A boot stage.
+This information is used for Virtual memory map generation
+during PEI phase and passed on to DXE phase as a HOB, where
+it is used in ConfigurationManagerDxe.
+
+Upstream-Status: Pending
+Signed-off-by: Adam Johnston <adam.johnston@arm.com>
+Signed-off-by: sahil <sahil@arm.com>
+Change-Id: Ib82571280bf1ca5febe5766e618de09e7b70bb02
+
+---
+ .../ConfigurationManager.c                    |  24 ++--
+ .../ConfigurationManagerDxe.inf               |   3 +-
+ .../ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h |  16 +--
+ .../Library/PlatformLib/AArch64/Helper.S      |   4 +-
+ .../Library/PlatformLib/PlatformLib.c         |  12 +-
+ .../Library/PlatformLib/PlatformLib.inf       |   8 +-
+ .../Library/PlatformLib/PlatformLibMem.c      | 103 +++++++++++++++++-
+ Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec   |   7 +-
+ 8 files changed, 152 insertions(+), 25 deletions(-)
+
+diff --git a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
+index f50623ae3f..e023d47cfd 100644
+--- a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
++++ b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
+@@ -1,7 +1,7 @@
+ /** @file
+   Configuration Manager Dxe
+ 
+-  Copyright (c) 2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2021 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -16,6 +16,7 @@
+ #include <IndustryStandard/SerialPortConsoleRedirectionTable.h>
+ #include <Library/ArmLib.h>
+ #include <Library/DebugLib.h>
++#include <Library/HobLib.h>
+ #include <Library/IoLib.h>
+ #include <Library/PcdLib.h>
+ #include <Library/UefiBootServicesTableLib.h>
+@@ -28,6 +29,7 @@
+ #include "Platform.h"
+ 
+ extern struct EFI_ACPI_HETEROGENEOUS_MEMORY_ATTRIBUTE_TABLE Hmat;
++static NEOVERSEN1SOC_PLAT_INFO *PlatInfo;
+ 
+ /** The platform configuration repository information.
+ */
+@@ -1242,13 +1244,11 @@ InitializePlatformRepository (
+   IN  EDKII_PLATFORM_REPOSITORY_INFO  * CONST PlatRepoInfo
+   )
+ {
+-  NEOVERSEN1SOC_PLAT_INFO       *PlatInfo;
+   UINT64                        Dram2Size;
+   UINT64                        RemoteDdrSize;
+ 
+   RemoteDdrSize = 0;
+ 
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+   Dram2Size = ((PlatInfo->LocalDdrSize - 2) * SIZE_1GB);
+ 
+   PlatRepoInfo->MemAffInfo[LOCAL_DDR_REGION2].Length = Dram2Size;
+@@ -1512,7 +1512,6 @@ GetGicCInfo (
+   )
+ {
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            TotalObjCount;
+   UINT32                            ObjIndex;
+ 
+@@ -1523,7 +1522,6 @@ GetGicCInfo (
+   }
+ 
+   PlatformRepo = This->PlatRepoInfo;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+ 
+   if (PlatInfo->MultichipMode == 1) {
+     TotalObjCount = PLAT_CPU_COUNT * 2;
+@@ -1623,7 +1621,6 @@ GetStandardNameSpaceObject (
+ {
+   EFI_STATUS                        Status;
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            AcpiTableCount;
+ 
+   if ((This == NULL) || (CmObject == NULL)) {
+@@ -1634,7 +1631,7 @@ GetStandardNameSpaceObject (
+ 
+   Status = EFI_NOT_FOUND;
+   PlatformRepo = This->PlatRepoInfo;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
++
+   AcpiTableCount = ARRAY_SIZE (PlatformRepo->CmAcpiTableList);
+   if (PlatInfo->MultichipMode == 0)
+         AcpiTableCount -= 1;
+@@ -1697,7 +1694,6 @@ GetArmNameSpaceObject (
+ {
+   EFI_STATUS                        Status;
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            GicRedistCount;
+   UINT32                            GicCpuCount;
+   UINT32                            ProcHierarchyInfoCount;
+@@ -1718,8 +1714,6 @@ GetArmNameSpaceObject (
+   Status = EFI_NOT_FOUND;
+   PlatformRepo = This->PlatRepoInfo;
+ 
+-  // Probe for multi chip information
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+   if (PlatInfo->MultichipMode == 1) {
+     GicRedistCount = 2;
+     GicCpuCount = PLAT_CPU_COUNT * 2;
+@@ -2162,8 +2156,18 @@ ConfigurationManagerDxeInitialize (
+   IN EFI_SYSTEM_TABLE  * SystemTable
+   )
+ {
++  VOID *PlatInfoHob;
+   EFI_STATUS  Status;
+ 
++  PlatInfoHob = GetFirstGuidHob (&gArmNeoverseN1SocPlatformInfoDescriptorGuid);
++
++  if (PlatInfoHob == NULL) {
++    DEBUG ((DEBUG_ERROR, "Platform HOB is NULL\n"));
++    return EFI_NOT_FOUND;
++  }
++
++  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)GET_GUID_HOB_DATA (PlatInfoHob);
++
+   // Initialize the Platform Configuration Repository before installing the
+   // Configuration Manager Protocol
+   Status = InitializePlatformRepository (
+diff --git a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
+index 4f8e7f1302..fb59c29501 100644
+--- a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
++++ b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Configuration Manager Dxe
+ #
+-#  Copyright (c) 2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2021 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -42,6 +42,7 @@
+ 
+ [LibraryClasses]
+   ArmPlatformLib
++  HobLib
+   PrintLib
+   UefiBootServicesTableLib
+   UefiDriverEntryPoint
+diff --git a/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h b/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
+index 097160c7e2..63cebaf0e0 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
++++ b/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
+@@ -1,6 +1,6 @@
+ /** @file
+ *
+-* Copyright (c) 2018 - 2020, ARM Limited. All rights reserved.
++* Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-2-Clause-Patent
+ *
+@@ -41,11 +41,6 @@
+ #define NEOVERSEN1SOC_EXP_PERIPH_BASE0               0x1C000000
+ #define NEOVERSEN1SOC_EXP_PERIPH_BASE0_SZ            0x1300000
+ 
+-// Base address to a structure of type NEOVERSEN1SOC_PLAT_INFO which is
+-// pre-populated by a earlier boot stage
+-#define NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE          (NEOVERSEN1SOC_NON_SECURE_SRAM_BASE + \
+-                                                      0x00008000)
+-
+ /*
+  * Platform information structure stored in Non-secure SRAM. Platform
+  * information are passed from the trusted firmware with the below structure
+@@ -55,12 +50,17 @@
+ typedef struct {
+   /*! 0 - Single Chip, 1 - Chip to Chip (C2C) */
+   UINT8   MultichipMode;
+-  /*! Slave count in C2C mode */
+-  UINT8   SlaveCount;
++  /*! Secondary chip count in C2C mode */
++  UINT8   SecondaryChipCount;
+   /*! Local DDR memory size in GigaBytes */
+   UINT8   LocalDdrSize;
+   /*! Remote DDR memory size in GigaBytes */
+   UINT8   RemoteDdrSize;
+ } NEOVERSEN1SOC_PLAT_INFO;
+ 
++// NT_FW_CONFIG DT structure
++typedef struct {
++  UINT64                  NtFwConfigDtAddr;
++} NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI;
++
+ #endif
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
+index 8d2069dea8..88ed640d29 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
+@@ -1,6 +1,6 @@
+ /** @file
+ *
+-*  Copyright (c) 2019 - 2020, ARM Limited. All rights reserved.
++*  Copyright (c) 2019 - 2022, ARM Limited. All rights reserved.
+ *
+ *  SPDX-License-Identifier: BSD-2-Clause-Patent
+ *
+@@ -25,6 +25,8 @@ GCC_ASM_EXPORT(ArmPlatformIsPrimaryCore)
+ // the UEFI firmware through the CPU registers.
+ //
+ ASM_PFX(ArmPlatformPeiBootAction):
++  adr  x10, NtFwConfigDtBlob
++  str  x0, [x10]
+   ret
+ 
+ //
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
+index c0effd37f3..fabe902cd0 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
+@@ -1,6 +1,6 @@
+ /** @file
+ 
+-  Copyright (c) 2018-2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -8,8 +8,12 @@
+ 
+ #include <Library/ArmPlatformLib.h>
+ #include <Library/BaseLib.h>
++#include <NeoverseN1Soc.h>
+ #include <Ppi/ArmMpCoreInfo.h>
+ 
++UINT64 NtFwConfigDtBlob;
++STATIC NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI mNtFwConfigDtInfoPpi;
++
+ STATIC ARM_CORE_INFO mCoreInfoTable[] = {
+   { 0x0, 0x0 }, // Cluster 0, Core 0
+   { 0x0, 0x1 }, // Cluster 0, Core 1
+@@ -46,6 +50,7 @@ ArmPlatformInitialize (
+   IN     UINTN                  MpId
+   )
+ {
++  mNtFwConfigDtInfoPpi.NtFwConfigDtAddr = NtFwConfigDtBlob;
+   return RETURN_SUCCESS;
+ }
+ 
+@@ -80,6 +85,11 @@ EFI_PEI_PPI_DESCRIPTOR gPlatformPpiTable[] = {
+     EFI_PEI_PPI_DESCRIPTOR_PPI,
+     &gArmMpCoreInfoPpiGuid,
+     &mMpCoreInfoPpi
++  },
++  {
++    EFI_PEI_PPI_DESCRIPTOR_PPI,
++    &gNtFwConfigDtInfoPpiGuid,
++    &mNtFwConfigDtInfoPpi
+   }
+ };
+ 
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
+index 96e590cdd8..6f9c9d5ab6 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Platform Library for N1Sdp.
+ #
+-#  Copyright (c) 2018-2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -18,10 +18,14 @@
+ [Packages]
+   ArmPkg/ArmPkg.dec
+   ArmPlatformPkg/ArmPlatformPkg.dec
++  EmbeddedPkg/EmbeddedPkg.dec
+   MdeModulePkg/MdeModulePkg.dec
+   MdePkg/MdePkg.dec
+   Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+ 
++[LibraryClasses]
++  FdtLib
++
+ [Sources.common]
+   PlatformLibMem.c
+   PlatformLib.c
+@@ -59,7 +63,9 @@
+   gEfiMdePkgTokenSpaceGuid.PcdPciExpressBaseAddress
+ 
+ [Guids]
++  gArmNeoverseN1SocPlatformInfoDescriptorGuid
+   gEfiHobListGuid          ## CONSUMES  ## SystemTable
+ 
+ [Ppis]
+   gArmMpCoreInfoPpiGuid
++  gNtFwConfigDtInfoPpiGuid
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
+index 339fa07b32..b58bda4b76 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
+@@ -1,6 +1,6 @@
+ /** @file
+ 
+-  Copyright (c) 2018 - 2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -10,11 +10,95 @@
+ #include <Library/DebugLib.h>
+ #include <Library/HobLib.h>
+ #include <Library/MemoryAllocationLib.h>
++#include <Library/PeiServicesLib.h>
++#include <libfdt.h>
+ #include <NeoverseN1Soc.h>
+ 
+ // The total number of descriptors, including the final "end-of-table" descriptor.
+ #define MAX_VIRTUAL_MEMORY_MAP_DESCRIPTORS 19
+ 
++/** A helper function to locate the NtFwConfig PPI and get the base address of
++  NT_FW_CONFIG DT from which values are obtained using FDT helper functions.
++
++  @param [out]  plat_info  Pointer to the NeoverseN1Soc PLATFORM_INFO HOB
++
++  @retval EFI_SUCCESS            Success.
++  returns EFI_INVALID_PARAMETER  A parameter is invalid.
++**/
++EFI_STATUS
++GetNeoverseN1SocPlatInfo (
++  OUT NEOVERSEN1SOC_PLAT_INFO *plat_info
++  )
++{
++  CONST UINT32                   *Property;
++  INT32                          Offset;
++  CONST VOID                     *NtFwCfgDtBlob;
++  NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI  *NtFwConfigInfoPpi;
++  EFI_STATUS                     Status;
++
++  Status = PeiServicesLocatePpi (
++             &gNtFwConfigDtInfoPpiGuid,
++             0,
++             NULL,
++             (VOID **)&NtFwConfigInfoPpi
++             );
++
++  if (EFI_ERROR (Status)) {
++    DEBUG ((
++      DEBUG_ERROR,
++      "PeiServicesLocatePpi failed with error %r\n",
++      Status
++      ));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  NtFwCfgDtBlob = (VOID *)(UINTN)NtFwConfigInfoPpi->NtFwConfigDtAddr;
++  if (fdt_check_header (NtFwCfgDtBlob) != 0) {
++    DEBUG ((DEBUG_ERROR, "Invalid DTB file %p passed\n", NtFwCfgDtBlob));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  Offset = fdt_subnode_offset (NtFwCfgDtBlob, 0, "platform-info");
++  if (Offset == -FDT_ERR_NOTFOUND) {
++    DEBUG ((DEBUG_ERROR, "Invalid DTB : platform-info node not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "local-ddr-size", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "local-ddr-size property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->LocalDdrSize = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "remote-ddr-size", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "remote-ddr-size property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->RemoteDdrSize = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "secondary-chip-count", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "secondary-chip-count property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->SecondaryChipCount = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "multichip-mode", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "multichip-mode property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->MultichipMode = fdt32_to_cpu (*Property);
++
++  return EFI_SUCCESS;
++}
++
+ /**
+   Returns the Virtual Memory Map of the platform.
+ 
+@@ -36,9 +120,24 @@ ArmPlatformGetVirtualMemoryMap (
+   NEOVERSEN1SOC_PLAT_INFO       *PlatInfo;
+   UINT64                        DramBlock2Size;
+   UINT64                        RemoteDdrSize;
++  EFI_STATUS                    Status;
+ 
+   Index = 0;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
++
++  // Create platform info HOB
++  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)BuildGuidHob (
++                                        &gArmNeoverseN1SocPlatformInfoDescriptorGuid,
++                                        sizeof (NEOVERSEN1SOC_PLAT_INFO)
++                                        );
++
++  if (PlatInfo == NULL) {
++    DEBUG ((DEBUG_ERROR, "Platform HOB is NULL\n"));
++    ASSERT (FALSE);
++    return;
++  }
++
++  Status = GetNeoverseN1SocPlatInfo (PlatInfo);
++  ASSERT (Status == 0);
+   DramBlock2Size = ((UINT64)(PlatInfo->LocalDdrSize -
+                              NEOVERSEN1SOC_DRAM_BLOCK1_SIZE / SIZE_1GB) *
+                             (UINT64)SIZE_1GB);
+diff --git a/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec b/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+index d59f25a5b9..4dea8fe1e8 100644
+--- a/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
++++ b/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Describes the entire platform configuration.
+ #
+-#  Copyright (c) 2018 - 2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -22,6 +22,8 @@
+   Include                        # Root include for the package
+ 
+ [Guids.common]
++  # ARM NeoverseN1Soc Platform Info descriptor
++  gArmNeoverseN1SocPlatformInfoDescriptorGuid = { 0x095cb024, 0x1e00, 0x4d6f, { 0xaa, 0x34, 0x4a, 0xf8, 0xaf, 0x0e, 0xad, 0x99 } }
+   gArmNeoverseN1SocTokenSpaceGuid = { 0xab93eb78, 0x60d7, 0x4099, { 0xac, 0xeb, 0x6d, 0xb5, 0x02, 0x58, 0x7c, 0x24 } }
+ 
+ [PcdsFixedAtBuild]
+@@ -83,3 +85,6 @@
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieMmio32Translation|0x40000000000|UINT64|0x0000004F
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieMmio64Translation|0x40000000000|UINT64|0x00000050
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieSegmentNumber|2|UINT32|0x00000051
++
++[Ppis]
++  gNtFwConfigDtInfoPpiGuid =  { 0xb50dee0e, 0x577f, 0x47fb, { 0x83, 0xd0, 0x41, 0x78, 0x61, 0x8b, 0x33, 0x8a } }
+-- 
+2.17.1
+