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"